PR #2839 from Arun-Prasad: Added ros2 examples

realsense2_camera/CMakeLists.txt

@@ -272,6 +272,12 @@ install(DIRECTORY
     DESTINATION share/${PROJECT_NAME}
     )
 
+# Install example files
+install(DIRECTORY 
+    examples
+    DESTINATION share/${PROJECT_NAME}
+)
+
 # Test
 if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)

realsense2_camera/examples/align_depth/rs_align_depth_launch.py

@@ -0,0 +1,56 @@
+# Copyright 2023 Intel Corporation. All Rights Reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+# DESCRIPTION #
+# ----------- #
+# Use this launch file to launch a device and align depth to color.
+# The Parameters available for definition in the command line for the camera are described in rs_launch.configurable_parameters
+# command line example:
+# ros2 launch realsense2_camera rs_align_depth_launch.py
+
+"""Launch realsense2_camera node."""
+from launch import LaunchDescription
+import launch_ros.actions
+from launch.actions import OpaqueFunction
+from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir
+import sys
+import pathlib
+sys.path.append(str(pathlib.Path(__file__).parent.absolute()))
+import os
+from ament_index_python.packages import get_package_share_directory
+sys.path.append(os.path.join(get_package_share_directory('realsense2_camera'), 'launch'))
+import rs_launch
+
+local_parameters = [{'name': 'camera_name',                  'default': 'camera', 'description': 'camera unique name'},
+                    {'name': 'camera_namespace',             'default': 'camera', 'description': 'camera namespace'},
+                    {'name': 'enable_color',                 'default': 'true', 'description': 'enable color stream'},
+                    {'name': 'enable_depth',                 'default': 'true', 'description': 'enable depth stream'},
+                    {'name': 'align_depth.enable',           'default': 'true', 'description': 'enable align depth filter'},
+                    {'name': 'enable_sync',                  'default': 'true', 'description': 'enable sync mode'},
+                   ]
+
+def set_configurable_parameters(local_params):
+    return dict([(param['name'], LaunchConfiguration(param['name'])) for param in local_params])
+
+
+def generate_launch_description():
+    params = rs_launch.configurable_parameters
+    return LaunchDescription(
+        rs_launch.declare_configurable_parameters(local_parameters) +
+        rs_launch.declare_configurable_parameters(params) + 
+        [
+        OpaqueFunction(function=rs_launch.launch_setup,
+                kwargs = {'params' : set_configurable_parameters(params)}
+        )
+    ])

realsense2_camera/examples/dual_camera/dual_camera_pointcloud.rviz

@@ -0,0 +1,194 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 0
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Grid1
+        - /PointCloud21
+        - /PointCloud22
+      Splitter Ratio: 0.5
+    Tree Height: 865
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: RGB8
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /camera1/depth/color/points
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: RGB8
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /camera2/depth/color/points
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: camera1_link
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /move_base_simple/goal
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 8.93685531616211
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: -0.18814913928508759
+        Y: -0.17941315472126007
+        Z: 0.14549313485622406
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: -1.5697963237762451
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz_default_plugins)
+      Yaw: 4.730405330657959
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1016
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 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
+  Selection:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1846
+  X: 74
+  Y: 27

realsense2_camera/examples/dual_camera/rs_dual_camera_launch.py

