Write a nano_bridge (#193)

* Add nano_bridge

* Set perception nodes default QoS to best effort, history 1

* Finalize nano_bridge

* Add README

* Fixes from sim testing

---------

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

Author: amalnanavati

ada_feeding_perception/ada_feeding_perception/face_detection.py

@@ -23,6 +23,7 @@
 from rclpy.node import Node
 from rclpy.parameter import Parameter
 from rclpy.executors import MultiThreadedExecutor
+from rclpy.qos import QoSProfile, ReliabilityPolicy
 from rcl_interfaces.msg import ParameterDescriptor, ParameterType
 from sensor_msgs.msg import CompressedImage, CameraInfo, Image
 from skspatial.objects import Plane, Points
@@ -159,7 +160,7 @@ def __init__(
             image_type,
             image_topic,
             self.camera_callback,
-            1,
+            QoSProfile(depth=1, reliability=ReliabilityPolicy.BEST_EFFORT),
             callback_group=MutuallyExclusiveCallbackGroup(),
         )
 
@@ -179,7 +180,7 @@ def __init__(
             aligned_depth_type,
             aligned_depth_topic,
             self.depth_callback,
-            1,
+            QoSProfile(depth=1, reliability=ReliabilityPolicy.BEST_EFFORT),
             callback_group=MutuallyExclusiveCallbackGroup(),
         )
 
@@ -192,7 +193,7 @@ def __init__(
             CameraInfo,
             "~/camera_info",
             self.camera_info_callback,
-            1,
+            QoSProfile(depth=1, reliability=ReliabilityPolicy.BEST_EFFORT),
             callback_group=MutuallyExclusiveCallbackGroup(),
         )
 

ada_feeding_perception/ada_feeding_perception/food_on_fork_detection.py

@@ -21,6 +21,7 @@ class to use and kwargs for the class's constructor; (b) exposes a ROS2 service
 from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
 from rclpy.executors import MultiThreadedExecutor
 from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy
 from sensor_msgs.msg import CameraInfo, CompressedImage, Image
 from std_srvs.srv import SetBool
 from tf2_ros.buffer import Buffer
@@ -129,7 +130,7 @@ def __init__(self):
             CameraInfo,
             "~/camera_info",
             self.camera_info_callback,
-            1,
+            QoSProfile(depth=1, reliability=ReliabilityPolicy.BEST_EFFORT),
             callback_group=MutuallyExclusiveCallbackGroup(),
         )
 
@@ -150,7 +151,7 @@ def __init__(self):
             aligned_depth_type,
             aligned_depth_topic,
             self.depth_callback,
-            1,
+            QoSProfile(depth=1, reliability=ReliabilityPolicy.BEST_EFFORT),
             callback_group=MutuallyExclusiveCallbackGroup(),
         )
 
@@ -174,7 +175,7 @@ def __init__(self):
                 image_type,
                 image_topic,
                 self.camera_callback,
-                1,
+                QoSProfile(depth=1, reliability=ReliabilityPolicy.BEST_EFFORT),
                 callback_group=MutuallyExclusiveCallbackGroup(),
             )
 

ada_feeding_perception/ada_feeding_perception/republisher.py

@@ -25,6 +25,7 @@
 from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
 from rclpy.executors import MultiThreadedExecutor
 from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy
 
 # Local imports
 from ada_feeding.helpers import import_from_string
@@ -208,7 +209,9 @@ def __init__(self) -> None:
                 msg_type=self.in_topic_types[i],
                 topic=self.from_topics[i],
                 callback=callback,
-                qos_profile=1,  # TODO: we should get and mirror the QOS profile of the from_topic
+                qos_profile=QoSProfile(
+                    depth=1, reliability=ReliabilityPolicy.BEST_EFFORT
+                ),  # TODO: we should get and mirror the QOS profile of the from_topic
                 callback_group=MutuallyExclusiveCallbackGroup(),
             )
             self.subs.append(subscriber)

ada_feeding_perception/ada_feeding_perception/segment_from_point.py

@@ -23,6 +23,7 @@
 from rclpy.executors import MultiThreadedExecutor
 from rclpy.node import Node
 from rclpy.parameter import Parameter
