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 @@
+
+
+
+
+
+
+
+ allow/forbid all collisions for given object/s
+
+ ModifyPlanningScene stage
+
+
+ second object of collision pair
+ first object of collision pair
+
+ ModifyPlanningScene stage
+
+
+ joint model group name
+ allow/forbid collisions with joint model group links
+
+ MTC task
+ ModifyPlanningScene stage
+
+
+
+
+ Name for property initialization sources. {DEFAULT, MANUAL, PARENT, INTERFACE}
+
+
+ step size between consecutive waypoints
+ Planner interface using pipeline motion solver
+ linear and rotational precision
+ fraction of motion required for success
+ scale down max acceleration by this factor
+ scale down max velocity by this factor
+
+
+
+ minimum distance between seperate IK solutions for the same target
+
+
+ max ik solutions
+ ignore collisions, true or false
+ name of active group (derived from eef if not provided)
+ name of end-effector
+
+
+
+ 0 = SEQUENTIAL, 1 = WAYPOINTS
+
+ connect
+
+ maximally accepted joint configuration distance between trajectory endpoint and goal state
+ timeout
+ Connect stage
+
+
+ MoveIt Task Constructor stage.
+
+
+ GenerateGraspPose Stage
+
+
+ pregrasp posture
+ object on which we generate the grasp poses
+ angular steps (rad)
+ rotate object pose about given axis
+ name of end-effector
+
+
+ GenerateGraspPose Stage
+ target pose to pass on in spawned states
+ allow placing objects upside down
+
+
+ object on which we generate the place poses
+
+
+ plan using simple interpolation in joint-space
+ max joint step
+ scale down max acceleration by this factor
+ scale down max velocity by this factor
+
+
+ ModifyPlanningScene stage
+ list of object names/id's.
+ attach or detach a list of objects to the given link
+
+
+
+ ModifyPlanningScene stage
+ list of object names/id's.
+ attach or detach a list of objects to the given link
+
+
+
+ MoveRelative stage
+ planner interface
+ move specified joint variables by given amount
+ maximum distance to move
+ frame to be moved in Cartesian direction
+ minimum distance to move
+
+ name of planning group
+ stage name
+
+
+ MoveRelative stage
+ planner interface
+ translate link along given direction
+ maximum distance to move
+ frame to be moved in Cartesian direction
+ minimum distance to move
+
+ name of planning group
+ stage name
+
+
+ MoveRelative stage
+ planner interface
+ perform twist motion on specified link
+ maximum distance to move
+ frame to be moved in Cartesian direction
+ minimum distance to move
+
+ name of planning group
+ stage name
+
+
+ move joints by name to their mapped target value
+ stage name
+ name of planning group
+
+ planner interface
+ MoveTo stage
+
+
+ move joint model group to given named pose
+ stage name
+ name of planning group
+
+ planner interface
+ MoveTo stage
+
+
+ move link to given point, keeping current orientation
+ frame to be moved in Cartesian direction
+ stage name
+ name of planning group
+
+ planner interface
+ MoveTo stage
+
+
+ move link to a given pose
+ frame to be moved in Cartesian direction
+ stage name
+ name of planning group
+
+ planner interface
+ MoveTo stage
+
+
+ scale down max acceleration by this factor
+ number of planning attempts
+
+ scale down max velocity by this factor
+
+ publish generated solutions on topic display_planned_path
+ tolerance for reaching orientation goals
+ tolerance for reaching position goals
+ tolerance for reaching joint goals
+ publish motion planning requests on topic motion_plan_request
+ Planner interface using pipeline motion solver
+
+
+ serial container
+ serial container name
+
+
+ {planning_scene_interface}
+
+
+ allow/forbid all collisions for given object/s
+
+ ModifyPlanningScene stage
+
+
+ second object of collision pair
+ first object of collision pair
+
+ ModifyPlanningScene stage
+
+
+ joint model group name
+ allow/forbid collisions with joint model group links
+
+ MTC task
+ ModifyPlanningScene stage
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ MTC stage raw pointer
+ MTC stage
+
+
+ MoveIt Task Constructor task.
+
+
+ Box's size along Z axis (m)
+ Box's size along X axis (m)
+
+
+
+ Box's size along Y axis (m)
+
+
+
+ cylinder height (m)
+
+
+
+ cylinder radius (m)
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
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
#include
-#include
+#include
#include
#include
#include
@@ -62,23 +62,23 @@ static const char* xml_text = R"(
collision_object="{cylinder}" />
-
+
-
+
-
+
-
+
@@ -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");
factory.registerNodeType("MoveItMsgsCollisionObjectCylinder");
factory.registerNodeType("MoveItMsgsCollisionObjectBox");
factory.registerNodeType("CreatePlanningSceneInterface");
factory.registerNodeType("AddObjectToPlanningScene");
- factory.registerNodeType("MoveMTCStageToContainer");
factory.registerNodeType("InitializeMTCTask", BT::RosNodeParams(node));
factory.registerNodeType("CreateMTCCurrentState");
factory.registerNodeType("CreateMTCPipelinePlanner", BT::RosNodeParams(node));
+ factory.registerNodeType>("MoveMTCStageToTask");
factory.registerNodeType("PlanMTCTask");
factory.registerNodeType("AllowCollisionPairs");
factory.registerNodeType("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
#include
-#include
-#include
+#include
#include
#include
#include
@@ -69,33 +68,33 @@ static const char* xml_text = R"(
collision_object="{cylinder}" />
-
+
-
+
-
+
-
-
+
+
-
+
@@ -107,7 +106,7 @@ static const char* xml_text = R"(
direction="{tcp_translate}"
min_distance="0.1"
max_distance="0.15" />
-
+
-
-
+
+
@@ -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");
factory.registerNodeType("GeometryMsgsPoseStamped");
factory.registerNodeType("GeometryMsgsVector3Stamped");
@@ -156,7 +154,6 @@ int main(int argc, char** argv)
factory.registerNodeType("CreatePlanningSceneInterface");
factory.registerNodeType("AddObjectToPlanningScene");
factory.registerNodeType("CreateMTCGenerateGraspPose");
- factory.registerNodeType("MoveMTCStageToContainer");
factory.registerNodeType("InitializeMTCTask", BT::RosNodeParams(node));
factory.registerNodeType("CreateMTCCurrentState");
factory.registerNodeType("CreateMTCPipelinePlanner", BT::RosNodeParams(node));
@@ -164,7 +161,9 @@ int main(int argc, char** argv)
factory.registerNodeType("GetMTCRawStage");
factory.registerNodeType("CreateMTCMoveToNamedJointPose");
factory.registerNodeType("CreateMTCConnect");
- factory.registerNodeType("MoveMTCContainerToParentContainer");
+ factory.registerNodeType>("MoveMTCStageToContainer");
+ factory.registerNodeType>("MoveMTCStageToTask");
+ factory.registerNodeType>("MoveMTCContainerToTask");
factory.registerNodeType("CreateMTCSerialContainer");
factory.registerNodeType("CreateMTCMoveRelativeTranslate");
factory.registerNodeType("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
-#include
-#include
-#include
-#include
#include
#include
#include
@@ -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");
factory.registerNodeType("MoveItMsgsCollisionObjectCylinder");
factory.registerNodeType("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
-#include
#include
-#include
+#include
+#include
#include
#include "behaviortree_cpp/bt_factory.h"
@@ -19,9 +19,9 @@ static const char* xml_text = R"(
-
+
-
+
@@ -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", BT::RosNodeParams(node));
factory.registerNodeType("CreateMTCCurrentState");
- factory.registerNodeType("MoveMTCStageToContainer");
+ factory.registerNodeType>("MoveMTCStageToTask");
factory.registerNodeType("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
#include
-#include
-#include
+#include
#include
#include
#include
@@ -72,7 +71,7 @@ static const char* xml_text = R"(
collision_object="{cylinder}" />
-
+
-
+
-
+
-
+
@@ -107,7 +106,7 @@ static const char* xml_text = R"(
direction="{tcp_translate}"
min_distance="0.1"
max_distance="0.15" />
-
+
-
+
-
+
-
-
+
+
-
+
-
+
-
-
+
-
-
-
-
+
+
+
-
-
-
-
+
+
+
-
+
-
-
+
+
@@ -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");
factory.registerNodeType("GeometryMsgsPoseStamped");
factory.registerNodeType("GeometryMsgsVector3Stamped");
@@ -230,7 +228,6 @@ int main(int argc, char** argv)
factory.registerNodeType("CreatePlanningSceneInterface");
factory.registerNodeType("AddObjectToPlanningScene");
factory.registerNodeType("CreateMTCGenerateGraspPose");
- factory.registerNodeType("MoveMTCStageToContainer");
factory.registerNodeType("InitializeMTCTask", BT::RosNodeParams(node));
factory.registerNodeType("CreateMTCCurrentState");
factory.registerNodeType("CreateMTCPipelinePlanner", BT::RosNodeParams(node));
@@ -238,7 +235,9 @@ int main(int argc, char** argv)
factory.registerNodeType("GetMTCRawStage");
factory.registerNodeType("CreateMTCMoveToNamedJointPose");
factory.registerNodeType("CreateMTCConnect");
- factory.registerNodeType("MoveMTCContainerToParentContainer");
+ factory.registerNodeType>("MoveMTCStageToContainer");
+ factory.registerNodeType>("MoveMTCStageToTask");
+ factory.registerNodeType>("MoveMTCContainerToTask");
factory.registerNodeType("CreateMTCSerialContainer");
factory.registerNodeType("CreateMTCMoveRelativeTranslate");
factory.registerNodeType("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;
}
-
-/**
- *
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- */
-
-//
-
-//
-
-//
-//
-//
-//
-//
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
#include
#include
-#include
+#include
#include
#include
@@ -23,10 +23,10 @@ static const char* xml_text = R"(
-
+
-
+
-
-
+
+
@@ -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");
factory.registerNodeType("CreateMTCCurrentState");
factory.registerNodeType("CreateMTCMoveRelativeJoint");
- factory.registerNodeType("MoveMTCStageToContainer");
+ factory.registerNodeType>("MoveMTCStageToTask");
factory.registerNodeType("PlanMTCTask");
factory.registerNodeType("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
#include
#include
-#include
+#include
#include
#include
@@ -23,12 +23,12 @@ static const char* xml_text = R"(
-
+
-
+
@@ -39,7 +39,7 @@ static const char* xml_text = R"(
ik_frame="{ik_frame}"
direction="{tcp_translate}"
stage="{stage_move_rel_translate}" />
-
+
@@ -50,7 +50,7 @@ static const char* xml_text = R"(
ik_frame="{ik_frame}"
direction="{tcp_twist}"
stage="{stage_move_rel_twist}" />
-
+
@@ -60,7 +60,7 @@ static const char* xml_text = R"(
ik_frame="{ik_frame}"
direction="panda_joint7:-1.57079632679"
stage="{stage_move_rel_joint}" />
-
+
@@ -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");
factory.registerNodeType("CreateMTCMoveRelativeTwist");
factory.registerNodeType("CreateMTCMoveRelativeJoint");
- factory.registerNodeType("MoveMTCStageToContainer");
+ factory.registerNodeType>("MoveMTCStageToTask");
factory.registerNodeType("PlanMTCTask");
factory.registerNodeType("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
#include
#include
-#include
+#include
#include
#include
@@ -23,12 +23,12 @@ static const char* xml_text = R"(
-
+
-
+
-
+
+
-
+
-
+
@@ -61,9 +62,9 @@ static const char* xml_text = R"(
ik_frame="{ik_frame}"
goal="{goal_pose}"
stage="{stage_move_to_pose}" />
-
+
-
+
-
+
@@ -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");
factory.registerNodeType("CreateMTCMoveToPose");
factory.registerNodeType("CreateMTCMoveToPoint");
- factory.registerNodeType("MoveMTCStageToContainer");
+ factory.registerNodeType>("MoveMTCStageToTask");
factory.registerNodeType("PlanMTCTask");
factory.registerNodeType("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
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#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"(
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ )";
+
+// 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");
+ factory.registerNodeType("GeometryMsgsPoseStamped");
+ factory.registerNodeType("GeometryMsgsVector3Stamped");
+ factory.registerNodeType("MoveItMsgsCollisionObjectCylinder");
+ factory.registerNodeType("MoveItMsgsCollisionObjectBox");
+ factory.registerNodeType("CreatePlanningSceneInterface");
+ factory.registerNodeType("AddObjectToPlanningScene");
+ factory.registerNodeType("CreateMTCGenerateGraspPose");
+ factory.registerNodeType("InitializeMTCTask", BT::RosNodeParams(node));
+ factory.registerNodeType("CreateMTCCurrentState");
+ factory.registerNodeType("CreateMTCPipelinePlanner", BT::RosNodeParams(node));
+ factory.registerNodeType("PlanMTCTask");
+ factory.registerNodeType("GetMTCRawStage");
+ factory.registerNodeType("CreateMTCMoveToNamedJointPose");
+ factory.registerNodeType("CreateMTCConnect");
+ factory.registerNodeType>("MoveMTCStageToContainer");
+ factory.registerNodeType>("MoveMTCStageToTask");
+ factory.registerNodeType>("MoveMTCContainerToContainer");
+ factory.registerNodeType>("MoveMTCContainerToTask");
+ factory.registerNodeType("CreateMTCSerialContainer");
+ factory.registerNodeType("CreateMTCMoveRelativeTranslate");
+ factory.registerNodeType("CreateMTCComputeIK");
+ factory.registerNodeType>("ExposeMTCContainerPropertiesToContainer");
+ factory.registerNodeType>("ExposeMTCTaskPropertiesToContainer");
+ factory.registerNodeType>("StageConfigureInitFromMTCProperties");
+ factory.registerNodeType>("ContainerConfigureInitFromMTCProperties");
+ factory.registerNodeType>("SetMTCStagePropertyString");
+ factory.registerNodeType>("SetMTCTaskPropertyString");
+ factory.registerScriptingEnums();
+ factory.registerNodeType("CreateMTCCartesianPath");
+ factory.registerNodeType("CreateMTCModifyPlanningSceneAttachObjects");
+ factory.registerNodeType("CreateMTCModifyPlanningSceneDetachObjects");
+ factory.registerNodeType("AllowCollisionPairs");
+ factory.registerNodeType("AllowCollisionsJointModelGroup");
+ factory.registerNodeType("ForbidCollisionsJointModelGroup");
+ factory.registerNodeType("ForbidCollisionPairs");
+ factory.registerNodeType("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
#include
#include
-#include
-#include
+#include
#include
#include
@@ -25,12 +24,12 @@ static const char* xml_text = R"(
-
+
-
+
@@ -41,7 +40,7 @@ static const char* xml_text = R"(
ik_frame="{ik_frame}"
direction="{tcp_translate}"
stage="{stage_move_rel_translate}" />
-
+
@@ -52,8 +51,8 @@ static const char* xml_text = R"(
ik_frame="{ik_frame}"
direction="{tcp_translate}"
stage="{stage_move_rel_translate2}" />
-
-
+
+
@@ -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");
factory.registerNodeType("CreateMTCMoveRelativeTranslate");
factory.registerNodeType("CreateMTCCurrentState");
- factory.registerNodeType("MoveMTCStageToContainer");
- factory.registerNodeType("MoveMTCContainerToParentContainer");
+ factory.registerNodeType>("MoveMTCStageToContainer");
+ factory.registerNodeType>("MoveMTCStageToTask");
+ factory.registerNodeType>("MoveMTCContainerToTask");
factory.registerNodeType("PlanMTCTask");
factory.registerNodeType("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
-
-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
+#include
+
+#include
+
+namespace BT {
+namespace MTC {
+
+/// Move MTC child stage/container to parent container/task
+template
+class MoveMTCStage : public BT::SyncActionNode
+{
+ static_assert(std::is_base_of::value, "C must inherit from moveit::task_constructor::Stage");
+ static_assert(std::is_base_of::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>())
+ {
+ 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>())
+ {
+ auto& parent = *parent_ptr;
+
+ parent->add(std::move(unique_child));
+
+ return NodeStatus::SUCCESS;
+ }
+ }
+
+ return NodeStatus::FAILURE;
+ }
+
+ static BT::PortsList providedPorts()
+ {
+ return {
+ BT::InputPort>("child"),
+ BT::BidirectionalPort>("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
-
-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
+#include
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 @@
ament_cmake
behaviortree_cpp
+ behaviortree_ros2
geometry_msgs
moveit_msgs
moveit_ros_planning_interface
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();
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("max_velocity_scaling_factor", 0.1, "scale down max velocity by this factor"),
BT::InputPort("max_acceleration_scaling_factor", 0.1, "scale down max acceleration by this factor"),
- BT::InputPort("step_size", 0.01, "step size between consecutive waypoints"),
BT::InputPort("min_fraction", 1.0, "fraction of motion required for success"),
- BT::InputPort("jump_threshold", 1.5, "acceptable fraction of mean joint motion per step"),
+ BT::InputPort("precision", moveit::core::CartesianPrecision({ .translational = 0.001, .rotational = 0.01, .max_resolution = 1e-5 }), "linear and rotational precision"),
+ BT::InputPort("step_size", 0.01, "step size between consecutive waypoints"),
BT::OutputPort("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 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_ptrsetGroup(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_ptrsetGroup(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(node_, pipeline_id);
- solver->setPlannerId(planner_id);
+ auto solver = std::make_shared(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("goal_joint_tolerance", "1e-4", "tolerance for reaching joint goals"),
BT::InputPort("goal_position_tolerance", "1e-4", "tolerance for reaching position goals"),
BT::InputPort