@@ -0,0 +1,109 @@
+# Copyright 2023 Intel Corporation. All Rights Reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+# DESCRIPTION #
+# ----------- #
+# Use this launch file to launch 2 devices.
+# The Parameters available for definition in the command line for each camera are described in rs_launch.configurable_parameters
+# For each device, the parameter name was changed to include an index.
+# For example: to set camera_name for device1 set parameter camera_name1.
+# command line example:
+# ros2 launch realsense2_camera rs_dual_camera_launch.py camera_name1:=D400 device_type2:=l5. device_type1:=d4..
+
+"""Launch realsense2_camera node."""
+import copy
+from launch import LaunchDescription, LaunchContext
+import launch_ros.actions
+from launch.actions import OpaqueFunction
+from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir
+import sys
+import pathlib
+sys.path.append(str(pathlib.Path(__file__).parent.absolute()))
+import os
+from ament_index_python.packages import get_package_share_directory
+sys.path.append(os.path.join(get_package_share_directory('realsense2_camera'), 'launch'))
+import rs_launch
+
+local_parameters = [{'name': 'camera_name1',            'default': 'camera1', 'description': 'camera unique name'},
+                    {'name': 'camera_name2',            'default': 'camera2', 'description': 'camera unique name'},
+                    {'name': 'camera_namespace1',       'default': 'camera1', 'description': 'camera1 namespace'},
+                    {'name': 'camera_namespace2',       'default': 'camera2', 'description': 'camera2 namespace'},
+                    {'name': 'enable_color1',           'default': 'true', 'description': 'enable color stream'},
+                    {'name': 'enable_color2',           'default': 'true', 'description': 'enable color stream'},
+                    {'name': 'enable_depth1',           'default': 'true', 'description': 'enable depth stream'},
+                    {'name': 'enable_depth2',           'default': 'true', 'description': 'enable depth stream'},
+                    {'name': 'pointcloud.enable1',      'default': 'true', 'description': 'enable pointcloud'},
+                    {'name': 'pointcloud.enable2',      'default': 'true', 'description': 'enable pointcloud'},
+                    {'name': 'spatial_filter.enable1',  'default': 'true', 'description': 'enable_spatial_filter'},
+                    {'name': 'spatial_filter.enable2',  'default': 'true', 'description': 'enable_spatial_filter'},
+                    {'name': 'temporal_filter.enable1', 'default': 'true', 'description': 'enable_temporal_filter'},
+                    {'name': 'temporal_filter.enable2', 'default': 'true', 'description': 'enable_temporal_filter'},
+                    {'name': 'tf.translation.x',        'default': '0.0', 'description': 'x'},
+                    {'name': 'tf.translation.y',        'default': '0.0', 'description': 'y'},
+                    {'name': 'tf.translation.z',        'default': '0.0', 'description': 'z'},
+                    {'name': 'tf.rotation.yaw',         'default': '0.0', 'description': 'yaw'},
+                    {'name': 'tf.rotation.pitch',       'default': '0.0', 'description': 'pitch'},
+                    {'name': 'tf.rotation.roll',        'default': '0.0', 'description': 'roll'},
+                   ]
+
+def set_configurable_parameters(local_params):
+    return dict([(param['original_name'], LaunchConfiguration(param['name'])) for param in local_params])
+
+def duplicate_params(general_params, posix):
+    local_params = copy.deepcopy(general_params)
+    for param in local_params:
+        param['original_name'] = param['name']
+        param['name'] += posix
+    return local_params
+
+def launch_static_transform_publisher_node(context : LaunchContext):
+    # dummy static transformation from camera1 to camera2
+    node = launch_ros.actions.Node(
+            name = "my_static_transform_publisher",
+            package = "tf2_ros",
+            executable = "static_transform_publisher",
+            arguments = [context.launch_configurations['tf.translation.x'],
+                         context.launch_configurations['tf.translation.y'],
+                         context.launch_configurations['tf.translation.z'],
+                         context.launch_configurations['tf.rotation.yaw'],
+                         context.launch_configurations['tf.rotation.pitch'],
+                         context.launch_configurations['tf.rotation.roll'],
+                         context.launch_configurations['camera_name1'] + "_link",
+                         context.launch_configurations['camera_name2'] + "_link"]
+    )
+    return [node]
+
+def generate_launch_description():
+    params1 = duplicate_params(rs_launch.configurable_parameters, '1')
+    params2 = duplicate_params(rs_launch.configurable_parameters, '2')
+    return LaunchDescription(
+        rs_launch.declare_configurable_parameters(local_parameters) +
+        rs_launch.declare_configurable_parameters(params1) + 
+        rs_launch.declare_configurable_parameters(params2) + 
+        [
+        OpaqueFunction(function=rs_launch.launch_setup,
+                       kwargs = {'params'           : set_configurable_parameters(params1),
+                                 'param_name_suffix': '1'}),
+        OpaqueFunction(function=rs_launch.launch_setup,
+                       kwargs = {'params'           : set_configurable_parameters(params2),
+                                 'param_name_suffix': '2'}),
+        OpaqueFunction(function=launch_static_transform_publisher_node),
+        launch_ros.actions.Node(
+            package='rviz2',
+            namespace='',
+            executable='rviz2',
+            name='rviz2',
+            arguments=['-d', [ThisLaunchFileDir(), '/dual_camera_pointcloud.rviz']]
+        )
+    ])