Split MoveToMouth Into Two Separate Actions (#128)

* [WIP] Mostly implemented, some debugging remaining

* Verified MoveToMouth works in sim

* Linting import order

* Linting allow lines of 120 chars

* Added MoveToMouth message

* Removed use_face_detection_msg field from MoveToMouth

* Fixed face detection encoding

* Added wait to MoveToMouth for mouth transform to register

* Added MoveFromMouthToStagingConfiguration

* Fixed log levels

* Updated README

* Updated README

* Change info logs to debug

* Reduced max distance for face to 1.25m

* Updated food detection

- Remove poitns with depth 0
- Use already-filtered octomap depth image
- Only annotate images with detected face in RGB *and* depth

.pylintrc

@@ -355,7 +355,7 @@ indent-after-paren=4
 indent-string='    '
 
 # Maximum number of characters on a single line.
-max-line-length=100
+max-line-length=120
 
 # Maximum number of lines in a module.
 max-module-lines=1000

ada_feeding/README.md

@@ -33,10 +33,12 @@ This code has been developed and tested with the Kinova JACO Gen2 Arm, on comput
         1. `ros2 action send_goal /MoveAbovePlate ada_feeding_msgs/action/MoveTo "{}" --feedback`
         2. `ros2 action send_goal /AcquireFood ada_feeding_msgs/action/AcquireFood "{header: {stamp: {sec: 0, nanosec: 0}, frame_id: ''}, detected_food: {roi: {x_offset: 0, y_offset: 0, height: 0, width: 0, do_rectify: false}, mask: {header: {stamp: {sec: 0, nanosec: 0}, frame_id: ''}, format: '', data: []}, item_id: '', confidence: 0.0}}" --feedback`
         3. `ros2 action send_goal /MoveToRestingPosition ada_feeding_msgs/action/MoveTo "{}" --feedback`
-        4. `ros2 action send_goal /MoveToMouth ada_feeding_msgs/action/MoveTo "{}" --feedback`
-        5. `ros2 action send_goal /MoveFromMouthToRestingPosition ada_feeding_msgs/action/MoveTo "{}" --feedback`
-        6. `ros2 action send_goal /MoveFromMouthToAbovePlate ada_feeding_msgs/action/MoveTo "{}" --feedback`
-        7. `ros2 action send_goal /MoveToStowLocation ada_feeding_msgs/action/MoveTo "{}" --feedback`
+        4. `ros2 action send_goal /MoveToStagingConfiguration ada_feeding_msgs/action/MoveTo "{}" --feedback`
+        5. `ros2 action send_goal /MoveToMouth ada_feeding_msgs/action/MoveToMouth "{}" --feedback`
+        6. `ros2 action send_goal /MoveFromMouthToRestingPosition ada_feeding_msgs/action/MoveTo "{}" --feedback`
+        7. `ros2 action send_goal /MoveFromMouthToAbovePlate ada_feeding_msgs/action/MoveTo "{}" --feedback`
+        8. `ros2 action send_goal /MoveToStowLocation ada_feeding_msgs/action/MoveTo "{}" --feedback`
+        9. `ros2 action send_goal /MoveFromMouthToStagingConfiguration ada_feeding_msgs/action/MoveTo "{}" --feedback`
     2. Test the individual actions with the web app:
         1. Launch the web app ([instructions here](https://github.com/personalrobotics/feeding_web_interface/tree/main/feedingwebapp))
         2. Go through the web app, ensure the expected actions happen on the robot.

ada_feeding/ada_feeding/behaviors/__init__.py

@@ -2,4 +2,10 @@
 This package contains custom py_tree behaviors for the Ada Feeding project.
 """
 from .blackboard_behavior import BlackboardBehavior
-from .ros_utility import UpdateTimestamp
+from .ros_utility import (
+    UpdateTimestamp,
+    GetTransform,
+    SetStaticTransform,
+    ApplyTransform,
+    CreatePoseStamped,
+)

ada_feeding/ada_feeding/behaviors/acquisition/compute_action_constraints.py

@@ -27,6 +27,8 @@
 from scipy.spatial.transform import Rotation as R
 
 # Local imports
+from ada_feeding_msgs.msg import AcquisitionSchema
+from ada_feeding_msgs.srv import AcquisitionSelect
 from ada_feeding.behaviors import BlackboardBehavior
 from ada_feeding.helpers import (
     BlackboardKey,
@@ -35,8 +37,6 @@
     set_static_tf,
 )
 from ada_feeding.idioms.pre_moveto_config import create_ft_thresh_request
-from ada_feeding_msgs.msg import AcquisitionSchema
-from ada_feeding_msgs.srv import AcquisitionSelect
 
 
 class ComputeActionConstraints(BlackboardBehavior):

ada_feeding/ada_feeding/behaviors/acquisition/compute_food_frame.py

@@ -21,14 +21,14 @@
 # Local imports
 from ada_feeding_msgs.msg import Mask
 from ada_feeding_msgs.srv import AcquisitionSelect
+from ada_feeding_perception.helpers import ros_msg_to_cv2_image
 from ada_feeding.helpers import (
     BlackboardKey,
     quat_between_vectors,
     get_tf_object,
     set_static_tf,
 )
 from ada_feeding.behaviors import BlackboardBehavior
-from ada_feeding_perception.helpers import ros_msg_to_cv2_image
 
 
 class ComputeFoodFrame(BlackboardBehavior):

ada_feeding/ada_feeding/behaviors/moveit2/modify_collision_object.py

@@ -105,7 +105,7 @@ def setup(self, **kwargs):
         # It is okay for attributes in behaviors to be
         # defined in the setup / initialise functions.
 
-        self.logger.info(f"{self.name} [ModifyCollisionObject::setup()]")
+        self.logger.debug(f"{self.name} [ModifyCollisionObject::setup()]")
 
         # Get Node from Kwargs
         self.node = kwargs["node"]
@@ -120,7 +120,7 @@ def setup(self, **kwargs):
     def initialise(self):
         # Docstring copied from @override
 
-        self.logger.info(f"{self.name} [ModifyCollisionObject::initialise()]")
+        self.logger.debug(f"{self.name} [ModifyCollisionObject::initialise()]")
 
         # Check that the right parameters have been passed
         operation = self.blackboard_get("operation")
@@ -155,7 +155,7 @@ def initialise(self):
     def update(self) -> py_trees.common.Status:
         # Docstring copied from @override
 
-        self.logger.info(f"{self.name} [ModifyCollisionObject::update()]")
+        self.logger.debug(f"{self.name} [ModifyCollisionObject::update()]")
 
         # Get the blackboard inputs for all operations
         if not self.blackboard_exists(["operation", "collision_object_id"]):

ada_feeding/ada_feeding/behaviors/moveit2/moveit2_constraints.py

@@ -401,9 +401,7 @@ def blackboard_inputs(
           - `frame_id` defaults to the base link
           - `target_link` defaults to end effector
 
-        Note: if pose is PoseStamped:
-          - `frame_id` is pose.header.frame_id (if not "")
-          - `target_link` is pose.child_frame_id (if not "")
+        Note: if pose is PoseStamped `frame_id` is pose.header.frame_id (if not "")
 
         Details on parameterization:
         https://github.com/ros-planning/moveit_msgs/blob/master/msg/OrientationConstraint.msg
@@ -468,9 +466,7 @@ def update(self) -> py_trees.common.Status:
                 "frame_id": pose.header.frame_id
                 if (isinstance(pose, PoseStamped) and len(pose.header.frame_id) > 0)
                 else self.blackboard_get("frame_id"),
-                "target_link": pose.child_frame_id
-                if (isinstance(pose, PoseStamped) and len(pose.child_frame_id) > 0)
-                else self.blackboard_get("target_link"),
+                "target_link": self.blackboard_get("target_link"),
                 "tolerance": self.blackboard_get("tolerance_orientation"),
                 "weight": self.blackboard_get("weight_orientation"),
                 "parameterization": self.blackboard_get("parameterization"),
@@ -486,9 +482,7 @@ def update(self) -> py_trees.common.Status:
                 "frame_id": pose.header.frame_id
                 if (isinstance(pose, PoseStamped) and len(pose.header.frame_id) > 0)
                 else self.blackboard_get("frame_id"),
-                "target_link": pose.child_frame_id
-                if (isinstance(pose, PoseStamped) and len(pose.child_frame_id) > 0)
-                else self.blackboard_get("target_link"),
+                "target_link": self.blackboard_get("target_link"),
                 "tolerance": self.blackboard_get("tolerance_position"),
                 "weight": self.blackboard_get("weight_position"),
             },

ada_feeding/ada_feeding/behaviors/moveit2/toggle_collision_object.py

@@ -63,7 +63,7 @@ def setup(self, **kwargs):
         # It is okay for attributes in behaviors to be
         # defined in the setup / initialise functions.
 
-        self.logger.info(f"{self.name} [ToggleCollisionObject::setup()]")
+        self.logger.debug(f"{self.name} [ToggleCollisionObject::setup()]")
 
         # Get Node from Kwargs
         self.node = kwargs["node"]
@@ -111,7 +111,7 @@ def update(self) -> py_trees.common.Status:
             return py_trees.common.Status.FAILURE
         collision_object_ids = self.blackboard_get("collision_object_ids")
 
-        self.logger.info(f"{self.name} [ToggleCollisionObject::update()]")
+        self.logger.debug(f"{self.name} [ToggleCollisionObject::update()]")
         # (Dis)allow collisions between the robot and the collision object
         if self.service_future is None:
             # Check if we have processed all collision objects
@@ -129,7 +129,7 @@ def update(self) -> py_trees.common.Status:
                 collision_object_id = collision_object_ids[
                     self.curr_collision_object_id_i
                 ]
-                self.logger.info(
+                self.logger.debug(
                     f"{self.name} [ToggleCollisionObject::update()] "
                     f"collision_object_id: {collision_object_id}"
                 )
@@ -150,7 +150,7 @@ def update(self) -> py_trees.common.Status:
             with self.moveit2_lock:
                 succ = self.moveit2.process_allow_collision_future(self.service_future)
             # Process success/failure
-            self.logger.info(
+            self.logger.debug(
                 f"{self.name} [ToggleCollisionObject::update()] "
                 f"service_future: {succ}"
             )