Merge pull request #46 from clearpathrobotics/lcamero/main-republishers

Republishers

clearpath_generator_robot/clearpath_generator_robot/launch/sensors.py

@@ -31,6 +31,7 @@
 # of Clearpath Robotics.
 
 from clearpath_config.sensors.types.sensor import BaseSensor
+from clearpath_config.sensors.types.cameras import BaseCamera
 
 from clearpath_generator_common.common import LaunchFile, Package, ParamFile
 from clearpath_generator_common.launch.writer import LaunchWriter
@@ -45,6 +46,10 @@ class BaseLaunch():
         # Launch arguments
         PARAMETERS = 'parameters'
         NAMESPACE = 'namespace'
+        # Republish arguments
+        INPUT = 'input_ns'
+        OUTPUT = 'output_ns'
+        CONTAINER = 'container'
 
         def __init__(self,
                      sensor: BaseSensor,
@@ -75,6 +80,20 @@ def generate(self):
             sensor_writer = LaunchWriter(self.launch_file)
             # Add default sensor launch file
             sensor_writer.add(self.default_sensor_launch_file)
+            # Cameras republishers
+            if self.sensor.get_sensor_type() == BaseCamera.get_sensor_type():
+                for republihser in self.sensor._republishers:
+                    sensor_writer.add(LaunchFile(
+                        "image_%s" % republihser.TYPE,
+                        package=self.CLEARPATH_SENSORS_PACKAGE,
+                        args=[
+                            (self.NAMESPACE, self.namespace),
+                            (self.PARAMETERS, self.parameters.full_path),
+                            (self.INPUT, republihser.input),
+                            (self.OUTPUT, republihser.output),
+                            (self.CONTAINER, 'image_processing_container')
+                        ]
+                    ))
             # Generate sensor launch file
             sensor_writer.generate_file()
 

clearpath_generator_robot/clearpath_generator_robot/param/sensors.py

@@ -31,6 +31,8 @@
 # of Clearpath Robotics.
 
 from clearpath_config.sensors.types.sensor import BaseSensor
+from clearpath_config.sensors.types.cameras import BaseCamera
+from clearpath_config.common.utils.dictionary import merge_dict
 
 from clearpath_generator_common.common import ParamFile, Package
 from clearpath_generator_common.param.writer import ParamWriter
@@ -63,12 +65,28 @@ def __init__(self,
                 parameters={})
             self.default_param_file.read()
 
+            default_parameters = self.default_param_file.parameters
+
+            # Camera republishers
+            if self.sensor.get_sensor_type() == BaseCamera.get_sensor_type():
+                for republisher in self.sensor._republishers:
+                    name = "image_%s" % republisher.TYPE
+                    rename = "image_%s_%s" % (republisher.TYPE, republisher.input)
+                    republisher_file = ParamFile(
+                        name=name,
+                        package=self.clearpath_sensors_package,
+                        parameters={})
+                    republisher_file.read()
+                    republisher_file.parameters[rename] = republisher_file.parameters.pop(name)
+                    default_parameters = merge_dict(default_parameters,
+                                                    republisher_file.parameters)
+
             # Parameter file to generate
             self.param_file = ParamFile(
                 name=self.sensor.name,
                 namespace=self.namespace,
                 path=self.param_path,
-                parameters=self.default_param_file.parameters)
+                parameters=default_parameters)
 
             self.param_file.update(self.sensor.get_ros_parameters())
 

clearpath_sensors/config/flir_blackfly.yaml

@@ -16,6 +16,8 @@ flir_blackfly:
     # image_height: 1080
     # offset_x: 16
     # offset_y: 0
+    # binning_x: 1
+    # binning_y: 1
     frame_rate_auto: Off
     frame_rate: 40.0
     frame_rate_enable: true
@@ -29,4 +31,4 @@ flir_blackfly:
     chunk_selector_gain: Gain
     chunk_enable_gain: true
     chunk_selector_timestamp: Timestamp
-    chunk_enable_timestamp: true
+    chunk_enable_timestamp: true

clearpath_sensors/config/image_rectify.yaml

