Split MoveToMouth Into Two Separate Actions

.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/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/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"),
             },