diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6ff7ae3..f6e69bc 100644
--- a/CMakeLists.txt
+++ b/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}
)
diff --git a/README.md b/README.md
index 384da12..4ff5e48 100644
--- a/README.md
+++ b/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.
diff --git a/config/bt_mtc_model.xml b/config/bt_mtc_model.xml
new file mode 100644
index 0000000..c6a8e09
--- /dev/null
+++ b/config/bt_mtc_model.xml
@@ -0,0 +1,269 @@
+<root BTCPP_format="4">
+ <TreeNodesModel>
+ <Action ID="AddObjectToPlanningScene">
+ <input_port name="collision_object" type="std::shared_ptr<moveit_msgs::CollisionObject_<std::allocator<void> > >"/>
+ <input_port name="planning_scene_interface" type="std::shared_ptr<moveit::planning_interface::PlanningSceneInterface>"/>
+ </Action>
+ <Action ID="AllowAllCollisions">
+ <input_port name="objects" type="std::shared_ptr<std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > >">allow/forbid all collisions for given object/s</input_port>
+ <input_port name="stage_name" type="std::string"/>
+ <output_port name="stage" type="std::shared_ptr<moveit::task_constructor::Stage>">ModifyPlanningScene stage</output_port>
+ </Action>
+ <Action ID="AllowCollisionPairs">
+ <input_port name="second" type="std::shared_ptr<std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > >">second object of collision pair</input_port>
+ <input_port name="first" type="std::shared_ptr<std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > >">first object of collision pair</input_port>
+ <input_port name="stage_name" type="std::string"/>
+ <output_port name="stage" type="std::shared_ptr<moveit::task_constructor::Stage>">ModifyPlanningScene stage</output_port>
+ </Action>
+ <Action ID="AllowCollisionsJointModelGroup">
+ <input_port name="jmg_name" type="std::string">joint model group name</input_port>
+ <input_port name="objects" type="std::shared_ptr<std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > >">allow/forbid collisions with joint model group links</input_port>
+ <input_port name="stage_name" type="std::string"/>
+ <input_port name="task" type="std::shared_ptr<moveit::task_constructor::Task>">MTC task</input_port>
+ <output_port name="stage" type="std::shared_ptr<moveit::task_constructor::Stage>">ModifyPlanningScene stage</output_port>
+ </Action>
+ <Action ID="ConfigureInitFromMTCProperties">
+ <inout_port name="stage" type="std::shared_ptr<moveit::task_constructor::Stage>"/>
+ <input_port name="property_names" type="std::shared_ptr<std::set<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::less<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > >"/>
+ <input_port name="source_flag" type="moveit::task_constructor::Stage::PropertyInitializerSource">Name for property initialization sources. {DEFAULT, MANUAL, PARENT, INTERFACE}</input_port>
+ </Action>
+ <Action ID="CreateMTCCartesianPath">
+ <input_port name="step_size" type="double" default="0.010000">step size between consecutive waypoints</input_port>
+ <output_port name="solver" type="std::shared_ptr<moveit::task_constructor::solvers::PlannerInterface>" default="{solver}">Planner interface using pipeline motion solver</output_port>
+ <input_port name="precision" type="moveit::core::CartesianPrecision" default="">linear and rotational precision</input_port>
+ <input_port name="min_fraction" type="double" default="1.000000">fraction of motion required for success</input_port>
+ <input_port name="max_acceleration_scaling_factor" type="double" default="0.100000">scale down max acceleration by this factor</input_port>
+ <input_port name="max_velocity_scaling_factor" type="double" default="0.100000">scale down max velocity by this factor</input_port>
+ </Action>
+ <Action ID="CreateMTCComputeIK">
+ <input_port name="ik_frame" type="std::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > >"/>
+ <input_port name="min_solution_distance" type="double" default="0.100000">minimum distance between seperate IK solutions for the same target</input_port>
+ <output_port name="stage" type="std::shared_ptr<moveit::task_constructor::Stage>"/>
+ <input_port name="wrapped_stage" type="std::shared_ptr<moveit::task_constructor::Stage>"/>
+ <input_port name="max_ik_solutions" type="unsigned int" default="1">max ik solutions</input_port>
+ <input_port name="ignore_collisions" type="bool" default="false">ignore collisions, true or false</input_port>
+ <input_port name="group" type="std::string">name of active group (derived from eef if not provided)</input_port>
+ <input_port name="eef" type="std::string">name of end-effector</input_port>
+ <input_port name="stage_name" type="std::string"/>
+ </Action>
+ <Action ID="CreateMTCConnect">
+ <input_port name="merge_mode" type="unsigned char" default="1">0 = SEQUENTIAL, 1 = WAYPOINTS</input_port>
+ <input_port name="group" type="std::string"/>
+ <input_port name="stage_name" type="std::string">connect</input_port>
+ <input_port name="planner" type="std::shared_ptr<moveit::task_constructor::solvers::PlannerInterface>"/>
+ <input_port name="max_distance" type="double" default="0.000100">maximally accepted joint configuration distance between trajectory endpoint and goal state</input_port>
+ <input_port name="timeout" type="double" default="1.000000">timeout</input_port>
+ <output_port name="stage" type="std::shared_ptr<moveit::task_constructor::Stage>" default="{connect_stage}">Connect stage</output_port>
+ </Action>
+ <Action ID="CreateMTCCurrentState">
+ <output_port name="stage" type="std::shared_ptr<moveit::task_constructor::Stage>" default="{mtc_stage}">MoveIt Task Constructor stage.</output_port>
+ </Action>
+ <Action ID="CreateMTCGenerateGraspPose">
+ <output_port name="stage" type="std::shared_ptr<moveit::task_constructor::Stage>" default="{generate_grasp_pose}">GenerateGraspPose Stage</output_port>
+ <input_port name="monitored_stage" type="moveit::task_constructor::Stage*"/>
+ <input_port name="stage_name" type="std::string"/>
+ <input_port name="pregrasp_pose" type="std::string">pregrasp posture</input_port>
+ <input_port name="object" type="std::string">object on which we generate the grasp poses</input_port>
+ <input_port name="angle_delta" type="double" default="0.100000">angular steps (rad)</input_port>
+ <input_port name="rotation_axis" type="Vector3D" default="0,0,1">rotate object pose about given axis</input_port>
+ <input_port name="eef" type="std::string">name of end-effector</input_port>
+ </Action>
+ <Action ID="CreateMTCGeneratePlacePose">
+ <output_port name="stage" type="std::shared_ptr<moveit::task_constructor::Stage>" default="{generate_grasp_pose}">GenerateGraspPose Stage</output_port>
+ <input_port name="pose" type="std::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > >">target pose to pass on in spawned states</input_port>
+ <input_port name="allow_z_flip" type="bool" default="false">allow placing objects upside down</input_port>
+ <input_port name="monitored_stage" type="moveit::task_constructor::Stage*"/>
+ <input_port name="stage_name" type="std::string"/>
+ <input_port name="object" type="std::string">object on which we generate the place poses</input_port>
+ </Action>
+ <Action ID="CreateMTCJointInterpolation">
+ <output_port name="solver" type="std::shared_ptr<moveit::task_constructor::solvers::PlannerInterface>" default="{solver}">plan using simple interpolation in joint-space</output_port>
+ <input_port name="max_step" type="double" default="0.100000">max joint step</input_port>
+ <input_port name="max_acceleration_scaling_factor" type="double" default="0.100000">scale down max acceleration by this factor</input_port>
+ <input_port name="max_velocity_scaling_factor" type="double" default="0.100000">scale down max velocity by this factor</input_port>
+ </Action>
+ <Action ID="CreateMTCModifyPlanningSceneAttachObjects">
+ <output_port name="stage" type="std::shared_ptr<moveit::task_constructor::Stage>">ModifyPlanningScene stage</output_port>
+ <input_port name="object_names" type="std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > >">list of object names/id's.</input_port>
+ <input_port name="link_name" type="std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > >">attach or detach a list of objects to the given link</input_port>
+ <input_port name="stage_name" type="std::string"/>
+ </Action>
+ <Action ID="CreateMTCModifyPlanningSceneDetachObjects">
+ <output_port name="stage" type="std::shared_ptr<moveit::task_constructor::Stage>">ModifyPlanningScene stage</output_port>
+ <input_port name="object_names" type="std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > >">list of object names/id's.</input_port>
+ <input_port name="link_name" type="std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > >">attach or detach a list of objects to the given link</input_port>
+ <input_port name="stage_name" type="std::string"/>
+ </Action>
+ <Action ID="CreateMTCMoveRelativeJoint">
+ <output_port name="stage" type="std::shared_ptr<moveit::task_constructor::Stage>">MoveRelative stage</output_port>
+ <input_port name="solver" type="std::shared_ptr<moveit::task_constructor::solvers::PlannerInterface>">planner interface</input_port>
+ <input_port name="direction" type="std::shared_ptr<std::map<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, double, std::less<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >, std::allocator<std::pair<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const, double> > > >">move specified joint variables by given amount</input_port>
+ <input_port name="max_distance" type="double" default="0.000000">maximum distance to move</input_port>
+ <input_port name="ik_frame" type="std::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > >">frame to be moved in Cartesian direction</input_port>
+ <input_port name="min_distance" type="double" default="-1.000000">minimum distance to move</input_port>
+ <input_port name="timeout" type="double" default="1.000000"/>
+ <input_port name="group" type="std::string">name of planning group</input_port>
+ <input_port name="stage_name" type="std::string" default="move relative">stage name</input_port>
+ </Action>
+ <Action ID="CreateMTCMoveRelativeTranslate">
+ <output_port name="stage" type="std::shared_ptr<moveit::task_constructor::Stage>">MoveRelative stage</output_port>
+ <input_port name="solver" type="std::shared_ptr<moveit::task_constructor::solvers::PlannerInterface>">planner interface</input_port>
+ <input_port name="direction" type="std::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > >">translate link along given direction</input_port>
+ <input_port name="max_distance" type="double" default="0.000000">maximum distance to move</input_port>
+ <input_port name="ik_frame" type="std::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > >">frame to be moved in Cartesian direction</input_port>
+ <input_port name="min_distance" type="double" default="-1.000000">minimum distance to move</input_port>
+ <input_port name="timeout" type="double" default="1.000000"/>
+ <input_port name="group" type="std::string">name of planning group</input_port>
+ <input_port name="stage_name" type="std::string" default="move relative">stage name</input_port>
+ </Action>
+ <Action ID="CreateMTCMoveRelativeTwist">
+ <output_port name="stage" type="std::shared_ptr<moveit::task_constructor::Stage>">MoveRelative stage</output_port>
+ <input_port name="solver" type="std::shared_ptr<moveit::task_constructor::solvers::PlannerInterface>">planner interface</input_port>
+ <input_port name="direction" type="std::shared_ptr<geometry_msgs::TwistStamped_<std::allocator<void> > >">perform twist motion on specified link</input_port>
+ <input_port name="max_distance" type="double" default="0.000000">maximum distance to move</input_port>
+ <input_port name="ik_frame" type="std::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > >">frame to be moved in Cartesian direction</input_port>
+ <input_port name="min_distance" type="double" default="-1.000000">minimum distance to move</input_port>
+ <input_port name="timeout" type="double" default="1.000000"/>
+ <input_port name="group" type="std::string">name of planning group</input_port>
+ <input_port name="stage_name" type="std::string" default="move relative">stage name</input_port>
+ </Action>
+ <Action ID="CreateMTCMoveToJoint">
+ <input_port name="goal" type="std::shared_ptr<std::map<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, double, std::less<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >, std::allocator<std::pair<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const, double> > > >">move joints by name to their mapped target value</input_port>
+ <input_port name="stage_name" type="std::string" default="move to">stage name</input_port>
+ <input_port name="group" type="std::string">name of planning group</input_port>
+ <input_port name="timeout" type="double" default="1.000000"/>
+ <input_port name="solver" type="std::shared_ptr<moveit::task_constructor::solvers::PlannerInterface>">planner interface</input_port>
+ <output_port name="stage" type="std::shared_ptr<moveit::task_constructor::Stage>">MoveTo stage</output_port>
+ </Action>
+ <Action ID="CreateMTCMoveToNamedJointPose">
+ <input_port name="goal" type="std::string">move joint model group to given named pose</input_port>
+ <input_port name="stage_name" type="std::string" default="move to">stage name</input_port>
+ <input_port name="group" type="std::string">name of planning group</input_port>
+ <input_port name="timeout" type="double" default="1.000000"/>
+ <input_port name="solver" type="std::shared_ptr<moveit::task_constructor::solvers::PlannerInterface>">planner interface</input_port>
+ <output_port name="stage" type="std::shared_ptr<moveit::task_constructor::Stage>">MoveTo stage</output_port>
+ </Action>
+ <Action ID="CreateMTCMoveToPoint">
+ <input_port name="goal" type="std::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > >">move link to given point, keeping current orientation</input_port>
+ <input_port name="ik_frame" type="std::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > >">frame to be moved in Cartesian direction</input_port>
+ <input_port name="stage_name" type="std::string" default="move to">stage name</input_port>
+ <input_port name="group" type="std::string">name of planning group</input_port>
+ <input_port name="timeout" type="double" default="1.000000"/>
+ <input_port name="solver" type="std::shared_ptr<moveit::task_constructor::solvers::PlannerInterface>">planner interface</input_port>
+ <output_port name="stage" type="std::shared_ptr<moveit::task_constructor::Stage>">MoveTo stage</output_port>
+ </Action>
+ <Action ID="CreateMTCMoveToPose">
+ <input_port name="goal" type="std::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > >">move link to a given pose</input_port>
+ <input_port name="ik_frame" type="std::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > >">frame to be moved in Cartesian direction</input_port>
+ <input_port name="stage_name" type="std::string" default="move to">stage name</input_port>
+ <input_port name="group" type="std::string">name of planning group</input_port>
+ <input_port name="timeout" type="double" default="1.000000"/>
+ <input_port name="solver" type="std::shared_ptr<moveit::task_constructor::solvers::PlannerInterface>">planner interface</input_port>
+ <output_port name="stage" type="std::shared_ptr<moveit::task_constructor::Stage>">MoveTo stage</output_port>
+ </Action>
+ <Action ID="CreateMTCPipelinePlanner">
+ <input_port name="max_acceleration_scaling_factor" type="double" default="0.100000">scale down max acceleration by this factor</input_port>
+ <input_port name="num_planning_attempts" type="unsigned int" default="1u">number of planning attempts</input_port>
+ <input_port name="pipeline_id" type="std::string"/>
+ <input_port name="max_velocity_scaling_factor" type="double" default="0.100000">scale down max velocity by this factor</input_port>
+ <input_port name="planner_id" type="std::string"/>
+ <input_port name="display_motion_plans" type="bool" default="false">publish generated solutions on topic display_planned_path</input_port>
+ <input_port name="goal_orientation_tolerance" type="double" default="1e-4">tolerance for reaching orientation goals</input_port>
+ <input_port name="goal_position_tolerance" type="double" default="1e-4">tolerance for reaching position goals</input_port>
+ <input_port name="goal_joint_tolerance" type="double" default="1e-4">tolerance for reaching joint goals</input_port>
+ <input_port name="publish_planning_requests" type="bool" default="false">publish motion planning requests on topic motion_plan_request</input_port>
+ <output_port name="solver" type="std::shared_ptr<moveit::task_constructor::solvers::PlannerInterface>" default="{solver}">Planner interface using pipeline motion solver</output_port>
+ </Action>
+ <Action ID="CreateMTCSerialContainer">
+ <output_port name="container" type="std::shared_ptr<moveit::task_constructor::ContainerBase>">serial container</output_port>
+ <input_port name="container_name" type="std::string">serial container name</input_port>
+ </Action>
+ <Action ID="CreatePlanningSceneInterface">
+ <output_port name="planning_scene_interface" type="std::shared_ptr<moveit::planning_interface::PlanningSceneInterface>">{planning_scene_interface}</output_port>
+ </Action>
+ <Action ID="ForbidAllCollisions">
+ <input_port name="objects" type="std::shared_ptr<std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > >">allow/forbid all collisions for given object/s</input_port>
+ <input_port name="stage_name" type="std::string"/>
+ <output_port name="stage" type="std::shared_ptr<moveit::task_constructor::Stage>">ModifyPlanningScene stage</output_port>
+ </Action>
+ <Action ID="ForbidCollisionPairs">
+ <input_port name="second" type="std::shared_ptr<std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > >">second object of collision pair</input_port>
+ <input_port name="first" type="std::shared_ptr<std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > >">first object of collision pair</input_port>
+ <input_port name="stage_name" type="std::string"/>
+ <output_port name="stage" type="std::shared_ptr<moveit::task_constructor::Stage>">ModifyPlanningScene stage</output_port>
+ </Action>
+ <Action ID="ForbidCollisionsJointModelGroup">
+ <input_port name="jmg_name" type="std::string">joint model group name</input_port>
+ <input_port name="objects" type="std::shared_ptr<std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > >">allow/forbid collisions with joint model group links</input_port>
+ <input_port name="stage_name" type="std::string"/>
+ <input_port name="task" type="std::shared_ptr<moveit::task_constructor::Task>">MTC task</input_port>
+ <output_port name="stage" type="std::shared_ptr<moveit::task_constructor::Stage>">ModifyPlanningScene stage</output_port>
+ </Action>
+ <Action ID="GeometryMsgsPose">
+ <output_port name="pose" type="std::shared_ptr<geometry_msgs::Pose_<std::allocator<void> > >"/>
+ <input_port name="quaternion" type="Vector4D"/>
+ <input_port name="position" type="Vector3D"/>
+ </Action>
+ <Action ID="GeometryMsgsPoseStamped">
+ <input_port name="quaternion" type="Vector4D"/>
+ <input_port name="position" type="Vector3D"/>
+ <output_port name="pose_stamped" type="std::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > >"/>
+ <input_port name="frame_id" type="std::string"/>
+ </Action>
+ <Action ID="GeometryMsgsTwistStamped">
+ <output_port name="twist_stamped" type="std::shared_ptr<geometry_msgs::TwistStamped_<std::allocator<void> > >"/>
+ <input_port name="angular_velocity" type="Vector3D"/>
+ <input_port name="linear_velocity" type="Vector3D"/>
+ <input_port name="frame_id" type="std::string"/>
+ </Action>
+ <Action ID="GeometryMsgsVector3Stamped">
+ <output_port name="vector3_stamped" type="std::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > >"/>
+ <input_port name="vector" type="Vector3D"/>
+ <input_port name="frame_id" type="std::string"/>
+ </Action>
+ <Action ID="GetMTCRawStage">
+ <output_port name="raw_stage" type="moveit::task_constructor::Stage*">MTC stage raw pointer</output_port>
+ <input_port name="stage" type="std::shared_ptr<moveit::task_constructor::Stage>">MTC stage</input_port>
+ </Action>
+ <Action ID="InitializeMTCTask">
+ <output_port name="task" type="std::shared_ptr<moveit::task_constructor::Task>" default="{mtc_task}">MoveIt Task Constructor task.</output_port>
+ </Action>
+ <Action ID="MoveItMsgsCollisionObjectBox">
+ <input_port name="height" type="double" default="1.0">Box's size along Z axis (m)</input_port>
+ <input_port name="length" type="double" default="1.0">Box's size along X axis (m)</input_port>
+ <input_port name="object_id" type="std::string"/>
+ <input_port name="frame_id" type="std::string"/>
+ <output_port name="collision_object" type="std::shared_ptr<moveit_msgs::CollisionObject_<std::allocator<void> > >"/>
+ <input_port name="width" type="double" default="1.0">Box's size along Y axis (m)</input_port>
+ <input_port name="pose" type="std::shared_ptr<geometry_msgs::Pose_<std::allocator<void> > >"/>
+ </Action>
+ <Action ID="MoveItMsgsCollisionObjectCylinder">
+ <input_port name="height" type="double" default="1.0">cylinder height (m)</input_port>
+ <input_port name="object_id" type="std::string"/>
+ <input_port name="frame_id" type="std::string"/>
+ <output_port name="collision_object" type="std::shared_ptr<moveit_msgs::CollisionObject_<std::allocator<void> > >"/>
+ <input_port name="radius" type="double" default="1.0">cylinder radius (m)</input_port>
+ <input_port name="pose" type="std::shared_ptr<geometry_msgs::Pose_<std::allocator<void> > >"/>
+ </Action>
+ <Action ID="MoveMTCContainerToContainer">
+ <inout_port name="parent" type="std::shared_ptr<moveit::task_constructor::ContainerBase>"/>
+ <input_port name="child" type="std::shared_ptr<moveit::task_constructor::ContainerBase>"/>
+ </Action>
+ <Action ID="MoveMTCContainerToTask">
+ <inout_port name="parent" type="std::shared_ptr<moveit::task_constructor::Task>"/>
+ <input_port name="child" type="std::shared_ptr<moveit::task_constructor::ContainerBase>"/>
+ </Action>
+ <Action ID="MoveMTCStageToContainer">
+ <inout_port name="parent" type="std::shared_ptr<moveit::task_constructor::ContainerBase>"/>
+ <input_port name="child" type="std::shared_ptr<moveit::task_constructor::Stage>"/>
+ </Action>
+ <Action ID="MoveMTCStageToTask">
+ <inout_port name="parent" type="std::shared_ptr<moveit::task_constructor::Task>"/>
+ <input_port name="child" type="std::shared_ptr<moveit::task_constructor::Stage>"/>
+ </Action>
+ <Action ID="PlanMTCTask">
+ <inout_port name="task" type="std::shared_ptr<moveit::task_constructor::Task>"/>
+ <input_port name="max_solutions" type="unsigned long"/>
+ </Action>
+ </TreeNodesModel>
+</root>
diff --git a/examples/bt_allow_collisions.cpp b/examples/bt_allow_collisions.cpp
index d52ab61..076c2e2 100644
--- a/examples/bt_allow_collisions.cpp
+++ b/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;
diff --git a/examples/bt_compute_ik.cpp b/examples/bt_compute_ik.cpp
index 0f75809..4f90af2 100644
--- a/examples/bt_compute_ik.cpp
+++ b/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,7 +154,6 @@ 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));
@@ -164,7 +161,9 @@ int main(int argc, char** argv)
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;
diff --git a/examples/bt_create_collision_object.cpp b/examples/bt_create_collision_object.cpp
index 46a7cb4..5cb8fb5 100644
--- a/examples/bt_create_collision_object.cpp
+++ b/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;
diff --git a/examples/bt_demo.cpp b/examples/bt_demo.cpp
index e21ae25..f648804 100644
--- a/examples/bt_demo.cpp
+++ b/examples/bt_demo.cpp
@@ -1,8 +1,8 @@
#include <rclcpp/rclcpp.hpp>
-#include <behaviortree_mtc/create_mtc_current_state.h>
#include <behaviortree_mtc/initialize_mtc_task.h>
-#include <behaviortree_mtc/move_mtc_stage_to_container.h>
+#include <behaviortree_mtc/create_mtc_current_state.h>
+#include <behaviortree_mtc/move_mtc_stage.h>
#include <behaviortree_mtc/plan_mtc_task.h>
#include "behaviortree_cpp/bt_factory.h"
@@ -19,9 +19,9 @@ static const char* xml_text = R"(
<BehaviorTree ID="MainTree">
<Sequence name="root">
- <InitializeMTCTask task="{mtc_task}" container="{container}" />
+ <InitializeMTCTask task="{mtc_task}" />
<CreateMTCCurrentState stage="{stage}" />
- <MoveMTCStageToContainer container="{container}" stage="{stage}" />
+ <MoveMTCStageToTask child="{stage}" parent="{mtc_task}" />
<PlanMTCTask task="{mtc_task}" max_solutions="5" />
</Sequence>
</BehaviorTree>
@@ -37,13 +37,13 @@ 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<InitializeMTCTask>("InitializeMTCTask", BT::RosNodeParams(node));
factory.registerNodeType<CreateMTCCurrentState>("CreateMTCCurrentState");
- factory.registerNodeType<MoveMTCStageToContainer>("MoveMTCStageToContainer");
+ factory.registerNodeType<MoveMTCStage<moveit::task_constructor::Stage, moveit::task_constructor::Task>>("MoveMTCStageToTask");
factory.registerNodeType<PlanMTCTask>("PlanMTCTask");
auto tree = factory.createTreeFromText(xml_text);
@@ -60,11 +60,14 @@ 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));
+ }
NodeStatus status = NodeStatus::IDLE;
diff --git a/examples/bt_generate_place_pose.cpp b/examples/bt_generate_place_pose.cpp
index 3df58ce..8ff65c7 100644
--- a/examples/bt_generate_place_pose.cpp
+++ b/examples/bt_generate_place_pose.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>
@@ -72,7 +71,7 @@ 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}"
@@ -81,7 +80,7 @@ static const char* xml_text = R"(
max_velocity_scaling_factor="1.0"
max_acceleration_scaling_factor="1.0" />
<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 -> open_hand motion"
group="{hand_group_name}"
@@ -89,13 +88,13 @@ static const char* xml_text = R"(
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}" />
+ <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,31 +126,31 @@ 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}" />
+ <MoveMTCStageToContainer child="{grasp_pose_IK}" parent="{pick_object}" />
<AllowCollisionsJointModelGroup stage_name="allow collisions -> hand,object"
stage="{allow_collisions}"
objects="{object_name}"
jmg_name="{hand_group_name}"
task="{mtc_task}" />
- <MoveMTCStageToContainer container="{pick_object}" stage="{allow_collisions}" />
+ <MoveMTCStageToContainer child="{allow_collisions}" parent="{pick_object}" />
<CreateMTCMoveToNamedJointPose name="close hand"
- group="{hand_group_name}"
- solver="{rrt_connect}"
+ group="{hand_group_name}"
+ solver="{rrt_connect}"
goal="close"
- stage="{stage_move_to_close_hand}" />
- <MoveMTCStageToContainer container="{pick_object}" stage="{stage_move_to_close_hand}" />
- <CreateMTCModifyPlanningSceneAttachObjects stage_name="attach object"
+ stage="{stage_move_to_close_hand}" />
+ <MoveMTCStageToContainer child="{stage_move_to_close_hand}" parent="{pick_object}" />
+ <CreateMTCModifyPlanningSceneAttachObjects stage_name="attach object"
stage="{attach_object}"
object_names="{object_name}"
link_name="{hand_frame}" />
- <MoveMTCStageToContainer container="{pick_object}" stage="{attach_object}" />
+ <MoveMTCStageToContainer child="{attach_object}" parent="{pick_object}" />
<AllowCollisionPairs stage_name="allow collisions -> object,support"
stage="{allow_collisions_table}"
first="{object_name}"
second="table" />
- <MoveMTCStageToContainer container="{pick_object}" stage="{allow_collisions_table}" />
+ <MoveMTCStageToContainer child="{allow_collisions_table}" parent="{pick_object}" />
<GeometryMsgsVector3Stamped frame_id="{reference_frame}" vector="0,0,1" vector3_stamped="{tcp_translate}"/>
- <CreateMTCMoveRelativeTranslate stage_name="lift object"
+ <CreateMTCMoveRelativeTranslate stage_name="lift object"
stage="{lift_object}"
group="{arm_group_name}"
solver="{cartesian}"
@@ -159,24 +158,24 @@ static const char* xml_text = R"(
direction="{tcp_translate}"
min_distance="0.01"
max_distance="0.1" />
- <MoveMTCStageToContainer container="{pick_object}" stage="{lift_object}" />
+ <MoveMTCStageToContainer child="{lift_object}" parent="{pick_object}" />
<ForbidCollisionPairs stage_name="forbid collisions -> object,support"
stage="{forbid_collisions_table}"
first="{object_name}"
second="table" />
- <GetMTCRawStage stage="{forbid_collisions_table}" raw_stage="{raw_forbid_collisions_table}" />
- <MoveMTCStageToContainer container="{pick_object}" stage="{forbid_collisions_table}" />
- <MoveMTCContainerToParentContainer parent_container="{task_container}" child_container="{pick_object}" />
- <CreateMTCConnect stage_name="Move To Place"
+ <GetMTCRawStage stage="{forbid_collisions_table}" raw_stage="{raw_forbid_collisions_table}" />
+ <MoveMTCStageToContainer child="{forbid_collisions_table}" parent="{pick_object}" />
+ <MoveMTCContainerToTask child="{pick_object}" parent="{mtc_task}" />
+ <CreateMTCConnect stage_name="Move To Place"
timeout="5.0"
group="{arm_group_name}"
planner="{rrt_connect}"
stage="{connectPlace}" />
- <MoveMTCStageToContainer container="{task_container}" stage="{connectPlace}" />
- <CreateMTCSerialContainer container_name="place object"
- container="{place_object}" />
- <GeometryMsgsVector3Stamped frame_id="{reference_frame}" vector="0,0,-1" vector3_stamped="{tcp_translate2}"/>
- <CreateMTCMoveRelativeTranslate stage_name="lower object"
+ <MoveMTCStageToTask child="{connectPlace}" parent="{mtc_task}" />
+ <CreateMTCSerialContainer container_name="place object"
+ container="{place_object}" />
+ <GeometryMsgsVector3Stamped frame_id="{reference_frame}" vector="0,0,-1" vector3_stamped="{tcp_translate2}"/>
+ <CreateMTCMoveRelativeTranslate stage_name="lower object"
stage="{lower_object}"
group="{arm_group_name}"
solver="{cartesian}"
@@ -184,7 +183,7 @@ static const char* xml_text = R"(
direction="{tcp_translate2}"
min_distance="0.03"
max_distance="0.13" />
- <MoveMTCStageToContainer container="{place_object}" stage="{lower_object}" />
+ <MoveMTCStageToContainer child="{lower_object}" parent="{place_object}" />
<GeometryMsgsPoseStamped frame_id="world" position="0.6,-0.15,0.1251" quaternion="1,0,0,0" pose_stamped="{place_pose}" />
<CreateMTCGeneratePlacePose stage="{generate_place_pose}"
stage_name="generate place pose"
@@ -201,8 +200,8 @@ static const char* xml_text = R"(
<ConfigureInitFromMTCProperties stage="{place_pose_IK}"
property_names="target_pose"
source_flag="INTERFACE" />
- <MoveMTCStageToContainer container="{place_object}" stage="{place_pose_IK}" />
- <MoveMTCContainerToParentContainer parent_container="{task_container}" child_container="{place_object}" />
+ <MoveMTCStageToContainer child="{place_pose_IK}" parent="{place_object}" />
+ <MoveMTCContainerToTask child="{place_object}" parent="{mtc_task}" />
<PlanMTCTask task="{mtc_task}" max_solutions="5" />
</Sequence>
</BehaviorTree>
@@ -218,10 +217,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");
@@ -230,7 +228,6 @@ 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));
@@ -238,7 +235,9 @@ int main(int argc, char** argv)
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");
@@ -266,12 +265,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;
@@ -297,70 +300,3 @@ int main(int argc, char** argv)
return 0;
}
-
-/**
- *
- <CreateMTCModifyPlanningSceneAttachObjects stage_name="attach object"
- stage="{attach_object}"
- object_names="{object_name}"
- link_name="{hand_frame}" />
- <MoveMTCStageToContainer container="{pick_object}" stage="{attach_object}" />
- <AllowCollisionPairs stage_name="allow collisions -> object,support"
- stage="{allow_collisions_table}"
- first="{object_name}"
- second="table" />
- <MoveMTCStageToContainer container="{pick_object}" stage="{allow_collisions_table}" />
- <GeometryMsgsVector3Stamped frame_id="{reference_frame}" vector="0,0,1" vector3_stamped="{tcp_translate}"/>
- <CreateMTCMoveRelativeTranslate stage_name="lift object"
- stage="{lift_object}"
- group="{arm_group_name}"
- solver="{cartesian}"
- ik_frame="{ik_frame}"
- direction="{tcp_translate}"
- min_distance="0.01"
- max_distance="0.1" />
- <MoveMTCStageToContainer container="{pick_object}" stage="{lift_object}" />
- <ForbidCollisionPairs stage_name="forbid collisions -> object,support"
- stage="{forbid_collisions_table}"
- first="{object_name}"
- second="table" />
- <GetMTCRawStage stage="{forbid_collisions_table}" raw_stage="{raw_forbid_collisions_table}}" />
- <MoveMTCStageToContainer container="{pick_object}" stage="{forbid_collisions_table}" />
- <MoveMTCContainerToParentContainer parent_container="{task_container}" child_container="{pick_object}" />
- <CreateMTCConnect stage_name="Move To Place"
- timeout="5.0"
- group="{arm_group_name}"
- planner="{rrt_connect}"
- stage="{connectPlace}" />
- <MoveMTCStageToContainer container="{task_container}" stage="{connectPlace}" />
- <CreateMTCSerialContainer container_name="place object"
- serial_container="{place_object}" />
- <GeometryMsgsVector3Stamped frame_id="{reference_frame}" vector="0,0,-1" vector3_stamped="{tcp_translate}"/>
- <CreateMTCMoveRelativeTranslate stage_name="lower object"
- stage="{lower_object}"
- group="{arm_group_name}"
- solver="{cartesian}"
- ik_frame="{ik_frame}"
- direction="{tcp_translate}"
- min_distance="0.03"
- max_distance="0.13" />
- <MoveMTCStageToContainer container="{place_object}" stage="{lower_object}" />
- */
-
-//
-
-//
-
-//
-// <CreateMTCSerialContainer container_name="place object"
-// serial_container="{place_object}" />
-// <GeometryMsgsVector3Stamped frame_id="{reference_frame}" vector="0,0,-1" vector3_stamped="{tcp_translate}"/>
-// <CreateMTCMoveRelativeTranslate stage_name="lower object"
-// stage="{lower_object}"
-// group="{arm_group_name}"
-// solver="{cartesian}"
-// ik_frame="{ik_frame}"
-// direction="{tcp_translate}"
-// min_distance="0.03"
-// max_distance="0.13" />
-// <MoveMTCStageToContainer container="{place_object}" stage="{lower_object}" />
diff --git a/examples/bt_joint_interpolation_solver.cpp b/examples/bt_joint_interpolation_solver.cpp
index a5928cf..a364e64 100644
--- a/examples/bt_joint_interpolation_solver.cpp
+++ b/examples/bt_joint_interpolation_solver.cpp
@@ -3,7 +3,7 @@
#include <behaviortree_mtc/initialize_mtc_task.h>
#include <behaviortree_mtc/create_mtc_current_state.h>
#include <behaviortree_mtc/create_mtc_move_relative.h>
-#include <behaviortree_mtc/move_mtc_stage_to_container.h>
+#include <behaviortree_mtc/move_mtc_stage.h>
#include <behaviortree_mtc/create_mtc_joint_interpolation.h>
#include <behaviortree_mtc/plan_mtc_task.h>
@@ -23,10 +23,10 @@ static const char* xml_text = R"(
<BehaviorTree ID="MainTree">
<Sequence name="root">
- <InitializeMTCTask task="{mtc_task}" container="{container}" />
+ <InitializeMTCTask task="{mtc_task}" />
<CreateMTCJointInterpolation solver="{joint_solver}" />
<CreateMTCCurrentState stage="{stage}" />
- <MoveMTCStageToContainer container="{container}" stage="{stage}" />
+ <MoveMTCStageToTask child="{stage}" parent="{mtc_task}" />
<!-- Joint Motion -> return end joint to default value -->
<GeometryMsgsPoseStamped frame_id="panda_link8" position="0,0,0" quaternion="1,0,0,0" pose_stamped="{ik_frame}"/>
<CreateMTCMoveRelativeJoint name="move relative -> joint motion"
@@ -35,8 +35,8 @@ static const char* xml_text = R"(
ik_frame="{ik_frame}"
direction="panda_joint7:-1.57079632679"
stage="{stage_move_rel_joint}" />
- <MoveMTCStageToContainer container="{container}" stage="{stage_move_rel_joint}" />
- <PlanMTCTask task="{mtc_task}" max_solutions="5" />
+ <MoveMTCStageToTask child="{stage_move_rel_joint}" parent="{mtc_task}" />
+ <PlanMTCTask task="{mtc_task}" max_solutions="5" />
</Sequence>
</BehaviorTree>
@@ -51,7 +51,7 @@ 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;
@@ -59,7 +59,7 @@ int main(int argc, char** argv)
factory.registerNodeType<CreateMTCJointInterpolation>("CreateMTCJointInterpolation");
factory.registerNodeType<CreateMTCCurrentState>("CreateMTCCurrentState");
factory.registerNodeType<CreateMTCMoveRelativeJoint>("CreateMTCMoveRelativeJoint");
- factory.registerNodeType<MoveMTCStageToContainer>("MoveMTCStageToContainer");
+ factory.registerNodeType<MoveMTCStage<moveit::task_constructor::Stage, moveit::task_constructor::Task>>("MoveMTCStageToTask");
factory.registerNodeType<PlanMTCTask>("PlanMTCTask");
factory.registerNodeType<GeometryMsgsPoseStamped>("GeometryMsgsPoseStamped");
@@ -78,11 +78,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::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;
diff --git a/examples/bt_move_relative.cpp b/examples/bt_move_relative.cpp
index 76b3d35..bab3807 100644
--- a/examples/bt_move_relative.cpp
+++ b/examples/bt_move_relative.cpp
@@ -4,7 +4,7 @@
#include <behaviortree_mtc/create_mtc_current_state.h>
#include <behaviortree_mtc/create_mtc_move_relative.h>
#include <behaviortree_mtc/create_mtc_pipeline_planner.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/geometry_msgs.h>
@@ -23,12 +23,12 @@ static const char* xml_text = R"(
<BehaviorTree ID="MainTree">
<Sequence name="root">
- <InitializeMTCTask task="{mtc_task}" container="{container}" />
+ <InitializeMTCTask task="{mtc_task}" />
<CreateMTCPipelinePlanner pipeline_id="ompl"
planner_id="RRTConnect"
solver="{rrt_connect}" />
<CreateMTCCurrentState stage="{stage}" />
- <MoveMTCStageToContainer container="{container}" stage="{stage}" />
+ <MoveMTCStageToTask child="{stage}" parent="{mtc_task}" />
<!-- Translate Motion -> 200mm towards x axis wrt. panda_link8 -->
<GeometryMsgsPoseStamped frame_id="panda_link8" position="0,0,0" quaternion="1,0,0,0" pose_stamped="{ik_frame}"/>
@@ -39,7 +39,7 @@ static const char* xml_text = R"(
ik_frame="{ik_frame}"
direction="{tcp_translate}"
stage="{stage_move_rel_translate}" />
- <MoveMTCStageToContainer container="{container}" stage="{stage_move_rel_translate}" />
+ <MoveMTCStageToTask child="{stage_move_rel_translate}" parent="{mtc_task}" />
<!-- Twist Motion -->
<GeometryMsgsPoseStamped frame_id="panda_link8" position="0,0,0" quaternion="1,0,0,0" pose_stamped="{ik_frame}"/>
@@ -50,7 +50,7 @@ static const char* xml_text = R"(
ik_frame="{ik_frame}"
direction="{tcp_twist}"
stage="{stage_move_rel_twist}" />
- <MoveMTCStageToContainer container="{container}" stage="{stage_move_rel_twist}" />
+ <MoveMTCStageToTask child="{stage_move_rel_twist}" parent="{mtc_task}" />
<!-- Joint Motion -> return end joint to default value -->
<GeometryMsgsPoseStamped frame_id="panda_link8" position="0,0,0" quaternion="1,0,0,0" pose_stamped="{ik_frame}"/>
@@ -60,7 +60,7 @@ static const char* xml_text = R"(
ik_frame="{ik_frame}"
direction="panda_joint7:-1.57079632679"
stage="{stage_move_rel_joint}" />
- <MoveMTCStageToContainer container="{container}" stage="{stage_move_rel_joint}" />
+ <MoveMTCStageToTask child="{stage_move_rel_joint}" parent="{mtc_task}" />
<PlanMTCTask task="{mtc_task}" max_solutions="5" />
@@ -78,7 +78,7 @@ 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;
@@ -88,7 +88,7 @@ int main(int argc, char** argv)
factory.registerNodeType<CreateMTCMoveRelativeTranslate>("CreateMTCMoveRelativeTranslate");
factory.registerNodeType<CreateMTCMoveRelativeTwist>("CreateMTCMoveRelativeTwist");
factory.registerNodeType<CreateMTCMoveRelativeJoint>("CreateMTCMoveRelativeJoint");
- factory.registerNodeType<MoveMTCStageToContainer>("MoveMTCStageToContainer");
+ factory.registerNodeType<MoveMTCStage<moveit::task_constructor::Stage, moveit::task_constructor::Task>>("MoveMTCStageToTask");
factory.registerNodeType<PlanMTCTask>("PlanMTCTask");
factory.registerNodeType<GeometryMsgsPoseStamped>("GeometryMsgsPoseStamped");
@@ -109,11 +109,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::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;
diff --git a/examples/bt_move_to.cpp b/examples/bt_move_to.cpp
index 3617440..d7ce64f 100644
--- a/examples/bt_move_to.cpp
+++ b/examples/bt_move_to.cpp
@@ -4,7 +4,7 @@
#include <behaviortree_mtc/create_mtc_current_state.h>
#include <behaviortree_mtc/create_mtc_move_to.h>
#include <behaviortree_mtc/create_mtc_pipeline_planner.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/geometry_msgs.h>
@@ -23,12 +23,12 @@ static const char* xml_text = R"(
<BehaviorTree ID="MainTree">
<Sequence name="root">
- <InitializeMTCTask task="{mtc_task}" container="{container}" />
+ <InitializeMTCTask task="{mtc_task}" />
<CreateMTCPipelinePlanner pipeline_id="ompl"
planner_id="RRTConnect"
solver="{rrt_connect}"/>
<CreateMTCCurrentState stage="{stage}" />
- <MoveMTCStageToContainer container="{container}" stage="{stage}" />
+ <MoveMTCStageToTask child="{stage}" parent="{mtc_task}" />
<!-- joint named position motion -->
<CreateMTCMoveToNamedJointPose name="move to -> close_hand motion"
@@ -36,13 +36,14 @@ static const char* xml_text = R"(
solver="{rrt_connect}"
goal="close"
stage="{stage_move_to_close_hand}" />
- <MoveMTCStageToContainer container="{container}" stage="{stage_move_to_close_hand}" />
+ <MoveMTCStageToTask parent="{mtc_task}" child="{stage_move_to_close_hand}" />
+
<CreateMTCMoveToNamedJointPose name="move to -> open_hand motion"
group="hand"
solver="{rrt_connect}"
goal="open"
stage="{stage_move_to_open_hand}" />
- <MoveMTCStageToContainer container="{container}" stage="{stage_move_to_open_hand}" />
+ <MoveMTCStageToTask child="{stage_move_to_open_hand}" parent="{mtc_task}" />
<!-- joint motion -->
<CreateMTCMoveToJoint name="move to -> joint motion"
@@ -50,7 +51,7 @@ static const char* xml_text = R"(
solver="{rrt_connect}"
goal="panda_joint7:-1.57079632679,panda_joint6:3.14"
stage="{stage_move_to_joint}" />
- <MoveMTCStageToContainer container="{container}" stage="{stage_move_to_joint}" />
+ <MoveMTCStageToTask parent="{mtc_task}" child="{stage_move_to_joint}" />
<!-- cartesian pose motion -->
<GeometryMsgsPoseStamped frame_id="panda_link8" position="0,0,0" quaternion="1,0,0,0" pose_stamped="{ik_frame}"/>
@@ -61,9 +62,9 @@ static const char* xml_text = R"(
ik_frame="{ik_frame}"
goal="{goal_pose}"
stage="{stage_move_to_pose}" />
- <MoveMTCStageToContainer container="{container}" stage="{stage_move_to_pose}" />
+ <MoveMTCStageToTask child="{stage_move_to_pose}" parent="{mtc_task}" />
- <!-- cartesian point motion -->
+ <!-- cartesian point motion -->
<GeometryMsgsPointStamped frame_id="panda_link8" point="0,0.05,0" point_stamped="{goal_point}"/>
<CreateMTCMoveToPoint name="move to -> cartesian point motion"
group="panda_arm"
@@ -71,7 +72,7 @@ static const char* xml_text = R"(
ik_frame="{ik_frame}"
goal="{goal_point}"
stage="{stage_move_to_point}" />
- <MoveMTCStageToContainer container="{container}" stage="{stage_move_to_point}" />
+ <MoveMTCStageToTask child="{stage_move_to_point}" parent="{mtc_task}" />
<PlanMTCTask task="{mtc_task}" max_solutions="5" />
</Sequence>
</BehaviorTree>
@@ -87,7 +88,7 @@ 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;
@@ -98,7 +99,7 @@ int main(int argc, char** argv)
factory.registerNodeType<CreateMTCMoveToJoint>("CreateMTCMoveToJoint");
factory.registerNodeType<CreateMTCMoveToPose>("CreateMTCMoveToPose");
factory.registerNodeType<CreateMTCMoveToPoint>("CreateMTCMoveToPoint");
- factory.registerNodeType<MoveMTCStageToContainer>("MoveMTCStageToContainer");
+ factory.registerNodeType<MoveMTCStage<moveit::task_constructor::Stage, moveit::task_constructor::Task>>("MoveMTCStageToTask");
factory.registerNodeType<PlanMTCTask>("PlanMTCTask");
factory.registerNodeType<GeometryMsgsPoseStamped>("GeometryMsgsPoseStamped");
@@ -118,12 +119,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::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;
diff --git a/examples/bt_pick_place.cpp b/examples/bt_pick_place.cpp
new file mode 100644
index 0000000..81ceebc
--- /dev/null
+++ b/examples/bt_pick_place.cpp
@@ -0,0 +1,395 @@
+#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.h>
+#include <behaviortree_mtc/create_mtc_serial_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_mtc_pipeline_planner.h>
+#include <behaviortree_mtc/create_planning_scene_interface.h>
+#include <behaviortree_mtc/add_object_to_planning_scene.h>
+#include <behaviortree_mtc/plan_mtc_task.h>
+#include <behaviortree_mtc/create_mtc_generate_grasp_pose.h>
+#include <behaviortree_mtc/get_mtc_raw_stage.h>
+#include <behaviortree_mtc/create_mtc_move_to.h>
+#include <behaviortree_mtc/create_mtc_connect.h>
+#include <behaviortree_mtc/create_mtc_move_relative.h>
+#include <behaviortree_mtc/create_mtc_compute_ik.h>
+#include <behaviortree_mtc/configure_init_from_mtc_properties.h>
+#include <behaviortree_mtc/std_containers.h>
+#include <behaviortree_mtc/create_mtc_cartesian_path.h>
+#include <behaviortree_mtc/create_mtc_modify_planning_scene.h>
+#include <behaviortree_mtc/create_mtc_generate_place_pose.h>
+#include <behaviortree_mtc/expose_mtc_properties.h>
+#include <behaviortree_mtc/set_mtc_properties.h>
+
+#include "behaviortree_cpp/bt_factory.h"
+#include "behaviortree_cpp/loggers/bt_file_logger_v2.h"
+#include "behaviortree_cpp/loggers/groot2_publisher.h"
+
+using namespace BT;
+using namespace BT::MTC;
+
+// clang-format off
+
+static const char* xml_text = R"(
+
+ <root BTCPP_format="4" >
+
+ <BehaviorTree ID="MainTree">
+ <Sequence name="root">
+
+ <!-- Set Parameters -->
+ <Script code="object_name:='object' " />
+ <Script code="table_name:='table' " />
+ <Script code="reference_frame:='world' " />
+ <Script code="eef:='hand' " />
+ <Script code="angle:=3.141592653589793/12 " />
+ <Script code="arm_group_name:='panda_arm'" />
+ <Script code="hand_group_name:='hand'" />
+ <Script code="hand_frame:='panda_link8'" />
+
+ <!-- PlanningSceneInterface -->
+ <CreatePlanningSceneInterface planning_scene_interface="{psi}" />
+
+ <!-- Create Table -->
+ <GeometryMsgsPose name="box's pose" position="0.5,-0.25,-0.05" quaternion="1,0,0,0" pose="{box_pose}" />
+ <MoveItMsgsCollisionObjectBox object_id="{table_name}" length="0.4" width="0.5" height="0.1" pose="{box_pose}" frame_id="{reference_frame}" collision_object="{box}" />
+ <AddObjectToPlanningScene planning_scene_interface="{psi}" collision_object="{box}" />
+
+ <!-- Create Cylinder -->
+ <GeometryMsgsPose name="cylinder's pose" position="0.5,-0.25,0.125" quaternion="1,0,0,0" pose="{cylinder_pose}" />
+ <MoveItMsgsCollisionObjectCylinder object_id="{object_name}"
+ radius="0.02"
+ height="0.25"
+ pose="{cylinder_pose}"
+ frame_id="{reference_frame}"
+ collision_object="{cylinder}" />
+ <AddObjectToPlanningScene planning_scene_interface="{psi}" collision_object="{cylinder}" />
+
+ <!-- Create Task -->
+ <InitializeMTCTask task="{mtc_task}" />
+
+ <!-- Add properpties to the Task -->
+ <SetMTCTaskPropertyString stage="{mtc_task}" property_name="eef" property="{eef}" />
+ <SetMTCTaskPropertyString stage="{mtc_task}" property_name="group" property="{arm_group_name}" />
+ <SetMTCTaskPropertyString stage="{mtc_task}" property_name="hand" property="{hand_group_name}" />
+ <SetMTCTaskPropertyString stage="{mtc_task}" property_name="hand_grasping_frame" property="{hand_frame}" />
+ <SetMTCTaskPropertyString stage="{mtc_task}" property_name="ik_frame" property="{hand_frame}" />
+
+ <!-- Create Solvers -->
+ <CreateMTCPipelinePlanner pipeline_id="ompl" planner_id="RRTConnect" solver="{rrt_connect}" goal_joint_tolerance="1e-5" />
+ <CreateMTCCartesianPath solver="{cartesian_planner}" max_velocity_scaling_factor="1.0" max_acceleration_scaling_factor="1.0" />
+
+ <!-- Current State -->
+ <CreateMTCCurrentState stage="{current_state}" />
+ <MoveMTCStageToTask child="{current_state}" parent="{mtc_task}" />
+
+
+ <!-- Open Hand -->
+ <GeometryMsgsPoseStamped frame_id="{hand_frame}" position="0,0,0" quaternion="1,0,0,0" pose_stamped="{ik_frame}" />
+ <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}" />
+ <MoveMTCStageToTask child="{stage_move_to_open_hand}" parent="{mtc_task}" />
+
+
+ <!-- Move to Pick -->
+ <CreateMTCConnect stage_name="MoveToPick"
+ timeout="5.0"
+ group="{arm_group_name}"
+ planner="{rrt_connect}"
+ stage="{connect}" />
+ <StageConfigureInitFromMTCProperties stage="{connect}" property_names="" source_flag="PARENT" />
+ <MoveMTCStageToTask child="{connect}" parent="{mtc_task}" />
+
+ <!-- Pick Object-->
+ <CreateMTCSerialContainer container_name="pick object" container="{pick_object}" />
+ <ExposeMTCTaskPropertiesToContainer from="{mtc_task}" to="{pick_object}" property_names="group,eef,hand,ik_frame" />
+ <ContainerConfigureInitFromMTCProperties stage="{pick_object}" property_names="group,eef,hand,ik_frame" source_flag="PARENT" />
+
+ <!-- Approach Object -->
+ <GeometryMsgsVector3Stamped frame_id="{hand_frame}" vector="0,0,1" vector3_stamped="{tcp_translate}"/>
+ <CreateMTCMoveRelativeTranslate stage_name="approach object"
+ stage="{approach_object}"
+ solver="{cartesian_planner}"
+ ik_frame="{ik_frame}"
+ direction="{tcp_translate}"
+ min_distance="0.1"
+ max_distance="0.15" />
+ <StageConfigureInitFromMTCProperties stage="{approach_object}" property_names="group" source_flag="PARENT" />
+ <MoveMTCStageToContainer child="{approach_object}" parent="{pick_object}" />
+
+ <!-- Generate Grasp Pose -->
+ <CreateMTCGenerateGraspPose stage_name="generate_grasp_pose"
+ object="{object_name}"
+ angle_delta="{angle}"
+ pregrasp_pose="open"
+ monitored_stage="{raw_stage}"
+ stage="{generate_pose}" />
+ <StageConfigureInitFromMTCProperties stage="{generate_pose}" property_names="" source_flag="PARENT" />
+ <GeometryMsgsPoseStamped frame_id="{hand_frame}" position="0,0,0.1" quaternion="0.270595,0.653228,-0.270861,0.653228" pose_stamped="{ik_frame2}"/>
+ <CreateMTCComputeIK stage_name="grasp pose IK"
+ wrapped_stage="{generate_pose}"
+ max_ik_solutions="8"
+ min_solution_distance="1.0"
+ stage="{grasp_pose_IK}"
+ ik_frame="{ik_frame2}" />
+ <StageConfigureInitFromMTCProperties stage="{grasp_pose_IK}" property_names="eef,group" source_flag="PARENT" />
+ <StageConfigureInitFromMTCProperties stage="{grasp_pose_IK}" property_names="target_pose" source_flag="INTERFACE" />
+ <MoveMTCStageToContainer child="{grasp_pose_IK}" parent="{pick_object}" />
+
+ <!-- Allow Collisions (hand object) -->
+ <AllowCollisionsJointModelGroup stage_name="allow collisions -> hand,object"
+ stage="{allow_collisions_object_robot}"
+ objects="{object_name}"
+ jmg_name="{hand_group_name}"
+ task="{mtc_task}" />
+ <MoveMTCStageToContainer child="{allow_collisions_object_robot}" parent="{pick_object}" />
+
+ <!-- Close Hand -->
+ <CreateMTCMoveToNamedJointPose name="close hand"
+ group="{hand_group_name}"
+ solver="{rrt_connect}"
+ goal="close"
+ stage="{stage_move_to_close_hand}" />
+ <MoveMTCStageToContainer child="{stage_move_to_close_hand}" parent="{pick_object}" />
+
+ <!-- Attach Object -->
+ <CreateMTCModifyPlanningSceneAttachObjects stage_name="attach object"
+ stage="{attach_object}"
+ object_names="{object_name}"
+ link_name="{hand_frame}" />
+ <MoveMTCStageToContainer child="{attach_object}" parent="{pick_object}" />
+
+ <!-- Allow Collision (object support) -->
+ <AllowCollisionPairs stage_name="allow collisions -> object,support"
+ stage="{allow_collisions_table}"
+ first="{object_name}"
+ second="table" />
+ <MoveMTCStageToContainer child="{allow_collisions_table}" parent="{pick_object}" />
+
+ <!-- Lift Object -->
+ <GeometryMsgsVector3Stamped frame_id="{reference_frame}" vector="0,0,1" vector3_stamped="{tcp_translate}"/>
+ <CreateMTCMoveRelativeTranslate stage_name="lift object"
+ stage="{lift_object}"
+ solver="{cartesian_planner}"
+ ik_frame="{ik_frame}"
+ direction="{tcp_translate}"
+ min_distance="0.01"
+ max_distance="0.1" />
+ <StageConfigureInitFromMTCProperties stage="{lift_object}" property_names="group" source_flag="PARENT" />
+ <MoveMTCStageToContainer child="{lift_object}" parent="{pick_object}" />
+
+ <!-- Forbid Collision (object support) -->
+ <ForbidCollisionPairs stage_name="forbid collisions -> object,support"
+ stage="{forbid_collisions_table}"
+ first="{object_name}"
+ second="table" />
+ <GetMTCRawStage stage="{forbid_collisions_table}" raw_stage="{raw_forbid_collisions_table}" />
+ <MoveMTCStageToContainer child="{forbid_collisions_table}" parent="{pick_object}" />
+
+ <!-- Add 'pick_object' Container to Task -->
+ <MoveMTCContainerToTask child="{pick_object}" parent="{mtc_task}" />
+
+ <!-- Move to Place -->
+ <CreateMTCConnect stage_name="Move To Place"
+ timeout="5.0"
+ group="{arm_group_name}"
+ planner="{rrt_connect}"
+ stage="{connectPlace}" />
+ <StageConfigureInitFromMTCProperties stage="{connectPlace}" property_names="" source_flag="PARENT" />
+ <MoveMTCStageToTask child="{connectPlace}" parent="{mtc_task}" />
+
+ <!-- Place Object -->
+ <CreateMTCSerialContainer container_name="place object" container="{place_object}" />
+ <ExposeMTCTaskPropertiesToContainer from="{mtc_task}" to="{place_object}" property_names="group,eef,hand" />
+ <ContainerConfigureInitFromMTCProperties stage="{place_object}" property_names="group,eef,hand" source_flag="PARENT" />
+
+
+ <!-- Lower Object -->
+ <GeometryMsgsVector3Stamped frame_id="{reference_frame}" vector="0,0,-1" vector3_stamped="{tcp_translate2}" />
+ <CreateMTCMoveRelativeTranslate stage_name="lower object"
+ stage="{lower_object}"
+ solver="{cartesian_planner}"
+ ik_frame="{ik_frame}"
+ direction="{tcp_translate2}"
+ min_distance="0.03"
+ max_distance="0.13" />
+ <StageConfigureInitFromMTCProperties stage="{lower_object}" property_names="group" source_flag="PARENT" />
+ <MoveMTCStageToContainer child="{lower_object}" parent="{place_object}" />
+
+ <!-- Generate Place Pose -->
+ <GeometryMsgsPoseStamped frame_id="world" position="0.6,-0.15,0.1251" quaternion="1,0,0,0" pose_stamped="{place_pose}" />
+ <CreateMTCGeneratePlacePose stage="{generate_place_pose}"
+ stage_name="generate place pose"
+ object="{object_name}"
+ pose="{place_pose}"
+ monitored_stage="{raw_forbid_collisions_table}" />
+ <CreateMTCComputeIK stage_name="place pose IK"
+ wrapped_stage="{generate_place_pose}"
+ max_ik_solutions="2"
+ stage="{place_pose_IK}"
+ ik_frame="{ik_frame2}" />
+ <StageConfigureInitFromMTCProperties stage="{place_pose_IK}" property_names="eef,group" source_flag="PARENT" />
+ <StageConfigureInitFromMTCProperties stage="{place_pose_IK}" property_names="target_pose" source_flag="INTERFACE" />
+ <MoveMTCStageToContainer child="{place_pose_IK}" parent="{place_object}" />
+
+ <!-- Open Hand -->
+ <CreateMTCMoveToNamedJointPose name="move to -> open_hand motion"
+ group="{hand_group_name}"
+ solver="{rrt_connect}"
+ goal="open"
+ stage="{stage_move_to_open_hand2}" />
+ <MoveMTCStageToContainer child="{stage_move_to_open_hand2}" parent="{place_object}" />
+
+ <!-- Forbid Collision (hand, object) -->
+ <ForbidCollisionsJointModelGroup stage_name="forbid collisions -> hand,object"
+ stage="{forbid_collisions_object_robot}"
+ objects="{object_name}"
+ jmg_name="{hand_group_name}"
+ task="{mtc_task}" />
+ <MoveMTCStageToContainer child="{forbid_collisions_object_robot}" parent="{place_object}" />
+
+ <!-- Detach Object -->
+ <CreateMTCModifyPlanningSceneDetachObjects stage_name="detach object"
+ stage="{detach_object}"
+ object_names="{object_name}"
+ link_name="{hand_frame}" />
+ <MoveMTCStageToContainer child="{detach_object}" parent="{place_object}" />
+
+ <!-- Retreat Motion -->
+ <GeometryMsgsVector3Stamped frame_id="{hand_frame}" vector="0,0,-1" vector3_stamped="{tcp_translate3}" />
+ <CreateMTCMoveRelativeTranslate stage_name="retreat after place"
+ stage="{retreat_after_place}"
+ solver="{cartesian_planner}"
+ ik_frame="{ik_frame}"
+ direction="{tcp_translate3}"
+ min_distance="0.12"
+ max_distance="0.25" />
+ <StageConfigureInitFromMTCProperties stage="{retreat_after_place}" property_names="group" source_flag="PARENT" />
+ <MoveMTCStageToContainer child="{retreat_after_place}" parent="{place_object}" />
+
+ <!-- Add 'place_object' Container to Task -->
+ <MoveMTCContainerToTask child="{place_object}" parent="{mtc_task}" />
+
+ <!-- Move to Home -->
+ <CreateMTCMoveToNamedJointPose name="move home"
+ solver="{rrt_connect}"
+ goal="ready"
+ stage="{stage_move_to_home}" />
+ <StageConfigureInitFromMTCProperties stage="{stage_move_to_home}" property_names="group" source_flag="PARENT" />
+ <MoveMTCStageToTask child="{stage_move_to_home}" parent="{mtc_task}" />
+
+ <!-- Plan the Task -->
+ <PlanMTCTask task="{mtc_task}" max_solutions="10" />
+ </Sequence>
+ </BehaviorTree>
+
+ </root>
+ )";
+
+// clang-format on
+
+int main(int argc, char** argv)
+{
+ rclcpp::init(argc, argv);
+ rclcpp::NodeOptions options;
+
+ // Declare parameters passed by a launch file
+ options.automatically_declare_parameters_from_overrides(true);
+ auto node = rclcpp::Node::make_shared("bt_mtc_demo", options);
+
+ BehaviorTreeFactory factory;
+ factory.registerNodeType<GeometryMsgsPose>("GeometryMsgsPose");
+ factory.registerNodeType<GeometryMsgsPoseStamped>("GeometryMsgsPoseStamped");
+ factory.registerNodeType<GeometryMsgsVector3Stamped>("GeometryMsgsVector3Stamped");
+ factory.registerNodeType<MoveItMsgsCollisionObjectCylinder>("MoveItMsgsCollisionObjectCylinder");
+ factory.registerNodeType<MoveItMsgsCollisionObjectBox>("MoveItMsgsCollisionObjectBox");
+ factory.registerNodeType<CreatePlanningSceneInterface>("CreatePlanningSceneInterface");
+ factory.registerNodeType<AddObjectToPlanningScene>("AddObjectToPlanningScene");
+ factory.registerNodeType<CreateMTCGenerateGraspPose>("CreateMTCGenerateGraspPose");
+ 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<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::ContainerBase>>("MoveMTCContainerToContainer");
+ factory.registerNodeType<MoveMTCStage<moveit::task_constructor::ContainerBase, moveit::task_constructor::Task>>("MoveMTCContainerToTask");
+ factory.registerNodeType<CreateMTCSerialContainer>("CreateMTCSerialContainer");
+ factory.registerNodeType<CreateMTCMoveRelativeTranslate>("CreateMTCMoveRelativeTranslate");
+ factory.registerNodeType<CreateMTCComputeIK>("CreateMTCComputeIK");
+ factory.registerNodeType<ExposeMTCProperties<moveit::task_constructor::ContainerBase, moveit::task_constructor::ContainerBase>>("ExposeMTCContainerPropertiesToContainer");
+ factory.registerNodeType<ExposeMTCProperties<moveit::task_constructor::Task, moveit::task_constructor::ContainerBase>>("ExposeMTCTaskPropertiesToContainer");
+ factory.registerNodeType<ConfigureInitFromMTCProperties<moveit::task_constructor::Stage>>("StageConfigureInitFromMTCProperties");
+ factory.registerNodeType<ConfigureInitFromMTCProperties<moveit::task_constructor::ContainerBase>>("ContainerConfigureInitFromMTCProperties");
+ factory.registerNodeType<SetMTCProperties<moveit::task_constructor::Stage, std::string>>("SetMTCStagePropertyString");
+ factory.registerNodeType<SetMTCProperties<moveit::task_constructor::Task, std::string>>("SetMTCTaskPropertyString");
+ factory.registerScriptingEnums<moveit::task_constructor::Stage::PropertyInitializerSource>();
+ factory.registerNodeType<CreateMTCCartesianPath>("CreateMTCCartesianPath");
+ factory.registerNodeType<CreateMTCModifyPlanningSceneAttachObjects>("CreateMTCModifyPlanningSceneAttachObjects");
+ factory.registerNodeType<CreateMTCModifyPlanningSceneDetachObjects>("CreateMTCModifyPlanningSceneDetachObjects");
+ factory.registerNodeType<AllowCollisionPairs>("AllowCollisionPairs");
+ factory.registerNodeType<AllowCollisionsJointModelGroup>("AllowCollisionsJointModelGroup");
+ factory.registerNodeType<ForbidCollisionsJointModelGroup>("ForbidCollisionsJointModelGroup");
+ factory.registerNodeType<ForbidCollisionPairs>("ForbidCollisionPairs");
+ factory.registerNodeType<CreateMTCGeneratePlacePose>("CreateMTCGeneratePlacePose");
+
+ auto tree = factory.createTreeFromText(xml_text);
+
+ // Connect the Groot2Publisher. This will allow Groot2 to
+ // get the tree and poll status updates.
+ const unsigned port = 1667;
+ BT::Groot2Publisher publisher(tree, port);
+
+ // Add two more loggers, to save the transitions into a file.
+ // Both formats are compatible with Groot2
+
+ // Logging with lightweight serialization
+ BT::FileLogger2 logger2(tree, "t12_logger2.btlog");
+
+ // Gives the user time to connect to Groot2
+ 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;
+
+ NodeStatus status = NodeStatus::IDLE;
+
+ // Lambda function for the timer callback to tick the behavior tree
+ auto tick_tree = [&tree, &status]() {
+ if (status == NodeStatus::IDLE || status == NodeStatus::RUNNING)
+ {
+ status = tree.tickExactlyOnce();
+ std::cout << "Status = " << status << std::endl;
+ }
+ };
+
+ // Timer to tick the behavior tree
+ auto timer = node->create_wall_timer(std::chrono::milliseconds(10), tick_tree);
+
+ // Spin the node (this will process callbacks including the timer)
+ rclcpp::spin(node);
+
+ // Shutdown when user press CTRL-C
+ rclcpp::shutdown();
+
+ return 0;
+}
diff --git a/examples/bt_serial_container.cpp b/examples/bt_serial_container.cpp
index 912a019..0728293 100644
--- a/examples/bt_serial_container.cpp
+++ b/examples/bt_serial_container.cpp
@@ -5,8 +5,7 @@
#include <behaviortree_mtc/create_mtc_move_relative.h>
#include <behaviortree_mtc/create_mtc_serial_container.h>
#include <behaviortree_mtc/create_mtc_pipeline_planner.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/plan_mtc_task.h>
#include <behaviortree_mtc/geometry_msgs.h>
@@ -25,12 +24,12 @@ static const char* xml_text = R"(
<BehaviorTree ID="MainTree">
<Sequence name="root">
- <InitializeMTCTask task="{mtc_task}" container="{container}" />
+ <InitializeMTCTask task="{mtc_task}" />
<CreateMTCPipelinePlanner pipeline_id="ompl"
planner_id="RRTConnect"
solver="{rrt_connect}" />
<CreateMTCCurrentState stage="{stage}" />
- <MoveMTCStageToContainer container="{container}" stage="{stage}" />
+ <MoveMTCStageToTask child="{stage}" parent="{mtc_task}" />
<!-- Translate Motion -> 200mm towards x axis wrt. panda_link8 -->
<GeometryMsgsPoseStamped frame_id="panda_link8" position="0,0,0" quaternion="1,0,0,0" pose_stamped="{ik_frame}"/>
@@ -41,7 +40,7 @@ static const char* xml_text = R"(
ik_frame="{ik_frame}"
direction="{tcp_translate}"
stage="{stage_move_rel_translate}" />
- <MoveMTCStageToContainer container="{container}" stage="{stage_move_rel_translate}" />
+ <MoveMTCStageToTask child="{stage_move_rel_translate}" parent="{mtc_task}" />
<CreateMTCSerialContainer container_name="approach_object"
container="{serial_container}"/>
<!-- Translate Motion-->
@@ -52,8 +51,8 @@ static const char* xml_text = R"(
ik_frame="{ik_frame}"
direction="{tcp_translate}"
stage="{stage_move_rel_translate2}" />
- <MoveMTCStageToContainer container="{serial_container}" stage="{stage_move_rel_translate2}" />
- <MoveMTCContainerToParentContainer parent_container="{container}" child_container="{serial_container}" />
+ <MoveMTCStageToContainer child="{stage_move_rel_translate2}" parent="{serial_container}" />
+ <MoveMTCContainerToTask child="{serial_container}" parent="{mtc_task}" />
<PlanMTCTask task="{mtc_task}" max_solutions="5" />
</Sequence>
</BehaviorTree>
@@ -69,7 +68,7 @@ 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;
@@ -78,8 +77,9 @@ int main(int argc, char** argv)
factory.registerNodeType<CreateMTCSerialContainer>("CreateMTCSerialContainer");
factory.registerNodeType<CreateMTCMoveRelativeTranslate>("CreateMTCMoveRelativeTranslate");
factory.registerNodeType<CreateMTCCurrentState>("CreateMTCCurrentState");
- factory.registerNodeType<MoveMTCStageToContainer>("MoveMTCStageToContainer");
- 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<PlanMTCTask>("PlanMTCTask");
factory.registerNodeType<GeometryMsgsPoseStamped>("GeometryMsgsPoseStamped");
@@ -99,11 +99,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::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;
diff --git a/include/behaviortree_mtc/move_mtc_container_to_parent_container.h b/include/behaviortree_mtc/move_mtc_container_to_parent_container.h
deleted file mode 100644
index cfd909f..0000000
--- a/include/behaviortree_mtc/move_mtc_container_to_parent_container.h
+++ /dev/null
@@ -1,18 +0,0 @@
-#pragma once
-
-#include <behaviortree_cpp/bt_factory.h>
-
-namespace BT {
-namespace MTC {
-
-class MoveMTCContainerToParentContainer : public BT::SyncActionNode
-{
-public:
- MoveMTCContainerToParentContainer(const std::string& name, const BT::NodeConfig& config);
-
- BT::NodeStatus tick() override;
- static BT::PortsList providedPorts();
-};
-
-} // namespace MTC
-} // namespace BT
diff --git a/include/behaviortree_mtc/move_mtc_stage.h b/include/behaviortree_mtc/move_mtc_stage.h
new file mode 100644
index 0000000..6314c74
--- /dev/null
+++ b/include/behaviortree_mtc/move_mtc_stage.h
@@ -0,0 +1,63 @@
+#pragma once
+
+#include <behaviortree_cpp/bt_factory.h>
+#include <behaviortree_mtc/shared_to_unique.h>
+
+#include <moveit/task_constructor/task.h>
+
+namespace BT {
+namespace MTC {
+
+/// Move MTC child stage/container to parent container/task
+template <typename C, typename P>
+class MoveMTCStage : public BT::SyncActionNode
+{
+ static_assert(std::is_base_of<moveit::task_constructor::Stage, C>::value, "C must inherit from moveit::task_constructor::Stage");
+ static_assert(std::is_base_of<moveit::task_constructor::Stage, P>::value, "P must inherit from moveit::task_constructor::Stage");
+
+public:
+ MoveMTCStage(const std::string& name, const BT::NodeConfig& config)
+ : SyncActionNode(name, config)
+ {}
+
+ BT::NodeStatus tick() override
+ {
+ // Transform stage from shared to unique
+ moveit::task_constructor::Stage::pointer unique_child{ nullptr };
+ if(auto any_child_ptr = getLockedPortContent("child"))
+ {
+ if(auto* stage_ptr = any_child_ptr->template castPtr<std::shared_ptr<C>>())
+ {
+ auto& stage = *stage_ptr;
+
+ unique_child = sharedToUnique(stage);
+ any_child_ptr.assign(nullptr); // set blackboard value to nullptr
+ }
+ }
+
+ if(auto any_parent_ptr = getLockedPortContent("parent"); unique_child)
+ {
+ if(auto* parent_ptr = any_parent_ptr->template castPtr<std::shared_ptr<P>>())
+ {
+ auto& parent = *parent_ptr;
+
+ parent->add(std::move(unique_child));
+
+ return NodeStatus::SUCCESS;
+ }
+ }
+
+ return NodeStatus::FAILURE;
+ }
+
+ static BT::PortsList providedPorts()
+ {
+ return {
+ BT::InputPort<std::shared_ptr<C>>("child"),
+ BT::BidirectionalPort<std::shared_ptr<P>>("parent"),
+ };
+ }
+};
+
+} // namespace MTC
+} // namespace BT
diff --git a/include/behaviortree_mtc/move_mtc_stage_to_container.h b/include/behaviortree_mtc/move_mtc_stage_to_container.h
deleted file mode 100644
index 99f1162..0000000
--- a/include/behaviortree_mtc/move_mtc_stage_to_container.h
+++ /dev/null
@@ -1,18 +0,0 @@
-#pragma once
-
-#include <behaviortree_cpp/bt_factory.h>
-
-namespace BT {
-namespace MTC {
-
-class MoveMTCStageToContainer : public BT::SyncActionNode
-{
-public:
- MoveMTCStageToContainer(const std::string& name, const BT::NodeConfig& config);
-
- BT::NodeStatus tick() override;
- static BT::PortsList providedPorts();
-};
-
-} // namespace MTC
-} // namespace BT
diff --git a/include/behaviortree_mtc/plan_mtc_task.h b/include/behaviortree_mtc/plan_mtc_task.h
index da37688..1879eec 100644
--- a/include/behaviortree_mtc/plan_mtc_task.h
+++ b/include/behaviortree_mtc/plan_mtc_task.h
@@ -1,6 +1,7 @@
#pragma once
#include <behaviortree_cpp/bt_factory.h>
+#include <moveit/task_constructor/task.h>
namespace BT {
namespace MTC {
@@ -11,7 +12,13 @@ class PlanMTCTask : public BT::ThreadedAction
PlanMTCTask(const std::string& name, const BT::NodeConfig& config);
BT::NodeStatus tick() override;
+ void halt() override;
static BT::PortsList providedPorts();
+
+private:
+ moveit::task_constructor::TaskPtr task_;
+
+ std::mutex task_mutex_;
};
} // namespace MTC
diff --git a/launch/run.launch.py b/launch/run.launch.py
new file mode 100644
index 0000000..5c4bb38
--- /dev/null
+++ b/launch/run.launch.py
@@ -0,0 +1,38 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from moveit_configs_utils import MoveItConfigsBuilder
+
+
+def generate_launch_description():
+ delay_ms_arg = DeclareLaunchArgument(
+ 'delay_ms',
+ default_value='0',
+ description='Delay (in milliseconds) to give the user time to connect to Groot2'
+ )
+
+ moveit_config = (
+ MoveItConfigsBuilder("moveit_resources_panda")
+ .robot_description(file_path="config/panda.urdf.xacro")
+ .to_moveit_configs()
+ )
+
+ node = Node(
+ package="behaviortree_mtc",
+ executable="bt_pick_place",
+ output="screen",
+ parameters=[
+ moveit_config.robot_description,
+ moveit_config.robot_description_semantic,
+ moveit_config.robot_description_kinematics,
+ moveit_config.joint_limits,
+ moveit_config.planning_pipelines,
+ os.path.join(get_package_share_directory("moveit_task_constructor_demo"), "config", "panda_config.yaml"),
+ {'delay_ms': LaunchConfiguration('delay_ms')}
+ ],
+ )
+
+ return LaunchDescription([delay_ms_arg, node])
\ No newline at end of file
diff --git a/package.xml b/package.xml
index 964b8af..0b0c076 100644
--- a/package.xml
+++ b/package.xml
@@ -11,6 +11,7 @@
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>behaviortree_cpp</depend>
+ <depend>behaviortree_ros2</depend>
<depend>geometry_msgs</depend>
<depend>moveit_msgs</depend>
<depend>moveit_ros_planning_interface</depend>
diff --git a/src/create_mtc_cartesian_path.cpp b/src/create_mtc_cartesian_path.cpp
index 5a621c3..0267279 100644
--- a/src/create_mtc_cartesian_path.cpp
+++ b/src/create_mtc_cartesian_path.cpp
@@ -18,23 +18,24 @@ BT::NodeStatus CreateMTCCartesianPath::tick()
double max_velocity_scaling_factor;
double max_acceleration_scaling_factor;
double step_size;
- double jump_treshold;
double min_fraction;
+ moveit::core::CartesianPrecision precision;
if(!getInput("max_velocity_scaling_factor", max_velocity_scaling_factor) ||
!getInput("max_acceleration_scaling_factor", max_acceleration_scaling_factor) ||
!getInput("step_size", step_size) ||
!getInput("min_fraction", min_fraction) ||
- !getInput("jump_threshold", jump_treshold))
+ !getInput("precision", precision))
return NodeStatus::FAILURE;
- //build solver
+ // Build solver
auto solver = std::make_shared<moveit::task_constructor::solvers::CartesianPath>();
solver->setMaxVelocityScalingFactor(max_velocity_scaling_factor);
solver->setMaxAccelerationScalingFactor(max_acceleration_scaling_factor);
solver->setStepSize(step_size);
- solver->setJumpThreshold(jump_treshold);
solver->setMinFraction(min_fraction);
+ solver->setPrecision(precision);
+
// Upcast to base class
moveit::task_constructor::solvers::PlannerInterfacePtr base_solver = solver;
@@ -47,9 +48,9 @@ BT::PortsList CreateMTCCartesianPath::providedPorts()
return {
BT::InputPort<double>("max_velocity_scaling_factor", 0.1, "scale down max velocity by this factor"),
BT::InputPort<double>("max_acceleration_scaling_factor", 0.1, "scale down max acceleration by this factor"),
- BT::InputPort<double>("step_size", 0.01, "step size between consecutive waypoints"),
BT::InputPort<double>("min_fraction", 1.0, "fraction of motion required for success"),
- BT::InputPort<double>("jump_threshold", 1.5, "acceptable fraction of mean joint motion per step"),
+ BT::InputPort<moveit::core::CartesianPrecision>("precision", moveit::core::CartesianPrecision({ .translational = 0.001, .rotational = 0.01, .max_resolution = 1e-5 }), "linear and rotational precision"),
+ BT::InputPort<double>("step_size", 0.01, "step size between consecutive waypoints"),
BT::OutputPort<moveit::task_constructor::solvers::PlannerInterfacePtr>("solver", "{solver}", "Planner interface using pipeline motion solver"),
};
}
diff --git a/src/create_mtc_compute_ik.cpp b/src/create_mtc_compute_ik.cpp
index aeef324..a32b96f 100644
--- a/src/create_mtc_compute_ik.cpp
+++ b/src/create_mtc_compute_ik.cpp
@@ -26,8 +26,6 @@ BT::NodeStatus CreateMTCComputeIK::tick()
std::shared_ptr<geometry_msgs::msg::PoseStamped> ik_frame{ nullptr };
if(!getInput("stage_name", name) ||
- !getInput("eef", eef) ||
- !getInput("group", group) ||
!getInput("ignore_collisions", ignore_collisions) ||
!getInput("min_solution_distance", min_solution_distance) ||
!getInput("max_ik_solutions", max_ik_solutions) ||
@@ -43,8 +41,13 @@ BT::NodeStatus CreateMTCComputeIK::tick()
new moveit::task_constructor::stages::ComputeIK(name, std::move(unique_stage)),
dirty::fake_deleter{}
};
- wrapper->setEndEffector(eef);
- wrapper->setGroup(group);
+
+ // Optionnal
+ if(getInput("group", group))
+ wrapper->setGroup(group);
+ if(getInput("eef", eef))
+ wrapper->setEndEffector(eef);
+
wrapper->setMaxIKSolutions(max_ik_solutions);
wrapper->setIgnoreCollisions(ignore_collisions);
wrapper->setMinSolutionDistance(min_solution_distance);
diff --git a/src/create_mtc_generate_grasp_pose.cpp b/src/create_mtc_generate_grasp_pose.cpp
index ee5bc6f..823e7f1 100644
--- a/src/create_mtc_generate_grasp_pose.cpp
+++ b/src/create_mtc_generate_grasp_pose.cpp
@@ -21,7 +21,6 @@ BT::NodeStatus CreateMTCGenerateGraspPose::tick()
Eigen::Vector3d rotation_axis;
moveit::task_constructor::Stage* monitored_stage;
if(!getInput("stage_name", name) ||
- !getInput("eef", eef) ||
!getInput("angle_delta", angle_delta) ||
!getInput("object", object) ||
!getInput("pregrasp_pose", pregrasp_pose) ||
@@ -35,10 +34,14 @@ BT::NodeStatus CreateMTCGenerateGraspPose::tick()
new moveit::task_constructor::stages::GenerateGraspPose(name),
dirty::fake_deleter{}
};
+
+ //Optionnal
+ if(getInput("eef", eef))
+ stage->setEndEffector(eef);
+
rotation_axis[0] = rotation_axis_input.x;
rotation_axis[1] = rotation_axis_input.y;
rotation_axis[2] = rotation_axis_input.z;
- stage->setEndEffector(eef);
stage->setObject(object);
stage->setAngleDelta(angle_delta);
stage->setRotationAxis(rotation_axis);
diff --git a/src/create_mtc_move_relative.cpp b/src/create_mtc_move_relative.cpp
index 289e274..b049120 100644
--- a/src/create_mtc_move_relative.cpp
+++ b/src/create_mtc_move_relative.cpp
@@ -23,7 +23,6 @@ BT::NodeStatus CreateMTCMoveRelativeBase::tick(std::shared_ptr<moveit::task_cons
double timeout;
if(!getInput("stage_name", name) ||
- !getInput("group", group) ||
!getInput("solver", solver) ||
!getInput("ik_frame", ik_frame) ||
!getInput("min_distance", min_distance) ||
@@ -37,7 +36,10 @@ BT::NodeStatus CreateMTCMoveRelativeBase::tick(std::shared_ptr<moveit::task_cons
dirty::fake_deleter{}
};
- stage->setGroup(group);
+ //Optionnal
+ if(getInput("group", group))
+ stage->setGroup(group);
+
stage->setIKFrame(*ik_frame);
stage->setMinMaxDistance(min_distance, max_distance);
stage->setTimeout(timeout);
diff --git a/src/create_mtc_move_to.cpp b/src/create_mtc_move_to.cpp
index b2726db..97fddcd 100644
--- a/src/create_mtc_move_to.cpp
+++ b/src/create_mtc_move_to.cpp
@@ -22,7 +22,6 @@ BT::NodeStatus CreateMTCMoveToBase::tick(std::shared_ptr<moveit::task_constructo
double timeout;
if(!getInput("stage_name", name) ||
- !getInput("group", group) ||
!getInput("solver", solver) ||
!getInput("timeout", timeout))
return NodeStatus::FAILURE;
@@ -32,8 +31,10 @@ BT::NodeStatus CreateMTCMoveToBase::tick(std::shared_ptr<moveit::task_constructo
new moveit::task_constructor::stages::MoveTo(name, solver),
dirty::fake_deleter{}
};
+ // Optionnal
+ if(getInput("group", group))
+ stage->setGroup(group);
- stage->setGroup(group);
stage->setTimeout(timeout);
return NodeStatus::SUCCESS;
diff --git a/src/create_mtc_pipeline_planner.cpp b/src/create_mtc_pipeline_planner.cpp
index 88c901e..7d77650 100644
--- a/src/create_mtc_pipeline_planner.cpp
+++ b/src/create_mtc_pipeline_planner.cpp
@@ -42,8 +42,7 @@ BT::NodeStatus CreateMTCPipelinePlanner::tick()
!getInput("num_planning_attempts", num_planning_attemps))
return NodeStatus::FAILURE;
//build solver
- auto solver = std::make_shared<moveit::task_constructor::solvers::PipelinePlanner>(node_, pipeline_id);
- solver->setPlannerId(planner_id);
+ auto solver = std::make_shared<moveit::task_constructor::solvers::PipelinePlanner>(node_, pipeline_id, planner_id);
solver->setMaxVelocityScalingFactor(max_velocity_scaling_factor);
solver->setMaxAccelerationScalingFactor(max_acceleration_scaling_factor);
solver->setProperty("goal_joint_tolerance", goal_joint_tolerance);
@@ -68,8 +67,8 @@ BT::PortsList CreateMTCPipelinePlanner::providedPorts()
BT::InputPort<double>("goal_joint_tolerance", "1e-4", "tolerance for reaching joint goals"),
BT::InputPort<double>("goal_position_tolerance", "1e-4", "tolerance for reaching position goals"),
BT::InputPort<double>("goal_orientation_tolerance", "1e-4", "tolerance for reaching orientation goals"),
- BT::InputPort<bool>("display_motion_plans", false, "publish generated solutions on topic " + planning_pipeline::PlanningPipeline::DISPLAY_PATH_TOPIC),
- BT::InputPort<bool>("publish_planning_requests", false, "publish motion planning requests on topic " + planning_pipeline::PlanningPipeline::MOTION_PLAN_REQUEST_TOPIC),
+ BT::InputPort<bool>("display_motion_plans", false, "publish generated solutions on topic "),
+ BT::InputPort<bool>("publish_planning_requests", false, "publish motion planning requests on topic "),
BT::InputPort<std::string>("planner_id"),
BT::InputPort<std::string>("pipeline_id"),
BT::InputPort<uint>("num_planning_attempts", "1u", "number of planning attempts"),
diff --git a/src/generate_xml_model.cpp b/src/generate_xml_model.cpp
new file mode 100644
index 0000000..3fbbd5e
--- /dev/null
+++ b/src/generate_xml_model.cpp
@@ -0,0 +1,101 @@
+#include <behaviortree_mtc/initialize_mtc_task.h>
+#include <behaviortree_mtc/create_mtc_current_state.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>
+#include <behaviortree_mtc/geometry_msgs.h>
+#include <behaviortree_mtc/create_mtc_pipeline_planner.h>
+#include <behaviortree_mtc/create_planning_scene_interface.h>
+#include <behaviortree_mtc/add_object_to_planning_scene.h>
+#include <behaviortree_mtc/plan_mtc_task.h>
+#include <behaviortree_mtc/create_mtc_generate_grasp_pose.h>
+#include <behaviortree_mtc/get_mtc_raw_stage.h>
+#include <behaviortree_mtc/create_mtc_move_to.h>
+#include <behaviortree_mtc/create_mtc_connect.h>
+#include <behaviortree_mtc/create_mtc_move_relative.h>
+#include <behaviortree_mtc/create_mtc_compute_ik.h>
+#include <behaviortree_mtc/configure_init_from_mtc_properties.h>
+#include <behaviortree_mtc/std_containers.h>
+#include <behaviortree_mtc/create_mtc_cartesian_path.h>
+#include <behaviortree_mtc/create_mtc_modify_planning_scene.h>
+#include <behaviortree_mtc/create_mtc_generate_place_pose.h>
+#include <behaviortree_mtc/create_mtc_joint_interpolation.h>
+
+#include "behaviortree_cpp/bt_factory.h"
+#include "behaviortree_cpp/loggers/bt_file_logger_v2.h"
+#include "behaviortree_cpp/loggers/groot2_publisher.h"
+#include "behaviortree_cpp/xml_parsing.h"
+
+using namespace BT;
+using namespace BT::MTC;
+
+int main(int argc, char** argv)
+{
+ rclcpp::init(argc, argv);
+ rclcpp::NodeOptions options;
+
+ // Declare parameters passed by a launch file
+ options.automatically_declare_parameters_from_overrides(true);
+ auto node = rclcpp::Node::make_shared("bt_mtc_demo", options);
+
+ BehaviorTreeFactory factory;
+
+ factory.registerNodeType<GeometryMsgsPose>("GeometryMsgsPose");
+ factory.registerNodeType<GeometryMsgsTwistStamped>("GeometryMsgsTwistStamped");
+ factory.registerNodeType<GeometryMsgsPoseStamped>("GeometryMsgsPoseStamped");
+ factory.registerNodeType<GeometryMsgsVector3Stamped>("GeometryMsgsVector3Stamped");
+ factory.registerNodeType<MoveItMsgsCollisionObjectCylinder>("MoveItMsgsCollisionObjectCylinder");
+ factory.registerNodeType<MoveItMsgsCollisionObjectBox>("MoveItMsgsCollisionObjectBox");
+ factory.registerNodeType<CreatePlanningSceneInterface>("CreatePlanningSceneInterface");
+ factory.registerNodeType<AddObjectToPlanningScene>("AddObjectToPlanningScene");
+ 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::ContainerBase>>("MoveMTCContainerToContainer");
+ factory.registerNodeType<MoveMTCStage<moveit::task_constructor::ContainerBase, moveit::task_constructor::Task>>("MoveMTCContainerToTask");
+ factory.registerNodeType<InitializeMTCTask>("InitializeMTCTask", BT::RosNodeParams(node));
+ factory.registerNodeType<CreateMTCCurrentState>("CreateMTCCurrentState");
+ factory.registerNodeType<CreateMTCPipelinePlanner>("CreateMTCPipelinePlanner", BT::RosNodeParams(node));
+ factory.registerNodeType<CreateMTCCartesianPath>("CreateMTCCartesianPath");
+ factory.registerNodeType<CreateMTCJointInterpolation>("CreateMTCJointInterpolation");
+ factory.registerNodeType<PlanMTCTask>("PlanMTCTask");
+ factory.registerNodeType<GetMTCRawStage>("GetMTCRawStage");
+ factory.registerNodeType<CreateMTCMoveToNamedJointPose>("CreateMTCMoveToNamedJointPose");
+ factory.registerNodeType<CreateMTCMoveToJoint>("CreateMTCMoveToJoint");
+ factory.registerNodeType<CreateMTCMoveToPoint>("CreateMTCMoveToPoint");
+ factory.registerNodeType<CreateMTCMoveToPose>("CreateMTCMoveToPose");
+ factory.registerNodeType<CreateMTCMoveRelativeTranslate>("CreateMTCMoveRelativeTranslate");
+ factory.registerNodeType<CreateMTCMoveRelativeJoint>("CreateMTCMoveRelativeJoint");
+ factory.registerNodeType<CreateMTCMoveRelativeTwist>("CreateMTCMoveRelativeTwist");
+ factory.registerNodeType<CreateMTCConnect>("CreateMTCConnect");
+ factory.registerNodeType<CreateMTCSerialContainer>("CreateMTCSerialContainer");
+ factory.registerNodeType<CreateMTCComputeIK>("CreateMTCComputeIK");
+ factory.registerNodeType<ConfigureInitFromMTCProperties<moveit::task_constructor::Stage>>("ConfigureInitFromMTCProperties");
+ factory.registerScriptingEnums<moveit::task_constructor::Stage::PropertyInitializerSource>();
+ factory.registerNodeType<CreateMTCModifyPlanningSceneAttachObjects>("CreateMTCModifyPlanningSceneAttachObjects");
+ factory.registerNodeType<CreateMTCModifyPlanningSceneDetachObjects>("CreateMTCModifyPlanningSceneDetachObjects");
+ factory.registerNodeType<ForbidCollisionPairs>("ForbidCollisionPairs");
+ factory.registerNodeType<AllowCollisionPairs>("AllowCollisionPairs");
+ factory.registerNodeType<AllowAllCollisions>("AllowAllCollisions");
+ factory.registerNodeType<ForbidAllCollisions>("ForbidAllCollisions");
+ factory.registerNodeType<AllowCollisionsJointModelGroup>("AllowCollisionsJointModelGroup");
+ factory.registerNodeType<ForbidCollisionsJointModelGroup>("ForbidCollisionsJointModelGroup");
+ factory.registerNodeType<CreateMTCGenerateGraspPose>("CreateMTCGenerateGraspPose");
+ factory.registerNodeType<CreateMTCGeneratePlacePose>("CreateMTCGeneratePlacePose");
+
+ std::string xml_models = BT::writeTreeNodesModelXML(factory);
+ std::string filePath = "./config/bt_mtc_model.xml";
+ std::ofstream file(filePath);
+ if(file.is_open())
+ {
+ file << xml_models;
+ file.close();
+ std::cout << "File saved successfully: " << filePath << std::endl;
+ }
+ else
+ {
+ std::cerr << "Unable to open file: " << filePath << std::endl;
+ }
+
+ return 0;
+}
diff --git a/src/initialize_mtc_task.cpp b/src/initialize_mtc_task.cpp
index 0467606..f4b2167 100644
--- a/src/initialize_mtc_task.cpp
+++ b/src/initialize_mtc_task.cpp
@@ -16,11 +16,7 @@ BT::NodeStatus InitializeMTCTask::tick()
auto task = std::make_shared<moveit::task_constructor::Task>();
task->loadRobotModel(node_);
- //Convert raw pointer to share pointer
- auto container = std::shared_ptr<moveit::task_constructor::ContainerBase>(task->stages());
-
setOutput("task", task);
- setOutput("container", container);
return NodeStatus::SUCCESS;
}
@@ -30,8 +26,6 @@ BT::PortsList InitializeMTCTask::providedPorts()
return {
BT::OutputPort<moveit::task_constructor::TaskPtr>("task", "{mtc_task}",
"MoveIt Task Constructor task."),
- BT::OutputPort<moveit::task_constructor::ContainerBasePtr>("container"),
-
};
}
diff --git a/src/move_mtc_container_to_parent_container.cpp b/src/move_mtc_container_to_parent_container.cpp
deleted file mode 100644
index 41bfd51..0000000
--- a/src/move_mtc_container_to_parent_container.cpp
+++ /dev/null
@@ -1,54 +0,0 @@
-#include <behaviortree_mtc/move_mtc_container_to_parent_container.h>
-#include <behaviortree_mtc/shared_to_unique.h>
-
-#include <moveit/task_constructor/container.h>
-#include <moveit/task_constructor/task.h>
-
-namespace BT {
-namespace MTC {
-
-MoveMTCContainerToParentContainer::MoveMTCContainerToParentContainer(const std::string& name,
- const BT::NodeConfig& config)
- : SyncActionNode(name, config)
-{}
-
-BT::NodeStatus MoveMTCContainerToParentContainer::tick()
-{
- // Transform container from shared to unique
- moveit::task_constructor::ContainerBase::pointer unique_child_container{ nullptr };
- if(auto any_container_ptr = getLockedPortContent("child_container"))
- {
- if(auto* container_ptr = any_container_ptr->castPtr<moveit::task_constructor::ContainerBasePtr>())
- {
- auto& child_container = *container_ptr;
-
- unique_child_container = sharedToUnique(child_container);
- any_container_ptr.assign(nullptr); // set blackboard value to nullptr
- }
- }
-
- if(auto any_container_ptr = getLockedPortContent("parent_container"); unique_child_container)
- {
- if(auto* container_ptr = any_container_ptr->castPtr<moveit::task_constructor::ContainerBasePtr>())
- {
- auto& parent_container = *container_ptr;
-
- parent_container->add(std::move(unique_child_container));
-
- return NodeStatus::SUCCESS;
- }
- }
-
- return NodeStatus::SUCCESS;
-}
-
-BT::PortsList MoveMTCContainerToParentContainer::providedPorts()
-{
- return {
- BT::InputPort<moveit::task_constructor::ContainerBasePtr>("child_container"),
- BT::BidirectionalPort<moveit::task_constructor::ContainerBasePtr>("parent_container"),
- };
-}
-
-} // namespace MTC
-} // namespace BT
diff --git a/src/move_mtc_stage_to_container.cpp b/src/move_mtc_stage_to_container.cpp
deleted file mode 100644
index db082d5..0000000
--- a/src/move_mtc_stage_to_container.cpp
+++ /dev/null
@@ -1,54 +0,0 @@
-#include <behaviortree_mtc/move_mtc_stage_to_container.h>
-#include <behaviortree_mtc/shared_to_unique.h>
-
-#include <moveit/task_constructor/container.h>
-#include <moveit/task_constructor/task.h>
-
-namespace BT {
-namespace MTC {
-
-MoveMTCStageToContainer::MoveMTCStageToContainer(const std::string& name,
- const BT::NodeConfig& config)
- : SyncActionNode(name, config)
-{}
-
-BT::NodeStatus MoveMTCStageToContainer::tick()
-{
- // Transform stage from shared to unique
- moveit::task_constructor::Stage::pointer unique_stage{ nullptr };
- if(auto any_stage_ptr = getLockedPortContent("stage"))
- {
- if(auto* stage_ptr = any_stage_ptr->castPtr<moveit::task_constructor::StagePtr>())
- {
- auto& stage = *stage_ptr;
-
- unique_stage = sharedToUnique(stage);
- any_stage_ptr.assign(nullptr); // set blackboard value to nullptr
- }
- }
-
- if(auto any_container_ptr = getLockedPortContent("container"); unique_stage)
- {
- if(auto* container_ptr = any_container_ptr->castPtr<moveit::task_constructor::ContainerBasePtr>())
- {
- auto& container = *container_ptr;
-
- container->add(std::move(unique_stage));
-
- return NodeStatus::SUCCESS;
- }
- }
-
- return NodeStatus::FAILURE;
-}
-
-BT::PortsList MoveMTCStageToContainer::providedPorts()
-{
- return {
- BT::InputPort<moveit::task_constructor::StagePtr>("stage"),
- BT::BidirectionalPort<moveit::task_constructor::ContainerBasePtr>("container"),
- };
-}
-
-} // namespace MTC
-} // namespace BT
diff --git a/src/plan_mtc_task.cpp b/src/plan_mtc_task.cpp
index 3d38db1..1da60f7 100644
--- a/src/plan_mtc_task.cpp
+++ b/src/plan_mtc_task.cpp
@@ -1,6 +1,5 @@
#include <behaviortree_mtc/plan_mtc_task.h>
-
-#include <moveit/task_constructor/task.h>
+#include <moveit/utils/moveit_error_code.h>
namespace BT {
namespace MTC {
@@ -11,21 +10,64 @@ PlanMTCTask::PlanMTCTask(const std::string& name, const BT::NodeConfig& config)
BT::NodeStatus PlanMTCTask::tick()
{
+ // Inputs
const size_t max_solutions = getInput<size_t>("max_solutions").value();
if(auto any_ptr = getLockedPortContent("task"))
{
if(auto* task_ptr = any_ptr->castPtr<moveit::task_constructor::TaskPtr>())
{
auto& task = *task_ptr;
- if(task->plan(max_solutions))
+
+ // copy task internally
{
- return NodeStatus::SUCCESS;
+ std::unique_lock<std::mutex> lock{ task_mutex_ };
+ task_ = task;
}
}
}
+
+ if(task_)
+ {
+ moveit::core::MoveItErrorCode ec;
+ try
+ {
+ task_->resetPreemptRequest(); // should not be needed
+ ec = task_->plan(max_solutions);
+ std::cout << moveit::core::errorCodeToString(ec) << std::endl;
+ }
+ catch(moveit::task_constructor::InitStageException e)
+ {
+ std::cout << e << std::endl;
+ return NodeStatus::FAILURE;
+ }
+
+ {
+ std::unique_lock<std::mutex> lock{ task_mutex_ };
+ task_ = nullptr;
+ }
+
+ if(ec.val == ec.SUCCESS)
+ return NodeStatus::SUCCESS;
+ }
+
return NodeStatus::FAILURE;
}
+void PlanMTCTask::halt()
+{
+ {
+ std::unique_lock<std::mutex> lock{ task_mutex_ };
+
+ if(task_)
+ {
+ task_->preempt();
+ }
+ }
+
+ // halt base class
+ ThreadedAction::halt();
+}
+
BT::PortsList PlanMTCTask::providedPorts()
{
return {
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index d7e8421..945a7eb 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -2,14 +2,19 @@
# TESTS
set(BT_MTC_TESTS
- gtest_mtc_properties.cpp
gtest_mtc_modify_planning_scene.cpp
+ gtest_mtc_properties.cpp
+ gtest_plan_mtc_task.cpp
)
set(TEST_DEPENDENCIES
+ gtest_utils
${PROJECT_NAME}
)
+add_library(gtest_utils models.cpp stage_mockups.cpp)
+target_link_libraries(gtest_utils ${PROJECT_NAME} ${moveit_task_constructor_LIBRARIES})
+
# Enable testing and find necessary packages
if(BUILD_TESTING)
find_package(ament_cmake_gtest REQUIRED)
@@ -20,4 +25,4 @@ if(BUILD_TESTING)
${TEST_DEPENDENCIES}
)
endif()
-endif()
+endif()
\ No newline at end of file
diff --git a/tests/gtest_plan_mtc_task.cpp b/tests/gtest_plan_mtc_task.cpp
new file mode 100644
index 0000000..cfd24ed
--- /dev/null
+++ b/tests/gtest_plan_mtc_task.cpp
@@ -0,0 +1,57 @@
+#include <gtest/gtest.h>
+// #include <ros/ros.h>
+#include <behaviortree_cpp/bt_factory.h>
+#include <behaviortree_mtc/plan_mtc_task.h>
+#include "stage_mockups.h"
+#include "models.h"
+
+using namespace BT;
+using namespace BT::MTC;
+
+TEST(PlanMTCTask, Preempt)
+{
+ BT::BehaviorTreeFactory factory;
+
+ // clang-format off
+
+ static const char* xml_text = R"(
+
+ <root BTCPP_format="4" >
+ <BehaviorTree ID="MainTree">
+ <Sequence>
+ <Script code="force_failure := false"/>
+
+ <ReactiveSequence name="root">
+
+ <AlwaysSuccess _failureIf="force_failure==true" _onSuccess="force_failure=true"/>
+ <PlanMTCTask task="{mtc_task}" max_solutions="1"/>
+ </ReactiveSequence>
+
+ </Sequence>
+
+ </BehaviorTree>
+ </root>)";
+
+ // clang-format off
+
+ factory.registerNodeType<PlanMTCTask>("PlanMTCTask");
+
+
+ // setup mtc task to be preempted
+ moveit::task_constructor::TaskPtr task = std::make_shared<moveit::task_constructor::Task>();
+ task->setRobotModel(getModel());
+
+ auto timeout = std::chrono::milliseconds(10);
+ task->add(std::make_unique<moveit::task_constructor::GeneratorMockup>(moveit::task_constructor::PredefinedCosts::constant(0.0)));
+ task->add(std::make_unique<moveit::task_constructor::TimedForwardMockup>(timeout));
+
+ // setup blackbaord
+ auto bb = BT::Blackboard::create();
+ bb->set("mtc_task", task);
+
+
+ // test
+ auto tree = factory.createTreeFromText(xml_text, bb);
+ EXPECT_EQ(tree.tickExactlyOnce(), NodeStatus::RUNNING);
+ EXPECT_EQ(tree.tickExactlyOnce(), NodeStatus::FAILURE);
+}
diff --git a/tests/models.cpp b/tests/models.cpp
new file mode 100644
index 0000000..a75208f
--- /dev/null
+++ b/tests/models.cpp
@@ -0,0 +1,29 @@
+#include "models.h"
+#include <moveit/utils/robot_model_test_utils.h>
+#include <moveit/robot_model_loader/robot_model_loader.h>
+#include <rclcpp/rclcpp.hpp>
+
+using namespace moveit::core;
+
+RobotModelPtr getModel()
+{
+ // suppress RobotModel errors and warnings
+ rclcpp::Logger logger = rclcpp::get_logger("moveit_core.robot_model");
+ logger.set_level(rclcpp::Logger::Level::Fatal);
+
+ // create dummy robot model
+ moveit::core::RobotModelBuilder builder("robot", "base");
+ builder.addChain("base->link1->link2->tip", "continuous");
+ builder.addGroupChain("base", "link2", "group");
+ builder.addGroupChain("link2", "tip", "eef_group");
+ builder.addEndEffector("eef", "link2", "group", "eef_group");
+ return builder.build();
+}
+
+moveit::core::RobotModelPtr loadModel()
+{
+ auto node = rclcpp::Node::make_shared("robot_model_loader_node");
+ robot_model_loader::RobotModelLoader::Options options;
+ robot_model_loader::RobotModelLoader loader(node, options);
+ return loader.getModel();
+}
diff --git a/tests/models.h b/tests/models.h
new file mode 100644
index 0000000..11cfcdb
--- /dev/null
+++ b/tests/models.h
@@ -0,0 +1,15 @@
+#pragma once
+
+#include <moveit/macros/class_forward.h>
+
+namespace moveit {
+namespace core {
+MOVEIT_CLASS_FORWARD(RobotModel);
+}
+} // namespace moveit
+
+// get a hard-coded model
+moveit::core::RobotModelPtr getModel();
+
+// load a model from robot_description
+moveit::core::RobotModelPtr loadModel();
diff --git a/tests/stage_mockups.cpp b/tests/stage_mockups.cpp
new file mode 100644
index 0000000..0fd9c26
--- /dev/null
+++ b/tests/stage_mockups.cpp
@@ -0,0 +1,163 @@
+#include <moveit/task_constructor/stage.h>
+#include <moveit/planning_scene/planning_scene.h>
+
+#include "stage_mockups.h"
+
+namespace moveit {
+namespace task_constructor {
+
+unsigned int GeneratorMockup::id_ = 0;
+unsigned int MonitoringGeneratorMockup::id_ = 0;
+unsigned int ConnectMockup::id_ = 0;
+unsigned int PropagatorMockup::id_ = 0;
+unsigned int ForwardMockup::id_ = 0;
+unsigned int BackwardMockup::id_ = 0;
+
+void resetMockupIds()
+{
+ GeneratorMockup::id_ = 0;
+ MonitoringGeneratorMockup::id_ = 0;
+ ConnectMockup::id_ = 0;
+ PropagatorMockup::id_ = 0;
+ ForwardMockup::id_ = 0;
+ BackwardMockup::id_ = 0;
+}
+
+PredefinedCosts::PredefinedCosts(std::list<double>&& costs, bool finite)
+ : costs_{ std::move(costs) }, finite_{ finite }
+{}
+
+bool PredefinedCosts::exhausted() const
+{
+ return costs_.empty();
+}
+
+double PredefinedCosts::cost() const
+{
+ // keep the tests going, but this should not happen
+ EXPECT_GT(costs_.size(), 0u);
+ if(costs_.empty())
+ return 0.0;
+
+ double c{ costs_.front() };
+
+ if(finite_ || costs_.size() > 1)
+ {
+ costs_.pop_front();
+ }
+
+ return c;
+}
+
+void DelayingWrapper::compute()
+{
+ if(!delay_.empty())
+ { // if empty, we don't delay
+ if(delay_.front() == 0)
+ delay_.pop_front(); // we can compute() now
+ else
+ {
+ --delay_.front(); // continue delaying
+ return;
+ }
+ }
+ // forward to child, eventually generating multiple solutions at once
+ WrapperBase::compute();
+}
+
+GeneratorMockup::GeneratorMockup(PredefinedCosts&& costs, std::size_t solutions_per_compute)
+ : Generator{ "GEN" + std::to_string(++id_) }
+ , costs_{ std::move(costs) }
+ , solutions_per_compute_{ solutions_per_compute }
+{}
+
+void GeneratorMockup::init(const moveit::core::RobotModelConstPtr& robot_model)
+{
+ ps_.reset(new planning_scene::PlanningScene(robot_model));
+ Generator::init(robot_model);
+}
+
+bool GeneratorMockup::canCompute() const
+{
+ // check if runs are being used and if there are still runs left (costs are then never exhausted) or if costs
+ // are being used and they are not exhausted yet
+ return !costs_.exhausted();
+}
+
+void GeneratorMockup::compute()
+{
+ ++runs_;
+
+ for(std::size_t i = 0; canCompute() && i < solutions_per_compute_; ++i)
+ spawn(InterfaceState(ps_), costs_.cost());
+}
+
+MonitoringGeneratorMockup::MonitoringGeneratorMockup(Stage* monitored, PredefinedCosts&& costs)
+ : MonitoringGenerator{ "MON" + std::to_string(++id_), monitored }, costs_{ std::move(costs) }
+{}
+
+void MonitoringGeneratorMockup::onNewSolution(const SolutionBase& s)
+{
+ ++runs_;
+
+ spawn(InterfaceState{ s.end()->scene()->diff() }, SubTrajectory{});
+}
+
+ConnectMockup::ConnectMockup(PredefinedCosts&& costs)
+ : Connecting{ "CON" + std::to_string(++id_) }, costs_{ std::move(costs) }
+{}
+
+void ConnectMockup::compute(const InterfaceState& from, const InterfaceState& to)
+{
+ ++runs_;
+
+ auto solution{ std::make_shared<SubTrajectory>() };
+ solution->setCost(costs_.cost());
+ solution->setComment(std::to_string(from.priority().cost()) + " -> " + std::to_string(to.priority().cost()));
+ connect(from, to, solution);
+}
+
+PropagatorMockup::PropagatorMockup(PredefinedCosts&& costs, std::size_t solutions_per_compute)
+ : PropagatingEitherWay{ "PRO" + std::to_string(++id_) }
+ , costs_{ std::move(costs) }
+ , solutions_per_compute_{ solutions_per_compute }
+{}
+
+void PropagatorMockup::computeForward(const InterfaceState& from)
+{
+ ++runs_;
+
+ for(std::size_t i = 0; i < solutions_per_compute_; ++i)
+ {
+ SubTrajectory solution{ robot_trajectory::RobotTrajectoryConstPtr(), costs_.cost() };
+ sendForward(from, InterfaceState{ from.scene()->diff() }, std::move(solution));
+ }
+}
+
+void PropagatorMockup::computeBackward(const InterfaceState& to)
+{
+ ++runs_;
+
+ for(std::size_t i = 0; i < solutions_per_compute_; ++i)
+ {
+ SubTrajectory solution(robot_trajectory::RobotTrajectoryConstPtr(), costs_.cost());
+ sendBackward(InterfaceState(to.scene()->diff()), to, std::move(solution));
+ }
+}
+
+ForwardMockup::ForwardMockup(PredefinedCosts&& costs, std::size_t solutions_per_compute)
+ : PropagatorMockup{ std::move(costs), solutions_per_compute }
+{
+ restrictDirection(FORWARD);
+ setName("FWD" + std::to_string(++id_));
+}
+
+BackwardMockup::BackwardMockup(PredefinedCosts&& costs, std::size_t solutions_per_compute)
+ : PropagatorMockup{ std::move(costs), solutions_per_compute }
+{
+ restrictDirection(BACKWARD);
+ setName("BWD" + std::to_string(++id_));
+}
+
+} // namespace task_constructor
+} // namespace moveit
diff --git a/tests/stage_mockups.h b/tests/stage_mockups.h
new file mode 100644
index 0000000..e2e9b01
--- /dev/null
+++ b/tests/stage_mockups.h
@@ -0,0 +1,201 @@
+#pragma once
+
+#include <thread>
+
+#include <moveit/task_constructor/task.h>
+#include <moveit/task_constructor/cost_terms.h>
+
+#include <moveit/planning_scene/planning_scene.h>
+
+#include <gtest/gtest.h>
+/* #include <gmock/gmock.h> */
+
+namespace moveit {
+namespace task_constructor {
+
+MOVEIT_STRUCT_FORWARD(PredefinedCosts);
+struct PredefinedCosts : CostTerm
+{
+ mutable std::list<double> costs_; // list of costs to assign
+ mutable bool finite_;
+
+ PredefinedCosts(std::list<double>&& costs, bool finite = true);
+
+ static PredefinedCosts constant(double c)
+ {
+ return PredefinedCosts{ std::list<double>{ c }, false };
+ }
+ static PredefinedCosts single(double c)
+ {
+ return PredefinedCosts{ std::list<double>{ c }, true };
+ }
+
+ bool exhausted() const;
+ double cost() const;
+
+ double operator()(const SubTrajectory& /*s*/, std::string& /*comment*/) const override
+ {
+ return cost();
+ }
+ double operator()(const SolutionSequence& /*s*/, std::string& /*comment*/) const override
+ {
+ return cost();
+ }
+ double operator()(const WrappedSolution& /*s*/, std::string& /*comment*/) const override
+ {
+ return cost();
+ }
+};
+
+constexpr double INF{ std::numeric_limits<double>::infinity() };
+
+/* wrapper stage to delay solutions by a given number of steps */
+struct DelayingWrapper : public WrapperBase
+{
+ std::list<unsigned int> delay_;
+ /* delay list specifies the number of steps each received solution should be delayed */
+ DelayingWrapper(std::list<unsigned int> delay, Stage::pointer&& child)
+ : WrapperBase("delayer", std::move(child)), delay_{ std::move(delay) }
+ {}
+
+ void compute() override;
+ void onNewSolution(const SolutionBase& s) override
+ {
+ liftSolution(s);
+ }
+};
+
+struct GeneratorMockup : public Generator
+{
+ planning_scene::PlanningScenePtr ps_;
+
+ PredefinedCosts costs_;
+ size_t runs_{ 0 };
+ std::size_t solutions_per_compute_;
+
+ static unsigned int id_;
+
+ // default to one solution to avoid infinity loops
+ GeneratorMockup(PredefinedCosts&& costs = PredefinedCosts{ std::list<double>{ 0.0 }, true },
+ std::size_t solutions_per_compute = 1);
+ GeneratorMockup(std::initializer_list<double> costs, std::size_t solutions_per_compute = 1)
+ : GeneratorMockup{ PredefinedCosts{ std::list<double>{ costs }, true }, solutions_per_compute }
+ {}
+
+ void init(const moveit::core::RobotModelConstPtr& robot_model) override;
+ bool canCompute() const override;
+ void compute() override;
+ virtual void reset() override
+ {
+ runs_ = 0;
+ };
+};
+
+struct MonitoringGeneratorMockup : public MonitoringGenerator
+{
+ PredefinedCosts costs_;
+ size_t runs_{ 0 };
+
+ static unsigned int id_;
+
+ MonitoringGeneratorMockup(Stage* monitored, PredefinedCosts&& costs = PredefinedCosts::constant(0.0));
+ MonitoringGeneratorMockup(Stage* monitored, std::initializer_list<double> costs)
+ : MonitoringGeneratorMockup{ monitored, PredefinedCosts{ std::list<double>{ costs }, true } }
+ {}
+
+ bool canCompute() const override
+ {
+ return false;
+ }
+ void compute() override
+ {}
+ void onNewSolution(const SolutionBase& s) override;
+ virtual void reset() override
+ {
+ runs_ = 0;
+ };
+};
+
+struct ConnectMockup : public Connecting
+{
+ PredefinedCosts costs_;
+ size_t runs_{ 0 };
+
+ static unsigned int id_;
+
+ ConnectMockup(PredefinedCosts&& costs = PredefinedCosts::constant(0.0));
+ ConnectMockup(std::initializer_list<double> costs)
+ : ConnectMockup{ PredefinedCosts{ std::list<double>{ costs }, true } }
+ {}
+
+ using Connecting::compatible; // make this accessible for testing
+
+ void compute(const InterfaceState& from, const InterfaceState& to) override;
+ virtual void reset() override
+ {
+ runs_ = 0;
+ };
+};
+
+struct PropagatorMockup : public PropagatingEitherWay
+{
+ PredefinedCosts costs_;
+ size_t runs_{ 0 };
+ std::size_t solutions_per_compute_;
+
+ static unsigned int id_;
+
+ PropagatorMockup(PredefinedCosts&& costs = PredefinedCosts::constant(0.0), std::size_t solutions_per_compute = 1);
+ PropagatorMockup(std::initializer_list<double> costs)
+ : PropagatorMockup{ PredefinedCosts{ std::list<double>{ costs }, true } }
+ {}
+
+ void computeForward(const InterfaceState& from) override;
+ void computeBackward(const InterfaceState& to) override;
+ virtual void reset() override
+ {
+ runs_ = 0;
+ };
+};
+
+struct ForwardMockup : public PropagatorMockup
+{
+ static unsigned int id_;
+
+ ForwardMockup(PredefinedCosts&& costs = PredefinedCosts::constant(0.0), std::size_t solutions_per_compute = 1);
+ ForwardMockup(std::initializer_list<double> costs)
+ : ForwardMockup{ PredefinedCosts{ std::list<double>{ costs }, true } }
+ {}
+};
+
+struct BackwardMockup : public PropagatorMockup
+{
+ static unsigned int id_;
+
+ BackwardMockup(PredefinedCosts&& costs = PredefinedCosts::constant(0.0), std::size_t solutions_per_compute = 1);
+ BackwardMockup(std::initializer_list<double> costs)
+ : BackwardMockup{ PredefinedCosts{ std::list<double>{ costs }, true } }
+ {}
+};
+
+// ForwardMockup that takes a while for its computation
+class TimedForwardMockup : public ForwardMockup
+{
+ std::chrono::milliseconds duration_;
+
+public:
+ TimedForwardMockup(std::chrono::milliseconds duration)
+ : ForwardMockup{}, duration_{ duration }
+ {}
+ void computeForward(const InterfaceState& from) override
+ {
+ std::this_thread::sleep_for(duration_);
+ ForwardMockup::computeForward(from);
+ }
+};
+
+// reset ids of all Mockup types (used to generate unique stage names)
+void resetMockupIds();
+
+} // namespace task_constructor
+} // namespace moveit