From 5c738a58811828bfac16e8df7e76a21b51d7f8f7 Mon Sep 17 00:00:00 2001 From: Taylor Kessler Faulkner Date: Thu, 29 Aug 2024 16:33:49 -0700 Subject: [PATCH 01/22] Add customized configurations --- .../ada_feeding_action_servers_custom.yaml | 54 ++++++++++--------- 1 file changed, 29 insertions(+), 25 deletions(-) diff --git a/ada_feeding/config/ada_feeding_action_servers_custom.yaml b/ada_feeding/config/ada_feeding_action_servers_custom.yaml index 20cd75c2..6d60a125 100644 --- a/ada_feeding/config/ada_feeding_action_servers_custom.yaml +++ b/ada_feeding/config/ada_feeding_action_servers_custom.yaml @@ -8,12 +8,12 @@ ada_feeding_action_servers: - hospital_table_mount_1 hospital_table_mount_1: AcquireFood.tree_kwargs.resting_joint_positions: - - -1.1717344206197549 - - 3.8349623560423542 - - 1.143531544675816 - - -1.4583640571583674 - - -1.8237450981853311 - - 7.026948142726954 + - -0.8326224011111689 + - 3.9280238571740833 + - 1.2793723345038692 + - -1.2987086150725533 + - -1.7135562754510847 + - 0.77158428024418 MoveAbovePlate.tree_kwargs.joint_positions: - -0.19797034068 - 3.6562432079254257 @@ -22,28 +22,32 @@ ada_feeding_action_servers: - 4.916644381246061 - 1.7380801275071338 MoveFromMouth.tree_kwargs.staging_configuration_position: - - -0.6014194269891053 - - 0.08069224383248258 - - 0.19766024762409728 + - -0.19780925396905233 + - 0.3418209608339783 + - 0.2413684489609259 MoveFromMouth.tree_kwargs.staging_configuration_quat_xyzw: - - 0.062023235263225555 - - -0.7245047876010136 - - -0.6768258670154677 - - 0.1146851200873633 + - 0.34614374143000426 + - 0.5525353940533169 + - 0.7087174551977422 + - 0.26946004766054077 + MoveToMouth.tree_kwargs.plan_distance_from_mouth: + - 0.1 + - 0.0 + - -0.01 MoveToRestingPosition.tree_kwargs.goal_configuration: - - -1.1717344206197549 - - 3.8349623560423542 - - 1.143531544675816 - - -1.4583640571583674 - - -1.8237450981853311 - - 7.026948142726954 + - -0.8326224011111689 + - 3.9280238571740833 + - 1.2793723345038692 + - -1.2987086150725533 + - -1.7135562754510847 + - 0.77158428024418 MoveToStagingConfiguration.tree_kwargs.goal_configuration: - - -0.5394174198411514 - - 4.294121954324551 - - 2.108729300644438 - - -1.0393311059687216 - - -2.1827671763194583 - - 1.6106314909061383 + - 0.15120789510808308 + - 4.259094195455066 + - 1.72927358641446 + - -0.6739427412487604 + - -3.0701456341812565 + - 1.8009590918986131 planning_scene_namespace_to_use: bedside namespace_to_use: hospital_table_mount_1 side_staging_1: From f82fc774a6e4d20366edf62dfcd33ab8a0c0e668 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Thu, 29 Aug 2024 16:41:06 -0700 Subject: [PATCH 02/22] Rotate the table 90 degrees --- .../ada_feeding_action_servers_custom.yaml | 18 +++++++++--------- .../config/ada_planning_scene.yaml | 11 ++++++++--- 2 files changed, 17 insertions(+), 12 deletions(-) diff --git a/ada_feeding/config/ada_feeding_action_servers_custom.yaml b/ada_feeding/config/ada_feeding_action_servers_custom.yaml index 6d60a125..6024c969 100644 --- a/ada_feeding/config/ada_feeding_action_servers_custom.yaml +++ b/ada_feeding/config/ada_feeding_action_servers_custom.yaml @@ -8,41 +8,41 @@ ada_feeding_action_servers: - hospital_table_mount_1 hospital_table_mount_1: AcquireFood.tree_kwargs.resting_joint_positions: - - -0.8326224011111689 + - -2.40341872791 - 3.9280238571740833 - 1.2793723345038692 - -1.2987086150725533 - -1.7135562754510847 - 0.77158428024418 MoveAbovePlate.tree_kwargs.joint_positions: - - -0.19797034068 + - -1.76876666747 - 3.6562432079254257 - 1.6497725020792509 - -2.659056359891099 - 4.916644381246061 - 1.7380801275071338 MoveFromMouth.tree_kwargs.staging_configuration_position: + - -0.3418209608339783 - -0.19780925396905233 - - 0.3418209608339783 - 0.2413684489609259 MoveFromMouth.tree_kwargs.staging_configuration_quat_xyzw: - - 0.34614374143000426 - - 0.5525353940533169 - - 0.7087174551977422 - - 0.26946004766054077 + - -0.14594094 + - 0.63546216 + - 0.691676 + - -0.31060195 MoveToMouth.tree_kwargs.plan_distance_from_mouth: - 0.1 - 0.0 - -0.01 MoveToRestingPosition.tree_kwargs.goal_configuration: - - -0.8326224011111689 + - -2.40341872791 - 3.9280238571740833 - 1.2793723345038692 - -1.2987086150725533 - -1.7135562754510847 - 0.77158428024418 MoveToStagingConfiguration.tree_kwargs.goal_configuration: - - 0.15120789510808308 + - -1.41958843169 - 4.259094195455066 - 1.72927358641446 - -0.6739427412487604 diff --git a/ada_planning_scene/config/ada_planning_scene.yaml b/ada_planning_scene/config/ada_planning_scene.yaml index e3b3ebdf..6afe9e3e 100644 --- a/ada_planning_scene/config/ada_planning_scene.yaml +++ b/ada_planning_scene/config/ada_planning_scene.yaml @@ -23,7 +23,7 @@ ada_planning_scene: namespaces: - seated - bedside - namespace_to_use: seated # bedside # + namespace_to_use: bedside # seated # ############################################################################ # Parameters related to the PlanningSceneInitializer class @@ -119,8 +119,8 @@ ada_planning_scene: filename: hospital_table.stl # Initial predicted position and orientation of the table; these values get # updated as the robot perceives the table. - position: [-0.68, 0.6, -1.04] - quat_xyzw: [0.0, 0.0, 0.0, 1.0] + 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 @@ -151,6 +151,11 @@ ada_planning_scene: 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 + # 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. From d0bf40e3911386351b7505d9735259c8281e0513 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Thu, 29 Aug 2024 20:14:25 -0700 Subject: [PATCH 03/22] [WIP] finalize rotated table planning scenes --- .../ada_feeding_action_servers_custom.yaml | 41 ++++++++++++++++++- ada_feeding/scripts/create_action_servers.py | 3 +- .../update_from_table_detection.py | 2 +- .../config/ada_planning_scene.yaml | 14 ++++--- 4 files changed, 52 insertions(+), 8 deletions(-) diff --git a/ada_feeding/config/ada_feeding_action_servers_custom.yaml b/ada_feeding/config/ada_feeding_action_servers_custom.yaml index 6024c969..b191009e 100644 --- a/ada_feeding/config/ada_feeding_action_servers_custom.yaml +++ b/ada_feeding/config/ada_feeding_action_servers_custom.yaml @@ -6,6 +6,7 @@ ada_feeding_action_servers: - side_staging_2 - side_staging_3 - hospital_table_mount_1 + - seated_side_table_1 hospital_table_mount_1: AcquireFood.tree_kwargs.resting_joint_positions: - -2.40341872791 @@ -49,7 +50,45 @@ ada_feeding_action_servers: - -3.0701456341812565 - 1.8009590918986131 planning_scene_namespace_to_use: bedside - namespace_to_use: hospital_table_mount_1 + namespace_to_use: seated_side_table_1 + seated_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 side_staging_1: AcquireFood.tree_kwargs.resting_joint_positions: - -0.8849039817349507 diff --git a/ada_feeding/scripts/create_action_servers.py b/ada_feeding/scripts/create_action_servers.py index f8b731c1..a8f585b0 100755 --- a/ada_feeding/scripts/create_action_servers.py +++ b/ada_feeding/scripts/create_action_servers.py @@ -1028,11 +1028,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( 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..6f69dcfe 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, diff --git a/ada_planning_scene/config/ada_planning_scene.yaml b/ada_planning_scene/config/ada_planning_scene.yaml index 6afe9e3e..c7f29104 100644 --- a/ada_planning_scene/config/ada_planning_scene.yaml +++ b/ada_planning_scene/config/ada_planning_scene.yaml @@ -23,7 +23,7 @@ ada_planning_scene: namespaces: - seated - bedside - namespace_to_use: bedside # seated # + namespace_to_use: seated # bedside # ############################################################################ # Parameters related to the PlanningSceneInitializer class @@ -71,8 +71,8 @@ ada_planning_scene: filename: table.stl # Initial predicted position and orientation of the table; these values get # updated as the robot perceives the table. - position: [0.08, -0.5, -0.56] - quat_xyzw: [0.0, 0.0, 0.0, 1.0] + 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 @@ -88,8 +88,8 @@ ada_planning_scene: 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.37, 0.17, 0.85] - quat_xyzw: [0.0, 0.0, 0.0, 1.0] + 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 @@ -119,6 +119,7 @@ ada_planning_scene: filename: hospital_table.stl # Initial predicted position and orientation of the table; these values get # updated as the robot perceives the table. + # TODO: Verify lateral position relative to face 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 @@ -134,6 +135,7 @@ ada_planning_scene: 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] + # TODO: rotate!!! position: [-0.37, 0.11, 0.26] quat_xyzw: [0.0, 0.0, 0.0, 1.0] frame_id: root # the frame_id that the pose is relative to @@ -226,10 +228,12 @@ ada_planning_scene: # - `table_pos_dist_thresh`: The threshold on the linear distance (in m) between the default table position # and the latest table position to determine whether to reject or accept the latest table position. seated: + # TODO: rotate!!! table_origin_offset: [-0.20, -0.25, -0.79] table_quat_dist_thresh: 0.349 table_pos_dist_thresh: 0.5 bedside: + # TODO: rotate!!! table_origin_offset: [-0.23, 0.71, -1.0067] table_quat_dist_thresh: 0.349 table_pos_dist_thresh: 0.5 From 37df3cc5ab765f2d31d8ea465c5b331f8d5a2f65 Mon Sep 17 00:00:00 2001 From: Taylor Kessler Faulkner Date: Fri, 30 Aug 2024 13:38:00 -0700 Subject: [PATCH 04/22] Add timeouts to MoveIt2 Plan --- .../ada_feeding/trees/acquire_food_tree.py | 192 +++++++++++------- .../ada_feeding/trees/move_from_mouth_tree.py | 92 +++++---- ...o_configuration_with_ft_thresholds_tree.py | 29 +-- ...configuration_with_wheelchair_wall_tree.py | 33 +-- .../ada_feeding/trees/move_to_pose_tree.py | 37 ++-- 5 files changed, 235 insertions(+), 148 deletions(-) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 804d8101..52d8c29c 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( @@ -181,15 +195,24 @@ 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("trajectory")}, + ), ), MoveIt2Execute( name="Resting", @@ -295,20 +318,29 @@ 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("trajectory") + }, + ), ), MoveIt2Execute( name="RecoveryOffset", @@ -418,20 +450,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("trajectory"), + "end_joint_state": BlackboardKey("test_into_joints"), + }, + ), ), ### Test MoveIntoFood MoveIt2PoseConstraint( @@ -445,20 +482,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") + }, + ), ), ], ) @@ -643,25 +688,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 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_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")}, + ), ), ) From 170b891b63067294901206cc9aad368d7f92f836 Mon Sep 17 00:00:00 2001 From: Taylor Kessler Faulkner Date: Fri, 30 Aug 2024 15:15:04 -0700 Subject: [PATCH 05/22] Only create a planning scene batch if are retrying until the add is succesful --- .../collision_object_manager.py | 73 +++++++++++-------- .../planning_scene_initializer.py | 3 + 2 files changed, 45 insertions(+), 31 deletions(-) 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..01f94c69 100644 --- a/ada_planning_scene/ada_planning_scene/collision_object_manager.py +++ b/ada_planning_scene/ada_planning_scene/collision_object_manager.py @@ -222,6 +222,9 @@ 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() @@ -232,21 +235,24 @@ def add_collision_objects( 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()) @@ -262,11 +268,14 @@ def add_collision_objects( 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( @@ -312,7 +321,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): @@ -323,13 +332,14 @@ def add_collision_objects( 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,9 +365,10 @@ 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) return True 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..5ada7478 100644 --- a/ada_planning_scene/ada_planning_scene/planning_scene_initializer.py +++ b/ada_planning_scene/ada_planning_scene/planning_scene_initializer.py @@ -442,6 +442,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 From e03cd4bb58f45cf4799941472fe398c3eb22678e Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Fri, 30 Aug 2024 16:54:35 -0700 Subject: [PATCH 06/22] Mouth should face the forktip --- .../behaviors/transfer/__init__.py | 5 + .../behaviors/transfer/compute_mouth_frame.py | 176 ++++++++++++++++++ .../ada_feeding/trees/move_to_mouth_tree.py | 37 +--- 3 files changed, 185 insertions(+), 33 deletions(-) create mode 100644 ada_feeding/ada_feeding/behaviors/transfer/__init__.py create mode 100644 ada_feeding/ada_feeding/behaviors/transfer/compute_mouth_frame.py 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..6d64cdfd --- /dev/null +++ b/ada_feeding/ada_feeding/behaviors/transfer/compute_mouth_frame.py @@ -0,0 +1,176 @@ +#!/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 + +# 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 + mouth_transform = self.tf_buffer.lookup_transform( + world_frame, + detected_mouth_center.header.frame_id, + timestamp, + ) + + # 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, + ) + + # Get the yaw of the face frame + x_unit = Vector3(x=1, y=0, z=0) + x_pos = Vector3( + x=mouth_transform.transform.translation.x - orientation_target_transform.transform.translation.x, + y=mouth_transform.transform.translation.y - orientation_target_transform.transform.translation.y, + z=0, + ) + quat = quat_between_vectors(x_unit, x_pos) + + # Create return object + mouth_pose = PoseStamped() + mouth_pose.header.frame_id = world_frame + mouth_pose.header.stamp = detected_mouth_center.header.stamp + mouth_pose.pose.position = detected_mouth_center.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/trees/move_to_mouth_tree.py b/ada_feeding/ada_feeding/trees/move_to_mouth_tree.py index 27459a30..8ebdae87 100644 --- a/ada_feeding/ada_feeding/trees/move_to_mouth_tree.py +++ b/ada_feeding/ada_feeding/trees/move_to_mouth_tree.py @@ -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 From c4a157743672a3d3d3a30f1bfa05226d36bc71f9 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Fri, 30 Aug 2024 18:47:41 -0700 Subject: [PATCH 07/22] Fixes from in-person testing --- .../behaviors/transfer/compute_mouth_frame.py | 35 ++++++++++---- .../ada_feeding/trees/move_to_mouth_tree.py | 3 +- .../update_from_face_detection.py | 47 ++++++++++++++----- 3 files changed, 63 insertions(+), 22 deletions(-) diff --git a/ada_feeding/ada_feeding/behaviors/transfer/compute_mouth_frame.py b/ada_feeding/ada_feeding/behaviors/transfer/compute_mouth_frame.py index 6d64cdfd..44bb8e75 100644 --- a/ada_feeding/ada_feeding/behaviors/transfer/compute_mouth_frame.py +++ b/ada_feeding/ada_feeding/behaviors/transfer/compute_mouth_frame.py @@ -14,6 +14,7 @@ from overrides import override import py_trees import rclpy +import tf2_ros # Local imports from ada_feeding.helpers import ( @@ -105,7 +106,12 @@ def update(self) -> py_trees.common.Status: # Validate inputs if not self.blackboard_exists( - ["detected_mouth_center", "timestamp", "world_frame", "frame_to_orient_towards"] + [ + "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." @@ -132,11 +138,15 @@ def update(self) -> py_trees.common.Status: # Use a Timeout decorator to determine failure. self.logger.warning("ComputeMouthFrame waiting on world/camera TF...") return py_trees.common.Status.RUNNING - mouth_transform = self.tf_buffer.lookup_transform( + 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( @@ -153,21 +163,28 @@ def update(self) -> py_trees.common.Status: 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, y=0, z=0) + x_unit = Vector3(x=1.0, y=0.0, z=0.0) x_pos = Vector3( - x=mouth_transform.transform.translation.x - orientation_target_transform.transform.translation.x, - y=mouth_transform.transform.translation.y - orientation_target_transform.transform.translation.y, - z=0, + 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 = world_frame - mouth_pose.header.stamp = detected_mouth_center.header.stamp - mouth_pose.pose.position = detected_mouth_center.point + 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 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 8ebdae87..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, @@ -494,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_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: From 86aed728ec29dd3c2509900f4fa6b82920eece2f Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Fri, 30 Aug 2024 18:48:33 -0700 Subject: [PATCH 08/22] Better logging --- ada_feeding/ada_feeding/behaviors/ros/tf.py | 1 + ada_feeding/scripts/create_action_servers.py | 16 +++++++++++++++- 2 files changed, 16 insertions(+), 1 deletion(-) 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/scripts/create_action_servers.py b/ada_feeding/scripts/create_action_servers.py index a8f585b0..fd0d405b 100755 --- a/ada_feeding/scripts/create_action_servers.py +++ b/ada_feeding/scripts/create_action_servers.py @@ -1080,7 +1080,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( From 897bc1be3d8f2db5f4f7e89d71560e89d4670c1a Mon Sep 17 00:00:00 2001 From: Taylor Kessler Faulkner Date: Fri, 30 Aug 2024 18:49:06 -0700 Subject: [PATCH 09/22] Fail planning scene initialization ifthe node doesn't get the robot configurations --- .../ada_planning_scene/workspace_walls.py | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/ada_planning_scene/ada_planning_scene/workspace_walls.py b/ada_planning_scene/ada_planning_scene/workspace_walls.py index 761a3402..a4590065 100644 --- a/ada_planning_scene/ada_planning_scene/workspace_walls.py +++ b/ada_planning_scene/ada_planning_scene/workspace_walls.py @@ -850,6 +850,9 @@ def __get_robot_configurations( 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( @@ -866,6 +869,9 @@ 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): @@ -893,6 +899,9 @@ 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.") + return False, {} # Add the current joint state if ( @@ -985,7 +994,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. From e5a3ee825d5d80c607c432f9f568610f9a29bef5 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Fri, 30 Aug 2024 18:49:48 -0700 Subject: [PATCH 10/22] Adjust table detection offsets --- ada_planning_scene/config/ada_planning_scene.yaml | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/ada_planning_scene/config/ada_planning_scene.yaml b/ada_planning_scene/config/ada_planning_scene.yaml index c7f29104..236ccbc6 100644 --- a/ada_planning_scene/config/ada_planning_scene.yaml +++ b/ada_planning_scene/config/ada_planning_scene.yaml @@ -228,8 +228,7 @@ ada_planning_scene: # - `table_pos_dist_thresh`: The threshold on the linear distance (in m) between the default table position # and the latest table position to determine whether to reject or accept the latest table position. seated: - # TODO: rotate!!! - table_origin_offset: [-0.20, -0.25, -0.79] + table_origin_offset: [-0.10, -0.25, -0.79] table_quat_dist_thresh: 0.349 table_pos_dist_thresh: 0.5 bedside: From 2c5cd78d214daf74a97e869bc9260b2730da35cc Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Fri, 30 Aug 2024 19:02:16 -0700 Subject: [PATCH 11/22] Re-add old planning scenes, keep the 90 degrees as separate options --- .../ada_feeding_action_servers_custom.yaml | 54 ++++++- .../ada_feeding_action_servers_default.yaml | 3 +- .../config/ada_planning_scene.yaml | 135 +++++++++++++++++- 3 files changed, 184 insertions(+), 8 deletions(-) diff --git a/ada_feeding/config/ada_feeding_action_servers_custom.yaml b/ada_feeding/config/ada_feeding_action_servers_custom.yaml index b191009e..3c316611 100644 --- a/ada_feeding/config/ada_feeding_action_servers_custom.yaml +++ b/ada_feeding/config/ada_feeding_action_servers_custom.yaml @@ -2,12 +2,53 @@ ada_feeding_action_servers: ros__parameters: custom_namespaces: + - bed_side_table_1 + - bed_front_table_1 + - wheelchair_side_table_1 + # TODO: For deployment, remove the below 3! - side_staging_1 - side_staging_2 - side_staging_3 - - hospital_table_mount_1 - - seated_side_table_1 - hospital_table_mount_1: + bed_front_table_1: + AcquireFood.tree_kwargs.resting_joint_positions: + - -1.1717344206197549 + - 3.8349623560423542 + - 1.143531544675816 + - -1.4583640571583674 + - -1.8237450981853311 + - 7.026948142726954 + MoveAbovePlate.tree_kwargs.joint_positions: + - -0.19797034068 + - 3.6562432079254257 + - 1.6497725020792509 + - -2.659056359891099 + - 4.916644381246061 + - 1.7380801275071338 + MoveFromMouth.tree_kwargs.staging_configuration_position: + - -0.6014194269891053 + - 0.08069224383248258 + - 0.19766024762409728 + MoveFromMouth.tree_kwargs.staging_configuration_quat_xyzw: + - 0.062023235263225555 + - -0.7245047876010136 + - -0.6768258670154677 + - 0.1146851200873633 + MoveToRestingPosition.tree_kwargs.goal_configuration: + - -1.1717344206197549 + - 3.8349623560423542 + - 1.143531544675816 + - -1.4583640571583674 + - -1.8237450981853311 + - 7.026948142726954 + MoveToStagingConfiguration.tree_kwargs.goal_configuration: + - -0.5394174198411514 + - 4.294121954324551 + - 2.108729300644438 + - -1.0393311059687216 + - -2.1827671763194583 + - 1.6106314909061383 + planning_scene_namespace_to_use: bedside + bed_side_table_1: AcquireFood.tree_kwargs.resting_joint_positions: - -2.40341872791 - 3.9280238571740833 @@ -49,9 +90,9 @@ ada_feeding_action_servers: - -0.6739427412487604 - -3.0701456341812565 - 1.8009590918986131 - planning_scene_namespace_to_use: bedside - namespace_to_use: seated_side_table_1 - seated_side_table_1: + planning_scene_namespace_to_use: bedside_90 + namespace_to_use: wheelchair_side_table_1 + wheelchair_side_table_1: AcquireFood.tree_kwargs.resting_joint_positions: - -1.0417226246276128 - 4.404614643739175 @@ -89,6 +130,7 @@ ada_feeding_action_servers: - -5.48339275809026 - 4.099009718117641 - 3.976385530989377 + planning_scene_namespace_to_use: seated_90 side_staging_1: AcquireFood.tree_kwargs.resting_joint_positions: - -0.8849039817349507 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_planning_scene/config/ada_planning_scene.yaml b/ada_planning_scene/config/ada_planning_scene.yaml index 236ccbc6..e4bf0390 100644 --- a/ada_planning_scene/config/ada_planning_scene.yaml +++ b/ada_planning_scene/config/ada_planning_scene.yaml @@ -23,13 +23,79 @@ 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 ############################################################################ seated: + # 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.08, -0.5, -0.56] + quat_xyzw: [0.0, 0.0, 0.0, 1.0] + 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.37, 0.17, 0.85] + quat_xyzw: [0.0, 0.0, 0.0, 1.0] + 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. @@ -94,6 +160,52 @@ ada_planning_scene: 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 + # 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.68, 0.4, -1.24] + 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.68, 0.4, -1.24] # 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.68, 0.6, -1.04] + 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 + 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.68, 0.22, 0.08] + 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.37, 0.11, 0.26] + quat_xyzw: [0.0, 0.0, 0.0, 1.0] + 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. @@ -151,8 +263,13 @@ 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 @@ -211,10 +328,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 @@ -228,10 +353,18 @@ ada_planning_scene: # - `table_pos_dist_thresh`: The threshold on the linear distance (in m) between the default table position # and the latest table position to determine whether to reject or accept the latest table position. seated: + 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: # TODO: rotate!!! table_origin_offset: [-0.23, 0.71, -1.0067] table_quat_dist_thresh: 0.349 From ade81d97dbab8cb9436eb13650c0ee7da344a9c0 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Sat, 31 Aug 2024 10:41:32 -0700 Subject: [PATCH 12/22] Add a timeout to retry_call_ros_service --- .../idioms/retry_call_ros_service.py | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) 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( From eda2f68b12d9f30db06a6316160f01a6bbc75f79 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Sat, 31 Aug 2024 11:24:14 -0700 Subject: [PATCH 13/22] Re-arrange custom params --- .../ada_feeding_action_servers_custom.yaml | 94 +++++++++---------- 1 file changed, 47 insertions(+), 47 deletions(-) diff --git a/ada_feeding/config/ada_feeding_action_servers_custom.yaml b/ada_feeding/config/ada_feeding_action_servers_custom.yaml index 3c316611..b5eb96dd 100644 --- a/ada_feeding/config/ada_feeding_action_servers_custom.yaml +++ b/ada_feeding/config/ada_feeding_action_servers_custom.yaml @@ -1,14 +1,6 @@ # This file is auto-generated by create_action_servers.py ada_feeding_action_servers: ros__parameters: - custom_namespaces: - - bed_side_table_1 - - bed_front_table_1 - - wheelchair_side_table_1 - # TODO: For deployment, remove the below 3! - - side_staging_1 - - side_staging_2 - - side_staging_3 bed_front_table_1: AcquireFood.tree_kwargs.resting_joint_positions: - -1.1717344206197549 @@ -91,46 +83,15 @@ ada_feeding_action_servers: - -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 + # TODO: For deployment, remove the below 3! + - side_staging_1 + - side_staging_2 + - side_staging_3 namespace_to_use: wheelchair_side_table_1 - 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 side_staging_1: AcquireFood.tree_kwargs.resting_joint_positions: - -0.8849039817349507 @@ -269,3 +230,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 From 25ce83aaf61bbd457bd5846d83f386854a1dd0ee Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Sat, 31 Aug 2024 14:58:38 -0700 Subject: [PATCH 14/22] Have actions plan deeper --- .../config/acquisition_library.yaml | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) 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 From 7da8f92e8129c7b15ae90dcba8dac774a92be733 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Sat, 31 Aug 2024 15:00:03 -0700 Subject: [PATCH 15/22] Improve logging --- ada_feeding/config/ada_feeding_action_servers_custom.yaml | 1 - .../ada_planning_scene/collision_object_manager.py | 6 ++++++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/ada_feeding/config/ada_feeding_action_servers_custom.yaml b/ada_feeding/config/ada_feeding_action_servers_custom.yaml index b5eb96dd..dce90c0c 100644 --- a/ada_feeding/config/ada_feeding_action_servers_custom.yaml +++ b/ada_feeding/config/ada_feeding_action_servers_custom.yaml @@ -87,7 +87,6 @@ ada_feeding_action_servers: - bed_side_table_1 - bed_front_table_1 - wheelchair_side_table_1 - # TODO: For deployment, remove the below 3! - side_staging_1 - side_staging_2 - side_staging_3 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 01f94c69..5a49fafd 100644 --- a/ada_planning_scene/ada_planning_scene/collision_object_manager.py +++ b/ada_planning_scene/ada_planning_scene/collision_object_manager.py @@ -312,7 +312,13 @@ def add_collision_objects( quat_xyzw=params.quat_xyzw, frame_id=params.frame_id, ) + self.__node.get_logger().info( + f"Added collision object {object_id}. About to sleep.", + ) rate.sleep() + self.__node.get_logger().info( + "Woke up from sleep after adding collision object.", + ) if not retry_until_added: break From fd094c83a5a783251ac2ba4dad36b3525598dfd6 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Sat, 31 Aug 2024 17:26:28 -0700 Subject: [PATCH 16/22] Destroy rates after use --- .../behaviors/moveit2/moveit2_execute.py | 1 + ada_feeding/scripts/create_action_servers.py | 7 +++++++ .../segment_from_point.py | 4 ++++ .../ada_planning_scene/ada_planning_scene.py | 6 ++++++ .../collision_object_manager.py | 17 +++++++++++++++ .../planning_scene_initializer.py | 4 ++++ .../ada_planning_scene/workspace_walls.py | 21 +++++++++++++++++++ 7 files changed, 60 insertions(+) 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/scripts/create_action_servers.py b/ada_feeding/scripts/create_action_servers.py index fd0d405b..ad67daa9 100755 --- a/ada_feeding/scripts/create_action_servers.py +++ b/ada_feeding/scripts/create_action_servers.py @@ -440,6 +440,8 @@ def set_planning_scene_namespace_to_use( start_time = self.get_clock().now() 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 @@ -469,10 +471,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 +508,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( @@ -1126,6 +1132,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_perception/ada_feeding_perception/segment_from_point.py b/ada_feeding_perception/ada_feeding_perception/segment_from_point.py index 75f8d368..8fc9cfa1 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,8 @@ 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 +731,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 +749,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_planning_scene/ada_planning_scene/ada_planning_scene.py b/ada_planning_scene/ada_planning_scene/ada_planning_scene.py index 0e2fee70..adfe144f 100644 --- a/ada_planning_scene/ada_planning_scene/ada_planning_scene.py +++ b/ada_planning_scene/ada_planning_scene/ada_planning_scene.py @@ -263,6 +263,8 @@ def __get_namespace_to_use( start_time = self.get_clock().now() 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( @@ -307,6 +309,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 +330,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 5a49fafd..8dd554f4 100644 --- a/ada_planning_scene/ada_planning_scene/collision_object_manager.py +++ b/ada_planning_scene/ada_planning_scene/collision_object_manager.py @@ -165,6 +165,8 @@ def get_global_collision_objects( # Start the time 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: @@ -173,6 +175,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 +195,7 @@ def get_global_collision_objects( batch_id_to_update=self.__GLOBAL_BATCH_ID, ) + cleanup() return True def add_collision_objects( @@ -229,6 +233,8 @@ def add_collision_objects( # 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): @@ -265,6 +271,7 @@ 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 @@ -335,6 +342,7 @@ 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 @@ -376,6 +384,7 @@ def add_collision_objects( self.__collision_objects_per_batch.pop(batch_id) self.__attached_collision_objects_per_batch.pop(batch_id) + cleanup() return True def move_collision_objects( @@ -401,6 +410,8 @@ def move_collision_objects( # 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): @@ -469,6 +480,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( @@ -489,6 +501,8 @@ def clear_all_collision_objects( # Start the time 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() @@ -496,6 +510,7 @@ def clear_all_collision_objects( self.__node.get_logger().error( "Could not clear planning scene; service is not ready" ) + cleanup() return False while not future.done(): @@ -504,11 +519,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 5ada7478..f62815c1 100644 --- a/ada_planning_scene/ada_planning_scene/planning_scene_initializer.py +++ b/ada_planning_scene/ada_planning_scene/planning_scene_initializer.py @@ -306,6 +306,8 @@ def __wait_for_moveit(self, timeout: Duration(seconds=10.0)) -> bool: # Get the start time 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 @@ -313,9 +315,11 @@ def __wait_for_moveit(self, timeout: Duration(seconds=10.0)) -> bool: if ( self.__collision_object_manager.moveit2._get_planning_scene_service.service_is_ready() ): + cleanup() return True rate.sleep() + cleanup() return False def initialize( diff --git a/ada_planning_scene/ada_planning_scene/workspace_walls.py b/ada_planning_scene/ada_planning_scene/workspace_walls.py index a4590065..f2b98950 100644 --- a/ada_planning_scene/ada_planning_scene/workspace_walls.py +++ b/ada_planning_scene/ada_planning_scene/workspace_walls.py @@ -693,6 +693,8 @@ def __get_robot_model( # Start the time 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(): @@ -700,6 +702,7 @@ def __get_robot_model( self.__node.get_logger().error( "Timeout while waiting for the get URDF parameter service." ) + cleanup() return False rate.sleep() @@ -712,6 +715,7 @@ def __get_robot_model( self.__node.get_logger().error( "Timeout while getting the robot's URDF." ) + cleanup() return False rate.sleep() @@ -720,11 +724,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 +743,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( @@ -758,6 +765,8 @@ def __get_parameter_prefix( # Start the time 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(): @@ -765,6 +774,7 @@ def __get_parameter_prefix( self.__node.get_logger().error( "Timeout while waiting for the get robot configurations parameter service." ) + cleanup() return False, "" rate.sleep() @@ -777,6 +787,7 @@ def __get_parameter_prefix( self.__node.get_logger().error( "Timeout while getting the namespace to use." ) + cleanup() return False, "" rate.sleep() @@ -787,6 +798,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 +808,7 @@ def __get_parameter_prefix( else: prefix = response.values[0].string_value + "." + cleanup() return True, prefix def __joint_states_callback(self, msg: JointState) -> None: @@ -840,6 +853,8 @@ def __get_robot_configurations( # Start the time 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( @@ -847,6 +862,7 @@ def __get_robot_configurations( ) if not success: self.__node.get_logger().error("Failed to get the parameter prefix.") + cleanup() return False, {} # Wait for the service to be ready @@ -858,6 +874,7 @@ def __get_robot_configurations( 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() @@ -878,6 +895,7 @@ def __get_robot_configurations( self.__node.get_logger().error( "Timeout while getting the robot configurations." ) + cleanup() return False, {} if publish_feedback is not None: publish_feedback() @@ -890,6 +908,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: @@ -901,6 +920,7 @@ def __get_robot_configurations( 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 @@ -916,6 +936,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( From 2715e59435a4aaca1e5f96e04e79eaa5b59b4c10 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Sat, 31 Aug 2024 17:26:58 -0700 Subject: [PATCH 17/22] LinUCB shoudl return all actions, with a one-hot probability vector --- .../ada_feeding_action_select/policies/linear_policies.py | 5 ++++- .../ada_feeding_action_select/policy_service.py | 4 +++- 2 files changed, 7 insertions(+), 2 deletions(-) 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..bc79ddcf 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,11 @@ 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: From cc3b9ae8de6823d7c68f9b2dd4cade6792da83dd Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Sat, 31 Aug 2024 18:31:37 -0700 Subject: [PATCH 18/22] Formatting --- ada_feeding/scripts/create_action_servers.py | 1 + .../ada_feeding_action_select/policies/linear_policies.py | 3 ++- .../ada_feeding_perception/segment_from_point.py | 2 ++ ada_planning_scene/ada_planning_scene/ada_planning_scene.py | 1 + .../ada_planning_scene/collision_object_manager.py | 4 ++++ .../ada_planning_scene/planning_scene_initializer.py | 1 + ada_planning_scene/ada_planning_scene/workspace_walls.py | 3 +++ 7 files changed, 14 insertions(+), 1 deletion(-) diff --git a/ada_feeding/scripts/create_action_servers.py b/ada_feeding/scripts/create_action_servers.py index ad67daa9..97bb5564 100755 --- a/ada_feeding/scripts/create_action_servers.py +++ b/ada_feeding/scripts/create_action_servers.py @@ -440,6 +440,7 @@ def set_planning_scene_namespace_to_use( start_time = self.get_clock().now() timeout = rclpy.time.Duration(seconds=timeout_secs) rate = self.create_rate(rate_hz) + def cleanup(): self.destroy_rate(rate) 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 bc79ddcf..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 @@ -344,7 +344,8 @@ def choice( action_i = np.argmin(lcb) return [ - (1.0 if i == action_i else 0.0, self.library[i]) for i in range(self.n_actions) + (1.0 if i == action_i else 0.0, self.library[i]) + for i in range(self.n_actions) ] @override 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 8fc9cfa1..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,8 +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 ) 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 adfe144f..e7b49487 100644 --- a/ada_planning_scene/ada_planning_scene/ada_planning_scene.py +++ b/ada_planning_scene/ada_planning_scene/ada_planning_scene.py @@ -263,6 +263,7 @@ def __get_namespace_to_use( start_time = self.get_clock().now() timeout = rclpy.time.Duration(seconds=timeout_secs) rate = self.create_rate(rate_hz) + def cleanup(): self.destroy_rate(rate) 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 8dd554f4..75c0e656 100644 --- a/ada_planning_scene/ada_planning_scene/collision_object_manager.py +++ b/ada_planning_scene/ada_planning_scene/collision_object_manager.py @@ -165,6 +165,7 @@ def get_global_collision_objects( # Start the time start_time = self.__node.get_clock().now() rate = self.__node.create_rate(rate_hz) + def cleanup(): self.__node.destroy_rate(rate) @@ -233,6 +234,7 @@ def add_collision_objects( # Start the time start_time = self.__node.get_clock().now() rate = self.__node.create_rate(rate_hz) + def cleanup(): self.__node.destroy_rate(rate) @@ -410,6 +412,7 @@ def move_collision_objects( # Start the time start_time = self.__node.get_clock().now() rate = self.__node.create_rate(rate_hz) + def cleanup(): self.__node.destroy_rate(rate) @@ -501,6 +504,7 @@ def clear_all_collision_objects( # Start the time start_time = self.__node.get_clock().now() rate = self.__node.create_rate(rate_hz) + def cleanup(): self.__node.destroy_rate(rate) 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 f62815c1..b1218e8d 100644 --- a/ada_planning_scene/ada_planning_scene/planning_scene_initializer.py +++ b/ada_planning_scene/ada_planning_scene/planning_scene_initializer.py @@ -306,6 +306,7 @@ def __wait_for_moveit(self, timeout: Duration(seconds=10.0)) -> bool: # Get the start time start_time = self.__node.get_clock().now() rate = self.__node.create_rate(self.__wait_for_moveit_hz) + def cleanup(): self.__node.destroy_rate(rate) diff --git a/ada_planning_scene/ada_planning_scene/workspace_walls.py b/ada_planning_scene/ada_planning_scene/workspace_walls.py index f2b98950..9d5e31b1 100644 --- a/ada_planning_scene/ada_planning_scene/workspace_walls.py +++ b/ada_planning_scene/ada_planning_scene/workspace_walls.py @@ -693,6 +693,7 @@ def __get_robot_model( # Start the time start_time = self.__node.get_clock().now() rate = self.__node.create_rate(rate_hz) + def cleanup(): self.__node.destroy_rate(rate) @@ -765,6 +766,7 @@ def __get_parameter_prefix( # Start the time start_time = self.__node.get_clock().now() rate = self.__node.create_rate(rate_hz) + def cleanup(): self.__node.destroy_rate(rate) @@ -853,6 +855,7 @@ def __get_robot_configurations( # Start the time start_time = self.__node.get_clock().now() rate = self.__node.create_rate(rate_hz) + def cleanup(): self.__node.destroy_rate(rate) From c30bf499b6c036b5256623d78d6ea7a8690b6fc5 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Sat, 31 Aug 2024 18:32:43 -0700 Subject: [PATCH 19/22] Send acquisition report info in feedback instead of result --- .../acquisition/compute_action_constraints.py | 3 + .../ada_feeding/trees/acquire_food_tree.py | 90 +++++++++++-------- ada_feeding_msgs/action/AcquireFood.action | 27 +++--- 3 files changed, 71 insertions(+), 49 deletions(-) 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/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 52d8c29c..293b4325 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -177,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, @@ -211,13 +211,15 @@ def create_tree( "max_acceleration_scale": self.max_acceleration_scaling_to_resting_configuration, "allowed_planning_time": self.allowed_planning_time_to_resting_configuration, }, - outputs={"trajectory": BlackboardKey("trajectory")}, + outputs={ + "trajectory": BlackboardKey("resting_trajectory") + }, ), ), MoveIt2Execute( name="Resting", ns=name, - inputs={"trajectory": BlackboardKey("trajectory")}, + inputs={"trajectory": BlackboardKey("resting_trajectory")}, outputs={}, ), ], @@ -338,14 +340,20 @@ def create_tree( "allowed_planning_time": self.allowed_planning_time_for_recovery, }, outputs={ - "trajectory": BlackboardKey("trajectory") + "trajectory": BlackboardKey( + "recovery_trajectory" + ) }, ), ), MoveIt2Execute( name="RecoveryOffset", ns=name, - inputs={"trajectory": BlackboardKey("trajectory")}, + inputs={ + "trajectory": BlackboardKey( + "recovery_trajectory" + ) + }, outputs={}, ), ], @@ -427,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"), }, ), ), @@ -465,7 +474,7 @@ def move_above_plan( "max_path_len_joint": max_path_len_joint, }, outputs={ - "trajectory": BlackboardKey("trajectory"), + "trajectory": BlackboardKey("move_above_trajectory"), "end_joint_state": BlackboardKey("test_into_joints"), }, ), @@ -515,7 +524,7 @@ def move_above_plan( memory=True, children=[ scoped_behavior( - name="TableCollision", + name="Success", # Set Approach F/T Thresh pre_behavior=( Success() @@ -543,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", @@ -611,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 @@ -934,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 @@ -984,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 @@ -997,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_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 From 7dec06570588c0605ab6c67a207e29e983d8f013 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Sun, 1 Sep 2024 15:38:43 -0700 Subject: [PATCH 20/22] Finish tuning planning scene for 90-degree rotated in-bed --- ada_planning_scene/config/ada_planning_scene.yaml | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/ada_planning_scene/config/ada_planning_scene.yaml b/ada_planning_scene/config/ada_planning_scene.yaml index e4bf0390..c05d34af 100644 --- a/ada_planning_scene/config/ada_planning_scene.yaml +++ b/ada_planning_scene/config/ada_planning_scene.yaml @@ -217,13 +217,13 @@ ada_planning_scene: - 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.68, 0.4, -1.24] + 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.68, 0.4, -1.24] # should match the wheelchair position + 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 @@ -231,7 +231,6 @@ ada_planning_scene: filename: hospital_table.stl # Initial predicted position and orientation of the table; these values get # updated as the robot perceives the table. - # TODO: Verify lateral position relative to face 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 @@ -240,16 +239,15 @@ ada_planning_scene: filename: tom.stl # This is an initial guess of head position; it will be updated as the # robot perceives the face. - position: [-0.68, 0.22, 0.08] + 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] - # TODO: rotate!!! - position: [-0.37, 0.11, 0.26] - quat_xyzw: [0.0, 0.0, 0.0, 1.0] + 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 @@ -365,7 +363,6 @@ ada_planning_scene: table_quat_dist_thresh: 0.349 table_pos_dist_thresh: 0.5 bedside_90: - # TODO: rotate!!! - table_origin_offset: [-0.23, 0.71, -1.0067] + table_origin_offset: [-0.71, -0.23, -1.0067] table_quat_dist_thresh: 0.349 table_pos_dist_thresh: 0.5 From fc1c04100aee985fdfc3b8bd7d9f7aa5216f19f1 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Sun, 1 Sep 2024 15:39:29 -0700 Subject: [PATCH 21/22] Improve logs --- .../ada_planning_scene/collision_object_manager.py | 4 ++-- .../ada_planning_scene/update_from_table_detection.py | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) 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 75c0e656..ce3e34a1 100644 --- a/ada_planning_scene/ada_planning_scene/collision_object_manager.py +++ b/ada_planning_scene/ada_planning_scene/collision_object_manager.py @@ -321,11 +321,11 @@ def cleanup(): quat_xyzw=params.quat_xyzw, frame_id=params.frame_id, ) - self.__node.get_logger().info( + self.__node.get_logger().debug( f"Added collision object {object_id}. About to sleep.", ) rate.sleep() - self.__node.get_logger().info( + self.__node.get_logger().debug( "Woke up from sleep after adding collision object.", ) if not retry_until_added: 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 6f69dcfe..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 @@ -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 From 04fe99d47c7885e187b29b68fb58a1fcbd5ac2e0 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Sun, 1 Sep 2024 16:10:27 -0700 Subject: [PATCH 22/22] Improve logs --- ada_feeding/config/ada_feeding_action_servers_custom.yaml | 6 +++++- .../ada_feeding_perception/face_detection.py | 4 +++- .../ada_feeding_perception/food_on_fork_detection.py | 4 +++- .../ada_feeding_perception/table_detection.py | 4 +++- 4 files changed, 14 insertions(+), 4 deletions(-) diff --git a/ada_feeding/config/ada_feeding_action_servers_custom.yaml b/ada_feeding/config/ada_feeding_action_servers_custom.yaml index dce90c0c..ada11edc 100644 --- a/ada_feeding/config/ada_feeding_action_servers_custom.yaml +++ b/ada_feeding/config/ada_feeding_action_servers_custom.yaml @@ -55,6 +55,8 @@ ada_feeding_action_servers: - -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 @@ -64,8 +66,10 @@ ada_feeding_action_servers: - 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.1 + - 0.025 - 0.0 - -0.01 MoveToRestingPosition.tree_kwargs.goal_configuration: 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/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}"