diff --git a/ada_feeding/config/ada_feeding_action_servers_custom.yaml b/ada_feeding/config/ada_feeding_action_servers_custom.yaml index ada11edc..f1062c61 100644 --- a/ada_feeding/config/ada_feeding_action_servers_custom.yaml +++ b/ada_feeding/config/ada_feeding_action_servers_custom.yaml @@ -94,7 +94,18 @@ ada_feeding_action_servers: - side_staging_1 - side_staging_2 - side_staging_3 - namespace_to_use: wheelchair_side_table_1 + - demo + demo: + MoveFromMouth.tree_kwargs.linear_speed_near_mouth: 0.08 + MoveFromMouth.tree_kwargs.max_linear_speed_to_staging_configuration: 0.15 + MoveToMouth.tree_kwargs.linear_speed_near_mouth: 0.06 + MoveToMouth.tree_kwargs.max_linear_speed: 0.15 + MoveToMouth.tree_kwargs.plan_distance_from_mouth: + - 0.05 + - 0.0 + - -0.01 + planning_scene_namespace_to_use: seated + namespace_to_use: demo 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 abdbdcec..cfc01871 100644 --- a/ada_feeding/config/ada_feeding_action_servers_default.yaml +++ b/ada_feeding/config/ada_feeding_action_servers_default.yaml @@ -5,8 +5,7 @@ ada_feeding_action_servers: watchdog_timeout_sec: 0.5 # sec default: - # TODO: Post-deployment, change this back! - planning_scene_namespace_to_use: seated_90 + planning_scene_namespace_to_use: seated # 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/ada_planning_scene/ada_planning_scene.py b/ada_planning_scene/ada_planning_scene/ada_planning_scene.py index e7b49487..8a6bd49e 100644 --- a/ada_planning_scene/ada_planning_scene/ada_planning_scene.py +++ b/ada_planning_scene/ada_planning_scene/ada_planning_scene.py @@ -5,6 +5,7 @@ # Standard imports import threading +import traceback from typing import List # Third-party imports @@ -381,6 +382,23 @@ def parameter_callback(self, params: List[Parameter]) -> SetParametersResult: return SetParametersResult(successful=True) +def spin(node: Node, executor: MultiThreadedExecutor) -> None: + """ + Spin the node in the executor. + """ + try: + rclpy.spin(node, executor=executor) + except rclpy._rclpy_pybind11.InvalidHandle: + # There is a known issue in rclpy where it doesn't properly handle destruction of + # elements in the executor. + # - https://github.com/ros2/rclpy/issues/1355 + # - https://github.com/ros2/rclpy/issues/1206 + # - https://github.com/ros2/rclpy/issues/1142 + # This is a **very hacky** workaround to prevent the node from crashing. + traceback.print_exc() + spin(node, executor) + + def main(args: List = None) -> None: """ Create the ROS2 node. diff --git a/ada_planning_scene/ada_planning_scene/workspace_walls.py b/ada_planning_scene/ada_planning_scene/workspace_walls.py index 9d5e31b1..4a591cac 100644 --- a/ada_planning_scene/ada_planning_scene/workspace_walls.py +++ b/ada_planning_scene/ada_planning_scene/workspace_walls.py @@ -808,7 +808,7 @@ def cleanup(): ): prefix = "" else: - prefix = response.values[0].string_value + "." + prefix = response.values[0].string_value cleanup() return True, prefix @@ -826,31 +826,33 @@ def __joint_states_callback(self, msg: JointState) -> None: if name in self.__articulated_joint_names: self.__joint_states[name] = msg.position[i] - def __get_robot_configurations( + def __get_robot_configurations_within_prefix( self, + prefix: str, + configurations_parameter_names: List[str], rate_hz: float = 10.0, timeout: Duration = Duration(seconds=5), publish_feedback: Optional[Callable[[], None]] = None, - include_current_robot_config: bool = True, ) -> Tuple[bool, Dict[str, List[float]]]: """ - Get the robot's configurations. + Get the robot's configurations within a prefix. Parameters ---------- + prefix: The prefix to add to the parameter name. + configurations_parameter_names: The names of the parameters that contain robot + joint configurations that should be contained within the workspace walls. rate_hz: The rate at which to call the service. timeout: The timeout for the service. publish_feedback: If not None, call this function periodically to publish feedback. - include_current_robot_config: Whether to include the current robot configuration. - This can override the value set in the parameter. Returns ------- success: True if successful, False otherwise. robot_configurations: A map from the parameter name to the configuration. """ - # pylint: disable=too-many-locals, too-many-branches - # A few above is fine + # pylint: disable=too-many-locals, too-many-arguments + # One over is fine. # Start the time start_time = self.__node.get_clock().now() @@ -859,35 +861,11 @@ def __get_robot_configurations( def cleanup(): self.__node.destroy_rate(rate) - # Get the prefix - success, prefix = self.__get_parameter_prefix( - rate_hz, get_remaining_time(self.__node, start_time, timeout) - ) - if not success: - self.__node.get_logger().error("Failed to get the parameter prefix.") - cleanup() - return False, {} - - # Wait for the service to be ready - self.__node.get_logger().info( - "Waiting for the get robot configurations parameter service." - ) - while not self.__get_robot_configurations_parameter_service.service_is_ready(): - if not check_ok(self.__node, start_time, timeout): - self.__node.get_logger().error( - "Timeout while waiting for the get robot configurations parameter service." - ) - cleanup() - return False, {} - if publish_feedback is not None: - publish_feedback() - rate.sleep() - # Get the robot configurations robot_configurations = {} request = GetParameters.Request() request.names = [ - prefix + name for name in self.__robot_configurations_parameter_names + ".".join([prefix, name]) for name in configurations_parameter_names ] self.__node.get_logger().info( f"Getting robot configurations from parameters: {request.names}" @@ -916,7 +894,7 @@ def cleanup(): for i, param in enumerate(response.values): if param.type != ParameterType.PARAMETER_DOUBLE_ARRAY: continue - robot_configurations[self.__robot_configurations_parameter_names[i]] = list( + robot_configurations[configurations_parameter_names[i]] = list( param.double_array_value ) if publish_feedback is not None: @@ -926,6 +904,96 @@ def cleanup(): cleanup() return False, {} + cleanup() + return True, robot_configurations + + def __get_robot_configurations( + self, + rate_hz: float = 10.0, + timeout: Duration = Duration(seconds=5), + publish_feedback: Optional[Callable[[], None]] = None, + include_current_robot_config: bool = True, + ) -> Tuple[bool, Dict[str, List[float]]]: + """ + Get the robot's configurations. + + Parameters + ---------- + rate_hz: The rate at which to call the service. + timeout: The timeout for the service. + publish_feedback: If not None, call this function periodically to publish feedback. + include_current_robot_config: Whether to include the current robot configuration. + This can override the value set in the parameter. + + Returns + ------- + success: True if successful, False otherwise. + robot_configurations: A map from the parameter name to the configuration. + """ + # pylint: disable=too-many-locals, too-many-branches + # A few above is fine + + # 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( + rate_hz, get_remaining_time(self.__node, start_time, timeout) + ) + if not success: + self.__node.get_logger().error("Failed to get the parameter prefix.") + cleanup() + return False, {} + + # Wait for the service to be ready + self.__node.get_logger().info( + "Waiting for the get robot configurations parameter service." + ) + while not self.__get_robot_configurations_parameter_service.service_is_ready(): + if not check_ok(self.__node, start_time, timeout): + self.__node.get_logger().error( + "Timeout while waiting for the get robot configurations parameter service." + ) + cleanup() + return False, {} + if publish_feedback is not None: + publish_feedback() + rate.sleep() + + # Get the robot configurations + _, robot_configurations = self.__get_robot_configurations_within_prefix( + prefix, + self.__robot_configurations_parameter_names, + rate_hz, + get_remaining_time(self.__node, start_time, timeout), + publish_feedback=publish_feedback, + ) + remaining_configurations_parameter_names = [ + name + for name in self.__robot_configurations_parameter_names + if name not in robot_configurations + ] + _, default_robot_configurations = self.__get_robot_configurations_within_prefix( + "default", + remaining_configurations_parameter_names, + rate_hz, + get_remaining_time(self.__node, start_time, timeout), + publish_feedback=publish_feedback, + ) + robot_configurations.update(default_robot_configurations) + # If we got some but not all of them, raise an error but continue + if len(robot_configurations) != len( + self.__robot_configurations_parameter_names + ): + self.__node.get_logger().error("Failed to get robot configurations.") + if len(robot_configurations) == 0: + cleanup() + return False, {} + # Add the current joint state if ( self.__workspace_walls_contain_current_robot_config diff --git a/ada_planning_scene/config/ada_planning_scene.yaml b/ada_planning_scene/config/ada_planning_scene.yaml index c05d34af..2ed17ac8 100644 --- a/ada_planning_scene/config/ada_planning_scene.yaml +++ b/ada_planning_scene/config/ada_planning_scene.yaml @@ -25,7 +25,7 @@ ada_planning_scene: - bedside - seated_90 - bedside_90 - namespace_to_use: seated_90 # bedside_90 # + namespace_to_use: seated ############################################################################ # Parameters related to the PlanningSceneInitializer class @@ -294,7 +294,7 @@ ada_planning_scene: - "MoveAbovePlate.tree_kwargs.joint_positions" - "MoveToRestingPosition.tree_kwargs.goal_configuration" - "MoveToStagingConfiguration.tree_kwargs.goal_configuration" - - "MoveToStowLocation.tree_kwargs.tree_kwargs" + - "MoveToStowLocation.tree_kwargs.joint_positions" # Which joints in the robot's URDF are fixed (along with their values). # These are used when computing the bounds of different robot arm configurationss.