Merge pull request #9 from NASA-JSC-Robotics/update-waypoints

updated waypoints to avoid ctb contact with trainer frame in sim

Author: eholum-nasa

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

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.");