@@ -0,0 +1,5 @@
+image_rectify:
+  ros__parameters:
+    image_transport: raw
+    interpolation: 1
+    queue_size: 5

clearpath_sensors/config/image_resize.yaml

@@ -0,0 +1,9 @@
+image_resize:
+  ros__parameters:
+    image_transport: raw
+    interpolation: 1
+    use_scale: True
+    scale_height: 1.0
+    scale_width: 1.0
+    height: -1
+    width: -1

clearpath_sensors/launch/flir_blackfly.launch.py

@@ -27,9 +27,8 @@
 # POSSIBILITY OF SUCH DAMAGE.
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
-from launch.conditions import LaunchConfigurationNotEquals
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
-from launch_ros.actions import Node, ComposableNodeContainer, LoadComposableNodes
+from launch_ros.actions import Node, ComposableNodeContainer
 from launch_ros.descriptions import ComposableNode
 from launch_ros.substitutions import FindPackageShare
 
@@ -38,7 +37,6 @@ def generate_launch_description():
     parameters = LaunchConfiguration('parameters')
     namespace = LaunchConfiguration('namespace')
     param_mapping_file = LaunchConfiguration('param_mapping_file')
-    rectify = LaunchConfiguration('rectify')
 
     arg_parameters = DeclareLaunchArgument(
         'parameters',
@@ -60,66 +58,47 @@ def generate_launch_description():
         'namespace',
         default_value='sensors/camera_0')
 
