From 12fd10fce158e152267b11eded058dc2f18949b4 Mon Sep 17 00:00:00 2001 From: marqrazz Date: Mon, 3 Jun 2024 09:47:55 -0600 Subject: [PATCH] Set controller names in Demo pick and place task --- demo/config/panda_config.yaml | 2 ++ demo/src/pick_place_demo_parameters.yaml | 4 ++++ demo/src/pick_place_task.cpp | 21 +++++++++++++++++++++ 3 files changed, 27 insertions(+) diff --git a/demo/config/panda_config.yaml b/demo/config/panda_config.yaml index 1c2b69bbe..ffe800d31 100644 --- 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 index a05c52702..b3c8e4f01 100644 --- 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 index 71c340f76..db0ec7d79 100644 --- 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("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("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("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)); }