Port Pick/Place demo to ROS 2

CMakeLists.txt

@@ -37,8 +37,6 @@ add_library(${PROJECT_NAME}
   src/get_mtc_raw_stage.cpp
   src/initialize_mtc_task.cpp
   src/moveit_msgs.cpp
-  src/move_mtc_stage_to_container.cpp
-  src/move_mtc_container_to_parent_container.cpp
   src/plan_mtc_task.cpp
 )
 
@@ -52,51 +50,56 @@ ament_target_dependencies(${PROJECT_NAME}
 )
 
 add_executable(bt_allow_collisions examples/bt_allow_collisions.cpp)
-target_link_libraries(bt_allow_collisions ${PROJECT_NAME} )
-
-add_executable(bt_create_collision_object examples/bt_create_collision_object.cpp)
-target_link_libraries(bt_create_collision_object ${PROJECT_NAME} )
-
 add_executable(bt_compute_ik examples/bt_compute_ik.cpp)
-target_link_libraries(bt_compute_ik ${PROJECT_NAME} )
-
+add_executable(bt_create_collision_object examples/bt_create_collision_object.cpp)
 add_executable(bt_demo examples/bt_demo.cpp)
-target_link_libraries(bt_demo ${PROJECT_NAME} )
-
 add_executable(bt_generate_place_pose examples/bt_generate_place_pose.cpp)
-target_link_libraries(bt_generate_place_pose ${PROJECT_NAME} )
-
 add_executable(bt_joint_interpolation_solver examples/bt_joint_interpolation_solver.cpp)
-target_link_libraries(bt_joint_interpolation_solver ${PROJECT_NAME} )
-
 add_executable(bt_move_relative examples/bt_move_relative.cpp)
-target_link_libraries(bt_move_relative ${PROJECT_NAME} )
-
 add_executable(bt_move_to examples/bt_move_to.cpp)
-target_link_libraries(bt_move_to ${PROJECT_NAME} )
-
+add_executable(bt_pick_place examples/bt_pick_place.cpp)
 add_executable(bt_serial_container examples/bt_serial_container.cpp)
-target_link_libraries(bt_serial_container ${PROJECT_NAME} )
+add_executable(generate_xml_model src/generate_xml_model.cpp)
+
+target_link_libraries(bt_allow_collisions ${PROJECT_NAME})
+target_link_libraries(bt_compute_ik ${PROJECT_NAME})
+target_link_libraries(bt_create_collision_object ${PROJECT_NAME})
+target_link_libraries(bt_demo ${PROJECT_NAME})
+target_link_libraries(bt_generate_place_pose ${PROJECT_NAME} )
+target_link_libraries(bt_joint_interpolation_solver ${PROJECT_NAME})
+target_link_libraries(bt_move_relative ${PROJECT_NAME})
+target_link_libraries(bt_move_to ${PROJECT_NAME})
+target_link_libraries(bt_pick_place ${PROJECT_NAME})
+target_link_libraries(bt_serial_container ${PROJECT_NAME})
+target_link_libraries(generate_xml_model ${PROJECT_NAME})
+
+
+add_subdirectory(tests)
 
-if(BUILD_TESTING)
-  add_subdirectory(tests)
-endif()
 
 install(
   DIRECTORY include/${PROJECT_NAME}
   DESTINATION include
 )
 
+install(DIRECTORY
+  config
+  launch
+  DESTINATION share/${PROJECT_NAME}/
+)
+
 install(TARGETS 
   bt_allow_collisions
-  bt_create_collision_object
   bt_compute_ik
+  bt_create_collision_object
   bt_demo
   bt_generate_place_pose
   bt_joint_interpolation_solver
   bt_move_relative
   bt_move_to
+  bt_pick_place
   bt_serial_container
+  generate_xml_model
   DESTINATION lib/${PROJECT_NAME}
 )
 

README.md

