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..4dbc11a493 --- /dev/null +++ b/realsense2_camera/examples/dual_camera/rs_dual_camera_launch.py @@ -0,0 +1,107 @@ +# 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': '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/pointcloud.rviz b/realsense2_camera/examples/pointcloud/pointcloud.rviz similarity index 100% rename from realsense2_camera/examples/pointcloud.rviz rename to realsense2_camera/examples/pointcloud/pointcloud.rviz diff --git a/realsense2_camera/examples/rs_d455_pointcloud_launch.py b/realsense2_camera/examples/pointcloud/rs_d455_pointcloud_launch.py similarity index 100% rename from realsense2_camera/examples/rs_d455_pointcloud_launch.py rename to realsense2_camera/examples/pointcloud/rs_d455_pointcloud_launch.py diff --git a/realsense2_camera/examples/rs_pointcloud_launch.py b/realsense2_camera/examples/pointcloud/rs_pointcloud_launch.py similarity index 100% rename from realsense2_camera/examples/rs_pointcloud_launch.py rename to realsense2_camera/examples/pointcloud/rs_pointcloud_launch.py diff --git a/realsense2_camera/examples/urdf_pointcloud.rviz b/realsense2_camera/examples/pointcloud/urdf_pointcloud.rviz similarity index 100% rename from realsense2_camera/examples/urdf_pointcloud.rviz rename to realsense2_camera/examples/pointcloud/urdf_pointcloud.rviz diff --git a/realsense2_camera/examples/rs_dual_camera_launch.py b/realsense2_camera/examples/rs_dual_camera_launch.py deleted file mode 100644 index df1ec0cf77..0000000000 --- a/realsense2_camera/examples/rs_dual_camera_launch.py +++ /dev/null @@ -1,78 +0,0 @@ -# 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 -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'}, - ] - -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 add_node_action(context : LaunchContext): - # dummy static transformation from camera1 to camera2 - node = launch_ros.actions.Node( - package = "tf2_ros", - executable = "static_transform_publisher", - arguments = ["0", "0", "0", "0", "0", "0", - 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=add_node_action) - ]) diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py index 2f93d4de32..60b95d5049 100644 --- a/realsense2_camera/launch/rs_launch.py +++ b/realsense2_camera/launch/rs_launch.py @@ -66,7 +66,9 @@ {'name': 'publish_tf', 'default': 'true', 'description': '[bool] enable/disable publishing static & dynamic TF'}, {'name': 'tf_publish_rate', 'default': '0.0', 'description': '[double] rate in Hz for publishing dynamic TF'}, {'name': 'diagnostics_period', 'default': '0.0', 'description': 'Rate of publishing diagnostics. 0=Disabled'}, - {'name': 'decimation_filter.enable', 'default': 'false', 'description': 'Rate of publishing static_tf'}, + {'name': 'decimation_filter.enable', 'default': 'false', 'description': 'enable decimation filter'}, + {'name': 'spatial_filter.enable', 'default': 'false', 'description': 'enable_spatial_filter'}, + {'name': 'temporal_filter.enable', 'default': 'false', 'description': 'enable_temporal_filter'}, {'name': 'rosbag_filename', 'default': "''", 'description': 'A realsense bagfile to run from as a device'}, {'name': 'depth_module.exposure.1', 'default': '7500', 'description': 'Depth module first exposure value. Used for hdr_merge filter'}, {'name': 'depth_module.gain.1', 'default': '16', 'description': 'Depth module first gain value. Used for hdr_merge filter'}, diff --git a/realsense2_camera/launch/rs_multi_camera_launch.py b/realsense2_camera/launch/rs_multi_camera_launch.py index d564bfffe3..272a280ef0 100644 --- a/realsense2_camera/launch/rs_multi_camera_launch.py +++ b/realsense2_camera/launch/rs_multi_camera_launch.py @@ -47,9 +47,10 @@ 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( + name = "my_static_transform_publisher", package = "tf2_ros", executable = "static_transform_publisher", arguments = ["0", "0", "0", "0", "0", "0", @@ -72,5 +73,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()