From 48494d6ca8a55cb7a35cca3cd355bda0ea004f18 Mon Sep 17 00:00:00 2001 From: Luis Camero Date: Thu, 1 Feb 2024 15:20:05 -0500 Subject: [PATCH 1/8] Added rectify and resize --- .../launch/sensors.py | 19 +++ .../param/sensors.py | 17 ++- clearpath_sensors/config/image_rectify.yaml | 5 + clearpath_sensors/config/image_resize.yaml | 9 ++ .../launch/flir_blackfly.launch.py | 60 +++------ .../launch/image_rectify.launch.py | 106 ++++++++++++++++ .../launch/image_resize.launch.py | 116 ++++++++++++++++++ 7 files changed, 290 insertions(+), 42 deletions(-) create mode 100644 clearpath_sensors/config/image_rectify.yaml create mode 100644 clearpath_sensors/config/image_resize.yaml create mode 100644 clearpath_sensors/launch/image_rectify.launch.py create mode 100644 clearpath_sensors/launch/image_resize.launch.py diff --git a/clearpath_generator_robot/clearpath_generator_robot/launch/sensors.py b/clearpath_generator_robot/clearpath_generator_robot/launch/sensors.py index 8b3ccd9..9a2c05b 100644 --- a/clearpath_generator_robot/clearpath_generator_robot/launch/sensors.py +++ b/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() diff --git a/clearpath_generator_robot/clearpath_generator_robot/param/sensors.py b/clearpath_generator_robot/clearpath_generator_robot/param/sensors.py index 243e153..05ac80b 100644 --- a/clearpath_generator_robot/clearpath_generator_robot/param/sensors.py +++ b/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,25 @@ 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: + republisher_file = ParamFile( + name="image_%s" % republisher.TYPE, + package=self.clearpath_sensors_package, + parameters={}) + republisher_file.read() + 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()) diff --git a/clearpath_sensors/config/image_rectify.yaml b/clearpath_sensors/config/image_rectify.yaml new file mode 100644 index 0000000..e0b1d14 --- /dev/null +++ b/clearpath_sensors/config/image_rectify.yaml @@ -0,0 +1,5 @@ +image_rectify: + ros__parameters: + image_transport: raw + interpolation: 1 + queue_size: 5 diff --git a/clearpath_sensors/config/image_resize.yaml b/clearpath_sensors/config/image_resize.yaml new file mode 100644 index 0000000..58d1ddd --- /dev/null +++ b/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 diff --git a/clearpath_sensors/launch/flir_blackfly.launch.py b/clearpath_sensors/launch/flir_blackfly.launch.py index 23701fd..7de75be 100644 --- a/clearpath_sensors/launch/flir_blackfly.launch.py +++ b/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,23 +58,22 @@ 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'), ] ) @@ -84,42 +81,24 @@ def generate_launch_description(): 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 diff --git a/clearpath_sensors/launch/image_rectify.launch.py b/clearpath_sensors/launch/image_rectify.launch.py new file mode 100644 index 0000000..4505524 --- /dev/null +++ b/clearpath_sensors/launch/image_rectify.launch.py @@ -0,0 +1,106 @@ +# Software License Agreement (BSD) +# +# @author Roni Kreinin +# @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.conditions import LaunchConfigurationEquals, LaunchConfigurationNotEquals +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression +from launch_ros.actions import ComposableNodeContainer, LoadComposableNodes +from launch_ros.descriptions import ComposableNode +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + parameters = LaunchConfiguration('parameters') + namespace = LaunchConfiguration('namespace') + input_ns = LaunchConfiguration('input_ns') + output_ns = LaunchConfiguration('output_ns') + container = LaunchConfiguration('container') + + arg_parameters = DeclareLaunchArgument( + 'parameters', + default_value=PathJoinSubstitution([ + FindPackageShare('clearpath_sensors'), + 'config', + 'flir_blackfly.yaml' + ])) + + arg_namespace = DeclareLaunchArgument( + 'namespace', + default_value='sensors/camera_0') + + arg_input_ns = DeclareLaunchArgument( + 'input_ns', + default_value='color' + ) + + arg_output_ns = DeclareLaunchArgument( + 'output_ns', + default_value='resize' + ) + + composable_nodes = [ + ComposableNode( + package='image_proc', + plugin='image_proc::RectifyNode', + name=PythonExpression(["'image_rectify_", input_ns, "'"]), + namespace=namespace, + # Remap subscribers and publishers + remappings=[ + ('image', 'mono/image'), + ('image_rect', 'mono/image_rect') + ], + parameters=[parameters] + ) + ] + + # Create container if none provided, by default look for `image_processing_node` + image_processing_container = ComposableNodeContainer( + condition=LaunchConfigurationEquals('container', ''), + name='image_processing_container', + namespace=namespace, + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=composable_nodes, + output='screen' + ) + + # Use default container that should have been launched by the camera launch file + load_composable_nodes = LoadComposableNodes( + condition=LaunchConfigurationNotEquals('container', ''), + composable_node_descriptions=composable_nodes, + target_container=PythonExpression(["'", namespace, "/", container, "'"]) + ) + + ld = LaunchDescription() + ld.add_action(arg_parameters) + ld.add_action(arg_namespace) + ld.add_action(arg_input_ns) + ld.add_action(arg_output_ns) + ld.add_action(image_processing_container) + ld.add_action(load_composable_nodes) + return ld diff --git a/clearpath_sensors/launch/image_resize.launch.py b/clearpath_sensors/launch/image_resize.launch.py new file mode 100644 index 0000000..3fb76a1 --- /dev/null +++ b/clearpath_sensors/launch/image_resize.launch.py @@ -0,0 +1,116 @@ +# Software License Agreement (BSD) +# +# @author Roni Kreinin +# @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.conditions import LaunchConfigurationEquals, LaunchConfigurationNotEquals +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression +from launch_ros.actions import ComposableNodeContainer, LoadComposableNodes +from launch_ros.descriptions import ComposableNode +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + parameters = LaunchConfiguration('parameters') + namespace = LaunchConfiguration('namespace') + input_ns = LaunchConfiguration('input_ns') + output_ns = LaunchConfiguration('output_ns') + container = LaunchConfiguration('container') + + arg_parameters = DeclareLaunchArgument( + 'parameters', + default_value=PathJoinSubstitution([ + FindPackageShare('clearpath_sensors'), + 'config', + 'image_resize.yaml' + ])) + + arg_namespace = DeclareLaunchArgument( + 'namespace', + default_value='sensors/camera_0') + + arg_input_ns = DeclareLaunchArgument( + 'input_ns', + default_value='color' + ) + + arg_output_ns = DeclareLaunchArgument( + 'output_ns', + default_value='resize' + ) + + arg_container = DeclareLaunchArgument( + 'container', + default_value='image_processing_container' + ) + + # Resize composable node + composable_nodes = [ + ComposableNode( + package='image_proc', + plugin='image_proc::ResizeNode', + name=PythonExpression(["'image_resize_", input_ns, "'"]), + namespace=namespace, + # Remap subscribers and publishers + remappings=[ + ('image', PathJoinSubstitution([input_ns, 'image'])), + ('resize', PathJoinSubstitution([output_ns, 'image'])), + ('resize/compressed', PathJoinSubstitution([output_ns, 'compressed'])), + ('resize/compressedDepth', PathJoinSubstitution([output_ns, 'compressedDepth'])), + ('resize/theora', PathJoinSubstitution([output_ns, 'theora'])), + ], + parameters=[parameters], + ), + ] + + # Create container if none provided, by default look for `image_processing_node` + image_processing_container = ComposableNodeContainer( + condition=LaunchConfigurationEquals('container', ''), + name='image_processing_container', + namespace=namespace, + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=composable_nodes, + output='screen' + ) + + # Use default container that should have been launched by the camera launch file + load_composable_nodes = LoadComposableNodes( + condition=LaunchConfigurationNotEquals('container', ''), + composable_node_descriptions=composable_nodes, + target_container=PythonExpression(["'", namespace, "/", container, "'"]) + ) + + ld = LaunchDescription() + ld.add_action(arg_parameters) + ld.add_action(arg_namespace) + ld.add_action(arg_input_ns) + ld.add_action(arg_output_ns) + ld.add_action(arg_container) + ld.add_action(image_processing_container) + ld.add_action(load_composable_nodes) + return ld From 791e334fe91a25d63793b95ee5d00c6ea0ab3534 Mon Sep 17 00:00:00 2001 From: Luis Camero Date: Mon, 5 Feb 2024 09:46:52 -0500 Subject: [PATCH 2/8] Add theora encode and decode launch --- .../launch/image_raw_to_theora.launch.py | 71 +++++++++++++++++++ .../launch/image_theora_to_raw.launch.py | 71 +++++++++++++++++++ 2 files changed, 142 insertions(+) create mode 100644 clearpath_sensors/launch/image_raw_to_theora.launch.py create mode 100644 clearpath_sensors/launch/image_theora_to_raw.launch.py diff --git a/clearpath_sensors/launch/image_raw_to_theora.launch.py b/clearpath_sensors/launch/image_raw_to_theora.launch.py new file mode 100644 index 0000000..f79d141 --- /dev/null +++ b/clearpath_sensors/launch/image_raw_to_theora.launch.py @@ -0,0 +1,71 @@ +# Software License Agreement (BSD) +# +# @author Roni Kreinin +# @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_theora = LaunchConfiguration('out_theora') + + arg_namespace = DeclareLaunchArgument( + 'namespace', + default_value='' + ) + + arg_in_raw = DeclareLaunchArgument( + 'in_raw', + default_value='image' + ) + + arg_out_theora = DeclareLaunchArgument( + 'out_theora', + default_value='theora' + ) + + theora_transport_node = Node( + name="image_raw_to_theora", + namespace=namespace, + package="image_transport", + executable="republish", + remappings=[ + ('in', in_raw), + ('out/theora', out_theora), + ], + arguments=['theora', 'raw'], + ) + + ld = LaunchDescription() + ld.add_action(arg_namespace) + ld.add_action(arg_in_raw) + ld.add_action(arg_out_theora) + ld.add_action(theora_transport_node) + return ld diff --git a/clearpath_sensors/launch/image_theora_to_raw.launch.py b/clearpath_sensors/launch/image_theora_to_raw.launch.py new file mode 100644 index 0000000..0795e28 --- /dev/null +++ b/clearpath_sensors/launch/image_theora_to_raw.launch.py @@ -0,0 +1,71 @@ +# Software License Agreement (BSD) +# +# @author Roni Kreinin +# @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_theora = LaunchConfiguration('in_theora') + out_raw = LaunchConfiguration('out_raw') + + arg_namespace = DeclareLaunchArgument( + 'namespace', + default_value='' + ) + + arg_in_theora = DeclareLaunchArgument( + 'in_theora', + default_value='theora' + ) + + arg_out_raw = DeclareLaunchArgument( + 'out_raw', + default_value='image' + ) + + theora_transport_node = Node( + name="image_theora_to_raw", + namespace=namespace, + package="image_transport", + executable="republish", + remappings=[ + ('in/theora', in_theora), + ('out', out_raw), + ], + arguments=['theora', 'raw'], + ) + + ld = LaunchDescription() + ld.add_action(arg_namespace) + ld.add_action(arg_in_theora) + ld.add_action(arg_out_raw) + ld.add_action(theora_transport_node) + return ld From c2925eb939ea2671384fb53525f2dc34b88244ff Mon Sep 17 00:00:00 2001 From: Luis Camero Date: Mon, 5 Feb 2024 10:16:09 -0500 Subject: [PATCH 3/8] Fixed theora encoding node --- clearpath_sensors/launch/image_raw_to_theora.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/clearpath_sensors/launch/image_raw_to_theora.launch.py b/clearpath_sensors/launch/image_raw_to_theora.launch.py index f79d141..74e9d81 100644 --- a/clearpath_sensors/launch/image_raw_to_theora.launch.py +++ b/clearpath_sensors/launch/image_raw_to_theora.launch.py @@ -60,7 +60,7 @@ def generate_launch_description(): ('in', in_raw), ('out/theora', out_theora), ], - arguments=['theora', 'raw'], + arguments=['raw', 'theora'], ) ld = LaunchDescription() From 95334058faa6de6f21975a4f0dde9c7353616a2f Mon Sep 17 00:00:00 2001 From: Luis Camero Date: Mon, 5 Feb 2024 13:10:21 -0500 Subject: [PATCH 4/8] Added compressed encode and decode launch files --- .../launch/image_compressed_to_raw.launch.py | 71 +++++++++++++++++++ .../launch/image_raw_to_compressed.launch.py | 71 +++++++++++++++++++ 2 files changed, 142 insertions(+) create mode 100644 clearpath_sensors/launch/image_compressed_to_raw.launch.py create mode 100644 clearpath_sensors/launch/image_raw_to_compressed.launch.py diff --git a/clearpath_sensors/launch/image_compressed_to_raw.launch.py b/clearpath_sensors/launch/image_compressed_to_raw.launch.py new file mode 100644 index 0000000..05b50fa --- /dev/null +++ b/clearpath_sensors/launch/image_compressed_to_raw.launch.py @@ -0,0 +1,71 @@ +# Software License Agreement (BSD) +# +# @author Roni Kreinin +# @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 diff --git a/clearpath_sensors/launch/image_raw_to_compressed.launch.py b/clearpath_sensors/launch/image_raw_to_compressed.launch.py new file mode 100644 index 0000000..0e3ae72 --- /dev/null +++ b/clearpath_sensors/launch/image_raw_to_compressed.launch.py @@ -0,0 +1,71 @@ +# Software License Agreement (BSD) +# +# @author Roni Kreinin +# @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 From f78c19dbfbfd69961a014523bafc45ed3603d120 Mon Sep 17 00:00:00 2001 From: Luis Camero Date: Wed, 7 Feb 2024 14:09:23 -0500 Subject: [PATCH 5/8] Recitfy node matches resize --- .../launch/image_rectify.launch.py | 22 ++++++++++++++----- 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/clearpath_sensors/launch/image_rectify.launch.py b/clearpath_sensors/launch/image_rectify.launch.py index 4505524..3cb5b8d 100644 --- a/clearpath_sensors/launch/image_rectify.launch.py +++ b/clearpath_sensors/launch/image_rectify.launch.py @@ -46,7 +46,7 @@ def generate_launch_description(): default_value=PathJoinSubstitution([ FindPackageShare('clearpath_sensors'), 'config', - 'flir_blackfly.yaml' + 'image_rectify.yaml' ])) arg_namespace = DeclareLaunchArgument( @@ -60,9 +60,15 @@ def generate_launch_description(): arg_output_ns = DeclareLaunchArgument( 'output_ns', - default_value='resize' + default_value='`rectif`y' ) + arg_container = DeclareLaunchArgument( + 'container', + default_value='image_processing_container' + ) + + # Rectify composable node composable_nodes = [ ComposableNode( package='image_proc', @@ -71,11 +77,14 @@ def generate_launch_description(): namespace=namespace, # Remap subscribers and publishers remappings=[ - ('image', 'mono/image'), - ('image_rect', 'mono/image_rect') + ('image', PathJoinSubstitution([input_ns, 'image'])), + ('rectify', PathJoinSubstitution([output_ns, 'image'])), + ('rectify/compressed', PathJoinSubstitution([output_ns, 'compressed'])), + ('rectify/compressedDepth', PathJoinSubstitution([output_ns, 'compressedDepth'])), + ('rectify/theora', PathJoinSubstitution([output_ns, 'theora'])), ], - parameters=[parameters] - ) + parameters=[parameters], + ), ] # Create container if none provided, by default look for `image_processing_node` @@ -101,6 +110,7 @@ def generate_launch_description(): ld.add_action(arg_namespace) ld.add_action(arg_input_ns) ld.add_action(arg_output_ns) + ld.add_action(arg_container) ld.add_action(image_processing_container) ld.add_action(load_composable_nodes) return ld From 8f15de8d31ef6d84a7a93e081bc944e3da9829a2 Mon Sep 17 00:00:00 2001 From: Luis Camero Date: Wed, 7 Feb 2024 14:09:37 -0500 Subject: [PATCH 6/8] Rename node based on input --- .../clearpath_generator_robot/param/sensors.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/clearpath_generator_robot/clearpath_generator_robot/param/sensors.py b/clearpath_generator_robot/clearpath_generator_robot/param/sensors.py index 05ac80b..8db7754 100644 --- a/clearpath_generator_robot/clearpath_generator_robot/param/sensors.py +++ b/clearpath_generator_robot/clearpath_generator_robot/param/sensors.py @@ -70,11 +70,14 @@ def __init__(self, # 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="image_%s" % republisher.TYPE, + 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) From f413f49081aa7d72efd98011eec441837a459dae Mon Sep 17 00:00:00 2001 From: Luis Camero Date: Mon, 12 Feb 2024 11:29:34 -0500 Subject: [PATCH 7/8] Added binning parameters --- clearpath_sensors/config/flir_blackfly.yaml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/clearpath_sensors/config/flir_blackfly.yaml b/clearpath_sensors/config/flir_blackfly.yaml index a95fe93..0452e09 100644 --- a/clearpath_sensors/config/flir_blackfly.yaml +++ b/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 \ No newline at end of file + chunk_enable_timestamp: true From 850c361297bcd9220f8c5226278ff634f280f87b Mon Sep 17 00:00:00 2001 From: Luis Camero Date: Mon, 12 Feb 2024 11:34:18 -0500 Subject: [PATCH 8/8] Added image_transport_plugins to package.xml --- clearpath_sensors/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/clearpath_sensors/package.xml b/clearpath_sensors/package.xml index be52d15..0151793 100644 --- a/clearpath_sensors/package.xml +++ b/clearpath_sensors/package.xml @@ -18,6 +18,7 @@ flir_camera_description flir_camera_msgs image_proc + image_transport_plugins microstrain_inertial_driver nmea_navsat_driver realsense2_camera