@@ -4,5 +4,26 @@
 
 This project is compatible with version 4.X of [BehaviorTree.CPP](https://github.com/BehaviorTree/BehaviorTree.CPP).
 
+It is recommanded that the [Moveit Task Constructor](https://github.com/moveit/moveit_task_constructor) library commit version `>=` [fdc06c3](https://github.com/moveit/moveit_task_constructor/commit/fdc06c3b9105dadec2f44ee3e4b3133986945e86). This is to ensure that one can preempt a plan gracefully.
+
+## Demo
+Check the available [examples](./examples).
+
+Running the Pick and Place MTC demo using BT's:
+
+``` sh
+# In one terminal
+$ ros2 launch moveit_task_constructor_demo demo.launch.py
+# ALTERNATE: Use the one supplied with MTC
+#            Don't forget to change the task solution topic
+
+# In another terminal
+$ ros2 launch behaviortree_mtc run.launch.py
+
+
+# OPTIONAL: Connect from Groot2 to visualize the BT's on port 1667
+$ ros2 launch behaviortree_mtc run.launch.py delay_ms:=5000
+```
+
 ## Roadmap
-We plan to release version `1.0.0` by the end of summer 2024. Check the [roadmap](https://github.com/captain-yoshi/BehaviorTree.MTC/issues/15) for more details.
+We plan to release version `1.0.0` by the end of Christmas 2024. Check the [roadmap](https://github.com/captain-yoshi/BehaviorTree.MTC/issues/15) for more details.

examples/bt_allow_collisions.cpp

@@ -2,7 +2,7 @@
 
 #include <behaviortree_mtc/initialize_mtc_task.h>
 #include <behaviortree_mtc/create_mtc_current_state.h>
-#include <behaviortree_mtc/move_mtc_stage_to_container.h>
+#include <behaviortree_mtc/move_mtc_stage.h>
 #include <behaviortree_mtc/plan_mtc_task.h>
 #include <behaviortree_mtc/moveit_msgs.h>
 #include <behaviortree_mtc/geometry_msgs.h>
@@ -62,23 +62,23 @@ static const char* xml_text = R"(
                                                 collision_object="{cylinder}"  />
             <AddObjectToPlanningScene  planning_scene_interface="{psi}" 
                                        collision_object="{cylinder}"/> 
-            <InitializeMTCTask        task="{mtc_task}" container="{task_container}" />
+            <InitializeMTCTask        task="{mtc_task}" />
             <CreateMTCPipelinePlanner pipeline_id="ompl"
                                  planner_id="RRTConnect"
                                  solver="{rrt_connect}"
                                  goal_joint_tolerance="1e-5" />
             <CreateMTCCurrentState    stage="{current_state}" />
-            <MoveMTCStageToContainer  container="{task_container}" stage="{current_state}" />
+            <MoveMTCStageToTask  child="{current_state}" parent="{mtc_task}" />
             <AllowCollisionsJointModelGroup stage_name="allow collisions -> hand,object"
                                             stage="{allow_collisions}"
                                             objects="{object_name}"
                                             jmg_name="{hand_group_name}"
                                             task="{mtc_task}" />
-            <MoveMTCStageToContainer  container="{task_container}"  stage="{allow_collisions}" />
+            <MoveMTCStageToTask  child="{allow_collisions}" parent="{mtc_task}" />
             <ForbidAllCollisions  stage_name="forbid collisions -> hand,object"
                                   stage="{forbid_collisions}"
                                   objects="{object_name}" />
-            <MoveMTCStageToContainer  container="{task_container}"  stage="{forbid_collisions}" />
+            <MoveMTCStageToTask  child="{forbid_collisions}" parent="{mtc_task}" />
             <PlanMTCTask              task="{mtc_task}" max_solutions="5" />
         </Sequence>
      </BehaviorTree>
@@ -95,19 +95,18 @@ int main(int argc, char** argv)
 
   // Declare parameters passed by a launch file
   options.automatically_declare_parameters_from_overrides(true);
-  auto node = rclcpp::Node::make_shared("test_behavior_tree", options);
+  auto node = rclcpp::Node::make_shared("bt_mtc_demo", options);
 
   BehaviorTreeFactory factory;
-
   factory.registerNodeType<GeometryMsgsPose>("GeometryMsgsPose");
   factory.registerNodeType<MoveItMsgsCollisionObjectCylinder>("MoveItMsgsCollisionObjectCylinder");
   factory.registerNodeType<MoveItMsgsCollisionObjectBox>("MoveItMsgsCollisionObjectBox");
   factory.registerNodeType<CreatePlanningSceneInterface>("CreatePlanningSceneInterface");
   factory.registerNodeType<AddObjectToPlanningScene>("AddObjectToPlanningScene");
-  factory.registerNodeType<MoveMTCStageToContainer>("MoveMTCStageToContainer");
   factory.registerNodeType<InitializeMTCTask>("InitializeMTCTask", BT::RosNodeParams(node));
   factory.registerNodeType<CreateMTCCurrentState>("CreateMTCCurrentState");
   factory.registerNodeType<CreateMTCPipelinePlanner>("CreateMTCPipelinePlanner", BT::RosNodeParams(node));
+  factory.registerNodeType<MoveMTCStage<moveit::task_constructor::Stage, moveit::task_constructor::Task>>("MoveMTCStageToTask");
   factory.registerNodeType<PlanMTCTask>("PlanMTCTask");
   factory.registerNodeType<AllowCollisionPairs>("AllowCollisionPairs");
   factory.registerNodeType<ForbidAllCollisions>("ForbidAllCollisions");
@@ -127,12 +126,16 @@ int main(int argc, char** argv)
   BT::FileLogger2 logger2(tree, "t12_logger2.btlog");
 
   // Gives the user time to connect to Groot2
-  int wait_time = 5000;
-  std::cout << "Waiting " << wait_time << " msec for connection with Groot2...\n\n"
-            << std::endl;
-  std::cout << "======================" << std::endl;
-  std::this_thread::sleep_for(std::chrono::milliseconds(wait_time));
-
+  int delay_ms = node->get_parameter("delay_ms").as_int();
+  if(delay_ms)
+  {
+    std::cout << "Waiting " << delay_ms << " msec for connection with Groot2...\n\n"
+              << std::endl;
+    std::cout << "======================" << std::endl;
+    std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
+  }
+
+  // Start ticking the Tree
   std::cout << "Starting Behavior Tree" << std::endl;
   std::cout << "======================" << std::endl;
 

examples/bt_compute_ik.cpp

@@ -2,8 +2,7 @@
 
 #include <behaviortree_mtc/initialize_mtc_task.h>
 #include <behaviortree_mtc/create_mtc_current_state.h>
-#include <behaviortree_mtc/move_mtc_stage_to_container.h>
-#include <behaviortree_mtc/move_mtc_container_to_parent_container.h>
+#include <behaviortree_mtc/move_mtc_stage.h>
 #include <behaviortree_mtc/create_mtc_serial_container.h>
 #include <behaviortree_mtc/plan_mtc_task.h>
 #include <behaviortree_mtc/moveit_msgs.h>
@@ -69,33 +68,33 @@ static const char* xml_text = R"(
                                                 collision_object="{cylinder}"  />
             <AddObjectToPlanningScene  planning_scene_interface="{psi}" 
                          collision_object="{cylinder}"/> 
-            <InitializeMTCTask        task="{mtc_task}" container="{task_container}" />
+            <InitializeMTCTask        task="{mtc_task}" />
             <CreateMTCPipelinePlanner pipeline_id="ompl"
                                  planner_id="RRTConnect"
                                  solver="{rrt_connect}"
                                  goal_joint_tolerance="1e-5" />
             <CreateMTCCurrentState    stage="{current_state}" />
-            <MoveMTCStageToContainer  container="{task_container}" stage="{current_state}" />
+            <MoveMTCStageToTask  child="{current_state}" parent="{mtc_task}" />
             <GeometryMsgsPoseStamped  frame_id="{hand_frame}" position="0,0,0" quaternion="1,0,0,0" pose_stamped="{ik_frame}"/>
             <CreateMTCMoveToNamedJointPose name="move to -> close_hand motion"
                                       group="{hand_group_name}"
                                       solver="{rrt_connect}"
                                       goal="close"
                                       stage="{stage_move_to_close_hand}" />
-            <MoveMTCStageToContainer  container="{task_container}" stage="{stage_move_to_close_hand}" />
+            <MoveMTCStageToTask  child="{stage_move_to_close_hand}" parent="{mtc_task}" />
             <CreateMTCMoveToNamedJointPose name="move to -> open_hand motion"
                                       group="{hand_group_name}"
                                       solver="{rrt_connect}"
                                       goal="open"
                                       stage="{stage_move_to_open_hand}" />
-            <GetMTCRawStage  stage="{stage_move_to_open_hand}" raw_stage="{raw_stage}"  />
-            <MoveMTCStageToContainer  container="{task_container}" stage="{stage_move_to_open_hand}" />
+            <GetMTCRawStage  stage="{stage_move_to_open_hand}" raw_stage="{raw_stage}" />
+            <MoveMTCStageToTask  child="{stage_move_to_open_hand}" parent="{mtc_task}" />
             <CreateMTCConnect  stage_name="MoveToPick"
                                timeout="5.0"
                                group="{arm_group_name}"
                                planner="{rrt_connect}"
                                stage="{connect}" />
-            <MoveMTCStageToContainer  container="{task_container}" stage="{connect}" />  
+            <MoveMTCStageToTask  child="{connect}" parent="{mtc_task}" />
             <CreateMTCSerialContainer  container_name="pick object"
                                        container="{pick_object}" />
             <GeometryMsgsVector3Stamped  frame_id="{hand_frame}" vector="0,0,1" vector3_stamped="{tcp_translate}"/>
@@ -107,7 +106,7 @@ static const char* xml_text = R"(
                                              direction="{tcp_translate}"
                                              min_distance="0.1"
                                              max_distance="0.15" />
-            <MoveMTCStageToContainer  container="{pick_object}" stage="{approach_object}" />    
+            <MoveMTCStageToContainer  child="{approach_object}" parent="{pick_object}" />
             <CreateMTCGenerateGraspPose  stage_name="generate_grasp_pose"
                                          eef="{eef}"
                                          object="{object_name}"
@@ -127,8 +126,8 @@ static const char* xml_text = R"(
             <ConfigureInitFromMTCProperties  stage="{grasp_pose_IK}"
                                              property_names="target_pose"
                                              source_flag="INTERFACE" />
-            <MoveMTCStageToContainer  container="{pick_object}" stage="{grasp_pose_IK}" />
-            <MoveMTCContainerToParentContainer parent_container="{task_container}"  child_container="{pick_object}" />
+            <MoveMTCStageToContainer child="{grasp_pose_IK}" parent="{pick_object}" />
+            <MoveMTCContainerToTask  child="{pick_object}" parent="{mtc_task}" />
             <PlanMTCTask              task="{mtc_task}" max_solutions="5" />
         </Sequence>
      </BehaviorTree>
@@ -144,10 +143,9 @@ int main(int argc, char** argv)
 
   // Declare parameters passed by a launch file
   options.automatically_declare_parameters_from_overrides(true);
-  auto node = rclcpp::Node::make_shared("test_behavior_tree", options);
+  auto node = rclcpp::Node::make_shared("bt_mtc_demo", options);
 
   BehaviorTreeFactory factory;
-
   factory.registerNodeType<GeometryMsgsPose>("GeometryMsgsPose");
   factory.registerNodeType<GeometryMsgsPoseStamped>("GeometryMsgsPoseStamped");
   factory.registerNodeType<GeometryMsgsVector3Stamped>("GeometryMsgsVector3Stamped");
@@ -156,15 +154,16 @@ int main(int argc, char** argv)
   factory.registerNodeType<CreatePlanningSceneInterface>("CreatePlanningSceneInterface");
   factory.registerNodeType<AddObjectToPlanningScene>("AddObjectToPlanningScene");
   factory.registerNodeType<CreateMTCGenerateGraspPose>("CreateMTCGenerateGraspPose");
-  factory.registerNodeType<MoveMTCStageToContainer>("MoveMTCStageToContainer");
   factory.registerNodeType<InitializeMTCTask>("InitializeMTCTask", BT::RosNodeParams(node));
   factory.registerNodeType<CreateMTCCurrentState>("CreateMTCCurrentState");
   factory.registerNodeType<CreateMTCPipelinePlanner>("CreateMTCPipelinePlanner", BT::RosNodeParams(node));
   factory.registerNodeType<PlanMTCTask>("PlanMTCTask");
   factory.registerNodeType<GetMTCRawStage>("GetMTCRawStage");
   factory.registerNodeType<CreateMTCMoveToNamedJointPose>("CreateMTCMoveToNamedJointPose");
   factory.registerNodeType<CreateMTCConnect>("CreateMTCConnect");
-  factory.registerNodeType<MoveMTCContainerToParentContainer>("MoveMTCContainerToParentContainer");
+  factory.registerNodeType<MoveMTCStage<moveit::task_constructor::Stage, moveit::task_constructor::ContainerBase>>("MoveMTCStageToContainer");
+  factory.registerNodeType<MoveMTCStage<moveit::task_constructor::Stage, moveit::task_constructor::Task>>("MoveMTCStageToTask");
+  factory.registerNodeType<MoveMTCStage<moveit::task_constructor::ContainerBase, moveit::task_constructor::Task>>("MoveMTCContainerToTask");
   factory.registerNodeType<CreateMTCSerialContainer>("CreateMTCSerialContainer");
   factory.registerNodeType<CreateMTCMoveRelativeTranslate>("CreateMTCMoveRelativeTranslate");
   factory.registerNodeType<CreateMTCComputeIK>("CreateMTCComputeIK");
@@ -185,12 +184,16 @@ int main(int argc, char** argv)
   BT::FileLogger2 logger2(tree, "t12_logger2.btlog");
 
   // Gives the user time to connect to Groot2
-  int wait_time = 5000;
-  std::cout << "Waiting " << wait_time << " msec for connection with Groot2...\n\n"
-            << std::endl;
-  std::cout << "======================" << std::endl;
-  std::this_thread::sleep_for(std::chrono::milliseconds(wait_time));
-
+  int delay_ms = node->get_parameter("delay_ms").as_int();
+  if(delay_ms)
+  {
+    std::cout << "Waiting " << delay_ms << " msec for connection with Groot2...\n\n"
+              << std::endl;
+    std::cout << "======================" << std::endl;
+    std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
+  }
+
+  // Start ticking the Tree
   std::cout << "Starting Behavior Tree" << std::endl;
   std::cout << "======================" << std::endl;
 

examples/bt_create_collision_object.cpp

@@ -1,9 +1,5 @@
 #include <rclcpp/rclcpp.hpp>
 
-#include <behaviortree_mtc/initialize_mtc_task.h>
-#include <behaviortree_mtc/create_mtc_current_state.h>
-#include <behaviortree_mtc/move_mtc_stage_to_container.h>
-#include <behaviortree_mtc/plan_mtc_task.h>
 #include <behaviortree_mtc/moveit_msgs.h>
 #include <behaviortree_mtc/geometry_msgs.h>
 #include <behaviortree_mtc/create_planning_scene_interface.h>
@@ -65,10 +61,9 @@ int main(int argc, char** argv)
 
   // Declare parameters passed by a launch file
   options.automatically_declare_parameters_from_overrides(true);
-  auto node = rclcpp::Node::make_shared("test_behavior_tree", options);
+  auto node = rclcpp::Node::make_shared("bt_mtc_demo", options);
 
   BehaviorTreeFactory factory;
-
   factory.registerNodeType<GeometryMsgsPose>("GeometryMsgsPose");
   factory.registerNodeType<MoveItMsgsCollisionObjectCylinder>("MoveItMsgsCollisionObjectCylinder");
   factory.registerNodeType<MoveItMsgsCollisionObjectBox>("MoveItMsgsCollisionObjectBox");
@@ -89,12 +84,16 @@ int main(int argc, char** argv)
   BT::FileLogger2 logger2(tree, "t12_logger2.btlog");
 
   // Gives the user time to connect to Groot2
-  int wait_time = 5000;
-  std::cout << "Waiting " << wait_time << " msec for connection with Groot2...\n\n"
-            << std::endl;
-  std::cout << "======================" << std::endl;
-  std::this_thread::sleep_for(std::chrono::milliseconds(wait_time));
-
+  int delay_ms = node->get_parameter("delay_ms").as_int();
+  if(delay_ms)
+  {
+    std::cout << "Waiting " << delay_ms << " msec for connection with Groot2...\n\n"
+              << std::endl;
+    std::cout << "======================" << std::endl;
+    std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
+  }
+
+  // Start ticking the Tree
   std::cout << "Starting Behavior Tree" << std::endl;
   std::cout << "======================" << std::endl;