diff --git a/clr_pick_and_place_demo/config/waypoints.yaml b/clr_pick_and_place_demo/config/waypoints.yaml index 5e3ff24..1a14641 100644 --- a/clr_pick_and_place_demo/config/waypoints.yaml +++ b/clr_pick_and_place_demo/config/waypoints.yaml @@ -120,7 +120,7 @@ waypoints: relative: true traverse_left_1: config: - - 0.700 + - 0.300 planning_group: "rail" cartesian: false pre_drop_ctb: @@ -139,6 +139,18 @@ waypoints: - 0.000 planning_group: "rail" cartesian: false + center_ctb: + pose: + x: 0.0 + y: -0.25 + z: 0.0 + qx: 0.0 + qy: 0.0 + qz: 0.0 + qw: 1.0 + planning_group: "ur_manipulator" + cartesian: true + relative: true drop_arm: pose: x: 0.0 diff --git a/clr_pick_and_place_demo/src/run_demo.cpp b/clr_pick_and_place_demo/src/run_demo.cpp index 282d37e..a476c2c 100644 --- a/clr_pick_and_place_demo/src/run_demo.cpp +++ b/clr_pick_and_place_demo/src/run_demo.cpp @@ -277,6 +277,11 @@ class RunDemoNode : public rclcpp::Node RCLCPP_ERROR(LOGGER, "Failed to traverse to CTB dropoff location. Exiting."); return; } + if (!this->center_ctb()) + { + RCLCPP_ERROR(LOGGER, "Failed to center CTB in bench. Exiting."); + return; + } if (!this->drop_ctb()) { RCLCPP_ERROR(LOGGER, "Failed to lower CTB. Exiting."); @@ -489,6 +494,12 @@ class RunDemoNode : public rclcpp::Node return plan_and_execute(wp_map.at("traverse_left_2")); } + bool center_ctb() + { + RCLCPP_INFO(LOGGER, "Centering CTB in bench."); + return plan_and_execute(wp_map.at("center_ctb")); + } + bool drop_ctb() { RCLCPP_INFO(LOGGER, "Dropping CTB.");