-    arg_rectify = DeclareLaunchArgument(
-        'rectify',
-        default_value=''
-    )
-
+    name = "flir_blackfly"
     blackfly_camera_node = Node(
         package='spinnaker_camera_driver',
         namespace=namespace,
-        name='flir_blackfly',
+        name=name,
         executable='camera_driver_node',
-        parameters=[parameters, {'parameter_file': param_mapping_file,}],
+        parameters=[parameters, {'parameter_file': param_mapping_file}],
         output='screen',
         remappings=[
-            ('flir_blackfly/camera_info', 'camera_info'),
-            ('flir_blackfly/control', 'control'),
-            ('flir_blackfly/image_raw', 'raw/image'),
-            ('flir_blackfly/meta', 'meta'),
+            (name + '/camera_info', 'camera_info'),
+            (name + '/control', 'control'),
+            (name + '/meta', 'meta'),
+            (name + '/image_raw', 'raw/image'),
+            (name + '/image_raw/compressed', 'raw/compressed'),
+            (name + '/image_raw/compressedDepth', 'raw/compressedDepth'),
+            (name + '/image_raw/theora', 'raw/theora'),
         ]
     )
 
     composable_nodes = [
         ComposableNode(
             package='image_proc',
             plugin='image_proc::DebayerNode',
-            name='debayer_node',
+            name='image_debayer',
             namespace=namespace,
             remappings=[
                 ('image_raw', 'raw/image'),
                 ('image_color', 'color/image'),
+                ('image_color/compressed', 'color/compressed'),
+                ('image_color/compressedDepth', 'color/compressedDepth'),
+                ('image_color/theora', 'color/theora'),
                 ('image_mono', 'mono/image'),
+                ('image_mono/compressed', 'mono/compressed'),
+                ('image_mono/compressedDepth', 'mono/compressedDepth'),
+                ('image_mono/theora', 'mono/theora'),
             ]
-        ),
-        ComposableNode(
-            condition=LaunchConfigurationNotEquals('rectify', ''),
-            package='image_proc',
-            plugin='image_proc::RectifyNode',
-            name='rectify_mono_node',
-            namespace=namespace,
-            # Remap subscribers and publishers
-            remappings=[
-                ('image', 'mono/image'),
-                ('image_rect', 'mono/image_rect')
-            ],
-        ),
-        ComposableNode(
-            condition=LaunchConfigurationNotEquals('rectify', ''),
-            package='image_proc',
-            plugin='image_proc::RectifyNode',
-            name='rectify_color_node',
-            namespace=namespace,
-            # Remap subscribers and publishers
-            remappings=[
-                ('image', 'color/image'),
-                ('image_rect', 'color/image_rect')
-            ],
         )
     ]
 
     image_processing_container = ComposableNodeContainer(
-        name='image_processing_node',
+        name='image_processing_container',
         namespace=namespace,
         package='rclcpp_components',
         executable='component_container',
@@ -131,7 +110,6 @@ def generate_launch_description():
     ld.add_action(arg_parameters)
     ld.add_action(arg_param_mapping_file)
     ld.add_action(arg_namespace)
-    ld.add_action(arg_rectify)
     ld.add_action(blackfly_camera_node)
     ld.add_action(image_processing_container)
     return ld

clearpath_sensors/launch/image_compressed_to_raw.launch.py

@@ -0,0 +1,71 @@
+# Software License Agreement (BSD)
+#
+# @author    Roni Kreinin <[email protected]>
+# @copyright (c) 2023, Clearpath Robotics, Inc., All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+# * Redistributions of source code must retain the above copyright notice,
+#   this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above copyright notice,
+#   this list of conditions and the following disclaimer in the documentation
+#   and/or other materials provided with the distribution.
+# * Neither the name of Clearpath Robotics nor the names of its contributors
+#   may be used to endorse or promote products derived from this software
+#   without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    namespace = LaunchConfiguration('namespace')
+    in_compressed = LaunchConfiguration('in_compressed')
+    out_raw = LaunchConfiguration('out_raw')
+
+    arg_namespace = DeclareLaunchArgument(
+        'namespace',
+        default_value=''
+    )
+
+    arg_in_compressed = DeclareLaunchArgument(
+        'in_compressed',
+        default_value='compressed'
+    )
+
+    arg_out_raw = DeclareLaunchArgument(
+        'out_raw',
+        default_value='image'
+    )
+
+    compressed_transport_node = Node(
+        name="image_compressed_to_raw",
+        namespace=namespace,
+        package="image_transport",
+        executable="republish",
+        remappings=[
+            ('in/compressed', in_compressed),
+            ('out', out_raw),
+        ],
+        arguments=['compressed', 'raw'],
+    )
+
+    ld = LaunchDescription()
+    ld.add_action(arg_namespace)
+    ld.add_action(arg_in_compressed)
+    ld.add_action(arg_out_raw)
+    ld.add_action(compressed_transport_node)
+    return ld

clearpath_sensors/launch/image_raw_to_compressed.launch.py

@@ -0,0 +1,71 @@
+# Software License Agreement (BSD)
+#
+# @author    Roni Kreinin <[email protected]>
+# @copyright (c) 2023, Clearpath Robotics, Inc., All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+# * Redistributions of source code must retain the above copyright notice,
+#   this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above copyright notice,
+#   this list of conditions and the following disclaimer in the documentation
+#   and/or other materials provided with the distribution.
+# * Neither the name of Clearpath Robotics nor the names of its contributors
+#   may be used to endorse or promote products derived from this software
+#   without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    namespace = LaunchConfiguration('namespace')
+    in_raw = LaunchConfiguration('in_raw')
+    out_compressed = LaunchConfiguration('out_compressed')
+
+    arg_namespace = DeclareLaunchArgument(
+        'namespace',
+        default_value=''
+    )
+
+    arg_in_raw = DeclareLaunchArgument(
+        'in_raw',
+        default_value='image'
+    )
+
+    arg_out_compressed = DeclareLaunchArgument(
+        'out_compressed',
+        default_value='compressed'
+    )
+
+    compressed_transport_node = Node(
+        name="image_raw_to_compressed",
+        namespace=namespace,
+        package="image_transport",
+        executable="republish",
+        remappings=[
+            ('in', in_raw),
+            ('out/compressed', out_compressed),
+        ],
+        arguments=['raw', 'compressed'],
+    )
+
+    ld = LaunchDescription()
+    ld.add_action(arg_namespace)
+    ld.add_action(arg_in_raw)
+    ld.add_action(arg_out_compressed)
+    ld.add_action(compressed_transport_node)
+    return ld