Change in_front_of_face_wall based on the planning scene namespace (#192)

* Add service to add or remove collision scene objects

* Remove hardcoded in-front-of-wheelchair wall from ada_feeding

* Changes from testing in real

* Fixes from testing

* Speed up adding in-front-of-face wall

---------

Co-authored-by: Taylor Kessler Faulkner <taylorkf@cs.washington.edu>

Author: amalnanavati

ada_feeding/ada_feeding/behaviors/moveit2/moveit2_plan.py

@@ -499,14 +499,21 @@ def terminate(self, new_status: py_trees.common.Status) -> None:
             self.moveit2.clear_path_constraints()
 
     @staticmethod
-    def get_path_len(path: JointTrajectory) -> Tuple[float, Dict[str, float]]:
+    def get_path_len(
+        path: JointTrajectory, exclude_j6: bool = True
+    ) -> Tuple[float, Dict[str, float]]:
         """
         Get the integrated path length of a trajectory in trajectory units.
 
         Uses only the position component of waypoints.
 
         Note: assumes joint values are in R1 (i.e. no wrap-around)
 
+        Parameters
+        ----------
+        exclude_j6: If True, exclude the last joint from the path length calculation. This is
+            because the last joint doesn't cause swivels, which is what we want to avoid.
+
         Returns:
         * length of the path in joint config space
         * Dict: joint_name -> distance that joint travels
@@ -521,11 +528,21 @@ def get_path_len(path: JointTrajectory) -> Tuple[float, Dict[str, float]]:
         if len(path.points) == 0:
             return total_len, joint_lens
 
+        j6_i = None
+        if exclude_j6 and "j2n6s200_joint_6" in path.joint_names:
+            j6_i = path.joint_names.index("j2n6s200_joint_6")
+
         prev_pos = np.array(path.points[0].positions)
         for point in path.points:
             curr_pos = np.array(point.positions)
             seg_len = np.abs(curr_pos - prev_pos)
-            total_len += np.linalg.norm(seg_len)
+            if j6_i is not None:
+                j6_len = seg_len[j6_i]
+                seg_len[j6_i] = 0.0
+                total_len += np.linalg.norm(seg_len)
+                seg_len[j6_i] = j6_len
+            else:
+                total_len += np.linalg.norm(seg_len)
             for index, name in enumerate(path.joint_names):
                 joint_lens[name] += seg_len[index]
             prev_pos = curr_pos

ada_feeding/ada_feeding/behaviors/ros/tf.py

@@ -341,6 +341,8 @@ def update(self) -> py_trees.common.Status:
         if self.tf_lock.locked():
             return py_trees.common.Status.RUNNING
 
+        self.logger.debug(f"Applying transform to {stamped_msg}")
+
         transformed_msg = None
         with self.tf_lock:
             if target_frame is not None:

ada_feeding/ada_feeding/idioms/bite_transfer.py

@@ -15,9 +15,8 @@
 from std_srvs.srv import SetBool
 
 # Local imports
+from ada_feeding_msgs.srv import ModifyCollisionObject
 from ada_feeding.behaviors.moveit2 import (
-    ModifyCollisionObject,
-    ModifyCollisionObjectOperation,
     ToggleCollisionObject,
 )
 from .retry_call_ros_service import retry_call_ros_service
@@ -127,9 +126,49 @@ def get_toggle_watchdog_listener_behavior(
     return toggle_watchdog_listener
 
 
-def get_add_in_front_of_wheelchair_wall_behavior(
+def get_modify_collision_object_behavior(
+    name: str,
+    object_id: str,
+    add: bool,
+) -> py_trees.behaviour.Behaviour:
+    """
+    Creates a behaviour that adds or removes a collision object.
+
+    Parameters
+    ----------
+    name: The name of the behaviour.
+    object_id: The ID of the collision object to add or remove.
+    add: Whether to add or remove the collision object.
+
+    Returns
+    -------
+    behavior: The behaviour that adds or removes the collision object.
+    """
+    modify_collision_object_key_response = Blackboard.separator.join([name, "response"])
+    return retry_call_ros_service(
+        name=name,
+        service_type=ModifyCollisionObject,
+        service_name="~/modify_collision_object",
+        key_request=None,
+        request=ModifyCollisionObject.Request(
+            operation=ModifyCollisionObject.Request.ADD
+            if add
+            else ModifyCollisionObject.Request.REMOVE,
+            object_id=object_id,
+        ),
+        key_response=modify_collision_object_key_response,
+        response_checks=[
+            py_trees.common.ComparisonExpression(
+                variable=modify_collision_object_key_response + ".success",
+                value=True,
+                operator=operator.eq,
+            )
+        ],
+    )
+
+
+def get_add_in_front_of_face_wall_behavior(
     name: str,
-    collision_object_id: str,
 ):
     """
     Creates a behavior that adds a collision wall between the staging pose and the user,
@@ -138,35 +177,20 @@ def get_add_in_front_of_wheelchair_wall_behavior(
     Parameters
     ----------
     name: The name of the behaviour.
-    collision_object_id: The ID of the collision object to add.
 
     Returns
     -------
     behavior: The behaviour that adds the collision wall.
     """
-    # Create the behavior to add a collision wall between the staging pose and the user,
-    # to prevent the robot from moving closer to the user.
-    return ModifyCollisionObject(
+    return get_modify_collision_object_behavior(
         name=name,
-        inputs={
-            "operation": ModifyCollisionObjectOperation.ADD,
-            "collision_object_id": collision_object_id,
-            "collision_object_position": (0.37, 0.17, 0.85),
-            "collision_object_orientation": (0.0, 0.0, 0.0, 1.0),
-            "prim_type": 1,  # Box=1. See shape_msgs/SolidPrimitive.msg
-            "dims": [
-                0.65,
-                0.01,
-                0.4,
-            ],  # Box has 3 dims: [x, y, z]
-            "frame_id": "root",
-        },
+        object_id="in_front_of_face_wall",
+        add=True,
     )
 
 
-def get_remove_in_front_of_wheelchair_wall_behavior(
+def get_remove_in_front_of_face_wall_behavior(
     name: str,
-    collision_object_id: str,
 ):
     """
     Creates a behavior that removes the collision wall between the staging pose and the user.
@@ -179,13 +203,8 @@ def get_remove_in_front_of_wheelchair_wall_behavior(
     -------
     behavior: The behaviour that removes the collision wall.
     """
-    # Create the behavior to remove the collision wall between the staging pose and the user.
-    remove_in_front_of_wheelchair_wall = ModifyCollisionObject(
+    return get_modify_collision_object_behavior(
         name=name,
-        inputs={
-            "operation": ModifyCollisionObjectOperation.REMOVE,
-            "collision_object_id": collision_object_id,
-        },
+        object_id="in_front_of_face_wall",
+        add=False,
     )
-
-    return remove_in_front_of_wheelchair_wall