[Bugfix] Workspace walls should get default configurations if customized ones don't exist (#201)

* Add a demo-specific custom configuration

* Hacky workaround to known rclpy bug with destroyables

* Get default robot arm configurations if the customized ones are undefined

---------

Co-authored-by: Taylor Kessler Faulkner <taylorkf@cs.washington.edu>

Author: amalnanavati

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

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.

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.

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

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.