Set controller names in Demo pick and place task

moveit · Jun 3, 2024 · 12fd10f · 12fd10f
1 parent 9c3655d
commit 12fd10f
Show file tree

Hide file tree

Showing 3 changed files with 27 additions and 0 deletions.
diff --git a/demo/config/panda_config.yaml b/demo/config/panda_config.yaml
@@ -2,12 +2,14 @@
   ros__parameters:
     # Total planning attempts
     max_solutions: 10
+    execute: false
 
     # Planning group and link names
     arm_group_name: "panda_arm"
     eef_name: "hand"
     hand_group_name: "hand"
     hand_frame: "panda_link8"
+    controller_names: ["panda_arm_controller", "panda_hand_controller"]
 
     # Poses
     hand_open_pose: "open"

diff --git a/demo/src/pick_place_demo_parameters.yaml b/demo/src/pick_place_demo_parameters.yaml
@@ -44,6 +44,10 @@ pick_place_task_demo:
     type: string
     validation:
       not_empty<>: []
+  controller_names:
+    type: string_array
+    validation:
+      not_empty<>: []
   eef_name:
     type: string
     validation:

diff --git a/demo/src/pick_place_task.cpp b/demo/src/pick_place_task.cpp
@@ -123,6 +123,7 @@ bool PickPlaceTask::init(const rclcpp::Node::SharedPtr& node, const pick_place_t
 	cartesian_planner->setStepSize(.01);
 
 	// Set task properties
+	t.setProperty("trajectory_execution_info", TrajectoryExecutionInfo().set__controller_names(params.controller_names));
 	t.setProperty("group", params.arm_group_name);
 	t.setProperty("eef", params.eef_name);
 	t.setProperty("hand", params.hand_group_name);
@@ -160,6 +161,8 @@ bool PickPlaceTask::init(const rclcpp::Node::SharedPtr& node, const pick_place_t
 		auto stage = std::make_unique<stages::MoveTo>("open hand", sampling_planner);
 		stage->setGroup(params.hand_group_name);
 		stage->setGoal(params.hand_open_pose);
+		stage->properties().set("trajectory_execution_info",
+		                        TrajectoryExecutionInfo().set__controller_names(params.controller_names));
 		initial_state_ptr = stage.get();  // remember start state for monitoring grasp pose generator
 		t.add(std::move(stage));
 	}
@@ -174,6 +177,8 @@ bool PickPlaceTask::init(const rclcpp::Node::SharedPtr& node, const pick_place_t
 		    "move to pick", stages::Connect::GroupPlannerVector{ { params.arm_group_name, sampling_planner } });
 		stage->setTimeout(5.0);
 		stage->properties().configureInitFrom(Stage::PARENT);
+		stage->properties().set("trajectory_execution_info",
+		                        TrajectoryExecutionInfo().set__controller_names(params.controller_names));
 		t.add(std::move(stage));
 	}
 
@@ -203,6 +208,8 @@ bool PickPlaceTask::init(const rclcpp::Node::SharedPtr& node, const pick_place_t
 			vec.header.frame_id = params.hand_frame;
 			vec.vector.z = 1.0;
 			stage->setDirection(vec);
+			stage->properties().set("trajectory_execution_info",
+			                        TrajectoryExecutionInfo().set__controller_names(params.controller_names));
 			grasp->insert(std::move(stage));
 		}
 
@@ -248,6 +255,8 @@ bool PickPlaceTask::init(const rclcpp::Node::SharedPtr& node, const pick_place_t
 			auto stage = std::make_unique<stages::MoveTo>("close hand", sampling_planner);
 			stage->setGroup(params.hand_group_name);
 			stage->setGoal(params.hand_close_pose);
+			stage->properties().set("trajectory_execution_info",
+			                        TrajectoryExecutionInfo().set__controller_names(params.controller_names));
 			grasp->insert(std::move(stage));
 		}
 
@@ -284,6 +293,8 @@ bool PickPlaceTask::init(const rclcpp::Node::SharedPtr& node, const pick_place_t
 			vec.header.frame_id = params.world_frame;
 			vec.vector.z = 1.0;
 			stage->setDirection(vec);
+			stage->properties().set("trajectory_execution_info",
+			                        TrajectoryExecutionInfo().set__controller_names(params.controller_names));
 			grasp->insert(std::move(stage));
 		}
 
@@ -312,6 +323,8 @@ bool PickPlaceTask::init(const rclcpp::Node::SharedPtr& node, const pick_place_t
 		    "move to place", stages::Connect::GroupPlannerVector{ { params.arm_group_name, sampling_planner } });
 		stage->setTimeout(5.0);
 		stage->properties().configureInitFrom(Stage::PARENT);
+		stage->properties().set("trajectory_execution_info",
+		                        TrajectoryExecutionInfo().set__controller_names(params.controller_names));
 		t.add(std::move(stage));
 	}
 
@@ -340,6 +353,8 @@ bool PickPlaceTask::init(const rclcpp::Node::SharedPtr& node, const pick_place_t
 			vec.header.frame_id = params.world_frame;
 			vec.vector.z = -1.0;
 			stage->setDirection(vec);
+			stage->properties().set("trajectory_execution_info",
+			                        TrajectoryExecutionInfo().set__controller_names(params.controller_names));
 			place->insert(std::move(stage));
 		}
 
@@ -377,6 +392,8 @@ bool PickPlaceTask::init(const rclcpp::Node::SharedPtr& node, const pick_place_t
 			auto stage = std::make_unique<stages::MoveTo>("open hand", sampling_planner);
 			stage->setGroup(params.hand_group_name);
 			stage->setGoal(params.hand_open_pose);
+			stage->properties().set("trajectory_execution_info",
+			                        TrajectoryExecutionInfo().set__controller_names(params.controller_names));
 			place->insert(std::move(stage));
 		}
 
@@ -412,6 +429,8 @@ bool PickPlaceTask::init(const rclcpp::Node::SharedPtr& node, const pick_place_t
 			vec.header.frame_id = params.hand_frame;
 			vec.vector.z = -1.0;
 			stage->setDirection(vec);
+			stage->properties().set("trajectory_execution_info",
+			                        TrajectoryExecutionInfo().set__controller_names(params.controller_names));
 			place->insert(std::move(stage));
 		}
 
@@ -429,6 +448,8 @@ bool PickPlaceTask::init(const rclcpp::Node::SharedPtr& node, const pick_place_t
 		stage->properties().configureInitFrom(Stage::PARENT, { "group" });
 		stage->setGoal(params.arm_home_pose);
 		stage->restrictDirection(stages::MoveTo::FORWARD);
+		stage->properties().set("trajectory_execution_info",
+		                        TrajectoryExecutionInfo().set__controller_names(params.controller_names));
 		t.add(std::move(stage));
 	}