diff --git a/ada_feeding/ada_feeding/behaviors/acquisition/compute_action_constraints.py b/ada_feeding/ada_feeding/behaviors/acquisition/compute_action_constraints.py index 02a91124..97015782 100644 --- a/ada_feeding/ada_feeding/behaviors/acquisition/compute_action_constraints.py +++ b/ada_feeding/ada_feeding/behaviors/acquisition/compute_action_constraints.py @@ -90,6 +90,7 @@ def blackboard_outputs( grasp_thresh: Optional[BlackboardKey], # SetParameters.Request ext_thresh: Optional[BlackboardKey], # SetParameters.Request action: Optional[BlackboardKey], # AcquisitionSchema.msg + action_index: Optional[BlackboardKey], # int ) -> None: """ Blackboard Outputs @@ -135,6 +136,7 @@ def update(self) -> py_trees.common.Status: self.logger.error(f"Bad Action Select Response: {response.status}") return py_trees.common.Status.FAILURE prob = np.array(response.probabilities) + self.logger.debug(f"Action Probabilities: {prob} (size {prob.size})") if ( len(response.actions) == 0 or len(response.actions) != len(response.probabilities) @@ -204,6 +206,7 @@ def update(self) -> py_trees.common.Status: ### Final write to Blackboard self.blackboard_set("action", action) + self.blackboard_set("action_index", index) return py_trees.common.Status.SUCCESS diff --git a/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_execute.py b/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_execute.py index a26f7298..105e6a2c 100644 --- a/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_execute.py +++ b/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_execute.py @@ -219,3 +219,4 @@ def terminate(self, new_status: py_trees.common.Status) -> None: break rate.sleep() + self.node.destroy_rate(rate) diff --git a/ada_feeding/ada_feeding/behaviors/ros/tf.py b/ada_feeding/ada_feeding/behaviors/ros/tf.py index 42829f75..75e427d6 100644 --- a/ada_feeding/ada_feeding/behaviors/ros/tf.py +++ b/ada_feeding/ada_feeding/behaviors/ros/tf.py @@ -433,6 +433,7 @@ def update(self) -> py_trees.common.Status: return py_trees.common.Status.FAILURE # Write the transformed_msg + self.logger.debug(f"Transformed message: {transformed_msg}") self.blackboard_set("transformed_msg", transformed_msg) return py_trees.common.Status.SUCCESS diff --git a/ada_feeding/ada_feeding/behaviors/transfer/__init__.py b/ada_feeding/ada_feeding/behaviors/transfer/__init__.py new file mode 100644 index 00000000..20aa3c8d --- /dev/null +++ b/ada_feeding/ada_feeding/behaviors/transfer/__init__.py @@ -0,0 +1,5 @@ +""" +This subpackage contains custom py_tree behaviors for Bite Transfer. +""" + +from .compute_mouth_frame import ComputeMouthFrame diff --git a/ada_feeding/ada_feeding/behaviors/transfer/compute_mouth_frame.py b/ada_feeding/ada_feeding/behaviors/transfer/compute_mouth_frame.py new file mode 100644 index 00000000..44bb8e75 --- /dev/null +++ b/ada_feeding/ada_feeding/behaviors/transfer/compute_mouth_frame.py @@ -0,0 +1,193 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +This module defines the ComputeMouthFrame behavior, which takes in the PointStamped +containing the (x, y, z) position of the mouth in the camera frame, and returns the +pose of the mouth in the base frame. +""" + +# Standard imports +from typing import Optional, Union + +# Third-party imports +from geometry_msgs.msg import PointStamped, PoseStamped, Vector3 +from overrides import override +import py_trees +import rclpy +import tf2_ros + +# Local imports +from ada_feeding.helpers import ( + BlackboardKey, + quat_between_vectors, + get_tf_object, +) +from ada_feeding.behaviors import BlackboardBehavior + + +class ComputeMouthFrame(BlackboardBehavior): + """ + Compute the mouth PoseStamped in the base frame, where the position is the output + of face detection and the orientation is facing the forkTip. + """ + + # pylint: disable=arguments-differ + # We *intentionally* violate Liskov Substitution Princple + # in that blackboard config (inputs + outputs) are not + # meant to be called in a generic setting. + + # pylint: disable=too-many-arguments + # These are effectively config definitions + # They require a lot of arguments. + + def blackboard_inputs( + self, + detected_mouth_center: Union[BlackboardKey, PointStamped], + timestamp: Union[BlackboardKey, rclpy.time.Time] = rclpy.time.Time(), + world_frame: Union[BlackboardKey, str] = "root", # +z will match this frame + frame_to_orient_towards: Union[ + BlackboardKey, str + ] = "forkTip", # +x will point towards this frame + ) -> None: + """ + Blackboard Inputs + + Parameters + ---------- + detected_mouth_center : Union[BlackboardKey, PointStamped] + The detected mouth center in the camera frame. + timestamp : Union[BlackboardKey, rclpy.time.Time] + The timestamp of the detected mouth center (default 0 for latest). + world_frame : Union[BlackboardKey, str] + The target frame to transform the mouth center to. + frame_to_orient_towards : Union[BlackboardKey, str] + The frame that the mouth should orient towards. + """ + # pylint: disable=unused-argument, duplicate-code + # Arguments are handled generically in base class. + super().blackboard_inputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + def blackboard_outputs( + self, mouth_pose: Optional[BlackboardKey] # PoseStamped + ) -> None: + """ + Blackboard Outputs + + Parameters + ---------- + mouth_pose : Optional[BlackboardKey] + The PoseStamped of the mouth in the base frame. + """ + # pylint: disable=unused-argument, duplicate-code + # Arguments are handled generically in base class. + super().blackboard_outputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + @override + def setup(self, **kwargs): + # Docstring copied from @override + + # pylint: disable=attribute-defined-outside-init + # It is okay for attributes in behaviors to be + # defined in the setup / initialise functions. + + # Get Node from Kwargs + self.node = kwargs["node"] + + # Get TF Listener from blackboard + self.tf_buffer, _, self.tf_lock = get_tf_object(self.blackboard, self.node) + + @override + def update(self) -> py_trees.common.Status: + # Docstring copied from @override + + # Validate inputs + if not self.blackboard_exists( + [ + "detected_mouth_center", + "timestamp", + "world_frame", + "frame_to_orient_towards", + ] + ): + self.logger.error( + "Missing detected_mouth_center, timestamp, world_frame, or frame_to_orient_towards." + ) + return py_trees.common.Status.FAILURE + detected_mouth_center = self.blackboard_get("detected_mouth_center") + timestamp = self.blackboard_get("timestamp") + world_frame = self.blackboard_get("world_frame") + frame_to_orient_towards = self.blackboard_get("frame_to_orient_towards") + + # Lock TF Buffer + if self.tf_lock.locked(): + # Not yet, wait for it + # Use a Timeout decorator to determine failure. + return py_trees.common.Status.RUNNING + with self.tf_lock: + # Transform detected_mouth_center to world_frame + if not self.tf_buffer.can_transform( + world_frame, + detected_mouth_center.header.frame_id, + timestamp, + ): + # Not yet, wait for it + # Use a Timeout decorator to determine failure. + self.logger.warning("ComputeMouthFrame waiting on world/camera TF...") + return py_trees.common.Status.RUNNING + camera_transform = self.tf_buffer.lookup_transform( + world_frame, + detected_mouth_center.header.frame_id, + timestamp, + ) + mouth_point = tf2_ros.TransformRegistration().get(PointStamped)( + detected_mouth_center, camera_transform + ) + self.logger.info(f"Computed mouth point: {mouth_point.point}") + + # Transform frame_to_orient_towards to world_frame + if not self.tf_buffer.can_transform( + world_frame, + frame_to_orient_towards, + timestamp, + ): + # Not yet, wait for it + # Use a Timeout decorator to determine failure. + self.logger.warning("ComputeMouthFrame waiting on world/forkTip TF...") + return py_trees.common.Status.RUNNING + orientation_target_transform = self.tf_buffer.lookup_transform( + world_frame, + frame_to_orient_towards, + timestamp, + ) + self.logger.info( + f"Computed orientation target transform: {orientation_target_transform.transform}" + ) + + # Get the yaw of the face frame + x_unit = Vector3(x=1.0, y=0.0, z=0.0) + x_pos = Vector3( + x=orientation_target_transform.transform.translation.x + - mouth_point.point.x, + y=orientation_target_transform.transform.translation.y + - mouth_point.point.y, + z=0.0, + ) + self.logger.info(f"Computed x_pos: {x_pos}") + quat = quat_between_vectors(x_unit, x_pos) + self.logger.info(f"Computed orientation: {quat}") + + # Create return object + mouth_pose = PoseStamped() + mouth_pose.header.frame_id = mouth_point.header.frame_id + mouth_pose.header.stamp = mouth_point.header.stamp + mouth_pose.pose.position = mouth_point.point + mouth_pose.pose.orientation = quat + + # Write to blackboard outputs + self.blackboard_set("mouth_pose", mouth_pose) + + return py_trees.common.Status.SUCCESS diff --git a/ada_feeding/ada_feeding/idioms/retry_call_ros_service.py b/ada_feeding/ada_feeding/idioms/retry_call_ros_service.py index 81acd48d..4cdaeda0 100644 --- a/ada_feeding/ada_feeding/idioms/retry_call_ros_service.py +++ b/ada_feeding/ada_feeding/idioms/retry_call_ros_service.py @@ -25,6 +25,7 @@ def retry_call_ros_service( response_checks: Optional[List[py_trees.common.ComparisonExpression]] = None, max_retries: int = 3, wait_for_server_timeout_sec: float = 0.0, + server_execution_timeout_sec: float = 10.0, ) -> py_trees.behaviour.Behaviour: """ Creates a behavior that calls a ROS service and optionally checkes the response. @@ -51,6 +52,8 @@ def retry_call_ros_service( max_retries: The maximum number of retries. wait_for_server_timeout_sec: The timeout for waiting for the server to be available. + server_execution_timeout_sec: The timeout for the server to execute the request. If + <= 0.0, no timeout is set. """ # pylint: disable=too-many-arguments, too-many-locals @@ -90,10 +93,20 @@ def retry_call_ros_service( key_request=key_request, ) + # Add a timeout + if server_execution_timeout_sec > 0.0: + call_ros_service_wrapper = py_trees.decorators.Timeout( + name=call_ros_service_behavior_name + "_timeout", + child=call_ros_service_behavior, + duration=server_execution_timeout_sec, + ) + else: + call_ros_service_wrapper = call_ros_service_behavior + # Create the check response behavior if response_checks is not None: # Add all the response checks - children = [call_ros_service_behavior] + children = [call_ros_service_wrapper] for i, response_check in enumerate(response_checks): check_response_behavior_name = Blackboard.separator.join( [name, check_response_namespace_prefix + str(i)] @@ -111,7 +124,7 @@ def retry_call_ros_service( children=children, ) else: - child = call_ros_service_behavior + child = call_ros_service_wrapper # Add a retry decorator on top of the child retry_behavior = py_trees.decorators.Retry( diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 804d8101..293b4325 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -86,6 +86,10 @@ def __init__( max_velocity_scaling_to_resting_configuration: Optional[float] = 0.8, max_acceleration_scaling_to_resting_configuration: Optional[float] = 0.8, pickle_goal_path: Optional[str] = None, + allowed_planning_time_for_move_above: float = 2.0, + allowed_planning_time_for_move_into: float = 0.5, + allowed_planning_time_to_resting_configuration: float = 0.5, + allowed_planning_time_for_recovery: float = 0.5, ): """ Initializes tree-specific parameters. @@ -100,6 +104,10 @@ def __init__( max_velocity_scaling_to_resting_configuration: Max velocity scaling for move to resting configuration max_acceleration_scaling_to_resting_configuration: Max acceleration scaling for move to resting configuration pickle_goal_path: Path to pickle goal for debugging + allowed_planning_time_for_move_above: Allowed planning time for move above + allowed_planning_time_for_move_into: Allowed planning time for move into + allowed_planning_time_to_resting_configuration: Allowed planning time for move to resting configuration + allowed_planning_time_for_recovery: Allowed planning time for recovery """ # Initialize ActionServerBT super().__init__(node) @@ -116,6 +124,12 @@ def __init__( max_acceleration_scaling_to_resting_configuration ) self.pickle_goal_path = pickle_goal_path + self.allowed_planning_time_for_move_above = allowed_planning_time_for_move_above + self.allowed_planning_time_for_move_into = allowed_planning_time_for_move_into + self.allowed_planning_time_to_resting_configuration = ( + allowed_planning_time_to_resting_configuration + ) + self.allowed_planning_time_for_recovery = allowed_planning_time_for_recovery @override def create_tree( @@ -163,14 +177,14 @@ def create_tree( name + "RemoveWheelchairWall", ), workers=[ - # py_trees_ros.service_clients.FromConstant( - # name="ClearOctomap", - # service_name="/clear_octomap", - # service_type=Empty, - # service_request=Empty.Request(), - # # Default fail if service is down - # wait_for_server_timeout_sec=0.0, - # ), + py_trees_ros.service_clients.FromConstant( + name="ClearOctomap", + service_name="/clear_octomap", + service_type=Empty, + service_request=Empty.Request(), + # Default fail if service is down + wait_for_server_timeout_sec=0.0, + ), MoveIt2JointConstraint( name="RestingConstraint", ns=name, @@ -181,20 +195,31 @@ def create_tree( "constraints": BlackboardKey("goal_constraints"), }, ), - MoveIt2Plan( - name="RestingPlan", - ns=name, - inputs={ - "goal_constraints": BlackboardKey("goal_constraints"), - "max_velocity_scale": self.max_velocity_scaling_to_resting_configuration, - "max_acceleration_scale": self.max_acceleration_scaling_to_resting_configuration, - }, - outputs={"trajectory": BlackboardKey("trajectory")}, + py_trees.decorators.Timeout( + name="RestingPlanTimeout", + # Increase allowed_planning_time to account for ROS2 overhead and MoveIt2 setup and such + duration=10.0 + * self.allowed_planning_time_to_resting_configuration, + child=MoveIt2Plan( + name="RestingPlan", + ns=name, + inputs={ + "goal_constraints": BlackboardKey( + "goal_constraints" + ), + "max_velocity_scale": self.max_velocity_scaling_to_resting_configuration, + "max_acceleration_scale": self.max_acceleration_scaling_to_resting_configuration, + "allowed_planning_time": self.allowed_planning_time_to_resting_configuration, + }, + outputs={ + "trajectory": BlackboardKey("resting_trajectory") + }, + ), ), MoveIt2Execute( name="Resting", ns=name, - inputs={"trajectory": BlackboardKey("trajectory")}, + inputs={"trajectory": BlackboardKey("resting_trajectory")}, outputs={}, ), ], @@ -295,25 +320,40 @@ def create_tree( ), }, ), - MoveIt2Plan( - name="RecoveryOffsetPlan", - ns=name, - inputs={ - "goal_constraints": BlackboardKey( - "goal_constraints" - ), - "max_velocity_scale": self.max_velocity_scaling_move_into, - "max_acceleration_scale": self.max_acceleration_scaling_move_into, - "cartesian": True, - "cartesian_max_step": 0.001, - "cartesian_fraction_threshold": 0.92, - }, - outputs={"trajectory": BlackboardKey("trajectory")}, + py_trees.decorators.Timeout( + name="RecoveryOffsetPlanTimeout", + # Increase allowed_planning_time to account for ROS2 overhead and MoveIt2 setup and such + duration=10.0 + * self.allowed_planning_time_for_recovery, + child=MoveIt2Plan( + name="RecoveryOffsetPlan", + ns=name, + inputs={ + "goal_constraints": BlackboardKey( + "goal_constraints" + ), + "max_velocity_scale": self.max_velocity_scaling_move_into, + "max_acceleration_scale": self.max_acceleration_scaling_move_into, + "cartesian": True, + "cartesian_max_step": 0.001, + "cartesian_fraction_threshold": 0.92, + "allowed_planning_time": self.allowed_planning_time_for_recovery, + }, + outputs={ + "trajectory": BlackboardKey( + "recovery_trajectory" + ) + }, + ), ), MoveIt2Execute( name="RecoveryOffset", ns=name, - inputs={"trajectory": BlackboardKey("trajectory")}, + inputs={ + "trajectory": BlackboardKey( + "recovery_trajectory" + ) + }, outputs={}, ), ], @@ -395,6 +435,7 @@ def move_above_plan( "grasp_thresh": BlackboardKey("grasp_thresh"), "ext_thresh": BlackboardKey("ext_thresh"), "action": BlackboardKey("action"), + "action_index": BlackboardKey("action_index"), }, ), ), @@ -418,20 +459,25 @@ def move_above_plan( "constraints": BlackboardKey("goal_constraints"), }, ), - MoveIt2Plan( - name="MoveAbovePlan", - ns=name, - inputs={ - "goal_constraints": BlackboardKey("goal_constraints"), - "max_velocity_scale": self.max_velocity_scaling_move_above, - "max_acceleration_scale": self.max_acceleration_scaling_move_above, - "allowed_planning_time": 2.0, - "max_path_len_joint": max_path_len_joint, - }, - outputs={ - "trajectory": BlackboardKey("trajectory"), - "end_joint_state": BlackboardKey("test_into_joints"), - }, + py_trees.decorators.Timeout( + name="MoveAbovePlanTimeout", + # Increase allowed_planning_time to account for ROS2 overhead and MoveIt2 setup and such + duration=10.0 * self.allowed_planning_time_for_move_above, + child=MoveIt2Plan( + name="MoveAbovePlan", + ns=name, + inputs={ + "goal_constraints": BlackboardKey("goal_constraints"), + "max_velocity_scale": self.max_velocity_scaling_move_above, + "max_acceleration_scale": self.max_acceleration_scaling_move_above, + "allowed_planning_time": self.allowed_planning_time_for_move_above, + "max_path_len_joint": max_path_len_joint, + }, + outputs={ + "trajectory": BlackboardKey("move_above_trajectory"), + "end_joint_state": BlackboardKey("test_into_joints"), + }, + ), ), ### Test MoveIntoFood MoveIt2PoseConstraint( @@ -445,20 +491,28 @@ def move_above_plan( "constraints": BlackboardKey("goal_constraints"), }, ), - MoveIt2Plan( - name="MoveIntoPlan", - ns=name, - inputs={ - "goal_constraints": BlackboardKey("goal_constraints"), - "max_velocity_scale": self.max_velocity_scaling_move_into, - "max_acceleration_scale": self.max_acceleration_scaling_move_into, - "cartesian": True, - "cartesian_max_step": 0.001, - "cartesian_fraction_threshold": 0.92, - "start_joint_state": BlackboardKey("test_into_joints"), - "max_path_len_joint": max_path_len_joint, - }, - outputs={"trajectory": BlackboardKey("move_into_trajectory")}, + py_trees.decorators.Timeout( + name="MoveIntoPlanTimeout", + # Increase allowed_planning_time to account for ROS2 overhead and MoveIt2 setup and such + duration=10.0 * self.allowed_planning_time_for_move_into, + child=MoveIt2Plan( + name="MoveIntoPlan", + ns=name, + inputs={ + "goal_constraints": BlackboardKey("goal_constraints"), + "max_velocity_scale": self.max_velocity_scaling_move_into, + "max_acceleration_scale": self.max_acceleration_scaling_move_into, + "cartesian": True, + "cartesian_max_step": 0.001, + "cartesian_fraction_threshold": 0.92, + "start_joint_state": BlackboardKey("test_into_joints"), + "max_path_len_joint": max_path_len_joint, + "allowed_planning_time": self.allowed_planning_time_for_move_into, + }, + outputs={ + "trajectory": BlackboardKey("move_into_trajectory") + }, + ), ), ], ) @@ -470,7 +524,7 @@ def move_above_plan( memory=True, children=[ scoped_behavior( - name="TableCollision", + name="Success", # Set Approach F/T Thresh pre_behavior=( Success() @@ -498,7 +552,7 @@ def move_above_plan( # Starts a new Sequence w/ Memory internally workers=[ scoped_behavior( - name="OctomapCollision", + name="OctomapAndTableCollision", # Set Approach F/T Thresh pre_behavior=py_trees.composites.Sequence( name="AllowOctomapAndTable", @@ -566,7 +620,11 @@ def move_above_plan( MoveIt2Execute( name="MoveAbove", ns=name, - inputs={"trajectory": BlackboardKey("trajectory")}, + inputs={ + "trajectory": BlackboardKey( + "move_above_trajectory" + ) + }, outputs={}, ), # If Anything goes wrong, reset FT to safe levels @@ -643,25 +701,32 @@ def move_above_plan( # From move_above_plan() py_trees.decorators.FailureIsSuccess( name="MoveIntoPlanFallbackPrecomputed", - child=MoveIt2Plan( - name="MoveIntoPlan", - ns=name, - inputs={ - "goal_constraints": BlackboardKey( - "goal_constraints" - ), - "max_velocity_scale": self.max_velocity_scaling_move_into, - "max_acceleration_scale": self.max_acceleration_scaling_move_into, - "cartesian": True, - "cartesian_max_step": 0.001, - "cartesian_fraction_threshold": 0.92, - "max_path_len_joint": max_path_len_joint, - }, - outputs={ - "trajectory": BlackboardKey( - "move_into_trajectory" - ) - }, + child=py_trees.decorators.Timeout( + name="MoveIntoPlanTimeout", + # Increase allowed_planning_time to account for ROS2 overhead and MoveIt2 setup and such + duration=10.0 + * self.allowed_planning_time_for_move_into, + child=MoveIt2Plan( + name="MoveIntoPlan", + ns=name, + inputs={ + "goal_constraints": BlackboardKey( + "goal_constraints" + ), + "max_velocity_scale": self.max_velocity_scaling_move_into, + "max_acceleration_scale": self.max_acceleration_scaling_move_into, + "cartesian": True, + "cartesian_max_step": 0.001, + "cartesian_fraction_threshold": 0.92, + "max_path_len_joint": max_path_len_joint, + "allowed_planning_time": self.allowed_planning_time_for_move_into, + }, + outputs={ + "trajectory": BlackboardKey( + "move_into_trajectory" + ) + }, + ), ), ), # MoveInto expect F/T failure @@ -882,10 +947,10 @@ def move_above_plan( ), # End MoveIt2Servo ], # End SafeFTPreempt.workers ), # End SafeFTPreempt - ], # End OctomapCollision.workers - ), # OctomapCollision + ], # End OctomapAndTableCollision.workers + ), # OctomapAndTableCollision ] - + resting_position_behaviors, # End TableCollision.workers + + resting_position_behaviors, # End Success.workers ), # End Success # TableCollision ], # End root_seq.children ) # End root_seq @@ -932,7 +997,27 @@ def get_feedback( if action_type is not AcquireFood: return None - return super().get_feedback(tree, action_type) + # Get MoveTo Params + feedback_msg = super().get_feedback(tree, action_type) + + name = tree.root.name + blackboard = py_trees.blackboard.Client(name=name, namespace=name) + blackboard.register_key( + key="action_response", access=py_trees.common.Access.READ + ) + blackboard.register_key(key="action_index", access=py_trees.common.Access.READ) + + feedback_msg.action_info_populated = True + try: + # TODO: add posthoc + feedback_msg.selection_id = blackboard.action_response.id + feedback_msg.action_index = int(blackboard.action_index) + except KeyError as e: + self._node.get_logger().debug( + f"Failed to populate action_info in AcquireFoodTree: {e}" + ) + feedback_msg.action_info_populated = False + return feedback_msg # Override result to add other elements to result msg @override @@ -945,21 +1030,4 @@ def get_result( return None # Get MoveTo Params response = super().get_result(tree, action_type) - - name = tree.root.name - blackboard = py_trees.blackboard.Client(name=name, namespace=name) - blackboard.register_key( - key="action_response", access=py_trees.common.Access.READ - ) - blackboard.register_key(key="action", access=py_trees.common.Access.READ) - - # TODO: add posthoc - response.selection_id = blackboard.action_response.id - try: - response.action_index = blackboard.action_response.actions.index( - blackboard.action - ) - except ValueError: - response.status = response.STATUS_ACTION_NOT_TAKEN - return response diff --git a/ada_feeding/ada_feeding/trees/move_from_mouth_tree.py b/ada_feeding/ada_feeding/trees/move_from_mouth_tree.py index 561361f3..711b868e 100644 --- a/ada_feeding/ada_feeding/trees/move_from_mouth_tree.py +++ b/ada_feeding/ada_feeding/trees/move_from_mouth_tree.py @@ -302,29 +302,35 @@ def moveit2_move_to_staging_pose( ), get_staging_path_constraints(), # Plan - MoveIt2Plan( - name="MoveToStagingPosePlan" + suffix, - ns=name, - inputs={ - "goal_constraints": BlackboardKey("goal_constraints"), - "path_constraints": BlackboardKey("path_constraints"), - "planner_id": self.planner_id, - "allowed_planning_time": ( - self.allowed_planning_time_to_staging_configuration - ), - "max_velocity_scale": ( - self.max_velocity_scaling_factor_to_staging_configuration - ), - "cartesian": cartesian, - "cartesian_jump_threshold": ( - self.cartesian_jump_threshold_to_staging_configuration - ), - "cartesian_fraction_threshold": 0.20, # Fine if its low since the user can retry - "cartesian_max_step": ( - self.cartesian_max_step_to_staging_configuration - ), - }, - outputs={"trajectory": BlackboardKey("trajectory")}, + py_trees.decorators.Timeout( + name="MoveToStagingPosePlanTimeout", + # Increase allowed_planning_time to account for ROS2 overhead and MoveIt2 setup and such + duration=10.0 + * self.allowed_planning_time_to_staging_configuration, + child=MoveIt2Plan( + name="MoveToStagingPosePlan" + suffix, + ns=name, + inputs={ + "goal_constraints": BlackboardKey("goal_constraints"), + "path_constraints": BlackboardKey("path_constraints"), + "planner_id": self.planner_id, + "allowed_planning_time": ( + self.allowed_planning_time_to_staging_configuration + ), + "max_velocity_scale": ( + self.max_velocity_scaling_factor_to_staging_configuration + ), + "cartesian": cartesian, + "cartesian_jump_threshold": ( + self.cartesian_jump_threshold_to_staging_configuration + ), + "cartesian_fraction_threshold": 0.20, # Fine if its low since the user can retry + "cartesian_max_step": ( + self.cartesian_max_step_to_staging_configuration + ), + }, + outputs={"trajectory": BlackboardKey("trajectory")}, + ), ), # Execute MoveIt2Execute( @@ -503,21 +509,31 @@ def speed(post_stamped: PoseStamped) -> Tuple[float, float]: ), end_path_constraints, # Plan - MoveIt2Plan( - name="MoveToEndingConfigurationPlan", - ns=name, - inputs={ - "goal_constraints": BlackboardKey("goal_constraints"), - "path_constraints": BlackboardKey("path_constraints"), - "planner_id": self.planner_id, - "allowed_planning_time": ( - self.allowed_planning_time_to_end_configuration - ), - "max_velocity_scale": ( - self.max_velocity_scaling_factor_to_end_configuration - ), - }, - outputs={"trajectory": BlackboardKey("trajectory")}, + py_trees.decorators.Timeout( + name="MoveToEndingConfigurationPlanTimeout", + # Increase allowed_planning_time to account for ROS2 overhead and MoveIt2 setup and such + duration=10.0 + * self.allowed_planning_time_to_end_configuration, + child=MoveIt2Plan( + name="MoveToEndingConfigurationPlan", + ns=name, + inputs={ + "goal_constraints": BlackboardKey( + "goal_constraints" + ), + "path_constraints": BlackboardKey( + "path_constraints" + ), + "planner_id": self.planner_id, + "allowed_planning_time": ( + self.allowed_planning_time_to_end_configuration + ), + "max_velocity_scale": ( + self.max_velocity_scaling_factor_to_end_configuration + ), + }, + outputs={"trajectory": BlackboardKey("trajectory")}, + ), ), # Execute MoveIt2Execute( diff --git a/ada_feeding/ada_feeding/trees/move_to_configuration_with_ft_thresholds_tree.py b/ada_feeding/ada_feeding/trees/move_to_configuration_with_ft_thresholds_tree.py index fd1ae8c4..3000b9ce 100644 --- a/ada_feeding/ada_feeding/trees/move_to_configuration_with_ft_thresholds_tree.py +++ b/ada_feeding/ada_feeding/trees/move_to_configuration_with_ft_thresholds_tree.py @@ -172,18 +172,23 @@ def create_tree( "constraints": BlackboardKey("goal_constraints"), }, ), - MoveIt2Plan( - name="MoveToConfigurationPlan", - ns=name, - inputs={ - "goal_constraints": BlackboardKey("goal_constraints"), - "pipeline_id": self.pipeline_id, - "planner_id": self.planner_id, - "allowed_planning_time": self.allowed_planning_time, - "max_velocity_scale": self.max_velocity_scaling_factor, - "max_acceleration_scale": self.max_acceleration_scaling_factor, - }, - outputs={"trajectory": BlackboardKey("trajectory")}, + py_trees.decorators.Timeout( + name="MoveIt2PlanTimeout", + # Increase allowed_planning_time to account for ROS2 overhead and MoveIt2 setup and such + duration=10.0 * self.allowed_planning_time, + child=MoveIt2Plan( + name="MoveToConfigurationPlan", + ns=name, + inputs={ + "goal_constraints": BlackboardKey("goal_constraints"), + "pipeline_id": self.pipeline_id, + "planner_id": self.planner_id, + "allowed_planning_time": self.allowed_planning_time, + "max_velocity_scale": self.max_velocity_scaling_factor, + "max_acceleration_scale": self.max_acceleration_scaling_factor, + }, + outputs={"trajectory": BlackboardKey("trajectory")}, + ), ), MoveIt2Execute( name="MoveToConfigurationExecute", diff --git a/ada_feeding/ada_feeding/trees/move_to_configuration_with_wheelchair_wall_tree.py b/ada_feeding/ada_feeding/trees/move_to_configuration_with_wheelchair_wall_tree.py index 2af21730..8ac0beac 100644 --- a/ada_feeding/ada_feeding/trees/move_to_configuration_with_wheelchair_wall_tree.py +++ b/ada_feeding/ada_feeding/trees/move_to_configuration_with_wheelchair_wall_tree.py @@ -170,18 +170,27 @@ def create_tree( workers=constraints + [ # Plan - MoveIt2Plan( - name="MoveToStagingConfigurationPlan", - ns=name, - inputs={ - "goal_constraints": BlackboardKey("goal_constraints"), - "path_constraints": BlackboardKey("path_constraints"), - "planner_id": self.planner_id, - "allowed_planning_time": self.allowed_planning_time, - "max_velocity_scale": self.max_velocity_scaling_factor, - "ignore_violated_path_constraints": True, - }, - outputs={"trajectory": BlackboardKey("trajectory")}, + py_trees.decorators.Timeout( + name="MoveToStagingConfigurationPlanTimeout", + # Increase allowed_planning_time to account for ROS2 overhead and MoveIt2 setup and such + duration=10.0 * self.allowed_planning_time, + child=MoveIt2Plan( + name="MoveToStagingConfigurationPlan", + ns=name, + inputs={ + "goal_constraints": BlackboardKey( + "goal_constraints" + ), + "path_constraints": BlackboardKey( + "path_constraints" + ), + "planner_id": self.planner_id, + "allowed_planning_time": self.allowed_planning_time, + "max_velocity_scale": self.max_velocity_scaling_factor, + "ignore_violated_path_constraints": True, + }, + outputs={"trajectory": BlackboardKey("trajectory")}, + ), ), # Execute MoveIt2Execute( diff --git a/ada_feeding/ada_feeding/trees/move_to_mouth_tree.py b/ada_feeding/ada_feeding/trees/move_to_mouth_tree.py index 27459a30..1b681942 100644 --- a/ada_feeding/ada_feeding/trees/move_to_mouth_tree.py +++ b/ada_feeding/ada_feeding/trees/move_to_mouth_tree.py @@ -37,8 +37,8 @@ GetTransform, SetStaticTransform, ApplyTransform, - CreatePoseStamped, ) +from ada_feeding.behaviors.transfer import ComputeMouthFrame from ada_feeding.helpers import BlackboardKey from ada_feeding.idioms import ( pre_moveto_config, @@ -97,12 +97,6 @@ def __init__( -0.5, 0.5, ), - mouth_orientation: Tuple[float, float, float, float] = ( - 0.0, - 0.0, - -0.7071068, - 0.7071068, - ), # Facing away from the wheelchair backrest ): """ Initializes tree-specific parameters. @@ -138,8 +132,6 @@ def __init__( plan_distance_from_mouth: The distance (m) to plan from the mouth center. fork_target_orientation_from_mouth: The fork's target orientation, in *mouth* frame. Pointing straight to the mouth is (0.5, -0.5, -0.5, 0.5). - mouth_orientation: The quaternion for the mouth pose. By default, it is facing - away from the wheelchair backrest in the seated planning scene. """ # pylint: disable=too-many-locals @@ -167,7 +159,6 @@ def __init__( self.face_detection_timeout = face_detection_timeout self.plan_distance_from_mouth = plan_distance_from_mouth self.fork_target_orientation_from_mouth = fork_target_orientation_from_mouth - self.mouth_orientation = mouth_orientation self.face_detection_relative_blackboard_key = "face_detection" @@ -348,37 +339,17 @@ def speed(post_stamped: PoseStamped) -> Tuple[float, float]: ), ], ), - # Convert `face_detection` to `mouth_position` in the - # base frame. - ApplyTransform( - name=name + " ConvertFaceDetectionToBaseFrame", + ComputeMouthFrame( + name=name + " ComputeMouthFrame", ns=name, inputs={ - "stamped_msg": BlackboardKey( + "detected_mouth_center": BlackboardKey( self.face_detection_relative_blackboard_key + ".detected_mouth_center" ), - "target_frame": "j2n6s200_link_base", }, outputs={ - "transformed_msg": BlackboardKey( - "mouth_position" - ), # PointStamped - }, - ), - # Convert `mouth_position` into a mouth pose using - # a fixed quaternion - CreatePoseStamped( - name=name + " FaceDetectionToPose", - ns=name, - inputs={ - "position": BlackboardKey("mouth_position"), - "quaternion": self.mouth_orientation, - }, - outputs={ - "pose_stamped": BlackboardKey( - "mouth_pose" - ), # PostStamped + "mouth_pose": BlackboardKey("mouth_pose"), }, ), # Cache the mouth pose on the static TF tree @@ -523,6 +494,7 @@ def speed(post_stamped: PoseStamped) -> Tuple[float, float]: ignore_orientation=True, subscribe_to_servo_status=False, pub_topic="~/cartesian_twist_cmds", + viz=True, ) ], ), diff --git a/ada_feeding/ada_feeding/trees/move_to_pose_tree.py b/ada_feeding/ada_feeding/trees/move_to_pose_tree.py index f405020b..a9a39ba9 100644 --- a/ada_feeding/ada_feeding/trees/move_to_pose_tree.py +++ b/ada_feeding/ada_feeding/trees/move_to_pose_tree.py @@ -181,22 +181,27 @@ def create_tree( # Add the planning behavior root_seq.add_child( - MoveIt2Plan( - name="MoveToPosePlan", - ns=name, - inputs={ - "goal_constraints": goal_constraints_key, - "pipeline_id": self.pipeline_id, - "planner_id": self.planner_id, - "allowed_planning_time": self.allowed_planning_time, - "max_velocity_scale": self.max_velocity_scaling_factor, - "max_acceleration_scale": self.max_acceleration_scaling_factor, - "cartesian": self.cartesian, - "cartesian_jump_threshold": self.cartesian_jump_threshold, - "cartesian_max_step": self.cartesian_max_step, - "cartesian_fraction_threshold": self.cartesian_fraction_threshold, - }, - outputs={"trajectory": BlackboardKey("trajectory")}, + py_trees.decorators.Timeout( + name="MoveToPosePlanTimeout", + # Increase allowed_planning_time to account for ROS2 overhead and MoveIt2 setup and such + duration=10.0 * self.allowed_planning_time, + child=MoveIt2Plan( + name="MoveToPosePlan", + ns=name, + inputs={ + "goal_constraints": goal_constraints_key, + "pipeline_id": self.pipeline_id, + "planner_id": self.planner_id, + "allowed_planning_time": self.allowed_planning_time, + "max_velocity_scale": self.max_velocity_scaling_factor, + "max_acceleration_scale": self.max_acceleration_scaling_factor, + "cartesian": self.cartesian, + "cartesian_jump_threshold": self.cartesian_jump_threshold, + "cartesian_max_step": self.cartesian_max_step, + "cartesian_fraction_threshold": self.cartesian_fraction_threshold, + }, + outputs={"trajectory": BlackboardKey("trajectory")}, + ), ), ) diff --git a/ada_feeding/config/ada_feeding_action_servers_custom.yaml b/ada_feeding/config/ada_feeding_action_servers_custom.yaml index 20cd75c2..ada11edc 100644 --- a/ada_feeding/config/ada_feeding_action_servers_custom.yaml +++ b/ada_feeding/config/ada_feeding_action_servers_custom.yaml @@ -1,12 +1,7 @@ # This file is auto-generated by create_action_servers.py ada_feeding_action_servers: ros__parameters: - custom_namespaces: - - side_staging_1 - - side_staging_2 - - side_staging_3 - - hospital_table_mount_1 - hospital_table_mount_1: + bed_front_table_1: AcquireFood.tree_kwargs.resting_joint_positions: - -1.1717344206197549 - 3.8349623560423542 @@ -45,7 +40,61 @@ ada_feeding_action_servers: - -2.1827671763194583 - 1.6106314909061383 planning_scene_namespace_to_use: bedside - namespace_to_use: hospital_table_mount_1 + bed_side_table_1: + AcquireFood.tree_kwargs.resting_joint_positions: + - -2.40341872791 + - 3.9280238571740833 + - 1.2793723345038692 + - -1.2987086150725533 + - -1.7135562754510847 + - 0.77158428024418 + MoveAbovePlate.tree_kwargs.joint_positions: + - -1.76876666747 + - 3.6562432079254257 + - 1.6497725020792509 + - -2.659056359891099 + - 4.916644381246061 + - 1.7380801275071338 + MoveFromMouth.tree_kwargs.linear_speed_near_mouth: 0.08 + MoveFromMouth.tree_kwargs.max_linear_speed_to_staging_configuration: 0.15 + MoveFromMouth.tree_kwargs.staging_configuration_position: + - -0.3418209608339783 + - -0.19780925396905233 + - 0.2413684489609259 + MoveFromMouth.tree_kwargs.staging_configuration_quat_xyzw: + - -0.14594094 + - 0.63546216 + - 0.691676 + - -0.31060195 + MoveToMouth.tree_kwargs.linear_speed_near_mouth: 0.06 + MoveToMouth.tree_kwargs.max_linear_speed: 0.14 + MoveToMouth.tree_kwargs.plan_distance_from_mouth: + - 0.025 + - 0.0 + - -0.01 + MoveToRestingPosition.tree_kwargs.goal_configuration: + - -2.40341872791 + - 3.9280238571740833 + - 1.2793723345038692 + - -1.2987086150725533 + - -1.7135562754510847 + - 0.77158428024418 + MoveToStagingConfiguration.tree_kwargs.goal_configuration: + - -1.41958843169 + - 4.259094195455066 + - 1.72927358641446 + - -0.6739427412487604 + - -3.0701456341812565 + - 1.8009590918986131 + planning_scene_namespace_to_use: bedside_90 + custom_namespaces: + - bed_side_table_1 + - bed_front_table_1 + - wheelchair_side_table_1 + - side_staging_1 + - side_staging_2 + - side_staging_3 + namespace_to_use: wheelchair_side_table_1 side_staging_1: AcquireFood.tree_kwargs.resting_joint_positions: - -0.8849039817349507 @@ -184,3 +233,42 @@ ada_feeding_action_servers: - 5.824282703942583 - 2.9376642889102835 planning_scene_namespace_to_use: seated + wheelchair_side_table_1: + AcquireFood.tree_kwargs.resting_joint_positions: + - -1.0417226246276128 + - 4.404614643739175 + - 5.699745834519425 + - -6.704937345842203 + - 4.10020174902156 + - 5.1406561763380765 + MoveAbovePlate.tree_kwargs.joint_positions: + - 0.02532932512774355 + - 3.1431881533988597 + - 1.732171105680466 + - -2.3604859079728984 + - 4.162401474177013 + - 4.090987211649885 + MoveFromMouth.tree_kwargs.staging_configuration_position: + - -0.011402569359730129 + - 0.029461197720583806 + - 0.6578790735619884 + MoveFromMouth.tree_kwargs.staging_configuration_quat_xyzw: + - 0.3333477503344343 + - 0.6602177768810074 + - 0.5795891150204631 + - 0.3421523376904928 + MoveToRestingPosition.tree_kwargs.goal_configuration: + - -1.0417226246276128 + - 4.404614643739175 + - 5.699745834519425 + - -6.704937345842203 + - 4.10020174902156 + - 5.1406561763380765 + MoveToStagingConfiguration.tree_kwargs.goal_configuration: + - -0.6009865084504753 + - 4.406308680508575 + - 4.342634895689761 + - -5.48339275809026 + - 4.099009718117641 + - 3.976385530989377 + planning_scene_namespace_to_use: seated_90 diff --git a/ada_feeding/config/ada_feeding_action_servers_default.yaml b/ada_feeding/config/ada_feeding_action_servers_default.yaml index cfc01871..abdbdcec 100644 --- a/ada_feeding/config/ada_feeding_action_servers_default.yaml +++ b/ada_feeding/config/ada_feeding_action_servers_default.yaml @@ -5,7 +5,8 @@ ada_feeding_action_servers: watchdog_timeout_sec: 0.5 # sec default: - planning_scene_namespace_to_use: seated + # TODO: Post-deployment, change this back! + planning_scene_namespace_to_use: seated_90 # A list of the names of the action servers to create. Each one must have # it's own parameters (below) specifying action_type and tree_class. diff --git a/ada_feeding/scripts/create_action_servers.py b/ada_feeding/scripts/create_action_servers.py index f8b731c1..97bb5564 100755 --- a/ada_feeding/scripts/create_action_servers.py +++ b/ada_feeding/scripts/create_action_servers.py @@ -441,6 +441,9 @@ def set_planning_scene_namespace_to_use( timeout = rclpy.time.Duration(seconds=timeout_secs) rate = self.create_rate(rate_hz) + def cleanup(): + self.destroy_rate(rate) + if not reinit_same_namespace: # Wait for the service to be ready self.planning_scene_set_parameters_client.wait_for_service( @@ -469,10 +472,12 @@ def set_planning_scene_namespace_to_use( self.get_logger().warn( "Failed to get parameters from ada_planning_scene." ) + cleanup() return False # If the parameter is the same, return if curr_planning_scene_namespace_to_use == planning_scene_namespace_to_use: + cleanup() return True # Wait for the service to be ready @@ -504,11 +509,13 @@ def set_planning_scene_namespace_to_use( self.get_logger().info( f"Successfully set planning scene namespace to {planning_scene_namespace_to_use}" ) + cleanup() return True self.get_logger().warn( f"Failed to set planning scene namespace to {planning_scene_namespace_to_use}. " f"Elapsed time: {(self.get_clock().now() - start_time).nanoseconds / 1.0e9} seconds." ) + cleanup() return False def declare_parameter_in_namespace( @@ -1028,11 +1035,12 @@ async def execute_callback(goal_handle: ServerGoalHandle) -> Awaitable: if goal_handle.is_cancel_requested: # Note that the body of this conditional may be called # multiple times until the preemption is complete. - self.get_logger().info("Goal canceled") + self.get_logger().info("Waiting for tree to preempt") tree_action_server.preempt_goal( tree ) # blocks until the preempt succeeds goal_handle.canceled() + self.get_logger().info("Canceled goal.") try: result = tree_action_server.get_result( @@ -1079,7 +1087,21 @@ async def execute_callback(goal_handle: ServerGoalHandle) -> Awaitable: py_trees.common.Status.INVALID, ) ): - self.get_logger().info("Tree failed") + # Get the name of the behavior that the tree failed on + names_of_failed_behavior = [] + for node in tree.root.iterate(): + if node.status in set( + ( + py_trees.common.Status.FAILURE, + py_trees.common.Status.INVALID, + ) + ): + names_of_failed_behavior.append(node.name) + if node.status == py_trees.common.Status.FAILURE: + break + self.get_logger().info( + f"Tree failed at behavior {names_of_failed_behavior}" + ) goal_handle.abort() try: result = tree_action_server.get_result( @@ -1111,6 +1133,7 @@ async def execute_callback(goal_handle: ServerGoalHandle) -> Awaitable: # Unset the goal and return the result with self.active_goal_request_lock: self.active_goal_request = None + self.destroy_rate(rate) return result return execute_callback diff --git a/ada_feeding_action_select/ada_feeding_action_select/policies/linear_policies.py b/ada_feeding_action_select/ada_feeding_action_select/policies/linear_policies.py index 9c352515..2952d5e5 100644 --- a/ada_feeding_action_select/ada_feeding_action_select/policies/linear_policies.py +++ b/ada_feeding_action_select/ada_feeding_action_select/policies/linear_policies.py @@ -341,8 +341,12 @@ def choice( lcb = self.checkpoint.linear_model @ context - ( self.alpha * np.sqrt(context.T @ self.covariance @ context) ) + action_i = np.argmin(lcb) - return [(1.0, self.library[np.argmin(lcb)])] + return [ + (1.0 if i == action_i else 0.0, self.library[i]) + for i in range(self.n_actions) + ] @override def update( diff --git a/ada_feeding_action_select/ada_feeding_action_select/policy_service.py b/ada_feeding_action_select/ada_feeding_action_select/policy_service.py index 6bdbca14..7eb087a5 100644 --- a/ada_feeding_action_select/ada_feeding_action_select/policy_service.py +++ b/ada_feeding_action_select/ada_feeding_action_select/policy_service.py @@ -358,7 +358,9 @@ def report_callback( """ Implement AcquisitionReport.srv """ - self.get_logger().info(f"AcquisitionReport Request with ID: '{request.id}'") + self.get_logger().info( + f"AcquisitionReport Request with ID: '{request.id}' and loss '{request.loss}'" + ) # Collect cached context if request.id not in self.cache: diff --git a/ada_feeding_action_select/config/acquisition_library.yaml b/ada_feeding_action_select/config/acquisition_library.yaml index 4e5a6efc..456e4f62 100644 --- a/ada_feeding_action_select/config/acquisition_library.yaml +++ b/ada_feeding_action_select/config/acquisition_library.yaml @@ -26,7 +26,7 @@ actions: pre_offset: - 0.0 - 0.0 - - -0.02 + - -0.025 pre_pos: - 0.0 - 0.0 @@ -63,7 +63,7 @@ actions: pre_offset: - 0.0 - 0.0 - - -0.02 + - -0.04 pre_pos: - 0.0 - 0.0 @@ -100,7 +100,7 @@ actions: pre_offset: - -0.01 - 0.0 - - -0.02 + - -0.025 pre_pos: - -0.4949747468305832 - 0.0 @@ -137,7 +137,7 @@ actions: pre_offset: - 0.0 - 0.0 - - -0.02 + - -0.025 pre_pos: - 0.0 - 0.0 @@ -174,7 +174,7 @@ actions: pre_offset: - 0.0 - 0.0 - - -0.02 + - -0.04 pre_pos: - 0.0 - 0.0 @@ -211,7 +211,7 @@ actions: pre_offset: - -0.01 - 0.0 - - -0.02 + - -0.025 pre_pos: - -0.4949747468305832 - 0.0 @@ -258,4 +258,4 @@ actions: - 0.9860264 - -0.0486593 - -0.1055426 - pre_torque: 4.0 \ No newline at end of file + pre_torque: 4.0 diff --git a/ada_feeding_msgs/action/AcquireFood.action b/ada_feeding_msgs/action/AcquireFood.action index c658d136..3a2557ba 100644 --- a/ada_feeding_msgs/action/AcquireFood.action +++ b/ada_feeding_msgs/action/AcquireFood.action @@ -21,18 +21,6 @@ uint8 STATUS_UNKNOWN=99 # Whether the planning and motion succeeded and if not, why uint8 status - -## Information to send to AcquisitionSelect service -# Action Taken -uint8 action_index - -# Posthoc context -float64[] posthoc - -# AcquisitionSelect call id -# Used in AcquisitionReport -string selection_id - --- # Whether the robot is currently planning or moving (required) bool is_planning @@ -44,3 +32,18 @@ builtin_interfaces/Duration motion_time float64 motion_initial_distance # How far the robot currently is from the goal (required if `is_planning == false`) float64 motion_curr_distance + +## Information to send to AcquisitionSelect service +## NOTE: Although the below is info one might expect in the result as opposed to feedback, +## we include it in feedback because ROSLibJS doesn't give us access to the result in error cases, +## even though we still want to know the action index. +bool action_info_populated +# Action Taken +uint8 action_index + +# Posthoc context +float64[] posthoc + +# AcquisitionSelect call id +# Used in AcquisitionReport +string selection_id diff --git a/ada_feeding_perception/ada_feeding_perception/face_detection.py b/ada_feeding_perception/ada_feeding_perception/face_detection.py index e3411f6f..6083cab5 100755 --- a/ada_feeding_perception/ada_feeding_perception/face_detection.py +++ b/ada_feeding_perception/ada_feeding_perception/face_detection.py @@ -320,7 +320,9 @@ def toggle_face_detection_callback( the face detection on or off depending on the request. """ - self._node.get_logger().info(f"Incoming service request. data: {request.data}") + self._node.get_logger().info( + f"Incoming service request for face detection. data: {request.data}" + ) response.success = False response.message = f"Failed to set is_on to {request.data}" with self.is_on_lock: diff --git a/ada_feeding_perception/ada_feeding_perception/food_on_fork_detection.py b/ada_feeding_perception/ada_feeding_perception/food_on_fork_detection.py index 1111e8b4..e8cc0e3c 100644 --- a/ada_feeding_perception/ada_feeding_perception/food_on_fork_detection.py +++ b/ada_feeding_perception/ada_feeding_perception/food_on_fork_detection.py @@ -468,7 +468,9 @@ def toggle_food_on_fork_detection( response: The response to toggle the perception algorithm on and off. """ - self._node.get_logger().info(f"Incoming service request. data: {request.data}") + self._node.get_logger().info( + f"Incoming service request for food-on-fork detection. data: {request.data}" + ) response.success = False response.message = f"Failed to set is_on to {request.data}" with self.is_on_lock: diff --git a/ada_feeding_perception/ada_feeding_perception/segment_from_point.py b/ada_feeding_perception/ada_feeding_perception/segment_from_point.py index 75f8d368..235ed01d 100644 --- a/ada_feeding_perception/ada_feeding_perception/segment_from_point.py +++ b/ada_feeding_perception/ada_feeding_perception/segment_from_point.py @@ -706,6 +706,10 @@ async def execute_callback( int(goal_handle.request.seed_point.point.y), ) rate = self._node.create_rate(self.rate_hz) + + def cleanup(): + self._node.destroy_rate(rate) + segment_image_task = self._node.executor.create_task( self.segment_image, seed_point, latest_img_msg ) @@ -729,6 +733,7 @@ async def execute_callback( result.status = result.STATUS_CANCELED with self.active_goal_request_lock: self.active_goal_request = None # Clear the active goal + cleanup() return result self._node.get_logger().info("Goal not canceled") @@ -746,6 +751,7 @@ async def execute_callback( "...Done. Got masks with average depth " f"{[m.average_depth for m in result.detected_items]} m." ) + cleanup() return result diff --git a/ada_feeding_perception/ada_feeding_perception/table_detection.py b/ada_feeding_perception/ada_feeding_perception/table_detection.py index 978b1b02..a5789ac6 100644 --- a/ada_feeding_perception/ada_feeding_perception/table_detection.py +++ b/ada_feeding_perception/ada_feeding_perception/table_detection.py @@ -197,7 +197,9 @@ def toggle_table_detection_callback( ---------- response: The updated response message based on the request. """ - self._node.get_logger().info(f"Incoming service request. data: {request.data}") + self._node.get_logger().info( + f"Incoming service request for table detection. data: {request.data}" + ) response.success = False response.message = f"Failed to set is_on to {request.data}" diff --git a/ada_planning_scene/ada_planning_scene/ada_planning_scene.py b/ada_planning_scene/ada_planning_scene/ada_planning_scene.py index 0e2fee70..e7b49487 100644 --- a/ada_planning_scene/ada_planning_scene/ada_planning_scene.py +++ b/ada_planning_scene/ada_planning_scene/ada_planning_scene.py @@ -264,6 +264,9 @@ def __get_namespace_to_use( timeout = rclpy.time.Duration(seconds=timeout_secs) rate = self.create_rate(rate_hz) + def cleanup(): + self.destroy_rate(rate) + # Create the client to get parameters ada_feeding_get_parameters_client = self.create_client( GetParameters, @@ -307,6 +310,7 @@ def __get_namespace_to_use( "`ada_feeding_action_servers`. Perhaps it is not running yet? Using the default namespace " "in the `ada_planning_scene` YAML file." ) + cleanup() return # Second, get `planning_scene_namespace_to_use` within the `namespace_to_use` namespace. @@ -327,14 +331,17 @@ def __get_namespace_to_use( # If the parameter is set, that is the namespace to use if response.values[0].type == ParameterType.PARAMETER_STRING: self.__namespace_to_use = response.values[0].string_value + cleanup() return # If the parameter is not set, use the default namespace in `ada_feeding` self.__namespace_to_use = ada_feeding_default_planning_scene_namespace + cleanup() return self.get_logger().warn( f"Failed to get `{request.names[0]}` from `ada_feeding_action_servers`. Using the default namespace " "in the `ada_planning_scene` YAML file." ) + cleanup() return def parameter_callback(self, params: List[Parameter]) -> SetParametersResult: diff --git a/ada_planning_scene/ada_planning_scene/collision_object_manager.py b/ada_planning_scene/ada_planning_scene/collision_object_manager.py index 291dd96a..ce3e34a1 100644 --- a/ada_planning_scene/ada_planning_scene/collision_object_manager.py +++ b/ada_planning_scene/ada_planning_scene/collision_object_manager.py @@ -166,6 +166,9 @@ def get_global_collision_objects( start_time = self.__node.get_clock().now() rate = self.__node.create_rate(rate_hz) + def cleanup(): + self.__node.destroy_rate(rate) + # Get the planning scene while self.moveit2.planning_scene is None: # Check if the node is still OK and if the timeout has been reached @@ -173,6 +176,7 @@ def get_global_collision_objects( self.__node.get_logger().error( "Timed out while getting the planning scene." ) + cleanup() return False # Attempt to get the planning scene @@ -192,6 +196,7 @@ def get_global_collision_objects( batch_id_to_update=self.__GLOBAL_BATCH_ID, ) + cleanup() return True def add_collision_objects( @@ -222,31 +227,40 @@ def add_collision_objects( # pylint: disable=too-many-arguments, too-many-branches, too-many-statements # This is the main bread and butter of adding to the planning scene, # so is expected to be complex. + self.__node.get_logger().info( + "Adding collision objects to the planning scene..." + ) # Start the time start_time = self.__node.get_clock().now() rate = self.__node.create_rate(rate_hz) + def cleanup(): + self.__node.destroy_rate(rate) + # Check if the objects are a single object if isinstance(objects, CollisionObjectParams): objects = {objects.object_id: objects} # Create a new batch for this add_collision_objects operation - with self.__collision_objects_lock: - batch_id = self.__BATCH_ID_FORMAT.format(self.__n_batches) - if ignore_existing: - self.__collision_objects_per_batch[batch_id] = set() - self.__attached_collision_objects_per_batch[batch_id] = set() - else: - self.__collision_objects_per_batch[ - batch_id - ] = self.__collision_objects_per_batch[self.__GLOBAL_BATCH_ID].copy() - self.__attached_collision_objects_per_batch[ - batch_id - ] = self.__attached_collision_objects_per_batch[ - self.__GLOBAL_BATCH_ID - ].copy() - self.__n_batches += 1 + if retry_until_added: + with self.__collision_objects_lock: + batch_id = self.__BATCH_ID_FORMAT.format(self.__n_batches) + if ignore_existing: + self.__collision_objects_per_batch[batch_id] = set() + self.__attached_collision_objects_per_batch[batch_id] = set() + else: + self.__collision_objects_per_batch[ + batch_id + ] = self.__collision_objects_per_batch[ + self.__GLOBAL_BATCH_ID + ].copy() + self.__attached_collision_objects_per_batch[ + batch_id + ] = self.__attached_collision_objects_per_batch[ + self.__GLOBAL_BATCH_ID + ].copy() + self.__n_batches += 1 # First, try to add all the collision objects collision_object_ids = set(objects.keys()) @@ -259,14 +273,18 @@ def add_collision_objects( "Timed out while adding collision objects. " f"May not have added {collision_object_ids}." ) + cleanup() return False # Remove any collision objects that have already been added - with self.__collision_objects_lock: - if ignore_existing or i > 0: - collision_object_ids -= self.__collision_objects_per_batch[batch_id] - if len(collision_object_ids) == 0: - break + if retry_until_added: + with self.__collision_objects_lock: + if ignore_existing or i > 0: + collision_object_ids -= self.__collision_objects_per_batch[ + batch_id + ] + if len(collision_object_ids) == 0: + break # Add the collision objects self.__node.get_logger().info( @@ -303,7 +321,13 @@ def add_collision_objects( quat_xyzw=params.quat_xyzw, frame_id=params.frame_id, ) + self.__node.get_logger().debug( + f"Added collision object {object_id}. About to sleep.", + ) rate.sleep() + self.__node.get_logger().debug( + "Woke up from sleep after adding collision object.", + ) if not retry_until_added: break @@ -312,7 +336,7 @@ def add_collision_objects( object_id for object_id, params in objects.items() if params.attached } i = -1 - while len(collision_object_ids) > 0: + while len(attached_collision_object_ids) > 0: i += 1 # Check if the node is still OK and if the timeout has been reached if not check_ok(self.__node, start_time, timeout): @@ -320,16 +344,18 @@ def add_collision_objects( "Timed out while attaching collision objects. " f"May not have attached {attached_collision_object_ids}." ) + cleanup() return False # Remove any attached collision objects that have already been attached - with self.__collision_objects_lock: - if ignore_existing or i > 0: - attached_collision_object_ids -= ( - self.__attached_collision_objects_per_batch[batch_id] - ) - if len(attached_collision_object_ids) == 0: - break + if retry_until_added: + with self.__collision_objects_lock: + if ignore_existing or i > 0: + attached_collision_object_ids -= ( + self.__attached_collision_objects_per_batch[batch_id] + ) + if len(attached_collision_object_ids) == 0: + break # Attach the collision objects self.__node.get_logger().info( @@ -355,10 +381,12 @@ def add_collision_objects( # Remove the batch that corresponds to this add_collision_objects # operation - with self.__collision_objects_lock: - self.__collision_objects_per_batch.pop(batch_id) - self.__attached_collision_objects_per_batch.pop(batch_id) + if retry_until_added: + with self.__collision_objects_lock: + self.__collision_objects_per_batch.pop(batch_id) + self.__attached_collision_objects_per_batch.pop(batch_id) + cleanup() return True def move_collision_objects( @@ -385,6 +413,9 @@ def move_collision_objects( start_time = self.__node.get_clock().now() rate = self.__node.create_rate(rate_hz) + def cleanup(): + self.__node.destroy_rate(rate) + # Check if the objects are a single object if isinstance(objects, CollisionObjectParams): objects = {objects.object_id: objects} @@ -452,6 +483,7 @@ def move_collision_objects( self.__collision_objects_per_batch.pop(batch_id) self.__attached_collision_objects_per_batch.pop(batch_id) + cleanup() return success def clear_all_collision_objects( @@ -473,12 +505,16 @@ def clear_all_collision_objects( start_time = self.__node.get_clock().now() rate = self.__node.create_rate(rate_hz) + def cleanup(): + self.__node.destroy_rate(rate) + # Clear the planning scene future = self.moveit2.clear_all_collision_objects() if future is None: self.__node.get_logger().error( "Could not clear planning scene; service is not ready" ) + cleanup() return False while not future.done(): @@ -487,11 +523,13 @@ def clear_all_collision_objects( "Timed out while clearing the planning scene." ) self.moveit2.cancel_clear_all_collision_objects_future(future) + cleanup() return False # Sleep rate.sleep() + cleanup() return self.moveit2.process_clear_all_collision_objects_future(future) def remove_collision_object( diff --git a/ada_planning_scene/ada_planning_scene/planning_scene_initializer.py b/ada_planning_scene/ada_planning_scene/planning_scene_initializer.py index 61777cb4..b1218e8d 100644 --- a/ada_planning_scene/ada_planning_scene/planning_scene_initializer.py +++ b/ada_planning_scene/ada_planning_scene/planning_scene_initializer.py @@ -307,15 +307,20 @@ def __wait_for_moveit(self, timeout: Duration(seconds=10.0)) -> bool: start_time = self.__node.get_clock().now() rate = self.__node.create_rate(self.__wait_for_moveit_hz) + def cleanup(): + self.__node.destroy_rate(rate) + while check_ok(self.__node, start_time, timeout): # pylint: disable=protected-access # This is necessary. Ideally, the service would not be protected. if ( self.__collision_object_manager.moveit2._get_planning_scene_service.service_is_ready() ): + cleanup() return True rate.sleep() + cleanup() return False def initialize( @@ -442,6 +447,9 @@ def __modify_collision_object_callback( ------- response: The response to modify the collision object. """ + self.__node.get_logger().info( + f"Modifying collision object '{request.object_id}'..." + ) object_id = request.object_id if object_id not in self.objects[self.__namespace_to_use]: success = False diff --git a/ada_planning_scene/ada_planning_scene/update_from_face_detection.py b/ada_planning_scene/ada_planning_scene/update_from_face_detection.py index 8b2ed635..50979fae 100644 --- a/ada_planning_scene/ada_planning_scene/update_from_face_detection.py +++ b/ada_planning_scene/ada_planning_scene/update_from_face_detection.py @@ -184,6 +184,24 @@ def __load_parameters(self) -> None: update_face_hz = self.__node.get_parameter("update_face_hz") self.__update_face_hz = update_face_hz.value + # Whether to publish the detected mouth pose as a static tf transform + try: + publish_mouth_tf = self.__node.declare_parameter( + "publish_mouth_tf", + False, + descriptor=ParameterDescriptor( + name="publish_mouth_tf", + type=ParameterType.PARAMETER_BOOL, + description=( + "Whether to publish the detected mouth pose as a static TF transform." + ), + read_only=True, + ), + ) + except ParameterAlreadyDeclaredException: + publish_mouth_tf = self.__node.get_parameter("publish_mouth_tf") + self.__publish_mouth_tf = publish_mouth_tf.value + # Load the parameters within each namespace self.__namespace_to_params = {} for namespace in self.__namespaces: @@ -389,20 +407,25 @@ def __update_face_detection(self) -> None: ) # Add the static mouth pose to TF. - self.__tf_broadcaster.sendTransform( - TransformStamped( - header=detected_mouth_pose.header, - child_frame_id=mouth_frame_id, - transform=Transform( - translation=Vector3( - x=detected_mouth_pose.pose.position.x, - y=detected_mouth_pose.pose.position.y, - z=detected_mouth_pose.pose.position.z, + # NOTE: Although in theory, publishing the mouth TF here can allow for visual servoing + # to the mouth, in practice we have to make some assumptions about the mouth orientation + # (e.g., the mouth is facing the fork). Those assumptions are made in the MoveToMouth tree, + # and would be overrided here if we published the mouth TF. + if self.__publish_mouth_tf: + self.__tf_broadcaster.sendTransform( + TransformStamped( + header=detected_mouth_pose.header, + child_frame_id=mouth_frame_id, + transform=Transform( + translation=Vector3( + x=detected_mouth_pose.pose.position.x, + y=detected_mouth_pose.pose.position.y, + z=detected_mouth_pose.pose.position.z, + ), + rotation=detected_mouth_pose.pose.orientation, ), - rotation=detected_mouth_pose.pose.orientation, - ), + ) ) - ) # Scale the body object based on the user's head pose. if update_body: diff --git a/ada_planning_scene/ada_planning_scene/update_from_table_detection.py b/ada_planning_scene/ada_planning_scene/update_from_table_detection.py index faa8f4b6..b876a492 100644 --- a/ada_planning_scene/ada_planning_scene/update_from_table_detection.py +++ b/ada_planning_scene/ada_planning_scene/update_from_table_detection.py @@ -137,7 +137,7 @@ def __load_parameters(self) -> None: # How often to update the planning scene based on table detection update_table_hz = self.__node.declare_parameter( "update_table_hz", - 3.0, # default value + 2.0, # default value descriptor=ParameterDescriptor( name="update_table_hz", type=ParameterType.PARAMETER_DOUBLE, @@ -320,7 +320,7 @@ def __update_table_detection(self) -> None: self.__namespace_to_use ] self.__node.get_logger().debug( - f"Detected table position: {default_table_position}" + f"Detected table position: {detected_table_pose.pose.position}" ) # Translate detected position of table into table's origin diff --git a/ada_planning_scene/ada_planning_scene/workspace_walls.py b/ada_planning_scene/ada_planning_scene/workspace_walls.py index 761a3402..9d5e31b1 100644 --- a/ada_planning_scene/ada_planning_scene/workspace_walls.py +++ b/ada_planning_scene/ada_planning_scene/workspace_walls.py @@ -694,12 +694,16 @@ def __get_robot_model( start_time = self.__node.get_clock().now() rate = self.__node.create_rate(rate_hz) + def cleanup(): + self.__node.destroy_rate(rate) + # Wait until the service is ready while not self.__get_urdf_parameter_service.service_is_ready(): if not check_ok(self.__node, start_time, timeout): self.__node.get_logger().error( "Timeout while waiting for the get URDF parameter service." ) + cleanup() return False rate.sleep() @@ -712,6 +716,7 @@ def __get_robot_model( self.__node.get_logger().error( "Timeout while getting the robot's URDF." ) + cleanup() return False rate.sleep() @@ -720,11 +725,13 @@ def __get_robot_model( response = future.result() except Exception as error: # pylint: disable=broad-except self.__node.get_logger().error(f"Failed to get the robot's URDF: {error}") + cleanup() return False if ( len(response.values) == 0 or response.values[0].type != ParameterType.PARAMETER_STRING ): + cleanup() return False robot_urdf = response.values[0].string_value robot_urdf = WorkspaceWalls.urdf_replace_package_paths(robot_urdf) @@ -737,6 +744,7 @@ def __get_robot_model( # `yourdfpy` only allows loading URDF from file, so we bypass its default load. self.__robot_model = URDF(robot=URDF._parse_robot(xml_element=xml_root)) + cleanup() return True def __get_parameter_prefix( @@ -759,12 +767,16 @@ def __get_parameter_prefix( start_time = self.__node.get_clock().now() rate = self.__node.create_rate(rate_hz) + def cleanup(): + self.__node.destroy_rate(rate) + # Wait for the service to be ready while not self.__get_robot_configurations_parameter_service.service_is_ready(): if not check_ok(self.__node, start_time, timeout): self.__node.get_logger().error( "Timeout while waiting for the get robot configurations parameter service." ) + cleanup() return False, "" rate.sleep() @@ -777,6 +789,7 @@ def __get_parameter_prefix( self.__node.get_logger().error( "Timeout while getting the namespace to use." ) + cleanup() return False, "" rate.sleep() @@ -787,6 +800,7 @@ def __get_parameter_prefix( self.__node.get_logger().error( f"Failed to get the namespace to use: {error}" ) + cleanup() return False, "" if ( len(response.values) == 0 @@ -796,6 +810,7 @@ def __get_parameter_prefix( else: prefix = response.values[0].string_value + "." + cleanup() return True, prefix def __joint_states_callback(self, msg: JointState) -> None: @@ -841,20 +856,28 @@ def __get_robot_configurations( start_time = self.__node.get_clock().now() rate = self.__node.create_rate(rate_hz) + def cleanup(): + self.__node.destroy_rate(rate) + # Get the prefix success, prefix = self.__get_parameter_prefix( rate_hz, get_remaining_time(self.__node, start_time, timeout) ) if not success: self.__node.get_logger().error("Failed to get the parameter prefix.") + cleanup() return False, {} # Wait for the service to be ready + self.__node.get_logger().info( + "Waiting for the get robot configurations parameter service." + ) while not self.__get_robot_configurations_parameter_service.service_is_ready(): if not check_ok(self.__node, start_time, timeout): self.__node.get_logger().error( "Timeout while waiting for the get robot configurations parameter service." ) + cleanup() return False, {} if publish_feedback is not None: publish_feedback() @@ -866,12 +889,16 @@ def __get_robot_configurations( request.names = [ prefix + name for name in self.__robot_configurations_parameter_names ] + self.__node.get_logger().info( + f"Getting robot configurations from parameters: {request.names}" + ) future = self.__get_robot_configurations_parameter_service.call_async(request) while not future.done(): if not check_ok(self.__node, start_time, timeout): self.__node.get_logger().error( "Timeout while getting the robot configurations." ) + cleanup() return False, {} if publish_feedback is not None: publish_feedback() @@ -884,6 +911,7 @@ def __get_robot_configurations( self.__node.get_logger().error( f"Failed to get robot configurations: {error}" ) + cleanup() return False, {} for i, param in enumerate(response.values): if param.type != ParameterType.PARAMETER_DOUBLE_ARRAY: @@ -893,6 +921,10 @@ def __get_robot_configurations( ) if publish_feedback is not None: publish_feedback() + if len(robot_configurations) == 0: + self.__node.get_logger().error("Failed to get robot configurations.") + cleanup() + return False, {} # Add the current joint state if ( @@ -907,6 +939,7 @@ def __get_robot_configurations( if len(current_config) == len(self.__articulated_joint_names): robot_configurations["current_joint_states"] = current_config + cleanup() return True, robot_configurations def __update_robot_configuration_bounds( @@ -985,7 +1018,7 @@ def __compute_and_add_workspace_walls( rate_hz: float = 10.0, timeout: Duration = Duration(seconds=5), publish_feedback: Optional[Callable[[], None]] = None, - ): + ) -> bool: """ Recomputes workspace walls and adds them to the planning scene. diff --git a/ada_planning_scene/config/ada_planning_scene.yaml b/ada_planning_scene/config/ada_planning_scene.yaml index e3b3ebdf..c05d34af 100644 --- a/ada_planning_scene/config/ada_planning_scene.yaml +++ b/ada_planning_scene/config/ada_planning_scene.yaml @@ -23,7 +23,9 @@ ada_planning_scene: namespaces: - seated - bedside - namespace_to_use: seated # bedside # + - seated_90 + - bedside_90 + namespace_to_use: seated_90 # bedside_90 # ############################################################################ # Parameters related to the PlanningSceneInitializer class @@ -93,6 +95,70 @@ ada_planning_scene: frame_id: root # the frame_id that the pose is relative to add_on_initialize: False # don't add this to the planning scene initially + seated_90: + # list of objects to add to the planning scene. Before changing names, + # check where else they are used (e.g., in `ada_feeding`). Ideally, the object_ids + # across different namespaces should be the same. + object_ids: + - wheelchair + - body # expanded mesh around the wheelchair to account for a user sitting in it + - table + - head + - in_front_of_face_wall # a wall in front of the user's face so robot motions don't unnecessarily move towards them. + # For each object, specify: + # - Shape: EITHER `filepath` (for a mesh) OR `primitive_type` and + # `primitive_dims`(for a primitive -- see shape_msgs/SolidPrimitive.msg) + # - Pose: `position` and `quat_xyzw` (see geometry_msgs/Pose.msg) + # - Frame ID: `frame_id` (the frame_id of the object that the pose is + # relative to) + # - [Optional] to attach the collision object specify `attached` to be True. + # In that case, `frame_id` must be a link on the robot to attach the object + # to, and `touch_links` must be a list of additional links that should be + # ignored for collision checking. + # - [Optional] to ensure the workspace walls include the object, specify + # `within_workspace_walls` to be True. + # - [Optional] to specify that the object should not initially be added to the + # planning scene, specify `add_on_initialize` to False. + wheelchair: # the wheelchair mesh + filename: wheelchair.stl + position: [0.02, -0.02, -0.05] + quat_xyzw: [0.0, 0.0, 0.0, 1.0] + frame_id: root # the frame_id that the pose is relative to + within_workspace_walls: True # whether to add workspace walls around the wheelchair + # NOTE: If you change this you must also change the hardcoded initial pose in + # bite transfer. + body: # an expanded mesh around the wheelchair to account for a user sitting in it + filename: body_collision_in_wheelchair.stl + position: [0.02, -0.02, -0.05] # should match the wheelchair position + quat_xyzw: [0.0, 0.0, 0.0, 1.0] # should match the wheelchair orientation + frame_id: root # the frame_id that the pose is relative to + within_workspace_walls: True # whether to add workspace walls around the wheelchair + table: # the table mesh + filename: table.stl + # Initial predicted position and orientation of the table; these values get + # updated as the robot perceives the table. + position: [-0.5, -0.08, -0.56] + quat_xyzw: [0.0, 0.0, -0.7071068, 0.7071068] + offsets: [-0.20, -0.25, -0.689] + frame_id: root # the frame_id that the pose is relative to + # NOTE: If you change this you must also change the hardcoded initial pose in + # bite transfer. + head: # the head mesh + filename: tom.stl + # This is an initial guess of head position; it will be updated as the + # robot perceives the face. + position: [0.29, 0.35, 0.85] + quat_xyzw: [-0.0616284, -0.0616284, -0.6804416, 0.704416] + frame_id: root # the frame_id that the pose is relative to + within_workspace_walls: True # whether to add workspace walls around the wheelchair + in_front_of_face_wall: # a wall in front of the user's face so robot motions don't unnecessarily move towards them. + primitive_type: 1 # shape_msgs/SolidPrimitive.BOX + primitive_dims: [0.65, 0.01, 0.4] + position: [0.17, -0.37, 0.85] + quat_xyzw: [0.0, 0.0, -0.7071068, 0.7071068] + frame_id: root # the frame_id that the pose is relative to + add_on_initialize: False # don't add this to the planning scene initially + bedside: # list of objects to add to the planning scene. Before changing names, # check where else they are used (e.g., in `ada_feeding`). Ideally, the object_ids @@ -139,6 +205,52 @@ ada_planning_scene: frame_id: root # the frame_id that the pose is relative to add_on_initialize: False # don't add this to the planning scene initially + bedside_90: + # list of objects to add to the planning scene. Before changing names, + # check where else they are used (e.g., in `ada_feeding`). Ideally, the object_ids + # across different namespaces should be the same. + object_ids: # list of objects to add to the planning scene + - bed + - body # expanded mesh above the bed to account for a user lying in it + - table + - head + - in_front_of_face_wall # a wall in front of the user's face so robot motions don't unnecessarily move towards them. + bed: # the wheelchair mesh + filename: bed_with_back.stl + position: [-0.58, 0.25, -1.09] + quat_xyzw: [0.0, 0.0, 0.0, 1.0] + frame_id: root # the frame_id that the pose is relative to + within_workspace_walls: False # whether to add workspace walls around the wheelchair + body: + filename: body_collision_in_bed.stl + position: [-0.58, 0.25, -1.09] # should match the wheelchair position + quat_xyzw: [0.0, 0.0, 0.0, 1.0] # should match the wheelchair orientation + frame_id: root # the frame_id that the pose is relative to + within_workspace_walls: True # whether to add workspace walls around the wheelchair + table: # the table mesh + filename: hospital_table.stl + # Initial predicted position and orientation of the table; these values get + # updated as the robot perceives the table. + position: [-0.6, -0.68, -1.04] + quat_xyzw: [0.0, 0.0, 0.7071068, 0.7071068] + frame_id: root # the frame_id that the pose is relative to + within_workspace_walls: True # whether to add workspace walls around the wheelchair + head: # the head mesh + filename: tom.stl + # This is an initial guess of head position; it will be updated as the + # robot perceives the face. + position: [-0.58, 0.07, 0.23] + quat_xyzw: [0.0010617, -0.0010994, -0.6946612, 0.7193354] + frame_id: root # the frame_id that the pose is relative to + within_workspace_walls: True # whether to add workspace walls around the wheelchair + in_front_of_face_wall: # a wall in front of the user's face so robot motions don't unnecessarily move towards them. + primitive_type: 1 # shape_msgs/SolidPrimitive.BOX + primitive_dims: [0.65, 0.01, 0.6] + position: [-0.27, 0.01, 0.30] + quat_xyzw: [0.0, 0.0, 0.3826834, 0.9238795] + frame_id: root # the frame_id that the pose is relative to + add_on_initialize: False # don't add this to the planning scene initially + ############################################################################ # Parameters related to the WorkspaceWalls class ############################################################################ @@ -149,8 +261,18 @@ ada_planning_scene: seated: workspace_wall_margin_y_min: 0.2 # The min y direction needs a larger margin since the robot moves into that for acquisition disable_workspace_wall_y_max: True # Remove the back wall, to be able to see in in RVIZ + seated_90: + workspace_wall_margin_y_min: 0.2 # The min y direction needs a larger margin since the robot moves into that for acquisition + disable_workspace_wall_y_max: True # Remove the back wall, to be able to see in in RVIZ bedside: disable_workspace_wall_y_min: True # Remove the back wall, to be able to see in in RVIZ + bedside_90: + disable_workspace_wall_y_min: True # Remove the back wall, to be able to see in in RVIZ + # disable_workspace_wall_y_max: True + # disable_workspace_wall_x_min: True + # disable_workspace_wall_x_max: True + # disable_workspace_wall_z_min: True + # disable_workspace_wall_z_max: True # Whether the workspace walls should contain the robot's current configuration # at time of recomputation. @@ -204,10 +326,18 @@ ada_planning_scene: head_object_id: head head_distance_threshold: 0.5 # m update_body: True + seated_90: + head_object_id: head + head_distance_threshold: 0.5 # m + update_body: True bedside: head_object_id: head head_distance_threshold: 0.5 # m update_body: False + bedside_90: + head_object_id: head + head_distance_threshold: 0.5 # m + update_body: False ############################################################################ # Parameters related to the UpdateFromTableDetectionpace class @@ -224,7 +354,15 @@ ada_planning_scene: table_origin_offset: [-0.20, -0.25, -0.79] table_quat_dist_thresh: 0.349 table_pos_dist_thresh: 0.5 + seated_90: + table_origin_offset: [-0.10, -0.25, -0.79] + table_quat_dist_thresh: 0.349 + table_pos_dist_thresh: 0.5 bedside: table_origin_offset: [-0.23, 0.71, -1.0067] table_quat_dist_thresh: 0.349 table_pos_dist_thresh: 0.5 + bedside_90: + table_origin_offset: [-0.71, -0.23, -1.0067] + table_quat_dist_thresh: 0.349 + table_pos_dist_thresh: 0.5