+from rclpy.qos import QoSProfile, ReliabilityPolicy
 from segment_anything import sam_model_registry, SamPredictor
 from sensor_msgs.msg import CameraInfo, CompressedImage, Image, RegionOfInterest
 import torch
@@ -98,7 +99,7 @@ def __init__(self) -> None:
             CameraInfo,
             "~/camera_info",
             self.camera_info_callback,
-            1,
+            QoSProfile(depth=1, reliability=ReliabilityPolicy.BEST_EFFORT),
             callback_group=MutuallyExclusiveCallbackGroup(),
         )
 
@@ -118,7 +119,7 @@ def __init__(self) -> None:
             aligned_depth_type,
             aligned_depth_topic,
             self.depth_image_callback,
-            1,
+            QoSProfile(depth=1, reliability=ReliabilityPolicy.BEST_EFFORT),
             callback_group=MutuallyExclusiveCallbackGroup(),
         )
 
@@ -137,7 +138,7 @@ def __init__(self) -> None:
             image_type,
             image_topic,
             self.image_callback,
-            1,
+            QoSProfile(depth=1, reliability=ReliabilityPolicy.BEST_EFFORT),
             callback_group=MutuallyExclusiveCallbackGroup(),
         )
 

ada_feeding_perception/ada_feeding_perception/table_detection.py

@@ -16,6 +16,7 @@
 from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
 from rclpy.executors import MultiThreadedExecutor
 from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy
 from rcl_interfaces.msg import ParameterDescriptor, ParameterType
 from sensor_msgs.msg import CameraInfo, CompressedImage, Image
 from std_srvs.srv import SetBool
@@ -85,7 +86,7 @@ def __init__(self):
             CameraInfo,
             "~/camera_info",
             self.camera_info_callback,
-            1,
+            QoSProfile(depth=1, reliability=ReliabilityPolicy.BEST_EFFORT),
             callback_group=MutuallyExclusiveCallbackGroup(),
         )
 
@@ -104,7 +105,7 @@ def __init__(self):
             aligned_depth_type,
             aligned_depth_topic,
             self.depth_image_callback,
-            1,
+            QoSProfile(depth=1, reliability=ReliabilityPolicy.BEST_EFFORT),
             callback_group=MutuallyExclusiveCallbackGroup(),
         )
 
@@ -123,7 +124,7 @@ def __init__(self):
             image_type,
             image_topic,
             self.image_callback,
-            1,
+            QoSProfile(depth=1, reliability=ReliabilityPolicy.BEST_EFFORT),
             callback_group=MutuallyExclusiveCallbackGroup(),
         )
 

ada_feeding_perception/config/republisher.yaml

@@ -3,36 +3,24 @@ republisher:
   ros__parameters:
     # The name of the topics to republish from
     from_topics:
-      - /camera/color/image_raw/compressed
-      - /camera/color/camera_info
-      - /camera/aligned_depth_to_color/image_raw/compressedDepth
       - /camera/aligned_depth_to_color/camera_info
       - /local/camera/aligned_depth_to_color/image_raw/compressedDepth
 
     # The types of topics to republish from
     in_topic_types:
-      - sensor_msgs.msg.CompressedImage
-      - sensor_msgs.msg.CameraInfo
-      - sensor_msgs.msg.CompressedImage
       - sensor_msgs.msg.CameraInfo
       - sensor_msgs.msg.CompressedImage
 
     # If the republisher should convert types, specify the output type.
     # Currently, the republisher only supports conversions from
     # Image->CompressedImage and vice-versa.
     out_topic_types:
-      - ""
-      - ""
-      - ""
       - ""
       - sensor_msgs.msg.Image
 
     # The name of the topics to republish to. NOTE: the `prefix` in the launchfile
     # must match the below pattern!
     to_topics:
-      - /local/camera/color/image_raw/compressed
-      - /local/camera/color/camera_info
-      - /local/camera/aligned_depth_to_color/image_raw/compressedDepth
       - /local/camera/aligned_depth_to_color/camera_info
       - /local/camera/aligned_depth_to_color/depth_octomap
 
@@ -41,9 +29,6 @@ republisher:
     target_rates:
       - 0
       - 0
