diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 4eeda43bdf..d6a2761a23 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/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) diff --git a/realsense2_camera/examples/align_depth/rs_align_depth_launch.py b/realsense2_camera/examples/align_depth/rs_align_depth_launch.py new file mode 100644 index 0000000000..cc579eada8 --- /dev/null +++ b/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)} + ) + ]) diff --git a/realsense2_camera/examples/dual_camera/dual_camera_pointcloud.rviz b/realsense2_camera/examples/dual_camera/dual_camera_pointcloud.rviz new file mode 100644 index 0000000000..3469eda9d8 --- /dev/null +++ b/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: + 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: + 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 diff --git a/realsense2_camera/examples/dual_camera/rs_dual_camera_launch.py b/realsense2_camera/examples/dual_camera/rs_dual_camera_launch.py new file mode 100644 index 0000000000..4a4db4a275 --- /dev/null +++ b/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']] + ) + ]) diff --git a/realsense2_camera/examples/launch_from_rosbag/rs_launch_from_rosbag.py b/realsense2_camera/examples/launch_from_rosbag/rs_launch_from_rosbag.py new file mode 100644 index 0000000000..ad9b76667b --- /dev/null +++ b/realsense2_camera/examples/launch_from_rosbag/rs_launch_from_rosbag.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 from rosbag file. +# 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_launch_from_rosbag.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_depth', 'default': 'true', 'description': 'enable depth stream'}, + {'name': 'enable_gyro', 'default': 'true', 'description': "'enable gyro stream'"}, + {'name': 'enable_accel', 'default': 'true', 'description': "'enable accel stream'"}, + {'name': 'rosbag_filename', 'default': [ThisLaunchFileDir(), "/D435i_Depth_and_IMU_Stands_still.bag"], 'description': 'A realsense bagfile to run from as a device'}, + ] + +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)} + ) + ]) diff --git a/realsense2_camera/examples/launch_params_from_file/config.yaml b/realsense2_camera/examples/launch_params_from_file/config.yaml new file mode 100644 index 0000000000..8b8bcc1709 --- /dev/null +++ b/realsense2_camera/examples/launch_params_from_file/config.yaml @@ -0,0 +1,6 @@ +enable_color: true +rgb_camera.profile: 1280x720x15 +enable_depth: true +align_depth.enable: true +enable_sync: true + diff --git a/realsense2_camera/examples/launch_params_from_file/rs_launch_get_params_from_yaml.py b/realsense2_camera/examples/launch_params_from_file/rs_launch_get_params_from_yaml.py new file mode 100644 index 0000000000..a6b5582039 --- /dev/null +++ b/realsense2_camera/examples/launch_params_from_file/rs_launch_get_params_from_yaml.py @@ -0,0 +1,53 @@ +# 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 get the params from a YAML file. +# 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_launch_get_params_from_yaml.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': 'config_file', 'default': [ThisLaunchFileDir(), "/config.yaml"], 'description': 'yaml config file'}, + ] + +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)} + ) + ]) diff --git a/realsense2_camera/examples/pointcloud/pointcloud.rviz b/realsense2_camera/examples/pointcloud/pointcloud.rviz new file mode 100644 index 0000000000..055431f228 --- /dev/null +++ b/realsense2_camera/examples/pointcloud/pointcloud.rviz @@ -0,0 +1,185 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Grid1 + - /PointCloud21 + - /Image1 + Splitter Ratio: 0.5 + Tree Height: 222 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Nav Goal1 + - /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: + 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 + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /camera/depth/color/points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 10 + Topic: /camera/color/image_raw + Unreliable: false + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 10 + Topic: /camera/depth/image_rect_raw + Unreliable: false + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 10 + Topic: /camera/infra1/image_rect_raw + Unreliable: false + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 10 + Topic: /camera/infra2/image_rect_raw + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: camera_depth_optical_frame + 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 + Topic: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: /move_base_simple/goal + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 1.0510121583938599 + 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: + Value: Orbit (rviz_default_plugins) + Yaw: 4.730405330657959 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1025 + Hide Left Dock: false + Hide Right Dock: true + Image: + collapsed: false + QMainWindow State: 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 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1853 + X: 67 + Y: 27 diff --git a/realsense2_camera/examples/pointcloud/rs_d455_pointcloud_launch.py b/realsense2_camera/examples/pointcloud/rs_d455_pointcloud_launch.py new file mode 100644 index 0000000000..4f013b1c87 --- /dev/null +++ b/realsense2_camera/examples/pointcloud/rs_d455_pointcloud_launch.py @@ -0,0 +1,92 @@ +# 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 visualize pointcloud along with the camera model D455. +# 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_d455_pointcloud_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 os +import sys +import pathlib +import xacro +import tempfile +from ament_index_python.packages import get_package_share_directory +sys.path.append(str(pathlib.Path(__file__).parent.absolute())) +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': 'device_type', 'default': "d455", 'description': 'choose device by type'}, + {'name': 'enable_color', 'default': 'true', 'description': 'enable color stream'}, + {'name': 'enable_depth', 'default': 'true', 'description': 'enable depth stream'}, + {'name': 'pointcloud.enable', 'default': 'true', 'description': 'enable pointcloud'}, + ] + +def to_urdf(xacro_path, parameters=None): + """Convert the given xacro file to URDF file. + * xacro_path -- the path to the xacro file + * parameters -- to be used when xacro file is parsed. + """ + urdf_path = tempfile.mktemp(prefix="%s_" % os.path.basename(xacro_path)) + + # open and process file + doc = xacro.process_file(xacro_path, mappings=parameters) + # open the output file + out = xacro.open_output(urdf_path) + out.write(doc.toprettyxml(indent=' ')) + + return urdf_path + +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 + xacro_path = os.path.join(get_package_share_directory('realsense2_description'), 'urdf', 'test_d455_camera.urdf.xacro') + urdf = to_urdf(xacro_path, {'use_nominal_extrinsics': 'true', 'add_plug': 'true'}) + 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)} + ), + launch_ros.actions.Node( + package='rviz2', + namespace='', + executable='rviz2', + name='rviz2', + arguments=['-d', [ThisLaunchFileDir(), '/urdf_pointcloud.rviz']], + output='screen', + parameters=[{'use_sim_time': False}] + ), + launch_ros.actions.Node( + name='model_node', + package='robot_state_publisher', + executable='robot_state_publisher', + namespace='', + output='screen', + arguments=[urdf] + ) + ]) diff --git a/realsense2_camera/examples/pointcloud/rs_pointcloud_launch.py b/realsense2_camera/examples/pointcloud/rs_pointcloud_launch.py new file mode 100644 index 0000000000..f0a5b541e0 --- /dev/null +++ b/realsense2_camera/examples/pointcloud/rs_pointcloud_launch.py @@ -0,0 +1,62 @@ +# 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 visualize pointcloud. +# 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_pointcloud_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': 'pointcloud.enable', 'default': 'true', 'description': 'enable pointcloud'}, + ] + +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)} + ), + launch_ros.actions.Node( + package='rviz2', + namespace='', + executable='rviz2', + name='rviz2', + arguments=['-d', [ThisLaunchFileDir(), '/pointcloud.rviz']] + ) + ]) diff --git a/realsense2_camera/examples/pointcloud/urdf_pointcloud.rviz b/realsense2_camera/examples/pointcloud/urdf_pointcloud.rviz new file mode 100644 index 0000000000..500e60114e --- /dev/null +++ b/realsense2_camera/examples/pointcloud/urdf_pointcloud.rviz @@ -0,0 +1,363 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1 + - /RobotModel1/Description Topic1 + - /TF1 + - /TF1/Frames1 + - /Image1 + - /Image2 + Splitter Ratio: 0.6360294222831726 + Tree Height: 362 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /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: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_accel_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_accel_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_bottom_screw_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_color_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_color_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_gyro_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_gyro_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_imu_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_infra1_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_infra1_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_infra2_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_infra2_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_usb_plug_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + base_link: + Value: true + camera_accel_frame: + Value: true + camera_accel_optical_frame: + Value: true + camera_bottom_screw_frame: + Value: true + camera_color_frame: + Value: true + camera_color_optical_frame: + Value: true + camera_depth_frame: + Value: true + camera_depth_optical_frame: + Value: true + camera_gyro_frame: + Value: true + camera_gyro_optical_frame: + Value: true + camera_imu_optical_frame: + Value: true + camera_infra1_frame: + Value: true + camera_infra1_optical_frame: + Value: true + camera_infra2_frame: + Value: true + camera_infra2_optical_frame: + Value: true + camera_link: + Value: true + camera_usb_plug_link: + Value: true + Marker Scale: 0.10000000149011612 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + base_link: + camera_bottom_screw_frame: + camera_link: + camera_accel_frame: + camera_accel_optical_frame: + {} + camera_color_frame: + camera_color_optical_frame: + {} + camera_depth_frame: + camera_depth_optical_frame: + {} + camera_gyro_frame: + camera_gyro_optical_frame: + camera_imu_optical_frame: + {} + camera_infra1_frame: + camera_infra1_optical_frame: + {} + camera_infra2_frame: + camera_infra2_optical_frame: + {} + camera_usb_plug_link: + {} + Update Interval: 0 + 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: /camera/depth/color/points + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera/color/image_raw + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera/depth/image_rect_raw + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - 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: /goal_pose + - 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: 3.9530911445617676 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.0008243769989348948 + Y: -0.00015415334200952202 + Z: 0.0003343875869177282 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.2247962951660156 + Target Frame: + Value: Orbit (rviz) + Yaw: 3.343583583831787 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1016 + Hide Left Dock: false + Hide Right Dock: false + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000001560000039efc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001f5000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000238000000b00000002800fffffffb0000000a0049006d00610067006501000002ee000000ed0000002800ffffff000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004c50000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1846 + X: 74 + Y: 27 diff --git a/realsense2_camera/launch/rs_multi_camera_launch.py b/realsense2_camera/launch/rs_multi_camera_launch.py index 5cb3ea5246..782b802c55 100644 --- a/realsense2_camera/launch/rs_multi_camera_launch.py +++ b/realsense2_camera/launch/rs_multi_camera_launch.py @@ -49,7 +49,7 @@ def duplicate_params(general_params, posix): param['name'] += posix return local_params -def add_node_action(context : LaunchContext): +def launch_static_transform_publisher_node(context : LaunchContext): # dummy static transformation from camera1 to camera2 node = launch_ros.actions.Node( package = "tf2_ros", @@ -74,5 +74,5 @@ def generate_launch_description(): OpaqueFunction(function=rs_launch.launch_setup, kwargs = {'params' : set_configurable_parameters(params2), 'param_name_suffix': '2'}), - OpaqueFunction(function=add_node_action) + OpaqueFunction(function=launch_static_transform_publisher_node) ]) diff --git a/realsense2_camera/scripts/set_cams_transforms.py b/realsense2_camera/scripts/set_cams_transforms.py index aa0186cac7..ecf466ac27 100644 --- a/realsense2_camera/scripts/set_cams_transforms.py +++ b/realsense2_camera/scripts/set_cams_transforms.py @@ -12,18 +12,18 @@ # See the License for the specific language governing permissions and # limitations under the License. -import rospy +import rclpy +from rclpy.node import Node import sys -import tf -import tf2_ros import geometry_msgs.msg import termios import tty import os -import time import math import json +from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster +from tf_transformations import quaternion_from_euler def getch(): @@ -46,9 +46,9 @@ def print_status(status): sys.stdout.write('%-8s%-8s%-8s%-40s\r' % (status['mode'], status[status['mode']]['value'], status[status['mode']]['step'], status['message'])) -def publish_status(broadcaster, status): +def publish_status(node, broadcaster, status): static_transformStamped = geometry_msgs.msg.TransformStamped() - static_transformStamped.header.stamp = rospy.Time.now() + static_transformStamped.header.stamp = node.get_clock().now().to_msg() static_transformStamped.header.frame_id = from_cam static_transformStamped.child_frame_id = to_cam @@ -56,7 +56,7 @@ def publish_status(broadcaster, status): static_transformStamped.transform.translation.y = status['y']['value'] static_transformStamped.transform.translation.z = status['z']['value'] - quat = tf.transformations.quaternion_from_euler(math.radians(status['roll']['value']), + quat = quaternion_from_euler(math.radians(status['roll']['value']), math.radians(status['pitch']['value']), math.radians(status['azimuth']['value'])) static_transformStamped.transform.rotation.x = quat[0] @@ -68,24 +68,22 @@ def publish_status(broadcaster, status): if __name__ == '__main__': if len(sys.argv) < 3: - print 'USAGE:' - print 'set_cams_transforms.py from_cam to_cam x y z azimuth pitch roll' - print 'x, y, z: in meters' - print 'azimuth, pitch, roll: in degrees' - print - print 'If parameters are not given read last used parameters.' - print - print '[OPTIONS]' - print '--file : if given, default values are loaded from file' + print ('USAGE:') + print ('set_cams_transforms.py from_cam to_cam x y z azimuth pitch roll') + print ('x, y, z: in meters') + print ('azimuth, pitch, roll: in degrees') + print ('If parameters are not given read last used parameters.') + print ('[OPTIONS]') + print ('--file : if given, default values are loaded from file') sys.exit(-1) from_cam, to_cam = sys.argv[1:3] try: filename = sys.argv[sys.argv.index('--file')+1] - print 'Using file %s' % os.path.abspath(filename) + print ('Using file %s' % os.path.abspath(filename)) except: filename = os.path.join(os.path.dirname(__file__), '_set_cams_info_file.txt') - print 'Using default file %s' % os.path.abspath(filename) + print ('Using default file %s' % os.path.abspath(filename)) if len(sys.argv) >= 9: x, y, z, yaw, pitch, roll = [float(arg) for arg in sys.argv[3:10]] @@ -97,37 +95,37 @@ def publish_status(broadcaster, status): 'pitch': {'value': pitch, 'step': 1}, 'roll': {'value': roll, 'step': 1}, 'message': ''} - print 'Use given initial values.' + print ('Use given initial values.') else: try: status = json.load(open(filename, 'r')) - print 'Read initial values from file.' + print ('Read initial values from file.') except IOError as e: - print 'Failed reading initial parameters from file %s' % filename - print 'Initial parameters must be given for initial run or if an un-initialized file has been given.' + print ('Failed reading initial parameters from file %s' % filename) + print ('Initial parameters must be given for initial run or if an un-initialized file has been given.') sys.exit(-1) - rospy.init_node('my_static_tf2_broadcaster') - broadcaster = tf2_ros.StaticTransformBroadcaster() + rclpy.init() + node = Node('my_static_tf2_broadcaster') + #rospy.init_node('my_static_tf2_broadcaster') + broadcaster = StaticTransformBroadcaster(node) print - print 'Press the following keys to change mode: x, y, z, (a)zimuth, (p)itch, (r)oll' - print 'For each mode, press 6 to increase by step and 4 to decrease' - print 'Press + to multiply step by 2 or - to divide' - print - print 'Press Q to quit' - print + print ('Press the following keys to change mode: x, y, z, (a)zimuth, (p)itch, (r)oll') + print ('For each mode, press 6 to increase by step and 4 to decrease') + print ('Press + to multiply step by 2 or - to divide') + print ('Press Q to quit') status_keys = [key[0] for key in status.keys()] - print '%-8s%-8s%-8s%s' % ('Mode', 'value', 'step', 'message') + print ('%-8s%-8s%-8s%s' % ('Mode', 'value', 'step', 'message')) print_status(status) - publish_status(broadcaster, status) + publish_status(node, broadcaster, status) while True: kk = getch() status['message'] = '' try: key_idx = status_keys.index(kk) - status['mode'] = status.keys()[key_idx] + status['mode'] = list(status.keys())[key_idx] except ValueError as e: if kk.upper() == 'Q': sys.stdout.write('\n') @@ -144,7 +142,7 @@ def publish_status(broadcaster, status): status['message'] = 'Invalid key:' + kk print_status(status) - publish_status(broadcaster, status) + publish_status(node, broadcaster, status) json.dump(status, open(filename, 'w'), indent=4) #rospy.spin()