sim works, need to test on hw, added mid-left-traverse waypoint to allow pre_drop_ctb space to plan, changed pre_drop_ctb to non-cartesian

Author: mtobia-nasa

clr_pick_and_place_demo/config/waypoints.yaml

@@ -51,7 +51,6 @@ waypoints:
       - 0.500
     planning_group: "rail"
     cartesian: false
-
   stow:
     # config:
     # - 4.17133 # 238 deg
@@ -119,22 +118,27 @@ waypoints:
     planning_group: "ur_manipulator"
     cartesian: true
     relative: true
-  traverse_left:
+  traverse_left_1:
     config:
-    - 0.000
+    - 0.700
     planning_group: "rail"
     cartesian: false
   pre_drop_ctb:
     pose:
-      x: -1.708
+      x: -1.006
       y: 0.811
       z: 1.580
       qx: 0.725
       qy: 0.688
       qz: 0.032
       qw: -0.004
     planning_group: "ur_manipulator"
-    cartesian: true
+    cartesian: false
+  traverse_left_2:
+    config:
+    - 0.000
+    planning_group: "rail"
+    cartesian: false
   drop_arm:
     pose:
       x: 0.0

clr_pick_and_place_demo/src/run_demo.cpp

@@ -262,16 +262,21 @@ class RunDemoNode : public rclcpp::Node
       RCLCPP_ERROR(LOGGER, "Failed to lift CTB. Exiting.");
       return;
     }
-    if (!this->traverse_left())
+    if (!this->traverse_left_1())
     {
-      RCLCPP_ERROR(LOGGER, "Failed to traverse to CTB dropoff location. Exiting.");
+      RCLCPP_ERROR(LOGGER, "Failed to traverse left. Exiting.");
       return;
     }
     if (!this->pre_drop_ctb())
     {
       RCLCPP_ERROR(LOGGER, "Failed to approach CTB dropoff location. Exiting.");
       return;
     }
+    if (!this->traverse_left_2())
+    {
+      RCLCPP_ERROR(LOGGER, "Failed to traverse to CTB dropoff location. Exiting.");
+      return;
+    }
     if (!this->drop_ctb())
     {
       RCLCPP_ERROR(LOGGER, "Failed to lower CTB. Exiting.");
@@ -456,10 +461,10 @@ class RunDemoNode : public rclcpp::Node
            plan_and_execute(wp_map.at("stow_ctb"));
   }
 
-  bool traverse_left()
+  bool traverse_left_1()
   {
     RCLCPP_INFO(LOGGER, "Traversing to bench seat location.");
-    return plan_and_execute(wp_map.at("traverse_left"));
+    return plan_and_execute(wp_map.at("traverse_left_1"));
   }
 
   bool pre_drop_ctb()
@@ -472,9 +477,17 @@ class RunDemoNode : public rclcpp::Node
     }
     Waypoint approach_wp_1 = Waypoint(eef.transform.translation.x, eef.transform.translation.y,
                                       eef.transform.translation.z, 0.725, 0.688, 0.032, -0.004, "ur_manipulator", true);
-    return plan_and_execute({ approach_wp_1, wp_map.at("pre_drop_ctb") });
+    // return plan_and_execute({ approach_wp_1, wp_map.at("pre_drop_ctb") });
+    return plan_and_execute(approach_wp_1) && plan_and_execute(wp_map.at("pre_drop_ctb"));
   }
 
+  bool traverse_left_2()
+  {
+    RCLCPP_INFO(LOGGER, "Traversing to bench seat location.");
+    return plan_and_execute(wp_map.at("traverse_left_2"));
+  }
+
+
   bool drop_ctb()
   {
     RCLCPP_INFO(LOGGER, "Dropping CTB.");