-      - 0
-      - 0
-      - 0
 
     # The names of a post-processing functions to apply to the message before
     # republishing it. Current options are:
@@ -58,9 +43,6 @@ republisher:
     # Any of these post-processing functions can be combined in a comma-separated list.
     # If an empty string, no post-processors are applied.
     post_processors:
-      - ""
-      - ""
-      - ""
       - ""
       - threshold,mask,temporal,spatial # Apply filters to the depth image for the Octomap
     # The binary mask used for "mask" post-processing. This mask will get scaled,

ada_feeding_perception/launch/ada_feeding_perception.launch.py

@@ -9,9 +9,14 @@
 from launch_ros.actions import Node
 from launch_ros.parameter_descriptions import ParameterValue
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
 from launch.conditions import IfCondition
-from launch.substitutions import LaunchConfiguration, PythonExpression
+from launch.launch_description_sources import AnyLaunchDescriptionSource
+from launch.substitutions import (
+    LaunchConfiguration,
+    PathJoinSubstitution,
+    PythonExpression,
+)
 
 
 # pylint: disable=too-many-locals
@@ -47,6 +52,20 @@ def generate_launch_description():
     )
     launch_description.add_action(republisher)
 
+    # Add the nano_bridge receiver node
+    nano_bridge_receiver = IncludeLaunchDescription(
+        AnyLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [
+                    get_package_share_directory("nano_bridge"),
+                    "launch",
+                    "receiver.launch.xml",
+                ]
+            ),
+        ),
+    )
+    launch_description.add_action(nano_bridge_receiver)
+
     # Remap from the perception nodes to the realsense topics
     prefix = "/local"  # NOTE: must match the topic names in the yaml file!
     realsense_remappings = [
@@ -58,7 +77,9 @@ def generate_launch_description():
         ),
         (
             "~/camera_info",
-            PythonExpression(expression=["'", prefix, "/camera/color/camera_info'"]),
+            PythonExpression(
+                expression=["'", prefix, "/camera/aligned_depth_to_color/camera_info'"]
+            ),
         ),
     ]
     aligned_depth_remapping = [

ada_planning_scene/ada_planning_scene/workspace_walls.py

@@ -1057,10 +1057,9 @@ def initialize(
             success = self.__update_robot_configuration_bounds(
                 rate_hz=rate_hz,
                 timeout=get_remaining_time(self.__node, start_time, timeout),
-                # For repeatability, during initialization we don't account for the
-                # current robot config, but during every other update, we do if
-                # the parameter is set.
-                include_current_robot_config=False,
+                # Although accounting for the current configuration during initialization
+                # harms repeatability, it does ensure the robot will always be able to move.
+                include_current_robot_config=True,
             )
             if not success:
                 self.__node.get_logger().info(

nano_bridge/CMakeLists.txt

@@ -0,0 +1,39 @@
+cmake_minimum_required(VERSION 3.8)
+project(nano_bridge)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(sensor_msgs REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/CompressedImage.msg"
+  DEPENDENCIES sensor_msgs
+)
+
+# Install Python executables
+install(PROGRAMS
+  nano_bridge/sender.py
+  nano_bridge/sender_compressed_image.py
+  nano_bridge/receiver.py
+  nano_bridge/receiver_compressed_image.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+# Install launch files.
+install(DIRECTORY
+  launch
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+# Install config files.
+install(DIRECTORY
+  config
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+ament_package()

nano_bridge/README.md

@@ -0,0 +1,7 @@
+# nano_bridge
+
+This package is very particular to our computer setting, intended to account for the fact that the publication rate of one of the two image topics (RGB and depth) becomes very unreliable when you have one subscriber each (which is common). See this issue for more: https://github.com/personalrobotics/ada_feeding/issues/73
+
+`sender` and `receiver` are generic nodes that can serialize any number of ROS topics of any type into one ROS topic. However, pickle serialization can take ~0.2s, which can slow down the publisher. Hence, `sender_compressed_image` and `receiver_compressed_image` can combine any number of CompressedImage topics into one, but can only do it for those topics (and it doesn't have the overhead of serialization).
+
+A future TODO is to create a generic way to handle cases where the input types are the same and are different, and to create a unified sender and receiver class.