diff --git a/ada_feeding/README.md b/ada_feeding/README.md index f9398aef..76e54602 100644 --- a/ada_feeding/README.md +++ b/ada_feeding/README.md @@ -8,7 +8,7 @@ This code has been developed and tested with the Kinova JACO Gen2 Arm, on comput - Install [ROS2 Humble](https://docs.ros.org/en/humble/Installation.html) - Install Python dependencies: `python3 -m pip install pyyaml py_trees pymongo tornado trimesh` - Install the code to command the real robot ([instructions here](https://github.com/personalrobotics/ada_ros2/blob/main/README.md)) -- Git clone the [PRL fork of pymoveit (branch: `amaln/allowed_collision_matrix`)](https://github.com/personalrobotics/pymoveit2) and the [PRL fork of py_trees_ros (branch: `amaln/service_client`)](https://github.com/personalrobotics/py_trees_ros/tree/amaln/service_client) into your ROS2 workspace's `src` folder. +- Git clone the [PRL fork of pymoveit (branch: `amaln/cartesian_allow_collision`)](https://github.com/personalrobotics/pymoveit2/tree/amaln/cartesian_avoid_collision) and the [PRL fork of py_trees_ros (branch: `amaln/service_client`)](https://github.com/personalrobotics/py_trees_ros/tree/amaln/service_client) into your ROS2 workspace's `src` folder. - Install additional dependencies: `sudo apt install ros-humble-py-trees-ros-interfaces`. - Install the web app into your workspace ([instructions here](https://github.com/personalrobotics/feeding_web_interface/tree/main/feedingwebapp)). @@ -21,7 +21,7 @@ This code has been developed and tested with the Kinova JACO Gen2 Arm, on comput 4. Launch the RealSense & Perception: 1. Dummy nodes: `ros2 launch feeding_web_app_ros2_test feeding_web_app_dummy_nodes_launch.xml run_motion:=false run_web_bridge:=false` 2. Real nodes: Follow the instructions in the [`ada_feeding_perception` README](https://github.com/personalrobotics/ada_feeding/blob/ros2-devel/ada_feeding_perception/README.md#usage) -5. Run the action servers: `ros2 launch ada_feeding ada_feeding_launch.xml` +5. Ensure the e-stop is plugged in. Run the action servers: `ros2 launch ada_feeding ada_feeding_launch.xml` 6. Launch MoveIt2: 1. Sim (RVIZ): `ros2 launch ada_moveit demo_feeding.launch.py sim:=mock` 2. Real Robot: `ros2 launch ada_moveit demo_feeding.launch.py` @@ -31,7 +31,9 @@ This code has been developed and tested with the Kinova JACO Gen2 Arm, on comput 2. `ros2 action send_goal /AcquireFood ada_feeding_msgs/action/AcquireFood "{header: {stamp: {sec: 0, nanosec: 0}, frame_id: ''}, detected_food: {roi: {x_offset: 0, y_offset: 0, height: 0, width: 0, do_rectify: false}, mask: {header: {stamp: {sec: 0, nanosec: 0}, frame_id: ''}, format: '', data: []}, item_id: '', confidence: 0.0}}" --feedback` 3. `ros2 action send_goal /MoveToRestingPosition ada_feeding_msgs/action/MoveTo "{}" --feedback` 4. `ros2 action send_goal /MoveToMouth ada_feeding_msgs/action/MoveTo "{}" --feedback` - 5. `ros2 action send_goal /MoveToStowLocation ada_feeding_msgs/action/MoveTo "{}" --feedback` + 5. `ros2 action send_goal /MoveFromMouthToRestingPosition ada_feeding_msgs/action/MoveTo "{}" --feedback` + 6. `ros2 action send_goal /MoveFromMouthToAbovePlate ada_feeding_msgs/action/MoveTo "{}" --feedback` + 7. `ros2 action send_goal /MoveToStowLocation ada_feeding_msgs/action/MoveTo "{}" --feedback` 2. Test the individual actions with the web app: 1. Launch the web app ([instructions here](https://github.com/personalrobotics/feeding_web_interface/tree/main/feedingwebapp)) 2. Go through the web app, ensure the expected actions happen on the robot. diff --git a/ada_feeding/ada_feeding/behaviors/move_to.py b/ada_feeding/ada_feeding/behaviors/move_to.py index 90fcd22f..312e5267 100644 --- a/ada_feeding/ada_feeding/behaviors/move_to.py +++ b/ada_feeding/ada_feeding/behaviors/move_to.py @@ -84,6 +84,10 @@ def __init__( self.move_to_blackboard.register_key( key="cartesian", access=py_trees.common.Access.READ ) + # Add the ability to set a pipeline_id + self.move_to_blackboard.register_key( + key="pipeline_id", access=py_trees.common.Access.READ + ) # Add the ability to set a planner_id self.move_to_blackboard.register_key( key="planner_id", access=py_trees.common.Access.READ @@ -96,6 +100,24 @@ def __init__( self.move_to_blackboard.register_key( key="max_velocity_scaling_factor", access=py_trees.common.Access.READ ) + # Add the ability to set acceleration scaling + self.move_to_blackboard.register_key( + key="max_acceleration_scaling_factor", access=py_trees.common.Access.READ + ) + # Add the ability to set the cartesian jump threshold + self.move_to_blackboard.register_key( + key="cartesian_jump_threshold", access=py_trees.common.Access.READ + ) + # Add the ability to set the cartesian max step + self.move_to_blackboard.register_key( + key="cartesian_max_step", access=py_trees.common.Access.READ + ) + # Add the ability to set a cartesian fraction threshold (e.g., only + # accept plans that completed at least this fraction of the path) + self.move_to_blackboard.register_key( + key="cartesian_fraction_threshold", access=py_trees.common.Access.READ + ) + # Initialize the blackboard to read from the parent behavior tree self.tree_blackboard = self.attach_blackboard_client( name=name + " MoveTo", namespace=tree_name @@ -144,6 +166,15 @@ def initialise(self) -> None: self.logger.info(f"{self.name} [MoveTo::initialise()]") with self.moveit2_lock: + # Set the planner_id + self.moveit2.planner_id = get_from_blackboard_with_default( + self.move_to_blackboard, "planner_id", "RRTstarkConfigDefault" + ) + # Set the pipeline_id + self.moveit2.pipeline_id = get_from_blackboard_with_default( + self.move_to_blackboard, "pipeline_id", "ompl" + ) + # Set the planner_id self.moveit2.planner_id = get_from_blackboard_with_default( self.move_to_blackboard, "planner_id", "RRTstarkConfigDefault" @@ -159,6 +190,39 @@ def initialise(self) -> None: self.move_to_blackboard, "allowed_planning_time", 0.5 ) + # Set the max acceleration + self.moveit2.max_acceleration = get_from_blackboard_with_default( + self.move_to_blackboard, "max_acceleration_scaling_factor", 0.1 + ) + + # Set the allowed planning time + self.moveit2.allowed_planning_time = get_from_blackboard_with_default( + self.move_to_blackboard, "allowed_planning_time", 0.5 + ) + + # Set the cartesian jump threshold + self.moveit2.cartesian_jump_threshold = get_from_blackboard_with_default( + self.move_to_blackboard, "cartesian_jump_threshold", 0.0 + ) + + # Get whether we should use the cartesian interpolator + self.cartesian = get_from_blackboard_with_default( + self.move_to_blackboard, "cartesian", False + ) + + # Get the cartesian max step + self.cartesian_max_step = get_from_blackboard_with_default( + self.move_to_blackboard, "cartesian_max_step", 0.0025 + ) + + # Get the cartesian fraction threshold + self.cartesian_fraction_threshold = get_from_blackboard_with_default( + self.move_to_blackboard, "cartesian_fraction_threshold", 0.0 + ) + + # If the plan is cartesian, it should always avoid collisions + self.moveit2.cartesian_avoid_collisions = True + # Reset local state variables self.prev_query_state = None self.planning_start_time = time.time() @@ -173,14 +237,8 @@ def initialise(self) -> None: self.tree_blackboard.motion_initial_distance = 0.0 self.tree_blackboard.motion_curr_distance = 0.0 - # Get all parameters for motion, resorting to default values if unset. - self.joint_names = kinova.joint_names() - self.cartesian = get_from_blackboard_with_default( - self.move_to_blackboard, "cartesian", False - ) - # Set the joint names - self.distance_to_goal.set_joint_names(self.joint_names) + self.distance_to_goal.set_joint_names(kinova.joint_names()) # pylint: disable=too-many-branches, too-many-return-statements # This is the heart of the MoveTo behavior, so the number of branches @@ -210,7 +268,9 @@ def update(self) -> py_trees.common.Status: return py_trees.common.Status.FAILURE # Initiate an asynchronous planning call with self.moveit2_lock: - planning_future = self.moveit2.plan_async(cartesian=self.cartesian) + planning_future = self.moveit2.plan_async( + cartesian=self.cartesian, max_step=self.cartesian_max_step + ) if planning_future is None: self.logger.error( f"{self.name} [MoveTo::update()] Failed to initiate planning!" @@ -228,7 +288,9 @@ def update(self) -> py_trees.common.Status: # Get the trajectory with self.moveit2_lock: traj = self.moveit2.get_trajectory( - self.planning_future, cartesian=self.cartesian + self.planning_future, + cartesian=self.cartesian, + cartesian_fraction_threshold=self.cartesian_fraction_threshold, ) self.logger.info(f"Trajectory: {traj} | type {type(traj)}") if traj is None: @@ -237,6 +299,11 @@ def update(self) -> py_trees.common.Status: ) return py_trees.common.Status.FAILURE + # MoveIt's default cartesian interpolator doesn't respect velocity + # scaling, so we need to manually add that. + if self.cartesian and self.moveit2.max_velocity > 0.0: + MoveTo.scale_velocity(traj, self.moveit2.max_velocity) + # Set the trajectory's initial distance to goal self.tree_blackboard.motion_initial_distance = ( self.distance_to_goal.set_trajectory(traj) @@ -350,6 +417,40 @@ def shutdown(self) -> None: """ self.logger.info(f"{self.name} [MoveTo::shutdown()]") + @staticmethod + def scale_velocity(traj: JointTrajectory, scale_factor: float) -> None: + """ + Scale the velocity of the trajectory by the given factor. The resulting + trajectory should execute the same trajectory with the same continuity, + but just take 1/scale_factor as long to execute. + + This function keeps positions the same and scales time, velocities, and + accelerations. It does not modify effort. + + Parameters + ---------- + traj: The trajectory to scale. + scale_factor: The factor to scale the velocity by, in [0, 1]. + """ + for point in traj.points: + # Scale time_from_start + nsec = point.time_from_start.sec * 10.0**9 + nsec += point.time_from_start.nanosec + nsec /= scale_factor # scale time + sec = int(math.floor(nsec / 10.0**9)) + point.time_from_start.sec = sec + point.time_from_start.nanosec = int(nsec - sec * 10.0**9) + + # Scale the velocities + # pylint: disable=consider-using-enumerate + # Necessary because we want to destructively modify the trajectory + for i in range(len(point.velocities)): + point.velocities[i] *= scale_factor + + # Scale the accelerations + for i in range(len(point.accelerations)): + point.accelerations[i] *= scale_factor**2 + class DistanceToGoal: """ diff --git a/ada_feeding/ada_feeding/behaviors/toggle_collision_object.py b/ada_feeding/ada_feeding/behaviors/toggle_collision_object.py index abbb8914..2ad01561 100644 --- a/ada_feeding/ada_feeding/behaviors/toggle_collision_object.py +++ b/ada_feeding/ada_feeding/behaviors/toggle_collision_object.py @@ -23,6 +23,9 @@ class ToggleCollisionObject(py_trees.behaviour.Behaviour): scene. """ + # pylint: disable=too-many-instance-attributes + # One over is fine. + def __init__( self, name: str, @@ -60,6 +63,18 @@ def __init__( self.node, ) + # pylint: disable=attribute-defined-outside-init + # For attributes that are only used during the execution of the tree + # and get reset before the next execution, it is reasonable to define + # them in `initialise`. + def initialise(self) -> None: + """ + Reset the service_future. + """ + self.logger.info(f"{self.name} [ToggleCollisionObject::initialise()]") + + self.service_future = None + def update(self) -> py_trees.common.Status: """ (Dis)allow collisions between the robot and a collision @@ -71,10 +86,24 @@ def update(self) -> py_trees.common.Status: """ self.logger.info(f"{self.name} [ToggleCollisionObject::update()]") # (Dis)allow collisions between the robot and the collision object - with self.moveit2_lock: - succ = self.moveit2.allow_collisions(self.collision_object_id, self.allow) + if self.service_future is None: + with self.moveit2_lock: + service_future = self.moveit2.allow_collisions( + self.collision_object_id, self.allow + ) + if service_future is None: # service not available + return py_trees.common.Status.FAILURE + self.service_future = service_future + return py_trees.common.Status.RUNNING + + # Check if the service future is done + if self.service_future.done(): + with self.moveit2_lock: + succ = self.moveit2.process_allow_collision_future(self.service_future) + # Return success/failure + if succ: + return py_trees.common.Status.SUCCESS + return py_trees.common.Status.FAILURE - # Return success - if succ: - return py_trees.common.Status.SUCCESS - return py_trees.common.Status.FAILURE + # Return running + return py_trees.common.Status.RUNNING diff --git a/ada_feeding/ada_feeding/idioms/bite_transfer.py b/ada_feeding/ada_feeding/idioms/bite_transfer.py new file mode 100644 index 00000000..4da90884 --- /dev/null +++ b/ada_feeding/ada_feeding/idioms/bite_transfer.py @@ -0,0 +1,147 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +This module contains helper functions to generate the behaviors used in bite +transfer, e.g., the MoveToMouth and MoveFromMouth behaviors. +""" + +# Standard imports +import logging +import operator + +# Third-party imports +import py_trees +from py_trees.blackboard import Blackboard +from rclpy.node import Node +from std_srvs.srv import SetBool + +# Local imports +from ada_feeding.behaviors import ToggleCollisionObject +from .retry_call_ros_service import retry_call_ros_service + + +def get_toggle_collision_object_behavior( + tree_name: str, + behavior_name_prefix: str, + node: Node, + collision_object_id: str, + allow: bool, + logger: logging.Logger, +) -> py_trees.behaviour.Behaviour: + """ + Creates a behaviour that toggles a collision object. + + Parameters + ---------- + tree_name: The name of the tree that this behaviour belongs to. + behavior_name_prefix: The prefix for the name of the behaviour. + node: The ROS2 node that this behaviour belongs to. + collision_object_id: The ID of the collision object to toggle. + allow: Whether to allow or disallow the collision object. + logger: The logger to use for the behaviour. + + Returns + ------- + behavior: The behaviour that toggles the collision object. + """ + + # pylint: disable=too-many-arguments + # This is acceptable, as these are the parameters necessary to create + # the behaviour. + + toggle_collision_object = ToggleCollisionObject( + name=Blackboard.separator.join([tree_name, behavior_name_prefix]), + node=node, + collision_object_id=collision_object_id, + allow=allow, + ) + toggle_collision_object.logger = logger + return toggle_collision_object + + +def get_toggle_face_detection_behavior( + tree_name: str, + behavior_name_prefix: str, + request_data: bool, + logger: logging.Logger, +) -> py_trees.behaviour.Behaviour: + """ + Creates a behaviour that toggles face detection. + + Parameters + ---------- + tree_name: The name of the tree that this behaviour belongs to. + behavior_name_prefix: The prefix for the name of the behaviour. + request_data: The request data to send to the toggle_face_detection service. + True to turn it on, False otherwise. + logger: The logger to use for the behaviour. + + Returns + ------- + behavior: The behaviour that toggles face detection. + """ + behavior_name = Blackboard.separator.join([tree_name, behavior_name_prefix]) + key_response = Blackboard.separator.join([behavior_name, "response"]) + toggle_face_detection_behavior = retry_call_ros_service( + name=behavior_name, + service_type=SetBool, + service_name="~/toggle_face_detection", + key_request=None, + request=SetBool.Request(data=request_data), + key_response=key_response, + response_checks=[ + py_trees.common.ComparisonExpression( + variable=key_response + ".success", + value=True, + operator=operator.eq, + ) + ], + logger=logger, + ) + return toggle_face_detection_behavior + + +def get_toggle_watchdog_listener_behavior( + tree_name: str, + behavior_name_prefix: str, + request_data: bool, + logger: logging.Logger, +) -> py_trees.behaviour.Behaviour: + """ + Creates a behaviour that toggles the watchdog listener. + + Parameters + ---------- + tree_name: The name of the tree that this behaviour belongs to. + behavior_name_prefix: The prefix for the name of the behaviour. + request_data: The request data to send to the toggle_watchdog_listener service. + True to turn it on, False otherwise. + logger: The logger to use for the behaviour. + + Returns + ------- + behavior: The behaviour that toggles the watchdog listener. + """ + toggle_watchdog_listener_name = Blackboard.separator.join( + [tree_name, behavior_name_prefix] + ) + toggle_watchdog_listener_key_response = Blackboard.separator.join( + [toggle_watchdog_listener_name, "response"] + ) + toggle_watchdog_listener = retry_call_ros_service( + name=toggle_watchdog_listener_name, + service_type=SetBool, + service_name="~/toggle_watchdog_listener", + key_request=None, + request=SetBool.Request(data=request_data), + key_response=toggle_watchdog_listener_key_response, + response_checks=[ + py_trees.common.ComparisonExpression( + variable=toggle_watchdog_listener_key_response + ".success", + value=True, + operator=operator.eq, + ) + ], + logger=logger, + ) + return toggle_watchdog_listener diff --git a/ada_feeding/ada_feeding/idioms/pre_moveto_config.py b/ada_feeding/ada_feeding/idioms/pre_moveto_config.py index d9e0d158..ce50afd6 100644 --- a/ada_feeding/ada_feeding/idioms/pre_moveto_config.py +++ b/ada_feeding/ada_feeding/idioms/pre_moveto_config.py @@ -27,6 +27,7 @@ from std_srvs.srv import SetBool # Local imports +from .bite_transfer import get_toggle_watchdog_listener_behavior from .retry_call_ros_service import retry_call_ros_service @@ -98,27 +99,11 @@ def pre_moveto_config( if re_tare: # Toggle the Watchdog Listener off if toggle_watchdog_listener: - turn_watchdog_listener_off_name = Blackboard.separator.join( - [name, turn_watchdog_listener_off_prefix] - ) - turn_watchdog_listener_off_key_response = Blackboard.separator.join( - [turn_watchdog_listener_off_name, "response"] - ) - turn_watchdog_listener_off = retry_call_ros_service( - name=turn_watchdog_listener_off_name, - service_type=SetBool, - service_name="~/toggle_watchdog_listener", - key_request=None, - request=SetBool.Request(data=False), - key_response=turn_watchdog_listener_off_key_response, - response_checks=[ - py_trees.common.ComparisonExpression( - variable=turn_watchdog_listener_off_key_response + ".success", - value=True, - operator=operator.eq, - ) - ], - logger=logger, + turn_watchdog_listener_off = get_toggle_watchdog_listener_behavior( + name, + turn_watchdog_listener_off_prefix, + False, + logger, ) children.append(turn_watchdog_listener_off) @@ -149,27 +134,11 @@ def pre_moveto_config( # Toggle the Watchdog Listener on if toggle_watchdog_listener: - turn_watchdog_listener_on_name = Blackboard.separator.join( - [name, turn_watchdog_listener_on_prefix] - ) - turn_watchdog_listener_on_key_response = Blackboard.separator.join( - [turn_watchdog_listener_on_name, "response"] - ) - turn_watchdog_listener_on = retry_call_ros_service( - name=turn_watchdog_listener_on_name, - service_type=SetBool, - service_name="~/toggle_watchdog_listener", - key_request=None, - request=SetBool.Request(data=True), - key_response=turn_watchdog_listener_on_key_response, - response_checks=[ - py_trees.common.ComparisonExpression( - variable=turn_watchdog_listener_on_key_response + ".success", - value=True, - operator=operator.eq, - ) - ], - logger=logger, + turn_watchdog_listener_on = get_toggle_watchdog_listener_behavior( + name, + turn_watchdog_listener_on_prefix, + True, + logger, ) children.append(turn_watchdog_listener_on) diff --git a/ada_feeding/ada_feeding/trees/__init__.py b/ada_feeding/ada_feeding/trees/__init__.py index af3a5b27..6649ee13 100644 --- a/ada_feeding/ada_feeding/trees/__init__.py +++ b/ada_feeding/ada_feeding/trees/__init__.py @@ -20,5 +20,6 @@ from .move_to_pose_with_pose_path_constraints_tree import ( MoveToPoseWithPosePathConstraintsTree, ) +from .move_from_mouth_tree import MoveFromMouthTree from .move_to_mouth_tree import MoveToMouthTree from .move_to_dummy_tree import MoveToDummyTree diff --git a/ada_feeding/ada_feeding/trees/move_from_mouth_tree.py b/ada_feeding/ada_feeding/trees/move_from_mouth_tree.py new file mode 100644 index 00000000..fa508a80 --- /dev/null +++ b/ada_feeding/ada_feeding/trees/move_from_mouth_tree.py @@ -0,0 +1,331 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +This module defines the MoveFromMouthTree behaviour tree and provides functions to +wrap that behaviour tree in a ROS2 action server. +""" +# pylint: disable=duplicate-code +# MoveFromMouth and MoveToMouth are inverses of each other, so it makes sense +# that they have similar code. + +# Standard imports +from functools import partial +import logging +from typing import List, Tuple + +# Third-party imports +import py_trees +from py_trees.blackboard import Blackboard +from rclpy.node import Node + +# Local imports +from ada_feeding.idioms import pre_moveto_config +from ada_feeding.idioms.bite_transfer import ( + get_toggle_collision_object_behavior, +) +from ada_feeding.trees import ( + MoveToTree, + MoveToConfigurationWithPosePathConstraintsTree, + MoveToPoseWithPosePathConstraintsTree, +) + + +class MoveFromMouthTree(MoveToTree): + """ + A behaviour tree that moves the robot back to the staging location in a + cartesian motion, and then moves the robot to a specified end configuration. + """ + + # pylint: disable=too-many-instance-attributes, too-many-arguments, too-many-locals + # Bite transfer is a big part of the robot's behavior, so it makes + # sense that it has lots of attributes/arguments. + + def __init__( + self, + staging_configuration_position: Tuple[float, float, float], + staging_configuration_quat_xyzw: Tuple[float, float, float, float], + end_configuration: List[float], + staging_configuration_tolerance: float = 0.001, + end_configuration_tolerance: float = 0.001, + orientation_constraint_to_staging_configuration_quaternion: List[float] = None, + orientation_constraint_to_staging_configuration_tolerances: List[float] = None, + orientation_constraint_to_end_configuration_quaternion: List[float] = None, + orientation_constraint_to_end_configuration_tolerances: List[float] = None, + planner_id: str = "RRTstarkConfigDefault", + allowed_planning_time_to_staging_configuration: float = 0.5, + allowed_planning_time_to_end_configuration: float = 0.5, + max_velocity_scaling_factor_to_staging_configuration: float = 0.1, + max_velocity_scaling_factor_to_end_configuration: float = 0.1, + cartesian_jump_threshold_to_staging_configuration: float = 0.0, + cartesian_max_step_to_staging_configuration: float = 0.0025, + wheelchair_collision_object_id: str = "wheelchair_collision", + force_threshold: float = 4.0, + torque_threshold: float = 4.0, + ): + """ + Initializes tree-specific parameters. + + Parameters + ---------- + staging_configuration_position: The position of the staging + configuration. + staging_configuration_quat_xyzw: The quaternion of the staging + configuration. + end_configuration: The joint positions to move the robot arm to after + going to the staging configuration. + staging_configuration_tolerance: The tolerance for the joint positions. + end_configuration_tolerance: The tolerance for the joint positions. + orientation_constraint_to_staging_configuration_quaternion: The + quaternion for the orientation constraint to the staging + configuration. + orientation_constraint_to_staging_configuration_tolerances: The + tolerances for the orientation constraint to the staging + configuration. + orientation_constraint_to_end_configuration_quaternion: The + quaternion for the orientation constraint to the end configuration. + orientation_constraint_to_end_configuration_tolerances: The + tolerances for the orientation constraint to the end configuration. + planner_id: The planner ID to use for the MoveIt2 motion planning. + allowed_planning_time_to_staging_configuration: The allowed planning + time for the MoveIt2 motion planner to move to the staging config. + allowed_planning_time_to_end_configuration: The allowed planning + time for the MoveIt2 motion planner to move to the end config. + max_velocity_scaling_factor_to_staging_configuration: The maximum + velocity scaling factor for the MoveIt2 motion planner to move to + the staging config. + max_velocity_scaling_factor_to_end_configuration: The maximum + velocity scaling factor for the MoveIt2 motion planner to move to + the end config. + cartesian_jump_threshold_to_staging_configuration: The cartesian + jump threshold for the MoveIt2 motion planner to move to the + staging config. + cartesian_max_step_to_staging_configuration: The cartesian + max step for the MoveIt2 motion planner to move to the + staging config. + wheelchair_collision_object_id: The ID of the wheelchair collision object + in the MoveIt2 planning scene. + force_threshold: The force threshold (N) for the ForceGateController. + For now, the same threshold is used to move to the staging location + and to the mouth. + torque_threshold: The torque threshold (N*m) for the ForceGateController. + For now, the same threshold is used to move to the staging location + and to the mouth. + """ + # Initialize MoveToTree + super().__init__() + + # Store the parameters + self.staging_configuration_position = staging_configuration_position + self.staging_configuration_quat_xyzw = staging_configuration_quat_xyzw + self.end_configuration = end_configuration + assert len(self.end_configuration) == 6, "Must provide 6 joint positions" + self.staging_configuration_tolerance = staging_configuration_tolerance + self.end_configuration_tolerance = end_configuration_tolerance + self.orientation_constraint_to_staging_configuration_quaternion = ( + orientation_constraint_to_staging_configuration_quaternion + ) + self.orientation_constraint_to_staging_configuration_tolerances = ( + orientation_constraint_to_staging_configuration_tolerances + ) + self.orientation_constraint_to_end_configuration_quaternion = ( + orientation_constraint_to_end_configuration_quaternion + ) + self.orientation_constraint_to_end_configuration_tolerances = ( + orientation_constraint_to_end_configuration_tolerances + ) + self.planner_id = planner_id + self.allowed_planning_time_to_staging_configuration = ( + allowed_planning_time_to_staging_configuration + ) + self.allowed_planning_time_to_end_configuration = ( + allowed_planning_time_to_end_configuration + ) + self.max_velocity_scaling_factor_to_staging_configuration = ( + max_velocity_scaling_factor_to_staging_configuration + ) + self.max_velocity_scaling_factor_to_end_configuration = ( + max_velocity_scaling_factor_to_end_configuration + ) + self.cartesian_jump_threshold_to_staging_configuration = ( + cartesian_jump_threshold_to_staging_configuration + ) + self.cartesian_max_step_to_staging_configuration = ( + cartesian_max_step_to_staging_configuration + ) + self.wheelchair_collision_object_id = wheelchair_collision_object_id + self.force_threshold = force_threshold + self.torque_threshold = torque_threshold + + def create_move_to_tree( + self, + name: str, + tree_root_name: str, + logger: logging.Logger, + node: Node, + ) -> py_trees.trees.BehaviourTree: + """ + Creates the MoveToMouth behaviour tree. + + Parameters + ---------- + name: The name of the behaviour tree. + tree_root_name: The name of the tree. This is necessary because sometimes + trees create subtrees, but still need to track the top-level tree + name to read/write the correct blackboard variables. + logger: The logger to use for the behaviour tree. + node: The ROS2 node that this tree is associated with. Necessary for + behaviours within the tree connect to ROS topics/services/actions. + + Returns + ------- + tree: The behaviour tree that moves the robot above the plate. + """ + + # Separate the namespace of each sub-behavior + pre_moveto_config_prefix = "pre_moveto_config" + allow_wheelchair_collision_prefix = "allow_wheelchair_collision" + move_to_staging_configuration_prefix = "move_to_staging_configuration" + disallow_wheelchair_collision_prefix = "disallow_wheelchair_collision" + move_to_end_configuration_prefix = "move_to_end_configuration" + + # Configure the force-torque sensor and thresholds before moving + pre_moveto_config_name = Blackboard.separator.join( + [name, pre_moveto_config_prefix] + ) + pre_moveto_config_behavior = pre_moveto_config( + name=pre_moveto_config_name, + toggle_watchdog_listener=False, + f_mag=self.force_threshold, + t_mag=self.torque_threshold, + logger=logger, + ) + + # Create the behavior to allow collisions between the robot and the + # wheelchair collision object. The wheelchair collision object is + # intentionally expanded to nsure the robot gets nowhere close to the + # user during acquisition, but during transfer the robot must get close + # to the user so the wheelchair collision object must be allowed. + allow_wheelchair_collision = get_toggle_collision_object_behavior( + name, + allow_wheelchair_collision_prefix, + node, + self.wheelchair_collision_object_id, + True, + logger, + ) + + # Create the behaviour to move the robot to the staging configuration + move_to_staging_configuration_name = Blackboard.separator.join( + [name, move_to_staging_configuration_prefix] + ) + move_to_staging_configuration = ( + MoveToPoseWithPosePathConstraintsTree( + position_goal=self.staging_configuration_position, + quat_xyzw_goal=self.staging_configuration_quat_xyzw, + tolerance_position_goal=self.staging_configuration_tolerance, + tolerance_orientation_goal=(0.6, 0.5, 0.5), + parameterization_orientation_goal=1, # Rotation vector + cartesian=True, + cartesian_jump_threshold=self.cartesian_jump_threshold_to_staging_configuration, + cartesian_max_step=self.cartesian_max_step_to_staging_configuration, + cartesian_fraction_threshold=0.60, + planner_id=self.planner_id, + allowed_planning_time=self.allowed_planning_time_to_staging_configuration, + max_velocity_scaling_factor=( + self.max_velocity_scaling_factor_to_staging_configuration + ), + quat_xyzw_path=self.orientation_constraint_to_staging_configuration_quaternion, + tolerance_orientation_path=( + self.orientation_constraint_to_staging_configuration_tolerances + ), + parameterization_orientation_path=1, # Rotation vector + ) + .create_tree( + move_to_staging_configuration_name, + self.action_type, + tree_root_name, + logger, + node, + ) + .root + ) + + # Create the behavior to disallow collisions between the robot and the + # wheelchair collision object. + gen_disallow_wheelchair_collision = partial( + get_toggle_collision_object_behavior, + name, + disallow_wheelchair_collision_prefix, + node, + self.wheelchair_collision_object_id, + False, + logger, + ) + + # Create the behaviour to move the robot to the end configuration + move_to_end_configuration_name = Blackboard.separator.join( + [name, move_to_end_configuration_prefix] + ) + move_to_end_configuration = ( + MoveToConfigurationWithPosePathConstraintsTree( + joint_positions_goal=self.end_configuration, + tolerance_joint_goal=self.end_configuration_tolerance, + planner_id=self.planner_id, + allowed_planning_time=self.allowed_planning_time_to_end_configuration, + max_velocity_scaling_factor=self.max_velocity_scaling_factor_to_end_configuration, + quat_xyzw_path=self.orientation_constraint_to_end_configuration_quaternion, + tolerance_orientation_path=( + self.orientation_constraint_to_end_configuration_tolerances + ), + parameterization_orientation_path=1, # Rotation vector + ) + .create_tree( + move_to_end_configuration_name, + self.action_type, + tree_root_name, + logger, + node, + ) + .root + ) + + # Link all the behaviours together in a sequence with memory + move_from_mouth = py_trees.composites.Sequence( + name=name + " Main", + memory=True, + children=[ + # For now, we only re-tare the F/T sensor once, since no large forces + # are expected during transfer. + pre_moveto_config_behavior, + allow_wheelchair_collision, + move_to_staging_configuration, + gen_disallow_wheelchair_collision(), + move_to_end_configuration, + ], + ) + move_from_mouth.logger = logger + + # Create a cleanup branch for the behaviors that should get executed if + # the main tree has a failure + cleanup_tree = gen_disallow_wheelchair_collision() + + # If move_from_mouth fails, we still want to do some cleanup (e.g., turn + # face detection off). + root = py_trees.composites.Selector( + name=name, + memory=True, + children=[ + move_from_mouth, + # Even though we are cleaning up the tree, it should still + # pass the failure up. + py_trees.decorators.SuccessIsFailure( + name + " Cleanup Root", cleanup_tree + ), + ], + ) + root.logger = logger + + # raise Exception(py_trees.display.unicode_blackboard()) + + tree = py_trees.trees.BehaviourTree(root) + return tree diff --git a/ada_feeding/ada_feeding/trees/move_to_configuration_tree.py b/ada_feeding/ada_feeding/trees/move_to_configuration_tree.py index 26a54578..5eddc5c3 100644 --- a/ada_feeding/ada_feeding/trees/move_to_configuration_tree.py +++ b/ada_feeding/ada_feeding/trees/move_to_configuration_tree.py @@ -46,9 +46,11 @@ def __init__( joint_positions: List[float], tolerance: float = 0.001, weight: float = 1.0, + pipeline_id: str = "ompl", planner_id: str = "RRTstarkConfigDefault", allowed_planning_time: float = 0.5, max_velocity_scaling_factor: float = 0.1, + max_acceleration_scaling_factor: float = 0.1, keys_to_not_write_to_blackboard: Set[str] = set(), clear_constraints: bool = True, ): @@ -60,11 +62,14 @@ def __init__( joint_positions: The joint positions to move the robot arm to. tolerance: The tolerance for the joint positions. weight: The weight for the joint goal constraint. + pipeline_id: The pipeline ID to use for the MoveIt2 motion planner. planner_id: The planner ID to use for the MoveIt2 motion planning. allowed_planning_time: The allowed planning time for the MoveIt2 motion planner. max_velocity_scaling_factor: The maximum velocity scaling factor for the MoveIt2 motion planner. + max_acceleration_scaling_factor: The maximum acceleration scaling factor + for the MoveIt2 motion planner. keys_to_not_write_to_blackboard: A set of keys that should not be written Note that the keys need to be exact e.g., "move_to.cartesian," "position_goal_constraint.tolerance," "orientation_goal_constraint.tolerance," @@ -81,9 +86,11 @@ def __init__( assert len(self.joint_positions) == 6, "Must provide 6 joint positions" self.tolerance = tolerance self.weight = weight + self.pipeline_id = pipeline_id self.planner_id = planner_id self.allowed_planning_time = allowed_planning_time self.max_velocity_scaling_factor = max_velocity_scaling_factor + self.max_acceleration_scaling_factor = max_acceleration_scaling_factor self.keys_to_not_write_to_blackboard = keys_to_not_write_to_blackboard self.clear_constraints = clear_constraints @@ -140,6 +147,12 @@ def create_move_to_tree( ) # Inputs for MoveTo + pipeline_id_key = Blackboard.separator.join( + [move_to_namespace_prefix, "pipeline_id"] + ) + self.blackboard.register_key( + key=pipeline_id_key, access=py_trees.common.Access.WRITE + ) planner_id_key = Blackboard.separator.join( [move_to_namespace_prefix, "planner_id"] ) @@ -158,6 +171,13 @@ def create_move_to_tree( self.blackboard.register_key( key=max_velocity_scaling_factor_key, access=py_trees.common.Access.WRITE ) + max_acceleration_scaling_factor_key = Blackboard.separator.join( + [move_to_namespace_prefix, "max_acceleration_scaling_factor"] + ) + self.blackboard.register_key( + key=max_acceleration_scaling_factor_key, + access=py_trees.common.Access.WRITE, + ) # Write the inputs to MoveToConfiguration to blackboard set_to_blackboard( @@ -178,6 +198,12 @@ def create_move_to_tree( self.weight, self.keys_to_not_write_to_blackboard, ) + set_to_blackboard( + self.blackboard, + pipeline_id_key, + self.pipeline_id, + self.keys_to_not_write_to_blackboard, + ) set_to_blackboard( self.blackboard, planner_id_key, @@ -196,6 +222,12 @@ def create_move_to_tree( self.max_velocity_scaling_factor, self.keys_to_not_write_to_blackboard, ) + set_to_blackboard( + self.blackboard, + max_acceleration_scaling_factor_key, + self.max_acceleration_scaling_factor, + self.keys_to_not_write_to_blackboard, + ) # Create the MoveTo behavior move_to_name = Blackboard.separator.join([name, move_to_namespace_prefix]) 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 48efd2f1..85c45b40 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 @@ -15,6 +15,7 @@ # Local imports from ada_feeding.idioms import pre_moveto_config +from ada_feeding.idioms.bite_transfer import get_toggle_watchdog_listener_behavior from ada_feeding.trees import MoveToTree, MoveToConfigurationTree @@ -36,9 +37,11 @@ def __init__( # Optional parameters for moving to a configuration tolerance_joint: float = 0.001, weight_joint: float = 1.0, + pipeline_id: str = "ompl", planner_id: str = "RRTstarkConfigDefault", allowed_planning_time: float = 0.5, max_velocity_scaling_factor: float = 0.1, + max_acceleration_scaling_factor: float = 0.1, # Optional parameters for the FT thresholds re_tare: bool = True, toggle_watchdog_listener: bool = True, @@ -61,11 +64,14 @@ def __init__( joint_positions: The joint positions for the goal constraint. tolerance_joint: The tolerance for the joint goal constraint. weight_joint: The weight for the joint goal constraint. + pipeline_id: The pipeline ID to use for MoveIt2 motion planning. planner_id: The planner ID to use for MoveIt2 motion planning. allowed_planning_time: The allowed planning time for the MoveIt2 motion planner. max_velocity_scaling_factor: The maximum velocity scaling factor for the MoveIt2 motion planner. + max_acceleration_scaling_factor: The maximum acceleration scaling factor for the + MoveIt2 motion planner. re_tare: Whether to re-tare the force-torque sensor. toggle_watchdog_listener: Whether to toggle the watchdog listener on and off. In practice, if the watchdog listener is on, you should toggle it. @@ -97,9 +103,11 @@ def __init__( assert len(self.joint_positions) == 6, "Must provide 6 joint positions" self.tolerance_joint = tolerance_joint self.weight_joint = weight_joint + self.pipeline_id = pipeline_id self.planner_id = planner_id self.allowed_planning_time = allowed_planning_time self.max_velocity_scaling_factor = max_velocity_scaling_factor + self.max_acceleration_scaling_factor = max_acceleration_scaling_factor # Store the parameters for the FT threshold self.re_tare = re_tare @@ -137,6 +145,8 @@ def create_move_to_tree( ------- tree: The behavior tree that moves the robot above the plate. """ + turn_watchdog_listener_on_prefix = "turn_watchdog_listener_on" + # First, create the MoveToConfiguration behavior tree, in the same # namespace as this tree move_to_configuration_root = ( @@ -144,9 +154,11 @@ def create_move_to_tree( joint_positions=self.joint_positions, tolerance=self.tolerance_joint, weight=self.weight_joint, + pipeline_id=self.pipeline_id, planner_id=self.planner_id, allowed_planning_time=self.allowed_planning_time, max_velocity_scaling_factor=self.max_velocity_scaling_factor, + max_acceleration_scaling_factor=self.max_acceleration_scaling_factor, keys_to_not_write_to_blackboard=self.keys_to_not_write_to_blackboard, clear_constraints=self.clear_constraints, ) @@ -171,11 +183,45 @@ def create_move_to_tree( ) # Combine them in a sequence with memory - root = py_trees.composites.Sequence( + main_tree = py_trees.composites.Sequence( name=name, memory=True, children=[pre_moveto_behavior, move_to_configuration_root], ) + main_tree.logger = logger + + if self.toggle_watchdog_listener: + # If there was a failure in the main tree, we want to ensure to turn + # the watchdog listener back on + # pylint: disable=duplicate-code + # This is similar to any other tree that needs to cleanup pre_moveto_config + turn_watchdog_listener_on = get_toggle_watchdog_listener_behavior( + name, + turn_watchdog_listener_on_prefix, + True, + logger, + ) + + # Create a cleanup branch for the behaviors that should get executed if + # the main tree has a failure + cleanup_tree = turn_watchdog_listener_on + + # If main_tree fails, we still want to do some cleanup. + root = py_trees.composites.Selector( + name=name, + memory=True, + children=[ + main_tree, + # Even though we are cleaning up the tree, it should still + # pass the failure up. + py_trees.decorators.SuccessIsFailure( + name + " Cleanup Root", cleanup_tree + ), + ], + ) + root.logger = logger + else: + root = main_tree tree = py_trees.trees.BehaviourTree(root) return tree diff --git a/ada_feeding/ada_feeding/trees/move_to_configuration_with_pose_path_constraints_tree.py b/ada_feeding/ada_feeding/trees/move_to_configuration_with_pose_path_constraints_tree.py index cb442512..c7913fd6 100644 --- a/ada_feeding/ada_feeding/trees/move_to_configuration_with_pose_path_constraints_tree.py +++ b/ada_feeding/ada_feeding/trees/move_to_configuration_with_pose_path_constraints_tree.py @@ -41,9 +41,11 @@ def __init__( # Optional parameters for moving to a configuration tolerance_joint_goal: float = 0.001, weight_joint_goal: float = 1.0, + pipeline_id: str = "ompl", planner_id: str = "RRTstarkConfigDefault", allowed_planning_time: float = 0.5, max_velocity_scaling_factor: float = 0.1, + max_acceleration_scaling_factor: float = 0.1, # Optional parameters for the pose path constraint position_path: Tuple[float, float, float] = None, quat_xyzw_path: Tuple[float, float, float, float] = None, @@ -65,11 +67,14 @@ def __init__( joint_positions_goal: The joint positions for the goal constraint. tolerance_joint_goal: The tolerance for the joint goal constraint. weight_joint_goal: The weight for the joint goal constraint. + pipeline_id: The pipeline ID to use for MoveIt2 motion planning. planner_id: The planner ID to use for MoveIt2 motion planning. allowed_planning_time: The allowed planning time for the MoveIt2 motion planner. max_velocity_scaling_factor: The maximum velocity scaling factor for the MoveIt2 motion planner. + max_acceleration_scaling_factor: The maximum acceleration scaling factor + for the MoveIt2 motion planner. position_path: the position for the path constraint. quat_xyzw_path: the orientation for the path constraint. frame_id: the frame id of the target pose, for the pose path constraint. @@ -101,9 +106,11 @@ def __init__( assert len(self.joint_positions_goal) == 6, "Must provide 6 joint positions" self.tolerance_joint_goal = tolerance_joint_goal self.weight_joint_goal = weight_joint_goal + self.pipeline_id = pipeline_id self.planner_id = planner_id self.allowed_planning_time = allowed_planning_time self.max_velocity_scaling_factor = max_velocity_scaling_factor + self.max_acceleration_scaling_factor = max_acceleration_scaling_factor # Store the parameters for the pose path constraint self.position_path = position_path @@ -150,9 +157,11 @@ def create_move_to_tree( joint_positions=self.joint_positions_goal, tolerance=self.tolerance_joint_goal, weight=self.weight_joint_goal, + pipeline_id=self.pipeline_id, planner_id=self.planner_id, allowed_planning_time=self.allowed_planning_time, max_velocity_scaling_factor=self.max_velocity_scaling_factor, + max_acceleration_scaling_factor=self.max_acceleration_scaling_factor, keys_to_not_write_to_blackboard=self.keys_to_not_write_to_blackboard, clear_constraints=False, ) 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 bf1c9231..74995d0c 100644 --- a/ada_feeding/ada_feeding/trees/move_to_mouth_tree.py +++ b/ada_feeding/ada_feeding/trees/move_to_mouth_tree.py @@ -4,30 +4,35 @@ This module defines the MoveToMouthTree behaviour tree and provides functions to wrap that behaviour tree in a ROS2 action server. """ +# pylint: disable=duplicate-code +# MoveFromMouth and MoveToMouth are inverses of each other, so it makes sense +# that they have similar code. # Standard imports +from functools import partial import logging -import operator -from typing import List +from typing import List, Tuple # Third-party imports import py_trees from py_trees.blackboard import Blackboard import py_trees_ros from rclpy.node import Node -from std_srvs.srv import SetBool # Local imports from ada_feeding_msgs.msg import FaceDetection from ada_feeding.behaviors import ( ComputeMoveToMouthPosition, MoveCollisionObject, - ToggleCollisionObject, ) from ada_feeding.helpers import ( POSITION_GOAL_CONSTRAINT_NAMESPACE_PREFIX, ) -from ada_feeding.idioms import pre_moveto_config, retry_call_ros_service +from ada_feeding.idioms import pre_moveto_config +from ada_feeding.idioms.bite_transfer import ( + get_toggle_collision_object_behavior, + get_toggle_face_detection_behavior, +) from ada_feeding.trees import ( MoveToTree, MoveToConfigurationWithPosePathConstraintsTree, @@ -37,7 +42,10 @@ class MoveToMouthTree(MoveToTree): """ - A behaviour tree that moves the robot to a specified configuration. + A behaviour tree that toggles face detection on, moves the robot to the + staging configuration, detects a face, checks whether it is within a + distance threshold from the camera, and does a cartesian motion to the + face. """ # pylint: disable=too-many-instance-attributes, too-many-arguments @@ -56,11 +64,13 @@ def __init__( allowed_planning_time_to_mouth: float = 0.5, max_velocity_scaling_factor_to_staging_configuration: float = 0.1, max_velocity_scaling_factor_to_mouth: float = 0.1, + cartesian_jump_threshold_to_mouth: float = 0.0, + cartesian_max_step_to_mouth: float = 0.0025, head_object_id: str = "head", wheelchair_collision_object_id: str = "wheelchair_collision", force_threshold: float = 4.0, torque_threshold: float = 4.0, - allowed_face_distance: float = 1.5, + allowed_face_distance: Tuple[float, float] = (0.4, 1.5), ): """ Initializes tree-specific parameters. @@ -86,6 +96,11 @@ def __init__( the staging config. max_velocity_scaling_factor_to_mouth: The maximum velocity scaling factor for the MoveIt2 motion planner to move to the user's mouth. + cartesian_jump_threshold_to_mouth: The maximum allowed jump in the + cartesian space for the MoveIt2 motion planner to move to the user's + mouth. + cartesian_max_step_to_mouth: The maximum allowed step in the cartesian + space for the MoveIt2 motion planner to move to the user's mouth. head_object_id: The ID of the head collision object in the MoveIt2 planning scene. wheelchair_collision_object_id: The ID of the wheelchair collision object @@ -96,7 +111,7 @@ def __init__( torque_threshold: The torque threshold (N*m) for the ForceGateController. For now, the same threshold is used to move to the staging location and to the mouth. - allowed_face_distance: The maximum distance (m) between a face and the + allowed_face_distance: The min and max distance (m) between a face and the **camera's optical frame** for the robot to move towards the face. """ @@ -122,6 +137,8 @@ def __init__( max_velocity_scaling_factor_to_staging_configuration ) self.max_velocity_scaling_factor_to_mouth = max_velocity_scaling_factor_to_mouth + self.cartesian_jump_threshold_to_mouth = cartesian_jump_threshold_to_mouth + self.cartesian_max_step_to_mouth = cartesian_max_step_to_mouth self.head_object_id = head_object_id self.wheelchair_collision_object_id = wheelchair_collision_object_id self.force_threshold = force_threshold @@ -154,7 +171,11 @@ def check_face_msg(self, msg: FaceDetection, _: FaceDetection) -> bool: + msg.detected_mouth_center.point.y**2.0 + msg.detected_mouth_center.point.z**2.0 ) ** 0.5 - if distance <= self.allowed_face_distance: + if ( + self.allowed_face_distance[0] + <= distance + <= self.allowed_face_distance[1] + ): return True return False @@ -194,31 +215,18 @@ def create_move_to_tree( get_face_prefix = "get_face" check_face_prefix = "check_face" compute_target_position_prefix = "compute_target_position" + move_head_prefix = "move_head" + allow_wheelchair_collision_prefix = "allow_wheelchair_collision" move_to_target_pose_prefix = "move_to_target_pose" + disallow_wheelchair_collision_prefix = "disallow_wheelchair_collision" turn_face_detection_off_prefix = "turn_face_detection_off" # Create the behaviour to turn face detection on - turn_face_detection_on_name = Blackboard.separator.join( - [name, turn_face_detection_on_prefix] - ) - turn_face_detection_on_key_response = Blackboard.separator.join( - [turn_face_detection_on_name, "response"] - ) - turn_face_detection_on = retry_call_ros_service( - name=turn_face_detection_on_name, - service_type=SetBool, - service_name="~/toggle_face_detection", - key_request=None, - request=SetBool.Request(data=True), - key_response=turn_face_detection_on_key_response, - response_checks=[ - py_trees.common.ComparisonExpression( - variable=turn_face_detection_on_key_response + ".success", - value=True, - operator=operator.eq, - ) - ], - logger=logger, + turn_face_detection_on = get_toggle_face_detection_behavior( + name, + turn_face_detection_on_prefix, + True, + logger, ) # Configure the force-torque sensor and thresholds before moving @@ -327,7 +335,6 @@ def create_move_to_tree( # Create the behavior to move the head in the collision scene to the mouth # position. For now, assume the head is always perpendicular to the back # of the wheelchair. - move_head_prefix = "move_head" move_head = MoveCollisionObject( name=Blackboard.separator.join([name, move_head_prefix]), node=node, @@ -360,14 +367,14 @@ def create_move_to_tree( # intentionally expanded to nsure the robot gets nowhere close to the # user during acquisition, but during transfer the robot must get close # to the user so the wheelchair collision object must be allowed. - allow_wheelchair_collision_prefix = "allow_wheelchair_collision" - allow_wheelchair_collision = ToggleCollisionObject( - name=Blackboard.separator.join([name, allow_wheelchair_collision_prefix]), - node=node, - collision_object_id=self.wheelchair_collision_object_id, - allow=True, + allow_wheelchair_collision = get_toggle_collision_object_behavior( + name, + allow_wheelchair_collision_prefix, + node, + self.wheelchair_collision_object_id, + True, + logger, ) - allow_wheelchair_collision.logger = logger # Create the behaviour to move the robot to the target pose # We want to add a position goal, but it should come from the @@ -387,7 +394,10 @@ def create_move_to_tree( tolerance_position_goal=self.mouth_pose_tolerance, tolerance_orientation_goal=(0.6, 0.5, 0.5), parameterization_orientation_goal=1, # Rotation vector - cartesian=False, + cartesian=True, + cartesian_jump_threshold=self.cartesian_jump_threshold_to_mouth, + cartesian_max_step=self.cartesian_max_step_to_mouth, + cartesian_fraction_threshold=0.60, planner_id=self.planner_id, allowed_planning_time=self.allowed_planning_time_to_mouth, max_velocity_scaling_factor=self.max_velocity_scaling_factor_to_mouth, @@ -406,49 +416,25 @@ def create_move_to_tree( .root ) - # # NOTE: The below is commented out for the time being, because if - # # we disallow collisions between the robot and the wheelchair - # # collision object, then the robot will not be able to do any motions - # # after this (since it is in collision with the wheelchair collision - # # object at the end of transfer). - # # - # # The right solution is to create a separate `MoveAwayFromMouth` tree - # # that moves the robot back to the staging location, and then disallows - # # this collision before moving back to the staging location. - - # # Create the behavior to disallow collisions between the robot and the - # # wheelchair collision object. - # disallow_wheelchair_collision_prefix = "disallow_wheelchair_collision" - # disallow_wheelchair_collision = ToggleCollisionObject( - # name=Blackboard.separator.join([name, disallow_wheelchair_collision_prefix]), - # node=node, - # collision_object_id=self.wheelchair_collision_object_id, - # allow=False, - # ) - # disallow_wheelchair_collision.logger = logger + # Create the behavior to disallow collisions between the robot and the + # wheelchair collision object. + gen_disallow_wheelchair_collision = partial( + get_toggle_collision_object_behavior, + name, + disallow_wheelchair_collision_prefix, + node, + self.wheelchair_collision_object_id, + False, + logger, + ) # Create the behaviour to turn face detection off - turn_face_detection_off_name = Blackboard.separator.join( - [name, turn_face_detection_off_prefix] - ) - turn_face_detection_off_key_response = Blackboard.separator.join( - [turn_face_detection_off_name, "response"] - ) - turn_face_detection_off = retry_call_ros_service( - name=turn_face_detection_off_name, - service_type=SetBool, - service_name="~/toggle_face_detection", - key_request=None, - request=SetBool.Request(data=False), - key_response=turn_face_detection_off_key_response, - response_checks=[ - py_trees.common.ComparisonExpression( - variable=turn_face_detection_off_key_response + ".success", - value=True, - operator=operator.eq, - ) - ], - logger=logger, + gen_turn_face_detection_off = partial( + get_toggle_face_detection_behavior, + name, + turn_face_detection_off_prefix, + False, + logger, ) # Link all the behaviours together in a sequence with memory @@ -466,12 +452,23 @@ def create_move_to_tree( move_head, allow_wheelchair_collision, move_to_target_pose, - # disallow_wheelchair_collision, - turn_face_detection_off, + gen_disallow_wheelchair_collision(), + gen_turn_face_detection_off(), ], ) move_to_mouth.logger = logger + # Create a cleanup branch for the behaviors that should get executed if + # the main tree has a failure + cleanup_tree = py_trees.composites.Sequence( + name=name + " Cleanup", + memory=True, + children=[ + gen_disallow_wheelchair_collision(), + gen_turn_face_detection_off(), + ], + ) + # If move_to_mouth fails, we still want to do some cleanup (e.g., turn # face detection off). root = py_trees.composites.Selector( @@ -482,7 +479,7 @@ def create_move_to_tree( # Even though we are cleaning up the tree, it should still # pass the failure up. py_trees.decorators.SuccessIsFailure( - name + " Cleanup", turn_face_detection_off + name + " Cleanup Root", cleanup_tree ), ], ) 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 58b0a01f..79661217 100644 --- a/ada_feeding/ada_feeding/trees/move_to_pose_tree.py +++ b/ada_feeding/ada_feeding/trees/move_to_pose_tree.py @@ -58,9 +58,14 @@ def __init__( weight_position: float = 1.0, weight_orientation: float = 1.0, cartesian: bool = False, + cartesian_jump_threshold: float = 0.0, + cartesian_max_step: float = 0.0025, + cartesian_fraction_threshold: float = 0.0, + pipeline_id: str = "ompl", planner_id: str = "RRTstarkConfigDefault", allowed_planning_time: float = 0.5, max_velocity_scaling_factor: float = 0.1, + max_acceleration_scaling_factor: float = 0.1, keys_to_not_write_to_blackboard: Set[str] = set(), clear_constraints: bool = True, ): @@ -81,10 +86,17 @@ def __init__( weight_position: the weight for the end effector position. weight_orientation: the weight for the end effector orientation. cartesian: whether to use cartesian path planning. + cartesian_jump_threshold: the jump threshold for cartesian path planning. + cartesian_max_step: the maximum step for cartesian path planning. + cartesian_fraction_threshold: Reject cartesian plans that don't reach + at least this fraction of the path to the goal. + pipeline_id: the pipeline to use for path planning. planner_id: the planner to use for path planning. allowed_planning_time: the allowed planning time for path planning. max_velocity_scaling_factor: the maximum velocity scaling factor for path planning. + max_acceleration_scaling_factor: the maximum acceleration scaling factor for + path planning. keys_to_not_write_to_blackboard: the keys to not write to the blackboard. Note that the keys need to be exact e.g., "move_to.cartesian," "position_goal_constraint.tolerance," "orientation_goal_constraint.tolerance," @@ -107,9 +119,14 @@ def __init__( self.weight_position = weight_position self.weight_orientation = weight_orientation self.cartesian = cartesian + self.cartesian_jump_threshold = cartesian_jump_threshold + self.cartesian_max_step = cartesian_max_step + self.cartesian_fraction_threshold = cartesian_fraction_threshold + self.pipeline_id = pipeline_id self.planner_id = planner_id self.allowed_planning_time = allowed_planning_time self.max_velocity_scaling_factor = max_velocity_scaling_factor + self.max_acceleration_scaling_factor = max_acceleration_scaling_factor self.keys_to_not_write_to_blackboard = keys_to_not_write_to_blackboard self.clear_constraints = clear_constraints @@ -232,6 +249,31 @@ def create_move_to_tree( self.blackboard.register_key( key=cartesian_key, access=py_trees.common.Access.WRITE ) + cartesian_jump_threshold_key = Blackboard.separator.join( + [move_to_namespace_prefix, "cartesian_jump_threshold"] + ) + self.blackboard.register_key( + key=cartesian_jump_threshold_key, access=py_trees.common.Access.WRITE + ) + cartesian_max_step_key = Blackboard.separator.join( + [move_to_namespace_prefix, "cartesian_max_step"] + ) + self.blackboard.register_key( + key=cartesian_max_step_key, access=py_trees.common.Access.WRITE + ) + cartesian_fraction_threshold_key = Blackboard.separator.join( + [move_to_namespace_prefix, "cartesian_fraction_threshold"] + ) + self.blackboard.register_key( + key=cartesian_fraction_threshold_key, + access=py_trees.common.Access.WRITE, + ) + pipeline_id_key = Blackboard.separator.join( + [move_to_namespace_prefix, "pipeline_id"] + ) + self.blackboard.register_key( + key=pipeline_id_key, access=py_trees.common.Access.WRITE + ) planner_id_key = Blackboard.separator.join( [move_to_namespace_prefix, "planner_id"] ) @@ -250,6 +292,13 @@ def create_move_to_tree( self.blackboard.register_key( key=max_velocity_scaling_factor_key, access=py_trees.common.Access.WRITE ) + max_acceleration_scaling_factor_key = Blackboard.separator.join( + [move_to_namespace_prefix, "max_acceleration_scaling_factor"] + ) + self.blackboard.register_key( + key=max_acceleration_scaling_factor_key, + access=py_trees.common.Access.WRITE, + ) # Write the inputs to MoveToPose to blackboard if self.position is not None: @@ -326,6 +375,30 @@ def create_move_to_tree( self.cartesian, self.keys_to_not_write_to_blackboard, ) + set_to_blackboard( + self.blackboard, + cartesian_jump_threshold_key, + self.cartesian_jump_threshold, + self.keys_to_not_write_to_blackboard, + ) + set_to_blackboard( + self.blackboard, + cartesian_max_step_key, + self.cartesian_max_step, + self.keys_to_not_write_to_blackboard, + ) + set_to_blackboard( + self.blackboard, + cartesian_fraction_threshold_key, + self.cartesian_fraction_threshold, + self.keys_to_not_write_to_blackboard, + ) + set_to_blackboard( + self.blackboard, + pipeline_id_key, + self.pipeline_id, + self.keys_to_not_write_to_blackboard, + ) set_to_blackboard( self.blackboard, planner_id_key, @@ -344,6 +417,12 @@ def create_move_to_tree( self.max_velocity_scaling_factor, self.keys_to_not_write_to_blackboard, ) + set_to_blackboard( + self.blackboard, + max_acceleration_scaling_factor_key, + self.max_acceleration_scaling_factor, + self.keys_to_not_write_to_blackboard, + ) # Create the MoveTo behavior move_to_name = Blackboard.separator.join([name, move_to_namespace_prefix]) diff --git a/ada_feeding/ada_feeding/trees/move_to_pose_with_pose_path_constraints_tree.py b/ada_feeding/ada_feeding/trees/move_to_pose_with_pose_path_constraints_tree.py index a01d3ad0..6b35e165 100644 --- a/ada_feeding/ada_feeding/trees/move_to_pose_with_pose_path_constraints_tree.py +++ b/ada_feeding/ada_feeding/trees/move_to_pose_with_pose_path_constraints_tree.py @@ -48,9 +48,14 @@ def __init__( weight_position_goal: float = 1.0, weight_orientation_goal: float = 1.0, cartesian: bool = False, + cartesian_jump_threshold: float = 0.0, + cartesian_max_step: float = 0.0025, + cartesian_fraction_threshold: float = 0.0, + pipeline_id: str = "ompl", planner_id: str = "RRTstarkConfigDefault", allowed_planning_time: float = 0.5, max_velocity_scaling_factor: float = 0.1, + max_acceleration_scaling_factor: float = 0.1, # Optional parameters for the pose path constraint position_path: Tuple[float, float, float] = None, quat_xyzw_path: Tuple[float, float, float, float] = None, @@ -81,11 +86,18 @@ def __init__( weight_position_goal: the weight for the position goal. weight_orientation_goal: the weight for the orientation goal. cartesian: whether to use cartesian path planning. + cartesian_jump_threshold: the jump threshold for cartesian path planning. + cartesian_max_step: the maximum step for cartesian path planning. + cartesian_fraction_threshold: if a cartesian plan does not reach at least + this fraction of the way to the goal, the plan is rejected. + pipeline_id: the pipeline ID to use for MoveIt2 motion planning. planner_id: the planner ID to use for MoveIt2 motion planning. allowed_planning_time: the allowed planning time for the MoveIt2 motion planner. max_velocity_scaling_factor: the maximum velocity scaling factor for MoveIt2 motion planning. + max_acceleration_scaling_factor: the maximum acceleration scaling factor + for MoveIt2 motion planning. position_path: the target position relative to frame_id for path constraints. quat_xyzw_path: the target orientation relative to frame_id for path constraints. frame_id_path: the frame id of the target pose for path constraints. If None, @@ -120,9 +132,14 @@ def __init__( self.weight_position_goal = weight_position_goal self.weight_orientation_goal = weight_orientation_goal self.cartesian = cartesian + self.cartesian_jump_threshold = cartesian_jump_threshold + self.cartesian_max_step = cartesian_max_step + self.cartesian_fraction_threshold = cartesian_fraction_threshold + self.pipeline_id = pipeline_id self.planner_id = planner_id self.allowed_planning_time = allowed_planning_time self.max_velocity_scaling_factor = max_velocity_scaling_factor + self.max_acceleration_scaling_factor = max_acceleration_scaling_factor # Store the parameters for the pose path constraint self.position_path = position_path @@ -176,9 +193,14 @@ def create_move_to_tree( weight_position=self.weight_position_goal, weight_orientation=self.weight_orientation_goal, cartesian=self.cartesian, + cartesian_jump_threshold=self.cartesian_jump_threshold, + cartesian_max_step=self.cartesian_max_step, + cartesian_fraction_threshold=self.cartesian_fraction_threshold, + pipeline_id=self.pipeline_id, planner_id=self.planner_id, allowed_planning_time=self.allowed_planning_time, max_velocity_scaling_factor=self.max_velocity_scaling_factor, + max_acceleration_scaling_factor=self.max_acceleration_scaling_factor, keys_to_not_write_to_blackboard=self.keys_to_not_write_to_blackboard, clear_constraints=False, ) diff --git a/ada_feeding/config/ada_feeding_action_servers.yaml b/ada_feeding/config/ada_feeding_action_servers.yaml index d9ea7d12..349ebee3 100644 --- a/ada_feeding/config/ada_feeding_action_servers.yaml +++ b/ada_feeding/config/ada_feeding_action_servers.yaml @@ -2,7 +2,7 @@ ada_feeding_action_servers: ros__parameters: # If we haven't received a message from the watchdog in this time, stop all robot motions - watchdog_timeout_sec: 0.1 # sec + watchdog_timeout_sec: 0.5 # sec # 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. @@ -10,6 +10,8 @@ ada_feeding_action_servers: - MoveAbovePlate - AcquireFood - MoveToMouth + - MoveFromMouthToAbovePlate + - MoveFromMouthToRestingPosition - MoveToRestingPosition - MoveToStowLocation - TestMoveToPose @@ -72,9 +74,6 @@ ada_feeding_action_servers: tick_rate: 10 # Hz, optional, default: 30 # Parameters for the MoveToMouth action - # TODO: This is a dummy MoveToMouth implementation used to test MoveToPose. - # It will be replaced with the actual MoveToMouth action once that is - # implemented. MoveToMouth: # required action_type: ada_feeding_msgs.action.MoveTo # required tree_class: ada_feeding.trees.MoveToMouthTree # required @@ -85,14 +84,31 @@ ada_feeding_action_servers: - allowed_planning_time_to_staging_configuration - max_velocity_scaling_factor_to_staging_configuration - max_velocity_scaling_factor_to_mouth + - cartesian_jump_threshold_to_mouth + - cartesian_max_step_to_mouth tree_kwargs: # optional staging_configuration: - - 2.74709 # j2n6s200_joint_1 - - 2.01400 # j2n6s200_joint_2 - - 1.85257 # j2n6s200_joint_3 - - -2.83523 # j2n6s200_joint_4 - - -1.61925 # j2n6s200_joint_5 - - -2.67325 # j2n6s200_joint_6 + # # Side-staging configuration + # - 2.74709 # j2n6s200_joint_1 + # - 2.01400 # j2n6s200_joint_2 + # - 1.85257 # j2n6s200_joint_3 + # - -2.83523 # j2n6s200_joint_4 + # - -1.61925 # j2n6s200_joint_5 + # - -2.67325 # j2n6s200_joint_6 + # # Front-staging configuration -- Taller + # - -2.30252 + # - 4.23221 + # - 3.84109 + # - -4.65546 + # - 3.94225 + # - 4.26543 + # Front-staging configuration -- Shorter + - -2.32526 + - 4.456298 + - 4.16769 + - 1.53262 + - -2.18359 + - -2.19525 orientation_constraint_quaternion: # perpendicular to the base link - 0.707168 # x - 0.0 # y @@ -102,9 +118,120 @@ ada_feeding_action_servers: - 0.6 # x, rad - 3.14159 # y, rad - 0.5 # z, rad - allowed_planning_time_to_staging_configuration: 1.0 # secs + allowed_planning_time_to_staging_configuration: 2.5 # secs max_velocity_scaling_factor_to_staging_configuration: 0.65 # optional in (0.0, 1.0], default: 0.1 max_velocity_scaling_factor_to_mouth: 0.4 # optional in (0.0, 1.0], default: 0.1 + cartesian_jump_threshold_to_mouth: 2.0 # If a jump for a joint across consecutive IK solutions is more than this factor greater than the average, it is rejected + cartesian_max_step_to_mouth: 0.01 # m + tick_rate: 10 # Hz, optional, default: 30 + + # Parameters for the MoveFromMouthToAbovePlate action. + # No need for orientation path constraints when moving above the plate + # because presumably there is no food on the fork. + MoveFromMouthToAbovePlate: # required + action_type: ada_feeding_msgs.action.MoveTo # required + tree_class: ada_feeding.trees.MoveFromMouthTree # required + tree_kws: # optional + - staging_configuration_position + - staging_configuration_quat_xyzw + - end_configuration + - orientation_constraint_to_staging_configuration_quaternion + - orientation_constraint_to_staging_configuration_tolerances + - max_velocity_scaling_factor_to_staging_configuration + - max_velocity_scaling_factor_to_end_configuration + - cartesian_jump_threshold_to_staging_configuration + - cartesian_max_step_to_staging_configuration + tree_kwargs: # optional + staging_configuration_position: + - 0.28323 # x + - 0.07289 # y + - 0.69509 # z + staging_configuration_quat_xyzw: + - 0.00472 # x + - 0.69757 # y + - 0.71645 # z + - -0.00811 # w + end_configuration: + - -2.11666 # j2n6s200_joint_1 + - 3.34967 # j2n6s200_joint_2 + - 2.04129 # j2n6s200_joint_3 + - -2.30031 # j2n6s200_joint_4 + - -2.34026 # j2n6s200_joint_5 + - 2.95450 # j2n6s200_joint_6 + orientation_constraint_to_staging_configuration_quaternion: # perpendicular to the base link + - 0.707168 # x + - 0.0 # y + - 0.0 # z + - 0.707168 # w + orientation_constraint_to_staging_configuration_tolerances: # Note that tolerances are w.r.t. the axes in quat_xyzw_path + - 0.6 # x, rad + - 3.14159 # y, rad + - 0.5 # z, rad + max_velocity_scaling_factor_to_staging_configuration: 0.4 # optional in (0.0, 1.0], default: 0.1 + max_velocity_scaling_factor_to_end_configuration: 0.65 # optional in (0.0, 1.0], default: 0.1 + cartesian_jump_threshold_to_staging_configuration: 2.0 # If a jump for a joint across consecutive IK solutions is more than this factor greater than the average, it is rejected + cartesian_max_step_to_staging_configuration: 0.01 # m + tick_rate: 10 # Hz, optional, default: 30 + + # Parameters for the MoveFromMouthToRestingPosition action. + # Need orientation path constraints to end config because + # presumably there is still food on the fork. + MoveFromMouthToRestingPosition: # required + action_type: ada_feeding_msgs.action.MoveTo # required + tree_class: ada_feeding.trees.MoveFromMouthTree # required + tree_kws: # optional + - staging_configuration_position + - staging_configuration_quat_xyzw + - end_configuration + - orientation_constraint_to_staging_configuration_quaternion + - orientation_constraint_to_staging_configuration_tolerances + - orientation_constraint_to_end_configuration_quaternion + - orientation_constraint_to_end_configuration_tolerances + - allowed_planning_time_to_end_configuration + - max_velocity_scaling_factor_to_staging_configuration + - max_velocity_scaling_factor_to_end_configuration + - cartesian_jump_threshold_to_staging_configuration + - cartesian_max_step_to_staging_configuration + tree_kwargs: # optional + staging_configuration_position: + - 0.28323 # x + - 0.07289 # y + - 0.69509 # z + staging_configuration_quat_xyzw: + - 0.00472 # x + - 0.69757 # y + - 0.71645 # z + - -0.00811 # w + end_configuration: + - -1.94672 # j2n6s200_joint_1 + - 2.51268 # j2n6s200_joint_2 + - 0.35653 # j2n6s200_joint_3 + - -4.76501 # j2n6s200_joint_4 + - 5.99991 # j2n6s200_joint_5 + - 4.99555 # j2n6s200_joint_6 + orientation_constraint_to_staging_configuration_quaternion: # perpendicular to the base link + - 0.707168 # x + - 0.0 # y + - 0.0 # z + - 0.707168 # w + orientation_constraint_to_staging_configuration_tolerances: # Note that tolerances are w.r.t. the axes in quat_xyzw_path + - 0.6 # x, rad + - 3.14159 # y, rad + - 0.5 # z, rad + orientation_constraint_to_end_configuration_quaternion: # perpendicular to the base link + - 0.707168 # x + - 0.0 # y + - 0.0 # z + - 0.707168 # w + orientation_constraint_to_end_configuration_tolerances: # Note that tolerances are w.r.t. the axes in quat_xyzw_path + - 0.6 # x, rad + - 3.14159 # y, rad + - 0.5 # z, rad + max_velocity_scaling_factor_to_staging_configuration: 0.4 # optional in (0.0, 1.0], default: 0.1 + max_velocity_scaling_factor_to_end_configuration: 0.65 # optional in (0.0, 1.0], default: 0.1 + cartesian_jump_threshold_to_staging_configuration: 2.0 # If a jump for a joint across consecutive IK solutions is more than this factor greater than the average, it is rejected + cartesian_max_step_to_staging_configuration: 0.01 # m + allowed_planning_time_to_end_configuration: 2.5 # secs tick_rate: 10 # Hz, optional, default: 30 # Parameters for the MoveToStowLocation action @@ -141,14 +268,14 @@ ada_feeding_action_servers: - max_velocity_scaling_factor tree_kwargs: # optional position: - - 0.27971 # x - - 0.07175 # y - - 0.77314 # z + - 0.28323 # x + - 0.07289 # y + - 0.69509 # z quat_xyzw: - - 0.00175 # x - - 0.70159 # y - - 0.71255 # z - - -0.00636 # w + - 0.00472 # x + - 0.69757 # y + - 0.71645 # z + - -0.00811 # w max_velocity_scaling_factor: 0.65 # optional in (0.0, 1.0], default: 0.1 tick_rate: 10 # Hz, optional, default: 30 diff --git a/ada_feeding/config/ada_planning_scene.yaml b/ada_feeding/config/ada_planning_scene.yaml index b8d51221..ab9fb33d 100644 --- a/ada_feeding/config/ada_planning_scene.yaml +++ b/ada_feeding/config/ada_planning_scene.yaml @@ -28,7 +28,7 @@ ada_planning_scene: frame_id: root # the frame_id that the pose is relative to table: # the table mesh filename: table.stl - position: [0.08, -0.5, -0.45] + position: [0.08, -0.5, -0.48] quat_xyzw: [0.0, 0.0, 0.0, 1.0] frame_id: root # the frame_id that the pose is relative to head: # the head mesh diff --git a/ada_feeding_perception/ada_feeding_perception/face_detection.py b/ada_feeding_perception/ada_feeding_perception/face_detection.py index 64407b87..edb0af38 100755 --- a/ada_feeding_perception/ada_feeding_perception/face_detection.py +++ b/ada_feeding_perception/ada_feeding_perception/face_detection.py @@ -415,8 +415,13 @@ def get_mouth_depth( Returns ------- + detected: whether the mouth was detected in the depth image. depth_mm: The depth of the mouth in mm. """ + + # pylint: disable=too-many-locals + # This function is not too complex, but it does have a lot of local variables. + # Find depth image closest in time to RGB image that face was in with self.depth_buffer_lock: min_time_diff = float("inf") @@ -449,10 +454,20 @@ def get_mouth_depth( # Retrieve the depth value averaged over all mouth coordinates depth_sum = 0 + num_points_in_frame = 0 for point in face_points: - depth_sum += image_depth[int(point[1])][int(point[0])] - depth_mm = depth_sum / float(len(face_points)) + x, y = int(point[0]), int(point[1]) + if 0 <= x < image_depth.shape[1] and 0 <= y < image_depth.shape[0]: + num_points_in_frame += 1 + depth_sum += image_depth[y][x] + if num_points_in_frame < 0.5 * len(face_points): + self.get_logger().warn( + "Detected face in the RGB image, but majority of mouth points " + "were outside the frame of the depth image. Ignoring this face." + ) + return False, 0 + depth_mm = depth_sum / float(num_points_in_frame) return True, depth_mm def get_stomion_point(self, u: int, v: int, depth_mm: float) -> Point: