Implement republishing to address RealSense issue (#49)

* Implemented republisher

* Remove non-aligned depth

* Added default parameter value

* Finalized republisher node

* Updated launchfile to include republisher

* Updated readme and CameraInfo topic

Author: amalnanavati

ada_feeding_perception/README.md

@@ -18,7 +18,7 @@ For testing, be sure to unzip `test/food_img.zip`.
 
 1. Build your workspace: `colcon build`
 2. Source your workspace: `source install/setup.bash`
-3. Run the perception nodes: `ros2 launch ada_feeding_perception ada_feeding_perception_launch.xml`
+3. Run the perception nodes: `ros2 launch ada_feeding_perception ada_feeding_perception.launch.py`
 4. Launch the motion nodes:
     1. Dummy nodes: `ros2 launch feeding_web_app_ros2_test feeding_web_app_dummy_nodes_launch.xml run_real_sense:=false run_face_detection:=false run_food_detection:=false`
     2. Real nodes: `ros2 launch ada_feeding ada_feeding_launch.xml`
@@ -60,6 +60,8 @@ See `config/test_segment_from_point.yaml` for other sample images and points. No
 
 #### Option B: Interactively Testing the ROS Action Server
 
+**NOTE**: On some machines Option B does not work (more generally, matplotlib interactive graphics don't work).
+
 We have provided a ROS node that displays the live image stream from a topic, let's users click on it, and sends that point click to the SegmentFromPoint action server.
 
 Run this script with: `ros2 launch ada_feeding_perception test_food_segmentation_launch.xml`

ada_feeding_perception/ada_feeding_perception/republisher.py

@@ -0,0 +1,190 @@
+#!/usr/bin/env python3
+"""
+This module defines the Republisher node, which takes in parameters for `from_topics`
+and `republished_namespace` and republishes the messages from the `from_topics` within
+the specified namespace.
+
+This node is intended to address the issue where, when subscribing to images on a
+different machine to the one you are publishing them on, the rate slows down a lot if
+you have >3 subscriptions.
+"""
+
+# Standard imports
+from typing import Any, Callable, List, Tuple
+
+# Third-party imports
+from rcl_interfaces.msg import ParameterDescriptor, ParameterType
+import rclpy
+from rclpy.executors import MultiThreadedExecutor
+from rclpy.node import Node
+
+# Local imports
+from ada_feeding.helpers import import_from_string
+
+
+class Republisher(Node):
+    """
+    A node that takes in parameters for `from_topics` and `republished_namespace` and
+    republishes the messages from the `from_topics` within the specified namespace.
+    """
+
+    def __init__(self) -> None:
+        """
+        Initialize the node.
+        """
+        super().__init__("republisher")
+
+        # Load the parameters
+        (
+            self.from_topics,
+            self.topic_type_strs,
+            self.republished_namespace,
+        ) = self.load_parameters()
+
+        # Import the topic types
+        self.topic_types = []
+        for topic_type_str in self.topic_type_strs:
+            self.topic_types.append(import_from_string(topic_type_str))
+
+        # For each topic, create a callback, publisher, and subscriber
+        num_topics = min(len(self.from_topics), len(self.topic_types))
+        self.callbacks = []
+        self.pubs = []
+        self.subs = []
+        for i in range(num_topics):
+            # Create the callback
+            callback = self.create_callback(i)
+            self.callbacks.append(callback)
+
+            # Create the publisher
+            to_topic = "/".join(
+                [
+                    "",
+                    self.republished_namespace.lstrip("/"),
+                    self.from_topics[i].lstrip("/"),
+                ]
+            )
+            publisher = self.create_publisher(
+                msg_type=self.topic_types[i],
+                topic=to_topic,
+                qos_profile=1,  # TODO: we should get and mirror the QOS profile of the from_topic
+            )
+            self.pubs.append(publisher)
+
+            # Create the subscriber
+            subscriber = self.create_subscription(
+                msg_type=self.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
+            )
+            self.subs.append(subscriber)
+
+    def load_parameters(self) -> Tuple[List[str], List[str], List[str]]:
+        """
+        Load the parameters for the republisher.
+
+        Returns
+        -------
+        from_topics : List[str]
+            The topics to subscribe to.
+        topic_types : List[str]
+            The types of the topics to subscribe to in format, e.g., `std_msgs.msg.String`.
+        republished_namespace : str
+            The namespace to republish topics under.
+        """
+        # Read the from topics
+        from_topics = self.declare_parameter(
+            "from_topics",
+            descriptor=ParameterDescriptor(
+                name="from_topics",
+                type=ParameterType.PARAMETER_STRING_ARRAY,
+                description="List of the topics to subscribe to.",
+                read_only=True,
+            ),
+        )
+
+        # Read the topic types
+        topic_types = self.declare_parameter(
+            "topic_types",
+            descriptor=ParameterDescriptor(
+                name="topic_types",
+                type=ParameterType.PARAMETER_STRING_ARRAY,
+                description=(
+                    "List of the types of the topics to subscribe to  in format, "
+                    "e.g., `std_msgs.msg.String`."
+                ),
+                read_only=True,
+            ),
+        )
+
+        # Read the to topics
+        republished_namespace = self.declare_parameter(
+            "republished_namespace",
+            "/local",
+            descriptor=ParameterDescriptor(
+                name="republished_namespace",
+                type=ParameterType.PARAMETER_STRING,
+                description="The namespace to republish topics under.",
+                read_only=True,
+            ),
+        )
+
+        # Replace unset parameters with empty list
+        from_topics_retval = from_topics.value
+        if from_topics_retval is None:
+            from_topics_retval = []
+        topic_types_retval = topic_types.value
+        if topic_types_retval is None:
+            topic_types_retval = []
+
+        return from_topics_retval, topic_types_retval, republished_namespace.value
+
+    def create_callback(self, i: int) -> Callable:
+        """
+        Create the callback for the subscriber.
+
+        Parameters
+        ----------
+        i : int
+            The index of the callback.
+
+        Returns
+        -------
+        callback : Callable
+            The callback for the subscriber.
+        """
+
+        def callback(msg: Any):
+            """
+            The callback for the subscriber.
+
+            Parameters
+            ----------
+            msg : Any
+                The message from the subscriber.
+            """
+            # self.get_logger().info(
+            #     f"Received message on topic {i} {self.from_topics[i]}"
+            # )
+            self.pubs[i].publish(msg)
+
+        return callback
+
+
+def main(args=None):
+    """
+    Launch the ROS node and spin.
+    """
+    rclpy.init(args=args)
+
+    republisher = Republisher()
+
+    # Use a MultiThreadedExecutor to enable processing goals concurrently
+    executor = MultiThreadedExecutor()
+
+    rclpy.spin(republisher, executor=executor)
+
+
+if __name__ == "__main__":
+    main()

ada_feeding_perception/config/republisher.yaml

@@ -0,0 +1,14 @@
+# NOTE: You have to change this node name if you change the node name in the launchfile.
+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
+
+    # The types of topics to republish from
+    topic_types:
+      - sensor_msgs.msg.CompressedImage
+      - sensor_msgs.msg.CameraInfo
+      - sensor_msgs.msg.Image

ada_feeding_perception/launch/ada_feeding_perception.launch.py

@@ -0,0 +1,130 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+Launch file for ada_feeding_perception
+"""
+
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition
+from launch.launch_context import LaunchContext
+from launch.substitutions import LaunchConfiguration, PythonExpression
+
+
+# pylint: disable=too-many-locals
+# This is a launch file, so it's okay to have a lot of local variables.
+def generate_launch_description():
+    """Creates launch description to run all perception nodes"""
+    launch_description = LaunchDescription()
+
+    # Get the ada_feeding_perception share directory
+    ada_feeding_perception_share_dir = get_package_share_directory(
+        "ada_feeding_perception"
+    )
+
+    # Declare launch arguments
+    use_republisher_da = DeclareLaunchArgument(
+        "use_republisher",
+        default_value="true",
+        description="Whether to use the republisher node",
+    )
+    use_republisher = LaunchConfiguration("use_republisher")
+    launch_description.add_action(use_republisher_da)
+    republished_namespace_da = DeclareLaunchArgument(
+        "republished_namespace",
+        default_value="/local",
+        description="The namespace to republish topics under.",
+    )
+    republished_namespace = LaunchConfiguration("republished_namespace")
+    launch_description.add_action(republished_namespace_da)
+
+    # If we are using the republisher, add the republisher node
+    republisher_config = os.path.join(
+        ada_feeding_perception_share_dir, "config", "republisher.yaml"
+    )
+    republisher_params = {}
+    republisher_params["republished_namespace"] = ParameterValue(
+        republished_namespace, value_type=str
+    )
+    republisher = Node(
+        package="ada_feeding_perception",
+        name="republisher",
+        executable="republisher",
+        parameters=[republisher_config, republisher_params],
+        condition=IfCondition(use_republisher),
+    )
+    launch_description.add_action(republisher)
+
+    # Remap from the perception nodes to the realsense topics
+    prefix = PythonExpression(
+        expression=[
+            "'",
+            republished_namespace,
+            "' if '",
+            use_republisher,
+            "'=='true' else ''",
+        ]
+    )
+    realsense_remappings = [
+        (
+            "~/image",
+            PythonExpression(
+                expression=["'", prefix, "/camera/color/image_raw/compressed'"]
+            ),
+        ),
+        (
+            "~/camera_info",
+            PythonExpression(expression=["'", prefix, "/camera/color/camera_info'"]),
+        ),
+        (
+            "~/aligned_depth",
+            PythonExpression(
+                expression=["'", prefix, "/camera/aligned_depth_to_color/image_raw'"]
+            ),
+        ),
+    ]
+
+    # Load the segment from point node
+    segment_from_point_config = os.path.join(
+        ada_feeding_perception_share_dir, "config", "segment_from_point.yaml"
+    )
+    segment_from_point_params = {}
+    segment_from_point_params["model_dir"] = ParameterValue(
+        os.path.join(ada_feeding_perception_share_dir, "model"), value_type=str
+    )
+    segment_from_point = Node(
+        package="ada_feeding_perception",
+        name="segment_from_point",
+        executable="segment_from_point",
+        parameters=[segment_from_point_config, segment_from_point_params],
+        remappings=realsense_remappings,
+    )
+    launch_description.add_action(segment_from_point)
+
+    # Load the face detection node
+    face_detection_config = os.path.join(
+        ada_feeding_perception_share_dir, "config", "face_detection.yaml"
+    )
+    face_detection_params = {}
+    face_detection_params["model_dir"] = ParameterValue(
+        os.path.join(ada_feeding_perception_share_dir, "model"), value_type=str
+    )
+    face_detection_remappings = [
+        ("~/face_detection", "/face_detection"),
+        ("~/face_detection_img", "/face_detection_img"),
+        ("~/toggle_face_detection", "/toggle_face_detection"),
+    ]
+    face_detection = Node(
+        package="ada_feeding_perception",
+        name="face_detection",
+        executable="face_detection",
+        parameters=[face_detection_config, face_detection_params],
+        remappings=realsense_remappings + face_detection_remappings,
+    )
+    launch_description.add_action(face_detection)
+
+    return launch_description

ada_feeding_perception/launch/ada_feeding_perception_launch.xml

@@ -4,7 +4,7 @@
     <param name="base_dir" value="$(find-pkg-share ada_feeding_perception)"/>
 
     <!-- The image topic must be the same topic that the SegmentFromPoint action server is listening to -->
-    <remap from="~/image" to="/camera/color/image_raw/compressed"/>
+    <remap from="~/image" to="/local/camera/color/image_raw/compressed"/>
   </node>
 
 </launch>

ada_feeding_perception/launch/test_food_segmentation_launch.xml

@@ -41,6 +41,7 @@
     tests_require=["pytest"],
     entry_points={
         "console_scripts": [
+            "republisher = ada_feeding_perception.republisher:main",
             "segment_from_point = ada_feeding_perception.segment_from_point:main",
             "test_segment_from_point = ada_feeding_perception.test_segment_from_point:main",
             "face_detection = ada_feeding_perception.face_detection:main",