From a3e046a2718e3dc0f3b748ad2e2533cf67f4209c Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Wed, 9 Aug 2023 18:40:44 +0530 Subject: [PATCH 01/76] Added pointcloud and dual camera examples --- realsense2_camera/CMakeLists.txt | 6 + .../dual_camera/dual_camera_pointcloud.rviz | 194 ++++++++++ .../dual_camera/rs_dual_camera_launch.py | 109 ++++++ .../examples/pointcloud/pointcloud.rviz | 185 +++++++++ .../pointcloud/rs_d455_pointcloud_launch.py | 91 +++++ .../pointcloud/rs_pointcloud_launch.py | 61 +++ .../examples/pointcloud/urdf_pointcloud.rviz | 363 ++++++++++++++++++ realsense2_camera/launch/rs_launch.py | 4 +- .../launch/rs_multi_camera_launch.py | 4 +- .../scripts/set_cams_transforms.py | 66 ++-- 10 files changed, 1046 insertions(+), 37 deletions(-) create mode 100644 realsense2_camera/examples/dual_camera/dual_camera_pointcloud.rviz create mode 100644 realsense2_camera/examples/dual_camera/rs_dual_camera_launch.py create mode 100644 realsense2_camera/examples/pointcloud/pointcloud.rviz create mode 100644 realsense2_camera/examples/pointcloud/rs_d455_pointcloud_launch.py create mode 100644 realsense2_camera/examples/pointcloud/rs_pointcloud_launch.py create mode 100644 realsense2_camera/examples/pointcloud/urdf_pointcloud.rviz 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/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/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..e4e908d4a3 --- /dev/null +++ b/realsense2_camera/examples/pointcloud/rs_d455_pointcloud_launch.py @@ -0,0 +1,91 @@ +# 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': '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..d1e464f9be --- /dev/null +++ b/realsense2_camera/examples/pointcloud/rs_pointcloud_launch.py @@ -0,0 +1,61 @@ +# 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': '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_launch.py b/realsense2_camera/launch/rs_launch.py index 9f56745988..3231fabf97 100644 --- a/realsense2_camera/launch/rs_launch.py +++ b/realsense2_camera/launch/rs_launch.py @@ -68,7 +68,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 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() From c785bf52caaab2babc444018ab3b0e567b5d77db Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Tue, 8 Aug 2023 00:19:17 +0000 Subject: [PATCH 02/76] support color and depth/ir formats --- .../include/base_realsense_node.h | 18 +-- realsense2_camera/src/base_realsense_node.cpp | 106 +++++++++++++----- 2 files changed, 92 insertions(+), 32 deletions(-) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index d6f0a1b97c..b414473e31 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -205,15 +205,15 @@ namespace realsense2_camera void publishDynamicTransforms(); void publishPointCloud(rs2::points f, const rclcpp::Time& t, const rs2::frameset& frameset); Extrinsics rsExtrinsicsToMsg(const rs2_extrinsics& extrinsics) const; - IMUInfo getImuInfo(const rs2::stream_profile& profile); - + void initializeFormatsMaps(); + void fillMessageImage( const cv::Mat& cv_matrix_image, const stream_index_pair& stream, unsigned int width, unsigned int height, - unsigned int bpp, + const rs2_format& stream_format, const rclcpp::Time& t, sensor_msgs::msg::Image* img_msg_ptr); @@ -222,7 +222,6 @@ namespace realsense2_camera std::map& images, unsigned int width, unsigned int height, - unsigned int bpp, const stream_index_pair& stream); void publishFrame( @@ -234,7 +233,12 @@ namespace realsense2_camera const std::map>& image_publishers, const bool is_publishMetadata = true); - void publishRGBD(const cv::Mat& rgb_cv_matrix, const cv::Mat& depth_cv_matrix, const rclcpp::Time& t); + void publishRGBD( + const cv::Mat& rgb_cv_matrix, + const rs2_format& color_format, + const cv::Mat& depth_cv_matrix, + const rs2_format& depth_format, + const rclcpp::Time& t); void publishMetadata(rs2::frame f, const rclcpp::Time& header_time, const std::string& frame_id); @@ -293,14 +297,14 @@ namespace realsense2_camera std::map::SharedPtr> _imu_publishers; std::shared_ptr> _odom_publisher; std::shared_ptr _synced_imu_publisher; - std::map _image_formats; std::map::SharedPtr> _info_publishers; std::map::SharedPtr> _metadata_publishers; std::map::SharedPtr> _imu_info_publishers; std::map::SharedPtr> _extrinsics_publishers; rclcpp::Publisher::SharedPtr _rgbd_publisher; std::map _images; - std::map _encoding; + std::map rs_format_to_ros_format; + std::map rs_format_to_cv_format; std::map _camera_info; std::atomic_bool _is_initialized_time_base; diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index ae6b50d33c..729ff66fba 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -123,13 +123,7 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node, ROS_INFO("Intra-Process communication enabled"); } - _image_formats[1] = CV_8UC1; // CVBridge type - _image_formats[2] = CV_16UC1; // CVBridge type - _image_formats[3] = CV_8UC3; // CVBridge type - _encoding[1] = sensor_msgs::image_encodings::MONO8; // ROS message type - _encoding[2] = sensor_msgs::image_encodings::TYPE_16UC1; // ROS message type - _encoding[3] = sensor_msgs::image_encodings::RGB8; // ROS message type - + initializeFormatsMaps(); _monitor_options = {RS2_OPTION_ASIC_TEMPERATURE, RS2_OPTION_PROJECTOR_TEMPERATURE}; } @@ -171,6 +165,45 @@ void BaseRealSenseNode::publishTopics() ROS_INFO_STREAM("RealSense Node Is Up!"); } +void BaseRealSenseNode::initializeFormatsMaps() +{ + // from rs2_format to OpenCV format + // https://docs.opencv.org/3.4/d1/d1b/group__core__hal__interface.html + // https://docs.opencv.org/2.4/modules/core/doc/basic_structures.html + // CV_{U|S|F}C() + // where U is unsigned integer type, S is signed integer type, and F is float type. + // For example, CV_8UC1 means a 8-bit single-channel array, + // CV_32FC2 means a 2-channel (complex) floating-point array, and so on. + rs_format_to_cv_format[RS2_FORMAT_Y8] = CV_8UC1; + rs_format_to_cv_format[RS2_FORMAT_Y16] = CV_16UC1; + rs_format_to_cv_format[RS2_FORMAT_Z16] = CV_16UC1; + rs_format_to_cv_format[RS2_FORMAT_RGB8] = CV_8UC3; + rs_format_to_cv_format[RS2_FORMAT_BGR8] = CV_8UC3; + rs_format_to_cv_format[RS2_FORMAT_RGBA8] = CV_8UC4; + rs_format_to_cv_format[RS2_FORMAT_BGRA8] = CV_8UC4; + rs_format_to_cv_format[RS2_FORMAT_YUYV] = CV_16UC3; + rs_format_to_cv_format[RS2_FORMAT_M420] = CV_16UC3; + rs_format_to_cv_format[RS2_FORMAT_RAW8] = CV_8UC1; + rs_format_to_cv_format[RS2_FORMAT_RAW10] = CV_16UC1; + rs_format_to_cv_format[RS2_FORMAT_RAW16] = CV_16UC1; + + // from rs2_format to ROS2 image msg encoding (format) + // http://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/Image.html + // http://docs.ros.org/en/jade/api/sensor_msgs/html/image__encodings_8h_source.html + rs_format_to_ros_format[RS2_FORMAT_Y8] = sensor_msgs::image_encodings::MONO8; + rs_format_to_ros_format[RS2_FORMAT_Y16] = sensor_msgs::image_encodings::MONO16; + rs_format_to_ros_format[RS2_FORMAT_Z16] = sensor_msgs::image_encodings::TYPE_16UC1; + rs_format_to_ros_format[RS2_FORMAT_RGB8] = sensor_msgs::image_encodings::RGB8; + rs_format_to_ros_format[RS2_FORMAT_BGR8] = sensor_msgs::image_encodings::BGR8; + rs_format_to_ros_format[RS2_FORMAT_RGBA8] = sensor_msgs::image_encodings::RGBA8; + rs_format_to_ros_format[RS2_FORMAT_BGRA8] = sensor_msgs::image_encodings::BGRA8; + rs_format_to_ros_format[RS2_FORMAT_YUYV] = sensor_msgs::image_encodings::YUV422; + //rs_format_to_ros_format[RS2_FORMAT_M420] = not supported in ROS2 image msg yet + rs_format_to_ros_format[RS2_FORMAT_RAW8] = sensor_msgs::image_encodings::TYPE_8UC1; + rs_format_to_ros_format[RS2_FORMAT_RAW10] = sensor_msgs::image_encodings::TYPE_16UC1; + rs_format_to_ros_format[RS2_FORMAT_RAW16] = sensor_msgs::image_encodings::TYPE_16UC1; +} + void BaseRealSenseNode::setupFilters() { _filters.push_back(std::make_shared(std::make_shared(), _parameters, _logger)); @@ -220,7 +253,13 @@ cv::Mat& BaseRealSenseNode::fix_depth_scale(const cv::Mat& from_image, cv::Mat& to_image.create(from_image.rows, from_image.cols, from_image.type()); } - CV_Assert(from_image.depth() == _image_formats[2]); + // Samer to fix + // ROS_INFO_STREAM(from_image.depth()); + // ROS_INFO_STREAM(from_image.channels()); + // ROS_INFO_STREAM(CV_MAKETYPE(from_image.depth(),from_image.channels())); + // ROS_INFO_STREAM(rs_format_to_cv_format[RS2_FORMAT_Z16]); + // CV_Assert(CV_MAKETYPE(from_image.depth(),from_image.channels()) == rs_format_to_cv_format[RS2_FORMAT_Z16]); + // CV_Assert(3 == 1); int nRows = from_image.rows; int nCols = from_image.cols; @@ -555,7 +594,9 @@ void BaseRealSenseNode::frame_callback(rs2::frame frame) // On this line we already know original_depth_frame is valid. if(_enable_rgbd && original_color_frame) { - publishRGBD(_images[COLOR], _depth_aligned_image[COLOR], t); + auto color_format = original_color_frame.get_profile().format(); + auto depth_format = original_depth_frame.get_profile().format(); + publishRGBD(_images[COLOR], color_format, _depth_aligned_image[COLOR], depth_format, t); } } } @@ -830,12 +871,16 @@ void BaseRealSenseNode::fillMessageImage( const stream_index_pair& stream, unsigned int width, unsigned int height, - unsigned int bpp, + const rs2_format& stream_format, const rclcpp::Time& t, sensor_msgs::msg::Image* img_msg_ptr) { + if(rs_format_to_ros_format.find(stream_format) == rs_format_to_ros_format.end()) + { + ROS_ERROR_STREAM("Format " << rs2_format_to_string(stream_format) << " is not supported in ROS2 image messages"); + } // Convert the CV::Mat into a ROS image message (1 copy is done here) - cv_bridge::CvImage(std_msgs::msg::Header(), _encoding.at(bpp), cv_matrix_image).toImageMsg(*img_msg_ptr); + cv_bridge::CvImage(std_msgs::msg::Header(), rs_format_to_ros_format[stream_format], cv_matrix_image).toImageMsg(*img_msg_ptr); // Convert OpenCV Mat to ROS Image img_msg_ptr->header.frame_id = OPTICAL_FRAME_ID(stream); @@ -843,7 +888,7 @@ void BaseRealSenseNode::fillMessageImage( img_msg_ptr->height = height; img_msg_ptr->width = width; img_msg_ptr->is_bigendian = false; - img_msg_ptr->step = width * bpp; + img_msg_ptr->step = width * cv_matrix_image.elemSize(); } cv::Mat& BaseRealSenseNode::getCVMatImage( @@ -851,13 +896,21 @@ cv::Mat& BaseRealSenseNode::getCVMatImage( std::map& images, unsigned int width, unsigned int height, - unsigned int bpp, const stream_index_pair& stream) { auto& image = images[stream]; - if (image.size() != cv::Size(width, height) || image.depth() != _image_formats[bpp]) + auto stream_format = frame.get_profile().format(); + + if (rs_format_to_cv_format.find(stream_format) == rs_format_to_cv_format.end()) + { + ROS_ERROR_STREAM("Format " << rs2_format_to_string(stream_format) << " is not supported in realsense2_camera node"); + } + // we try to reduce image creation as much we can, so we check if the same image structure + // was already created before, and we fill this image next with the frame data + // image.create() should be called once per __ + if (image.size() != cv::Size(width, height) || CV_MAKETYPE(image.depth(), image.channels()) != rs_format_to_cv_format[stream_format]) { - image.create(height, width, _image_formats[bpp]); + image.create(height, width, rs_format_to_cv_format[stream_format]); } image.data = (uint8_t*)frame.get_data(); @@ -883,13 +936,13 @@ void BaseRealSenseNode::publishFrame( ROS_DEBUG("publishFrame(...)"); unsigned int width = 0; unsigned int height = 0; - unsigned int bpp = 1; + auto stream_format = f.get_profile().format(); if (f.is()) { auto timage = f.as(); width = timage.get_width(); height = timage.get_height(); - bpp = timage.get_bytes_per_pixel(); + stream_format = timage.get_profile().format(); } // Publish stream image @@ -901,7 +954,7 @@ void BaseRealSenseNode::publishFrame( // if rgbd has subscribers we fetch the CV image here if (_rgbd_publisher && 0 != _rgbd_publisher->get_subscription_count()) { - image_cv_matrix = getCVMatImage(f, images, width, height, bpp, stream); + image_cv_matrix = getCVMatImage(f, images, width, height, stream); } // if depth/color has subscribers, ask first if rgbd already fetched @@ -910,13 +963,13 @@ void BaseRealSenseNode::publishFrame( { if(image_cv_matrix.empty()) { - image_cv_matrix = getCVMatImage(f, images, width, height, bpp, stream); + image_cv_matrix = getCVMatImage(f, images, width, height, stream); } // Prepare image topic to be published // We use UniquePtr for allow intra-process publish when subscribers of that type are available sensor_msgs::msg::Image::UniquePtr img_msg_ptr(new sensor_msgs::msg::Image()); - fillMessageImage(image_cv_matrix, stream, width, height, bpp, t, img_msg_ptr.get()); + fillMessageImage(image_cv_matrix, stream, width, height, stream_format, t, img_msg_ptr.get()); if (!img_msg_ptr) { ROS_ERROR("sensor image message allocation failed, frame was dropped"); @@ -977,22 +1030,25 @@ void BaseRealSenseNode::publishFrame( } -void BaseRealSenseNode::publishRGBD(const cv::Mat& rgb_cv_matrix, const cv::Mat& depth_cv_matrix, const rclcpp::Time& t) +void BaseRealSenseNode::publishRGBD( + const cv::Mat& rgb_cv_matrix, + const rs2_format& color_format, + const cv::Mat& depth_cv_matrix, + const rs2_format& depth_format, + const rclcpp::Time& t) { if (_rgbd_publisher && 0 != _rgbd_publisher->get_subscription_count()) { ROS_DEBUG_STREAM("Publishing RGBD message"); unsigned int rgb_width = rgb_cv_matrix.size().width; unsigned int rgb_height = rgb_cv_matrix.size().height; - unsigned int rgb_bpp = rgb_cv_matrix.elemSize(); unsigned int depth_width = depth_cv_matrix.size().width; unsigned int depth_height = depth_cv_matrix.size().height; - unsigned int depth_bpp = depth_cv_matrix.elemSize(); realsense2_camera_msgs::msg::RGBD::UniquePtr msg(new realsense2_camera_msgs::msg::RGBD()); - fillMessageImage(rgb_cv_matrix, COLOR, rgb_width, rgb_height, rgb_bpp, t, &msg->rgb); - fillMessageImage(depth_cv_matrix, DEPTH, depth_width, depth_height, depth_bpp, t, &msg->depth); + fillMessageImage(rgb_cv_matrix, COLOR, rgb_width, rgb_height, color_format, t, &msg->rgb); + fillMessageImage(depth_cv_matrix, DEPTH, depth_width, depth_height, depth_format, t, &msg->depth); msg->header.frame_id = "camera_rgbd_optical_frame"; msg->header.stamp = t; From e05a75b0c6d37d595eb9dd91fafa28b5386b63be Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Thu, 10 Aug 2023 01:14:17 +0000 Subject: [PATCH 03/76] remove unsupported models --- .travis.yml | 71 ------ README.md | 15 +- realsense2_camera/CMakeLists.txt | 18 +- .../include/base_realsense_node.h | 4 - realsense2_camera/include/constants.h | 26 +-- realsense2_camera/include/image_publisher.h | 4 - realsense2_camera/include/ros_utils.h | 4 - realsense2_camera/launch/rs_launch.py | 46 ++-- realsense2_camera/package.xml | 2 +- realsense2_camera/scripts/rs2_listener.py | 14 +- realsense2_camera/scripts/rs2_test.py | 38 ++-- realsense2_camera/src/base_realsense_node.cpp | 22 +- realsense2_camera/src/dynamic_params.cpp | 4 - realsense2_camera/src/parameters.cpp | 4 - .../src/realsense_node_factory.cpp | 5 - realsense2_camera/src/ros_utils.cpp | 8 +- realsense2_camera/src/rs_node_setup.cpp | 7 - realsense2_camera/src/tfs.cpp | 93 -------- .../test/utils/pytest_rs_utils.py | 2 +- realsense2_description/urdf/_l515.urdf.xacro | 213 ------------------ realsense2_description/urdf/_r430.urdf.xacro | 14 -- .../urdf/test_l515_camera.urdf.xacro | 12 - 22 files changed, 46 insertions(+), 580 deletions(-) delete mode 100644 .travis.yml delete mode 100644 realsense2_description/urdf/_l515.urdf.xacro delete mode 100644 realsense2_description/urdf/test_l515_camera.urdf.xacro diff --git a/.travis.yml b/.travis.yml deleted file mode 100644 index fca14f802a..0000000000 --- a/.travis.yml +++ /dev/null @@ -1,71 +0,0 @@ -sudo: required -matrix: - include: - - dist: bionic - - dist: focal - - dist: jammy - -env: - # - git clone -v --progress https://github.com/doronhi/realsense.git # This is Done automatically by TravisCI -before_install: - - if [[ $(lsb_release -sc) == "bionic" ]]; then _python=python; _ros_dist=dashing; - elif [[ $(lsb_release -sc) == "focal" ]]; then _python=python3; _ros_dist=foxy; fi - elif [[ $(lsb_release -sc) == "jammy" ]]; then _python=python3; _ros_dist=iron; fi - - echo _python:$_python - - echo _ros_dist:$_ros_dist - - - sudo apt-key adv --keyserver keyserver.ubuntu.com --recv-key C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C8B3A55A6F3EFCDE - - sudo add-apt-repository "deb https://librealsense.intel.com/Debian/apt-repo $(lsb_release -cs) main" - - sudo apt-get update -qq - - sudo apt-get install librealsense2-dkms --allow-unauthenticated -y - - sudo apt-get install librealsense2-dev --allow-unauthenticated -y - -install: - # install ROS: - - sudo apt update && sudo apt install curl gnupg2 lsb-release -y - - curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add - - - sudo sh -c 'echo "deb [arch=$(dpkg --print-architecture)] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" > /etc/apt/sources.list.d/ros2-latest.list' - - sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 - - sudo apt update -qq - - sudo apt install ros-$_ros_dist-ros-base -y - - sudo apt install python3-colcon-common-extensions -y - - sudo apt-get install python3-rosdep -y - - #Environment setup - - echo "source /opt/ros/$_ros_dist/setup.bash" >> ~/.bashrc - - source ~/.bashrc - - # install realsense2-camera - - mkdir -p ~/ros2_ws/src/realsense-ros - - mv * ~/ros2_ws/src/realsense-ros/ # This leaves behind .git, .gitignore and .travis.yml but no matter. - - cd ~/ros2_ws - - sudo rosdep init - - rosdep update - - rosdep install -i --from-path src --rosdistro $_ros_dist -y - - sudo apt purge ros-$_ros_dist-librealsense2 -y - - colcon build - - - . install/local_setup.bash - -script: - # download data: - - bag_filename="https://librealsense.intel.com/rs-tests/TestData/outdoors_1color.bag"; - - wget $bag_filename -P "records/" - - bag_filename="https://librealsense.intel.com/rs-tests/D435i_Depth_and_IMU_Stands_still.bag"; - - wget $bag_filename -P "records/" - - # install packages for tests: - - sudo apt-get install python3-pip -y - - pip3 install numpy --upgrade - - pip3 install numpy-quaternion tqdm - # Run test: - - python3 src/realsense-ros/realsense2_camera/scripts/rs2_test.py --all - -before_cache: - - rm -f $HOME/.gradle/caches/modules-2/modules-2.lock - - rm -fr $HOME/.gradle/caches/*/plugin-resolution/ -cache: - directories: - - $HOME/.gradle/caches/ - - $HOME/.gradle/wrapper/ - - $HOME/.android/build-cache diff --git a/README.md b/README.md index ec670e6614..262600151f 100644 --- a/README.md +++ b/README.md @@ -142,8 +142,8 @@ - Install dependencies ```bash sudo apt-get install python3-rosdep -y - sudo rosdep init # "sudo rosdep init --include-eol-distros" for Eloquent and earlier - rosdep update # "sudo rosdep update --include-eol-distros" for Eloquent and earlier + sudo rosdep init # "sudo rosdep init --include-eol-distros" for Foxy and earlier + rosdep update # "sudo rosdep update --include-eol-distros" for Foxy and earlier rosdep install -i --from-path src --rosdistro $ROS_DISTRO --skip-keys=librealsense2 -y ``` @@ -200,13 +200,13 @@ - They have, at least, the **profile** parameter. - The profile parameter is a string of the following format: \X\X\ (The dividing character can be X, x or ",". Spaces are ignored.) - For example: ```depth_module.profile:=640x480x30 rgb_camera.profile:=1280x720x30``` - - Since infra, infra1, infra2, fisheye, fisheye1, fisheye2 and depth are all streams of the depth_module, their width, height and fps are defined by the same param **depth_module.profile** + - Since infra, infra1, infra2 and depth are all streams of the depth_module, their width, height and fps are defined by the same param **depth_module.profile** - If the specified combination of parameters is not available by the device, the default or previously set configuration will be used. - Run ```ros2 param describe ``` to get the list of supported profiles. - Note: Should re-enable the stream for the change to take effect. - ****_format** - This parameter is a string used to select the stream format. - - can be any of *infra, infra1, infra2, color, depth, fisheye, fisheye1, fisheye2*. + - can be any of *infra, infra1, infra2, color, depth*. - For example: ```depth_module.depth_format:=Z16 depth_module.infra1_format:=y8 rgb_camera.color_format:=RGB8``` - This parameter supports both lower case and upper case letters. - If the specified parameter is not available by the stream, the default or previously set configuration will be used. @@ -217,14 +217,14 @@ - Run ```rs-enumerate-devices``` command to know the list of profiles supported by the connected sensors. - **enable_****: - Choose whether to enable a specified stream or not. Default is true for images and false for orientation streams. - - can be any of *infra, infra1, infra2, color, depth, fisheye, fisheye1, fisheye2, gyro, accel, pose*. + - can be any of *infra, infra1, infra2, color, depth, gyro, accel*. - For example: ```enable_infra1:=true enable_color:=false``` - **enable_sync**: - gathers closest frames of different sensors, infra red, color and depth, to be sent with the same timetag. - This happens automatically when such filters as pointcloud are enabled. - ****_qos**: - Sets the QoS by which the topic is published. - - can be any of *infra, infra1, infra2, color, depth, fisheye, fisheye1, fisheye2, gyro, accel, pose*. + - can be any of *infra, infra1, infra2, color, depth, gyro, accel*. - Available values are the following strings: `SYSTEM_DEFAULT`, `DEFAULT`, `PARAMETER_EVENTS`, `SERVICES_DEFAULT`, `PARAMETERS`, `SENSOR_DATA`. - For example: ```depth_qos:=SENSOR_DATA``` - Reference: [ROS2 QoS profiles formal documentation](https://docs.ros.org/en/rolling/Concepts/About-Quality-of-Service-Settings.html#qos-profiles) @@ -270,7 +270,6 @@ - For example: `initial_reset:=true` - ****_frame_id**, ****_optical_frame_id**, **aligned_depth_to_**_frame_id**: Specify the different frame_id for the different frames. Especially important when using multiple cameras. - **base_frame_id**: defines the frame_id all static transformations refers to. -- **odom_frame_id**: defines the origin coordinate system in ROS convention (X-Forward, Y-Left, Z-Up). pose topic defines the pose relative to that system. - **unite_imu_method**: - D400 cameras have built in IMU components which produce 2 unrelated streams, each with it's own frequency: @@ -299,8 +298,6 @@ - 0 or negative values mean no diagnostics topic is published. Defaults to 0.
The `/diagnostics` topic includes information regarding the device temperatures and actual frequency of the enabled streams. -- **publish_odom_tf**: If True (default) publish TF from odom_frame to pose_frame. -

diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 4eeda43bdf..96c51f249d 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -144,23 +144,7 @@ set(SOURCES if(NOT DEFINED ENV{ROS_DISTRO}) message(FATAL_ERROR "ROS_DISTRO is not defined." ) endif() -if("$ENV{ROS_DISTRO}" STREQUAL "dashing") - message(STATUS "Build for ROS2 Dashing") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DDASHING") - set(SOURCES "${SOURCES}" src/ros_param_backend_dashing.cpp) -elseif("$ENV{ROS_DISTRO}" STREQUAL "eloquent") - message(STATUS "Build for ROS2 eloquent") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DELOQUENT") - set(SOURCES "${SOURCES}" src/ros_param_backend_foxy.cpp) -elseif("$ENV{ROS_DISTRO}" STREQUAL "foxy") - message(STATUS "Build for ROS2 Foxy") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DFOXY") - set(SOURCES "${SOURCES}" src/ros_param_backend_foxy.cpp) -elseif("$ENV{ROS_DISTRO}" STREQUAL "galactic") - message(STATUS "Build for ROS2 Galactic") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DGALACTIC") - set(SOURCES "${SOURCES}" src/ros_param_backend_foxy.cpp) -elseif("$ENV{ROS_DISTRO}" STREQUAL "humble") +if("$ENV{ROS_DISTRO}" STREQUAL "humble") message(STATUS "Build for ROS2 Humble") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DHUMBLE") set(SOURCES "${SOURCES}" src/ros_param_backend_foxy.cpp) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index b414473e31..3e0d727701 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -40,7 +40,6 @@ #include #include #include -#include #include #include @@ -249,7 +248,6 @@ namespace realsense2_camera void FillImuData_LinearInterpolation(const CimuData imu_data, std::deque& imu_msgs); void imu_callback(rs2::frame frame); void imu_callback_sync(rs2::frame frame, imu_sync_method sync_method=imu_sync_method::COPY); - void pose_callback(rs2::frame frame); void multiple_message_callback(rs2::frame frame, imu_sync_method sync_method); void frame_callback(rs2::frame frame); @@ -295,7 +293,6 @@ namespace realsense2_camera std::map> _image_publishers; std::map::SharedPtr> _imu_publishers; - std::shared_ptr> _odom_publisher; std::shared_ptr _synced_imu_publisher; std::map::SharedPtr> _info_publishers; std::map::SharedPtr> _metadata_publishers; @@ -316,7 +313,6 @@ namespace realsense2_camera bool _is_color_enabled; bool _is_depth_enabled; bool _pointcloud; - bool _publish_odom_tf; imu_sync_method _imu_sync_method; stream_index_pair _pointcloud_texture; PipelineSyncer _syncer; diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index cd53468e0f..a9321d9190 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -31,18 +31,6 @@ #define ROS_WARN(...) RCLCPP_WARN(_logger, __VA_ARGS__) #define ROS_ERROR(...) RCLCPP_ERROR(_logger, __VA_ARGS__) -#ifdef DASHING -// Based on: https://docs.ros2.org/dashing/api/rclcpp/logging_8hpp.html -#define MSG(msg) (static_cast(std::ostringstream() << msg)).str() -#define ROS_DEBUG_STREAM(msg) RCLCPP_DEBUG(_logger, MSG(msg)) -#define ROS_INFO_STREAM(msg) RCLCPP_INFO(_logger, MSG(msg)) -#define ROS_WARN_STREAM(msg) RCLCPP_WARN(_logger, MSG(msg)) -#define ROS_ERROR_STREAM(msg) RCLCPP_ERROR(_logger, MSG(msg)) -#define ROS_FATAL_STREAM(msg) RCLCPP_FATAL(_logger, MSG(msg)) -#define ROS_DEBUG_STREAM_ONCE(msg) RCLCPP_DEBUG_ONCE(_logger, MSG(msg)) -#define ROS_INFO_STREAM_ONCE(msg) RCLCPP_INFO_ONCE(_logger, MSG(msg)) -#define ROS_WARN_STREAM_COND(cond, msg) RCLCPP_WARN_EXPRESSION(_logger, cond, MSG(msg)) -#else // Based on: https://docs.ros2.org/foxy/api/rclcpp/logging_8hpp.html #define ROS_DEBUG_STREAM(msg) RCLCPP_DEBUG_STREAM(_logger, msg) #define ROS_INFO_STREAM(msg) RCLCPP_INFO_STREAM(_logger, msg) @@ -52,15 +40,12 @@ #define ROS_DEBUG_STREAM_ONCE(msg) RCLCPP_DEBUG_STREAM_ONCE(_logger, msg) #define ROS_INFO_STREAM_ONCE(msg) RCLCPP_INFO_STREAM_ONCE(_logger, msg) #define ROS_WARN_STREAM_COND(cond, msg) RCLCPP_WARN_STREAM_EXPRESSION(_logger, cond, msg) -#endif #define ROS_WARN_ONCE(msg) RCLCPP_WARN_ONCE(_logger, msg) #define ROS_WARN_COND(cond, ...) RCLCPP_WARN_EXPRESSION(_logger, cond, __VA_ARGS__) namespace realsense2_camera { - const uint16_t SR300_PID = 0x0aa5; // SR300 - const uint16_t SR300v2_PID = 0x0B48; // SR305 const uint16_t RS400_PID = 0x0ad1; // PSR const uint16_t RS410_PID = 0x0ad2; // ASR const uint16_t RS415_PID = 0x0ad3; // ASRC @@ -80,11 +65,7 @@ namespace realsense2_camera const uint16_t RS430i_PID = 0x0b4b; // D430i const uint16_t RS405_PID = 0x0B5B; // DS5U const uint16_t RS455_PID = 0x0B5C; // D455 - const uint16_t RS457_PID = 0xABCD; // D457 - const uint16_t RS_L515_PID_PRE_PRQ = 0x0B3D; // - const uint16_t RS_L515_PID = 0x0B64; // - const uint16_t RS_L535_PID = 0x0b68; - + const uint16_t RS457_PID = 0xABCD; // D457 const bool ALLOW_NO_TEXTURE_POINTS = false; const bool ORDERED_PC = false; @@ -100,15 +81,10 @@ namespace realsense2_camera const std::string HID_QOS = "SENSOR_DATA"; const bool HOLD_BACK_IMU_FOR_FRAMES = false; - const bool PUBLISH_ODOM_TF = true; const std::string DEFAULT_BASE_FRAME_ID = "link"; - const std::string DEFAULT_ODOM_FRAME_ID = "odom_frame"; const std::string DEFAULT_IMU_OPTICAL_FRAME_ID = "camera_imu_optical_frame"; - const std::string DEFAULT_UNITE_IMU_METHOD = ""; - const std::string DEFAULT_FILTERS = ""; - const float ROS_DEPTH_SCALE = 0.001; static const rmw_qos_profile_t rmw_qos_profile_latched = diff --git a/realsense2_camera/include/image_publisher.h b/realsense2_camera/include/image_publisher.h index 6bc0bab8e6..3d7d004c74 100644 --- a/realsense2_camera/include/image_publisher.h +++ b/realsense2_camera/include/image_publisher.h @@ -17,11 +17,7 @@ #include #include -#if defined( DASHING ) || defined( ELOQUENT ) -#include -#else #include -#endif namespace realsense2_camera { class image_publisher diff --git a/realsense2_camera/include/ros_utils.h b/realsense2_camera/include/ros_utils.h index 4df1def396..feccd4647d 100644 --- a/realsense2_camera/include/ros_utils.h +++ b/realsense2_camera/include/ros_utils.h @@ -31,12 +31,8 @@ namespace realsense2_camera const stream_index_pair INFRA0{RS2_STREAM_INFRARED, 0}; const stream_index_pair INFRA1{RS2_STREAM_INFRARED, 1}; const stream_index_pair INFRA2{RS2_STREAM_INFRARED, 2}; - const stream_index_pair FISHEYE{RS2_STREAM_FISHEYE, 0}; - const stream_index_pair FISHEYE1{RS2_STREAM_FISHEYE, 1}; - const stream_index_pair FISHEYE2{RS2_STREAM_FISHEYE, 2}; const stream_index_pair GYRO{RS2_STREAM_GYRO, 0}; const stream_index_pair ACCEL{RS2_STREAM_ACCEL, 0}; - const stream_index_pair POSE{RS2_STREAM_POSE, 0}; bool isValidCharInName(char c); diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py index 9f56745988..9e95144169 100644 --- a/realsense2_camera/launch/rs_launch.py +++ b/realsense2_camera/launch/rs_launch.py @@ -51,8 +51,6 @@ {'name': 'accel_fps', 'default': '0', 'description': "''"}, {'name': 'enable_gyro', 'default': 'false', 'description': "'enable gyro stream'"}, {'name': 'enable_accel', 'default': 'false', 'description': "'enable accel stream'"}, - {'name': 'enable_pose', 'default': 'true', 'description': "'enable pose stream'"}, - {'name': 'pose_fps', 'default': '200', 'description': "''"}, {'name': 'pointcloud.enable', 'default': 'false', 'description': ''}, {'name': 'pointcloud.stream_filter', 'default': '2', 'description': 'texture stream for pointcloud'}, {'name': 'pointcloud.stream_index_filter','default': '0', 'description': 'texture stream index for pointcloud'}, @@ -96,36 +94,20 @@ def launch_setup(context, params, param_name_suffix=''): _config_file = LaunchConfiguration('config_file' + param_name_suffix).perform(context) params_from_file = {} if _config_file == "''" else yaml_to_dict(_config_file) # Realsense - if (os.getenv('ROS_DISTRO') == "dashing") or (os.getenv('ROS_DISTRO') == "eloquent"): - return [ - launch_ros.actions.Node( - package='realsense2_camera', - node_namespace=LaunchConfiguration('camera_namespace' + param_name_suffix), - node_name=LaunchConfiguration('camera_name' + param_name_suffix), - node_executable='realsense2_camera_node', - prefix=['stdbuf -o L'], - parameters=[params - , params_from_file - ], - output='screen', - arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level' + param_name_suffix)], - ) - ] - else: - return [ - launch_ros.actions.Node( - package='realsense2_camera', - namespace=LaunchConfiguration('camera_namespace' + param_name_suffix), - name=LaunchConfiguration('camera_name' + param_name_suffix), - executable='realsense2_camera_node', - parameters=[params - , params_from_file - ], - output='screen', - arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level' + param_name_suffix)], - emulate_tty=True, - ) - ] + return [ + launch_ros.actions.Node( + package='realsense2_camera', + namespace=LaunchConfiguration('camera_namespace' + param_name_suffix), + name=LaunchConfiguration('camera_name' + param_name_suffix), + executable='realsense2_camera_node', + parameters=[params + , params_from_file + ], + output='screen', + arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level' + param_name_suffix)], + emulate_tty=True, + ) + ] def generate_launch_description(): return LaunchDescription(declare_configurable_parameters(configurable_parameters) + [ diff --git a/realsense2_camera/package.xml b/realsense2_camera/package.xml index faedde2dd2..ab4d1d763b 100644 --- a/realsense2_camera/package.xml +++ b/realsense2_camera/package.xml @@ -3,7 +3,7 @@ realsense2_camera 4.54.1 - RealSense camera package allowing access to Intel SR300 and D400 3D cameras + RealSense camera package allowing access to Intel D400 3D cameras LibRealSense ROS Team Apache License 2.0 diff --git a/realsense2_camera/scripts/rs2_listener.py b/realsense2_camera/scripts/rs2_listener.py index 1fcbc97834..b56185cd35 100644 --- a/realsense2_camera/scripts/rs2_listener.py +++ b/realsense2_camera/scripts/rs2_listener.py @@ -24,13 +24,8 @@ import struct import quaternion import os -if (os.getenv('ROS_DISTRO') != "dashing"): - import tf2_ros -if (os.getenv('ROS_DISTRO') == "humble"): - from sensor_msgs.msg import PointCloud2 as msg_PointCloud2 - from sensor_msgs_py import point_cloud2 as pc2 -# from sensor_msgs.msg import PointCloud2 as msg_PointCloud2 -# import sensor_msgs.point_cloud2 as pc2 +from sensor_msgs.msg import PointCloud2 as msg_PointCloud2 +from sensor_msgs_py import point_cloud2 as pc2 from sensor_msgs.msg import Imu as msg_Imu try: @@ -220,9 +215,8 @@ def wait_for_messages(self, themes): node.get_logger().info('Subscribing %s on topic: %s' % (theme_name, theme['topic'])) self.func_data[theme_name]['sub'] = node.create_subscription(theme['msg_type'], theme['topic'], theme['callback'](theme_name), qos.qos_profile_sensor_data) - if (os.getenv('ROS_DISTRO') != "dashing"): - self.tfBuffer = tf2_ros.Buffer() - self.tf_listener = tf2_ros.TransformListener(self.tfBuffer, node) + self.tfBuffer = tf2_ros.Buffer() + self.tf_listener = tf2_ros.TransformListener(self.tfBuffer, node) self.prev_time = time.time() break_timeout = False diff --git a/realsense2_camera/scripts/rs2_test.py b/realsense2_camera/scripts/rs2_test.py index 697f72ad5b..c22f35efc2 100644 --- a/realsense2_camera/scripts/rs2_test.py +++ b/realsense2_camera/scripts/rs2_test.py @@ -21,8 +21,7 @@ from rclpy.node import Node from importRosbag.importRosbag import importRosbag import numpy as np -if (os.getenv('ROS_DISTRO') != "dashing"): - import tf2_ros +import tf2_ros import itertools import subprocess import time @@ -277,20 +276,16 @@ def print_results(results): def get_tfs(coupled_frame_ids): res = dict() - if (os.getenv('ROS_DISTRO') == "dashing"): - for couple in coupled_frame_ids: + tfBuffer = tf2_ros.Buffer() + node = Node('tf_listener') + listener = tf2_ros.TransformListener(tfBuffer, node) + rclpy.spin_once(node) + for couple in coupled_frame_ids: + from_id, to_id = couple + if (tfBuffer.can_transform(from_id, to_id, rclpy.time.Time(), rclpy.time.Duration(nanoseconds=3e6))): + res[couple] = tfBuffer.lookup_transform(from_id, to_id, rclpy.time.Time(), rclpy.time.Duration(nanoseconds=1e6)).transform + else: res[couple] = None - else: - tfBuffer = tf2_ros.Buffer() - node = Node('tf_listener') - listener = tf2_ros.TransformListener(tfBuffer, node) - rclpy.spin_once(node) - for couple in coupled_frame_ids: - from_id, to_id = couple - if (tfBuffer.can_transform(from_id, to_id, rclpy.time.Time(), rclpy.time.Duration(nanoseconds=3e6))): - res[couple] = tfBuffer.lookup_transform(from_id, to_id, rclpy.time.Time(), rclpy.time.Duration(nanoseconds=1e6)).transform - else: - res[couple] = None return res def kill_realsense2_camera_node(): @@ -372,17 +367,12 @@ def main(): #{'name': 'points_cloud_1', 'type': 'pointscloud_avg', 'params': {'rosbag_filename': outdoors_filename, 'pointcloud.enable': 'true'}}, {'name': 'depth_w_cloud_1', 'type': 'depth_avg', 'params': {'rosbag_filename': outdoors_filename, 'pointcloud.enable': 'true'}}, {'name': 'align_depth_color_1', 'type': 'align_depth_color', 'params': {'rosbag_filename': outdoors_filename, 'align_depth.enable':'true'}}, - {'name': 'align_depth_ir1_1', 'type': 'align_depth_ir1', 'params': {'rosbag_filename': outdoors_filename, 'align_depth.enable': 'true', - 'enable_infra1':'true', 'enable_infra2':'true'}}, + {'name': 'align_depth_ir1_1', 'type': 'align_depth_ir1', 'params': {'rosbag_filename': outdoors_filename, 'align_depth.enable': 'true', 'enable_infra1':'true', 'enable_infra2':'true'}}, {'name': 'depth_avg_decimation_1', 'type': 'depth_avg_decimation', 'params': {'rosbag_filename': outdoors_filename, 'decimation_filter.enable':'true'}}, {'name': 'align_depth_ir1_decimation_1', 'type': 'align_depth_ir1_decimation', 'params': {'rosbag_filename': outdoors_filename, 'align_depth.enable':'true', 'decimation_filter.enable':'true'}}, - ] - if (os.getenv('ROS_DISTRO') != "dashing"): - all_tests.extend([ - {'name': 'static_tf_1', 'type': 'static_tf', 'params': {'rosbag_filename': outdoors_filename, - 'enable_infra1':'true', 'enable_infra2':'true'}}, - {'name': 'accel_up_1', 'type': 'accel_up', 'params': {'rosbag_filename': './records/D435i_Depth_and_IMU_Stands_still.bag', 'enable_accel': 'true', 'accel_fps': '0.0'}}, - ]) + {'name': 'static_tf_1', 'type': 'static_tf', 'params': {'rosbag_filename': outdoors_filename, 'enable_infra1':'true', 'enable_infra2':'true'}}, + {'name': 'accel_up_1', 'type': 'accel_up', 'params': {'rosbag_filename': './records/D435i_Depth_and_IMU_Stands_still.bag', 'enable_accel': 'true', 'accel_fps': '0.0'}}, + ] # Normalize parameters: for test in all_tests: diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 729ff66fba..11e8b2b6d8 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -22,10 +22,8 @@ // Header files for disabling intra-process comms for static broadcaster. #include -// This header file is not available in ROS 2 Dashing. -#ifndef DASHING + #include -#endif using namespace realsense2_camera; @@ -113,7 +111,6 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node, _is_color_enabled(false), _is_depth_enabled(false), _pointcloud(false), - _publish_odom_tf(false), _imu_sync_method(imu_sync_method::NONE), _is_profile_changed(false), _is_align_depth_changed(false) @@ -182,7 +179,6 @@ void BaseRealSenseNode::initializeFormatsMaps() rs_format_to_cv_format[RS2_FORMAT_RGBA8] = CV_8UC4; rs_format_to_cv_format[RS2_FORMAT_BGRA8] = CV_8UC4; rs_format_to_cv_format[RS2_FORMAT_YUYV] = CV_16UC3; - rs_format_to_cv_format[RS2_FORMAT_M420] = CV_16UC3; rs_format_to_cv_format[RS2_FORMAT_RAW8] = CV_8UC1; rs_format_to_cv_format[RS2_FORMAT_RAW10] = CV_16UC1; rs_format_to_cv_format[RS2_FORMAT_RAW16] = CV_16UC1; @@ -198,7 +194,6 @@ void BaseRealSenseNode::initializeFormatsMaps() rs_format_to_ros_format[RS2_FORMAT_RGBA8] = sensor_msgs::image_encodings::RGBA8; rs_format_to_ros_format[RS2_FORMAT_BGRA8] = sensor_msgs::image_encodings::BGRA8; rs_format_to_ros_format[RS2_FORMAT_YUYV] = sensor_msgs::image_encodings::YUV422; - //rs_format_to_ros_format[RS2_FORMAT_M420] = not supported in ROS2 image msg yet rs_format_to_ros_format[RS2_FORMAT_RAW8] = sensor_msgs::image_encodings::TYPE_8UC1; rs_format_to_ros_format[RS2_FORMAT_RAW10] = sensor_msgs::image_encodings::TYPE_16UC1; rs_format_to_ros_format[RS2_FORMAT_RAW16] = sensor_msgs::image_encodings::TYPE_16UC1; @@ -630,9 +625,6 @@ void BaseRealSenseNode::multiple_message_callback(rs2::frame frame, imu_sync_met if (sync_method > imu_sync_method::NONE) imu_callback_sync(frame, sync_method); else imu_callback(frame); break; - case RS2_STREAM_POSE: - pose_callback(frame); - break; default: frame_callback(frame); } @@ -672,17 +664,7 @@ rclcpp::Time BaseRealSenseNode::frameSystemTimeSec(rs2::frame frame) { double elapsed_camera_ns = millisecondsToNanoseconds(timestamp_ms - _camera_time_base); - /* - Fixing deprecated-declarations compilation warning. - Duration(rcl_duration_value_t) is deprecated in favor of - static Duration::from_nanoseconds(rcl_duration_value_t) - starting from GALACTIC. - */ -#if defined(FOXY) || defined(ELOQUENT) || defined(DASHING) - auto duration = rclcpp::Duration(elapsed_camera_ns); -#else auto duration = rclcpp::Duration::from_nanoseconds(elapsed_camera_ns); -#endif return rclcpp::Time(_ros_time_base + duration); } @@ -788,7 +770,7 @@ void BaseRealSenseNode::updateExtrinsicsCalibData(const rs2::video_stream_profil void BaseRealSenseNode::SetBaseStream() { - const std::vector base_stream_priority = {DEPTH, POSE}; + const std::vector base_stream_priority = {DEPTH}; std::set checked_sips; std::map available_profiles; for(auto&& sensor : _available_ros_sensors) diff --git a/realsense2_camera/src/dynamic_params.cpp b/realsense2_camera/src/dynamic_params.cpp index 618da0766a..4fb87d67f7 100644 --- a/realsense2_camera/src/dynamic_params.cpp +++ b/realsense2_camera/src/dynamic_params.cpp @@ -113,11 +113,7 @@ namespace realsense2_camera try { ROS_DEBUG_STREAM("setParam::Setting parameter: " << param_name); -#if defined(DASHING) || defined(ELOQUENT) || defined(FOXY) - //do nothing for old versions -#else descriptor.dynamic_typing=true; // Without this, undeclare_parameter() throws in Galactic onward. -#endif if (!_node.get_parameter(param_name, result_value)) { result_value = _node.declare_parameter(param_name, initial_value, descriptor); diff --git a/realsense2_camera/src/parameters.cpp b/realsense2_camera/src/parameters.cpp index 16965f27cf..e98a48d65b 100644 --- a/realsense2_camera/src/parameters.cpp +++ b/realsense2_camera/src/parameters.cpp @@ -77,10 +77,6 @@ void BaseRealSenseNode::getParameters() _hold_back_imu_for_frames = _parameters->setParam(param_name, HOLD_BACK_IMU_FOR_FRAMES); _parameters_names.push_back(param_name); - param_name = std::string("publish_odom_tf"); - _publish_odom_tf = _parameters->setParam(param_name, PUBLISH_ODOM_TF); - _parameters_names.push_back(param_name); - param_name = std::string("base_frame_id"); _base_frame_id = _parameters->setParam(param_name, DEFAULT_BASE_FRAME_ID); _base_frame_id = (static_cast(std::ostringstream() << _camera_name << "_" << _base_frame_id)).str(); diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index 410aacc78f..d57ff44e54 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -358,8 +358,6 @@ void RealSenseNodeFactory::startDevice() { switch(pid) { - case SR300_PID: - case SR300v2_PID: case RS400_PID: case RS405_PID: case RS410_PID: @@ -377,9 +375,6 @@ void RealSenseNodeFactory::startDevice() case RS457_PID: case RS465_PID: case RS_USB2_PID: - case RS_L515_PID_PRE_PRQ: - case RS_L515_PID: - case RS_L535_PID: _realSenseNode = std::unique_ptr(new BaseRealSenseNode(*this, _device, _parameters, this->get_node_options().use_intra_process_comms())); break; default: diff --git a/realsense2_camera/src/ros_utils.cpp b/realsense2_camera/src/ros_utils.cpp index b4661126cf..0ee2172904 100644 --- a/realsense2_camera/src/ros_utils.cpp +++ b/realsense2_camera/src/ros_utils.cpp @@ -111,10 +111,8 @@ static const rmw_qos_profile_t rmw_qos_profile_latched = const rmw_qos_profile_t qos_string_to_qos(std::string str) { -#if !defined(DASHING) && !defined(ELOQUENT) if (str == "UNKNOWN") return rmw_qos_profile_unknown; -#endif if (str == "SYSTEM_DEFAULT") return rmw_qos_profile_system_default; if (str == "DEFAULT") @@ -133,10 +131,8 @@ const rmw_qos_profile_t qos_string_to_qos(std::string str) const std::string list_available_qos_strings() { std::stringstream res; -#ifndef DASHING - res << "UNKNOWN" << "\n"; -#endif - res << "SYSTEM_DEFAULT" << "\n" + res << "UNKNOWN" << "\n" + << "SYSTEM_DEFAULT" << "\n" << "DEFAULT" << "\n" << "PARAMETER_EVENTS" << "\n" << "SERVICES_DEFAULT" << "\n" diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index fcdb673e34..69ca91cc34 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -281,13 +281,6 @@ void BaseRealSenseNode::startPublishers(const std::vector& profi IMUInfo info_msg = getImuInfo(profile); _imu_info_publishers[sip]->publish(info_msg); } - else if (profile.is()) - { - std::stringstream data_topic_name, info_topic_name; - data_topic_name << stream_name << "/sample"; - _odom_publisher = _node.create_publisher(data_topic_name.str(), - rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos), qos)); - } std::string topic_metadata(stream_name + "/metadata"); _metadata_publishers[sip] = _node.create_publisher(topic_metadata, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(info_qos), info_qos)); diff --git a/realsense2_camera/src/tfs.cpp b/realsense2_camera/src/tfs.cpp index 3e82666c9f..8e6201761b 100644 --- a/realsense2_camera/src/tfs.cpp +++ b/realsense2_camera/src/tfs.cpp @@ -31,15 +31,9 @@ void BaseRealSenseNode::restartStaticTransformBroadcaster() rclcpp::PublisherOptionsWithAllocator> options; options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; - #ifndef DASHING _static_tf_broadcaster = std::make_shared(_node, tf2_ros::StaticBroadcasterQoS(), std::move(options)); - #else - _static_tf_broadcaster = std::make_shared(_node, - rclcpp::QoS(100), - std::move(options)); - #endif } void BaseRealSenseNode::append_static_tf_msg(const rclcpp::Time& t, @@ -310,90 +304,3 @@ void BaseRealSenseNode::startDynamicTf() } } -void BaseRealSenseNode::pose_callback(rs2::frame frame) -{ - double frame_time = frame.get_timestamp(); - bool placeholder_false(false); - if (_is_initialized_time_base.compare_exchange_strong(placeholder_false, true) ) - { - _is_initialized_time_base = setBaseTime(frame_time, frame.get_frame_timestamp_domain()); - } - - ROS_DEBUG("Frame arrived: stream: %s ; index: %d ; Timestamp Domain: %s", - rs2_stream_to_string(frame.get_profile().stream_type()), - frame.get_profile().stream_index(), - rs2_timestamp_domain_to_string(frame.get_frame_timestamp_domain())); - rs2_pose pose = frame.as().get_pose_data(); - rclcpp::Time t(frameSystemTimeSec(frame)); - - geometry_msgs::msg::PoseStamped pose_msg; - pose_msg.pose.position.x = -pose.translation.z; - pose_msg.pose.position.y = -pose.translation.x; - pose_msg.pose.position.z = pose.translation.y; - pose_msg.pose.orientation.x = -pose.rotation.z; - pose_msg.pose.orientation.y = -pose.rotation.x; - pose_msg.pose.orientation.z = pose.rotation.y; - pose_msg.pose.orientation.w = pose.rotation.w; - - static tf2_ros::TransformBroadcaster br(_node); - geometry_msgs::msg::TransformStamped msg; - msg.header.stamp = t; - msg.header.frame_id = DEFAULT_ODOM_FRAME_ID; - msg.child_frame_id = FRAME_ID(POSE); - msg.transform.translation.x = pose_msg.pose.position.x; - msg.transform.translation.y = pose_msg.pose.position.y; - msg.transform.translation.z = pose_msg.pose.position.z; - msg.transform.rotation.x = pose_msg.pose.orientation.x; - msg.transform.rotation.y = pose_msg.pose.orientation.y; - msg.transform.rotation.z = pose_msg.pose.orientation.z; - msg.transform.rotation.w = pose_msg.pose.orientation.w; - - if (_publish_odom_tf) br.sendTransform(msg); - - if (0 != _odom_publisher->get_subscription_count()) - { - double cov_pose(_linear_accel_cov * pow(10, 3-(int)pose.tracker_confidence)); - double cov_twist(_angular_velocity_cov * pow(10, 1-(int)pose.tracker_confidence)); - - geometry_msgs::msg::Vector3Stamped v_msg; - tf2::Vector3 tfv(-pose.velocity.z, -pose.velocity.x, pose.velocity.y); - tf2::Quaternion q(-msg.transform.rotation.x, - -msg.transform.rotation.y, - -msg.transform.rotation.z, - msg.transform.rotation.w); - tfv=tf2::quatRotate(q,tfv); - v_msg.vector.x = tfv.x(); - v_msg.vector.y = tfv.y(); - v_msg.vector.z = tfv.z(); - - tfv = tf2::Vector3(-pose.angular_velocity.z, -pose.angular_velocity.x, pose.angular_velocity.y); - tfv=tf2::quatRotate(q,tfv); - geometry_msgs::msg::Vector3Stamped om_msg; - om_msg.vector.x = tfv.x(); - om_msg.vector.y = tfv.y(); - om_msg.vector.z = tfv.z(); - - nav_msgs::msg::Odometry odom_msg; - - odom_msg.header.frame_id = DEFAULT_ODOM_FRAME_ID; - odom_msg.child_frame_id = FRAME_ID(POSE); - odom_msg.header.stamp = t; - odom_msg.pose.pose = pose_msg.pose; - odom_msg.pose.covariance = {cov_pose, 0, 0, 0, 0, 0, - 0, cov_pose, 0, 0, 0, 0, - 0, 0, cov_pose, 0, 0, 0, - 0, 0, 0, cov_twist, 0, 0, - 0, 0, 0, 0, cov_twist, 0, - 0, 0, 0, 0, 0, cov_twist}; - odom_msg.twist.twist.linear = v_msg.vector; - odom_msg.twist.twist.angular = om_msg.vector; - odom_msg.twist.covariance ={cov_pose, 0, 0, 0, 0, 0, - 0, cov_pose, 0, 0, 0, 0, - 0, 0, cov_pose, 0, 0, 0, - 0, 0, 0, cov_twist, 0, 0, - 0, 0, 0, 0, cov_twist, 0, - 0, 0, 0, 0, 0, cov_twist}; - _odom_publisher->publish(odom_msg); - ROS_DEBUG("Publish %s stream", rs2_stream_to_string(frame.get_profile().stream_type())); - } -} diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index 0fd4563381..4f7d0657f7 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/realsense2_camera/test/utils/pytest_rs_utils.py @@ -576,7 +576,7 @@ def create_subscription(self, msg_type, topic , data_type, store_raw_data): super().create_subscription(msg_type, topic , self.rsCallback(topic,msg_type, store_raw_data), data_type) self.data[topic] = deque() self.frame_counter[topic] = 0 - if (os.getenv('ROS_DISTRO') != "dashing") and (self.tfBuffer == None): + if (self.tfBuffer == None): self.tfBuffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tfBuffer, super()) diff --git a/realsense2_description/urdf/_l515.urdf.xacro b/realsense2_description/urdf/_l515.urdf.xacro deleted file mode 100644 index c761ddf734..0000000000 --- a/realsense2_description/urdf/_l515.urdf.xacro +++ /dev/null @@ -1,213 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/realsense2_description/urdf/_r430.urdf.xacro b/realsense2_description/urdf/_r430.urdf.xacro index d4ea736bc8..28f65f76bf 100644 --- a/realsense2_description/urdf/_r430.urdf.xacro +++ b/realsense2_description/urdf/_r430.urdf.xacro @@ -125,20 +125,6 @@ aluminum peripherial evaluation case. - - - - - - - - - - - - - - diff --git a/realsense2_description/urdf/test_l515_camera.urdf.xacro b/realsense2_description/urdf/test_l515_camera.urdf.xacro deleted file mode 100644 index 6b1c3354a0..0000000000 --- a/realsense2_description/urdf/test_l515_camera.urdf.xacro +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - From 1901d2cf69af802f8233161569ca53750e8435d1 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Mon, 14 Aug 2023 18:07:56 +0530 Subject: [PATCH 04/76] added align_depth and get params from YAML examples --- .../align_depth/rs_align_depth_launch.py | 56 +++++++++++++++++++ .../rs_launch_from_rosbag.py | 56 +++++++++++++++++++ .../launch_params_from_file/config.yaml | 6 ++ .../rs_launch_get_params_from_yaml.py | 53 ++++++++++++++++++ .../pointcloud/rs_d455_pointcloud_launch.py | 1 + .../pointcloud/rs_pointcloud_launch.py | 1 + 6 files changed, 173 insertions(+) create mode 100644 realsense2_camera/examples/align_depth/rs_align_depth_launch.py create mode 100644 realsense2_camera/examples/launch_from_rosbag/rs_launch_from_rosbag.py create mode 100644 realsense2_camera/examples/launch_params_from_file/config.yaml create mode 100644 realsense2_camera/examples/launch_params_from_file/rs_launch_get_params_from_yaml.py 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/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/rs_d455_pointcloud_launch.py b/realsense2_camera/examples/pointcloud/rs_d455_pointcloud_launch.py index e4e908d4a3..4f013b1c87 100644 --- a/realsense2_camera/examples/pointcloud/rs_d455_pointcloud_launch.py +++ b/realsense2_camera/examples/pointcloud/rs_d455_pointcloud_launch.py @@ -35,6 +35,7 @@ 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'}, diff --git a/realsense2_camera/examples/pointcloud/rs_pointcloud_launch.py b/realsense2_camera/examples/pointcloud/rs_pointcloud_launch.py index d1e464f9be..f0a5b541e0 100644 --- a/realsense2_camera/examples/pointcloud/rs_pointcloud_launch.py +++ b/realsense2_camera/examples/pointcloud/rs_pointcloud_launch.py @@ -33,6 +33,7 @@ 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'}, From 5663e713d7a11d88202e86b00c13fb4a7103e204 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Mon, 14 Aug 2023 22:25:53 +0530 Subject: [PATCH 05/76] Updated rs_launch param names --- README.md | 7 +-- realsense2_camera/launch/rs_launch.py | 63 ++++++++++++++------------- 2 files changed, 33 insertions(+), 37 deletions(-) diff --git a/README.md b/README.md index ec670e6614..64c890e833 100644 --- a/README.md +++ b/README.md @@ -464,12 +464,7 @@ The following post processing filters are available: * pointcloud is of an unordered format by default. This can be changed by setting `pointcloud.ordered_pc` to true. - ```hdr_merge```: Allows depth image to be created by merging the information from 2 consecutive frames, taken with different exposure and gain values. - The way to set exposure and gain values for each sequence in runtime is by first selecting the sequence id, using the `depth_module.sequence_id` parameter and then modifying the `depth_module.gain`, and `depth_module.exposure`. - - To view the effect on the infrared image for each sequence id use the `sequence_id_filter.sequence_id` parameter. - - To initialize these parameters in start time use the following parameters: - - `depth_module.exposure.1` - - `depth_module.gain.1` - - `depth_module.exposure.2` - - `depth_module.gain.2` + - To view the effect on the infrared image for each sequence id use the `filter_by_sequence_id.sequence_id` parameter. - For in-depth review of the subject please read the accompanying [white paper](https://dev.intelrealsense.com/docs/high-dynamic-range-with-stereoscopic-depth-cameras). - The following filters have detailed descriptions in : https://github.com/IntelRealSense/librealsense/blob/master/doc/post-processing-filters.md diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py index 9f56745988..acecb8965f 100644 --- a/realsense2_camera/launch/rs_launch.py +++ b/realsense2_camera/launch/rs_launch.py @@ -27,56 +27,57 @@ {'name': 'usb_port_id', 'default': "''", 'description': 'choose device by usb port id'}, {'name': 'device_type', 'default': "''", 'description': 'choose device by type'}, {'name': 'config_file', 'default': "''", 'description': 'yaml config file'}, - {'name': 'unite_imu_method', 'default': "0", 'description': '[0-None, 1-copy, 2-linear_interpolation]'}, {'name': 'json_file_path', 'default': "''", 'description': 'allows advanced configuration'}, + {'name': 'initial_reset', 'default': 'false', 'description': "''"}, + {'name': 'rosbag_filename', 'default': "''", 'description': 'A realsense bagfile to run from as a device'}, {'name': 'log_level', 'default': 'info', 'description': 'debug log level [DEBUG|INFO|WARN|ERROR|FATAL]'}, - {'name': 'output', 'default': 'screen', 'description': 'pipe node output [screen|log]'}, - {'name': 'depth_module.profile', 'default': '0,0,0', 'description': 'depth module profile'}, - {'name': 'depth_module.depth_format', 'default': 'Z16', 'description': 'depth stream format'}, - {'name': 'depth_module.infra_format', 'default': 'RGB8', 'description': 'infra0 stream format'}, - {'name': 'depth_module.infra1_format', 'default': 'Y8', 'description': 'infra1 stream format'}, - {'name': 'depth_module.infra2_format', 'default': 'Y8', 'description': 'infra2 stream format'}, - {'name': 'enable_depth', 'default': 'true', 'description': 'enable depth stream'}, + {'name': 'enable_color', 'default': 'true', 'description': 'enable color stream'}, {'name': 'rgb_camera.profile', 'default': '0,0,0', 'description': 'color image width'}, {'name': 'rgb_camera.color_format', 'default': 'RGB8', 'description': 'color stream format'}, {'name': 'rgb_camera.enable_auto_exposure', 'default': 'true', 'description': 'enable/disable auto exposure for color image'}, - {'name': 'enable_color', 'default': 'true', 'description': 'enable color stream'}, + {'name': 'enable_depth', 'default': 'true', 'description': 'enable depth stream'}, {'name': 'enable_infra', 'default': 'false', 'description': 'enable infra0 stream'}, {'name': 'enable_infra1', 'default': 'false', 'description': 'enable infra1 stream'}, {'name': 'enable_infra2', 'default': 'false', 'description': 'enable infra2 stream'}, {'name': 'enable_fisheye1', 'default': 'true', 'description': 'enable fisheye1 stream'}, {'name': 'enable_fisheye2', 'default': 'true', 'description': 'enable fisheye2 stream'}, + {'name': 'depth_module.profile', 'default': '0,0,0', 'description': 'depth module profile'}, + {'name': 'depth_module.depth_format', 'default': 'Z16', 'description': 'depth stream format'}, + {'name': 'depth_module.infra_format', 'default': 'RGB8', 'description': 'infra0 stream format'}, + {'name': 'depth_module.infra1_format', 'default': 'Y8', 'description': 'infra1 stream format'}, + {'name': 'depth_module.infra2_format', 'default': 'Y8', 'description': 'infra2 stream format'}, + {'name': 'depth_module.exposure', 'default': '8500', 'description': 'Depth module manual exposure value'}, + {'name': 'depth_module.gain', 'default': '16', 'description': 'Depth module manual gain value'}, + {'name': 'depth_module.hdr_enabled', 'default': 'false', 'description': 'Depth module hdr enablement flag. Used for hdr_merge filter'}, + {'name': 'depth_module.enable_auto_exposure', 'default': 'true', 'description': 'enable/disable auto exposure for depth image'}, {'name': 'enable_confidence', 'default': 'true', 'description': 'enable confidence'}, - {'name': 'gyro_fps', 'default': '0', 'description': "''"}, - {'name': 'accel_fps', 'default': '0', 'description': "''"}, + {'name': 'enable_sync', 'default': 'false', 'description': "'enable sync mode'"}, + {'name': 'enable_rgbd', 'default': 'false', 'description': "'enable rgbd topic'"}, {'name': 'enable_gyro', 'default': 'false', 'description': "'enable gyro stream'"}, {'name': 'enable_accel', 'default': 'false', 'description': "'enable accel stream'"}, + {'name': 'gyro_fps', 'default': '0', 'description': "''"}, + {'name': 'accel_fps', 'default': '0', 'description': "''"}, + {'name': 'unite_imu_method', 'default': "0", 'description': '[0-None, 1-copy, 2-linear_interpolation]'}, {'name': 'enable_pose', 'default': 'true', 'description': "'enable pose stream'"}, {'name': 'pose_fps', 'default': '200', 'description': "''"}, - {'name': 'pointcloud.enable', 'default': 'false', 'description': ''}, - {'name': 'pointcloud.stream_filter', 'default': '2', 'description': 'texture stream for pointcloud'}, - {'name': 'pointcloud.stream_index_filter','default': '0', 'description': 'texture stream index for pointcloud'}, - {'name': 'pointcloud.ordered_pc', 'default': 'false', 'description': ''}, - {'name': 'enable_sync', 'default': 'false', 'description': "'enable sync mode'"}, - {'name': 'enable_rgbd', 'default': 'false', 'description': "'enable rgbd topic'"}, - {'name': 'align_depth.enable', 'default': 'false', 'description': "'enable align depth filter'"}, - {'name': 'colorizer.enable', 'default': 'false', 'description': "''"}, {'name': 'clip_distance', 'default': '-2.', 'description': "''"}, + {'name': 'angular_velocity_cov', 'default': '0.01', 'description': "''"}, {'name': 'linear_accel_cov', 'default': '0.01', 'description': "''"}, - {'name': 'initial_reset', 'default': 'false', 'description': "''"}, - {'name': 'allow_no_texture_points', 'default': 'false', 'description': "''"}, + {'name': 'diagnostics_period', 'default': '0.0', 'description': 'Rate of publishing diagnostics. 0=Disabled'}, {'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': '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'}, - {'name': 'depth_module.exposure.2', 'default': '1', 'description': 'Depth module second exposure value. Used for hdr_merge filter'}, - {'name': 'depth_module.gain.2', 'default': '16', 'description': 'Depth module second gain value. Used for hdr_merge filter'}, - {'name': 'depth_module.exposure', 'default': '8500', 'description': 'Depth module manual exposure value'}, - {'name': 'depth_module.hdr_enabled', 'default': 'false', 'description': 'Depth module hdr enablement flag. Used for hdr_merge filter'}, - {'name': 'depth_module.enable_auto_exposure', 'default': 'true', 'description': 'enable/disable auto exposure for depth image'}, + {'name': 'pointcloud.enable', 'default': 'false', 'description': ''}, + {'name': 'pointcloud.stream_filter', 'default': '2', 'description': 'texture stream for pointcloud'}, + {'name': 'pointcloud.stream_index_filter','default': '0', 'description': 'texture stream index for pointcloud'}, + {'name': 'pointcloud.ordered_pc', 'default': 'false', 'description': ''}, + {'name': 'pointcloud.allow_no_texture_points', 'default': 'false', 'description': "''"}, + {'name': 'align_depth.enable', 'default': 'false', 'description': 'enable align depth filter'}, + {'name': 'colorizer.enable', 'default': 'false', 'description': 'enable colorizer filter'}, + {'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': 'disparity_filter.enable', 'default': 'false', 'description': 'enable_disparity_filter'}, + {'name': 'hole_filling_filter.enable', 'default': 'false', 'description': 'enable_hole_filling_filter'}, {'name': 'hdr_merge.enable', 'default': 'false', 'description': 'hdr_merge filter enablement flag'}, {'name': 'wait_for_device_timeout', 'default': '-1.', 'description': 'Timeout for waiting for device to connect (Seconds)'}, {'name': 'reconnect_timeout', 'default': '6.', 'description': 'Timeout(seconds) between consequtive reconnection attempts'}, From b5e57d5fedc186e1a9cf534eb0ab7e2b6df451b1 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Mon, 14 Aug 2023 22:29:30 +0530 Subject: [PATCH 06/76] updated rs_launch --- realsense2_camera/launch/rs_launch.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py index 3231fabf97..9f56745988 100644 --- a/realsense2_camera/launch/rs_launch.py +++ b/realsense2_camera/launch/rs_launch.py @@ -68,9 +68,7 @@ {'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': 'enable_decimation_filter'}, - {'name': 'spatial_filter.enable', 'default': 'false', 'description': 'enable_spatial_filter'}, - {'name': 'temporal_filter.enable', 'default': 'false', 'description': 'enable_temporal_filter'}, + {'name': 'decimation_filter.enable', 'default': 'false', 'description': 'Rate of publishing static_tf'}, {'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'}, From 116ac2e83af598786142361f8009a05f32a2bdf4 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Tue, 15 Aug 2023 02:11:42 +0530 Subject: [PATCH 07/76] Updated the rs_launch.py params --- README.md | 5 +++++ realsense2_camera/launch/rs_launch.py | 4 ++++ 2 files changed, 9 insertions(+) diff --git a/README.md b/README.md index 64c890e833..95446dd36a 100644 --- a/README.md +++ b/README.md @@ -465,6 +465,11 @@ The following post processing filters are available: - ```hdr_merge```: Allows depth image to be created by merging the information from 2 consecutive frames, taken with different exposure and gain values. - The way to set exposure and gain values for each sequence in runtime is by first selecting the sequence id, using the `depth_module.sequence_id` parameter and then modifying the `depth_module.gain`, and `depth_module.exposure`. - To view the effect on the infrared image for each sequence id use the `filter_by_sequence_id.sequence_id` parameter. + - To initialize these parameters in start time use the following parameters: + - `depth_module.exposure.1` + - `depth_module.gain.1` + - `depth_module.exposure.2` + - `depth_module.gain.2` - For in-depth review of the subject please read the accompanying [white paper](https://dev.intelrealsense.com/docs/high-dynamic-range-with-stereoscopic-depth-cameras). - The following filters have detailed descriptions in : https://github.com/IntelRealSense/librealsense/blob/master/doc/post-processing-filters.md diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py index acecb8965f..16599ebc18 100644 --- a/realsense2_camera/launch/rs_launch.py +++ b/realsense2_camera/launch/rs_launch.py @@ -50,6 +50,10 @@ {'name': 'depth_module.gain', 'default': '16', 'description': 'Depth module manual gain value'}, {'name': 'depth_module.hdr_enabled', 'default': 'false', 'description': 'Depth module hdr enablement flag. Used for hdr_merge filter'}, {'name': 'depth_module.enable_auto_exposure', 'default': 'true', 'description': 'enable/disable auto exposure for depth image'}, + {'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'}, + {'name': 'depth_module.exposure.2', 'default': '1', 'description': 'Depth module second exposure value. Used for hdr_merge filter'}, + {'name': 'depth_module.gain.2', 'default': '16', 'description': 'Depth module second gain value. Used for hdr_merge filter'}, {'name': 'enable_confidence', 'default': 'true', 'description': 'enable confidence'}, {'name': 'enable_sync', 'default': 'false', 'description': "'enable sync mode'"}, {'name': 'enable_rgbd', 'default': 'false', 'description': "'enable rgbd topic'"}, From 7fef703cce12a57a564f8cb75792f9958bd915db Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Fri, 18 Aug 2023 02:31:07 +0530 Subject: [PATCH 08/76] updated node output setting --- realsense2_camera/launch/rs_launch.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py index 16599ebc18..ef0f167f7e 100644 --- a/realsense2_camera/launch/rs_launch.py +++ b/realsense2_camera/launch/rs_launch.py @@ -31,6 +31,7 @@ {'name': 'initial_reset', 'default': 'false', 'description': "''"}, {'name': 'rosbag_filename', 'default': "''", 'description': 'A realsense bagfile to run from as a device'}, {'name': 'log_level', 'default': 'info', 'description': 'debug log level [DEBUG|INFO|WARN|ERROR|FATAL]'}, + {'name': 'output', 'default': 'screen', 'description': 'pipe node output [screen|log]'}, {'name': 'enable_color', 'default': 'true', 'description': 'enable color stream'}, {'name': 'rgb_camera.profile', 'default': '0,0,0', 'description': 'color image width'}, {'name': 'rgb_camera.color_format', 'default': 'RGB8', 'description': 'color stream format'}, @@ -112,7 +113,7 @@ def launch_setup(context, params, param_name_suffix=''): parameters=[params , params_from_file ], - output='screen', + output=LaunchConfiguration('output' + param_name_suffix), arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level' + param_name_suffix)], ) ] @@ -126,7 +127,7 @@ def launch_setup(context, params, param_name_suffix=''): parameters=[params , params_from_file ], - output='screen', + output=LaunchConfiguration('output' + param_name_suffix), arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level' + param_name_suffix)], emulate_tty=True, ) From e14ba753ef2b90218f92bbe4655011f54505bfcb Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Fri, 18 Aug 2023 22:09:43 +0530 Subject: [PATCH 09/76] Create /imu topic only when motion streams enabled --- .../include/base_realsense_node.h | 3 ++- realsense2_camera/src/base_realsense_node.cpp | 10 +++++++--- realsense2_camera/src/rs_node_setup.cpp | 19 +++++++++++++------ 3 files changed, 22 insertions(+), 10 deletions(-) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index d6f0a1b97c..865ff9d13a 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -252,7 +252,6 @@ namespace realsense2_camera void startDiagnosticsUpdater(); void monitoringProfileChanges(); void publish_temperature(); - void setupFiltersPublishers(); void setAvailableSensors(); void setCallbackFunctions(); void updateSensors(); @@ -311,6 +310,8 @@ namespace realsense2_camera bool _enable_rgbd; bool _is_color_enabled; bool _is_depth_enabled; + bool _is_accel_enabled; + bool _is_gyro_enabled; bool _pointcloud; bool _publish_odom_tf; imu_sync_method _imu_sync_method; diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index ae6b50d33c..e079ff556d 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -112,6 +112,8 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node, _enable_rgbd(ENABLE_RGBD), _is_color_enabled(false), _is_depth_enabled(false), + _is_accel_enabled(false), + _is_gyro_enabled(false), _pointcloud(false), _publish_odom_tf(false), _imu_sync_method(imu_sync_method::NONE), @@ -381,7 +383,7 @@ void BaseRealSenseNode::imu_callback_sync(rs2::frame frame, imu_sync_method sync _is_initialized_time_base = setBaseTime(frame_time, frame.get_frame_timestamp_domain()); } - if (0 != _synced_imu_publisher->getNumSubscribers()) + if (_synced_imu_publisher && (0 != _synced_imu_publisher->getNumSubscribers())) { auto crnt_reading = *(reinterpret_cast(frame.get_data())); Eigen::Vector3d v(crnt_reading.x, crnt_reading.y, crnt_reading.z); @@ -464,7 +466,8 @@ void BaseRealSenseNode::imu_callback(rs2::frame frame) void BaseRealSenseNode::frame_callback(rs2::frame frame) { - _synced_imu_publisher->Pause(); + if (_synced_imu_publisher) + _synced_imu_publisher->Pause(); double frame_time = frame.get_timestamp(); // We compute a ROS timestamp which is based on an initial ROS time at point of first frame, @@ -576,7 +579,8 @@ void BaseRealSenseNode::frame_callback(rs2::frame frame) } publishFrame(frame, t, sip, _images, _info_publishers, _image_publishers); } - _synced_imu_publisher->Resume(); + if (_synced_imu_publisher) + _synced_imu_publisher->Resume(); } // frame_callback void BaseRealSenseNode::multiple_message_callback(rs2::frame frame, imu_sync_method sync_method) diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index fcdb673e34..652eb63ca7 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -27,18 +27,12 @@ void BaseRealSenseNode::setup() setAvailableSensors(); SetBaseStream(); setupFilters(); - setupFiltersPublishers(); setCallbackFunctions(); monitoringProfileChanges(); updateSensors(); publishServices(); } -void BaseRealSenseNode::setupFiltersPublishers() -{ - _synced_imu_publisher = std::make_shared(_node.create_publisher("imu", 5)); -} - void BaseRealSenseNode::monitoringProfileChanges() { int time_interval(10000); @@ -190,6 +184,9 @@ void BaseRealSenseNode::stopPublishers(const std::vector& profil } else if (profile.is()) { + _is_accel_enabled = false; + _is_gyro_enabled = false; + _synced_imu_publisher.reset(); _imu_publishers.erase(sip); _imu_info_publishers.erase(sip); } @@ -270,6 +267,11 @@ void BaseRealSenseNode::startPublishers(const std::vector& profi } else if (profile.is()) { + if(profile.stream_type() == RS2_STREAM_ACCEL) + _is_accel_enabled = true; + else if (profile.stream_type() == RS2_STREAM_GYRO) + _is_gyro_enabled = true; + std::stringstream data_topic_name, info_topic_name; data_topic_name << stream_name << "/sample"; _imu_publishers[sip] = _node.create_publisher(data_topic_name.str(), @@ -305,6 +307,11 @@ void BaseRealSenseNode::startPublishers(const std::vector& profi rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(extrinsics_qos), extrinsics_qos), std::move(options)); } } + if (_is_accel_enabled && _is_gyro_enabled && (_imu_sync_method > imu_sync_method::NONE)) + { + _synced_imu_publisher = std::make_shared(_node.create_publisher("imu", 5)); + } + } void BaseRealSenseNode::startRGBDPublisherIfNeeded() From 00621dcd3d366b6c48e0352b349d78abea52f4ac Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Tue, 22 Aug 2023 15:08:22 +0530 Subject: [PATCH 10/76] Get the topic to subscribe from user --- .../launch/rs_intra_process_demo_launch.py | 2 ++ .../tools/frame_latency/frame_latency.cpp | 11 ++++++++--- 2 files changed, 10 insertions(+), 3 deletions(-) diff --git a/realsense2_camera/launch/rs_intra_process_demo_launch.py b/realsense2_camera/launch/rs_intra_process_demo_launch.py index 103afee220..9bdf75111f 100644 --- a/realsense2_camera/launch/rs_intra_process_demo_launch.py +++ b/realsense2_camera/launch/rs_intra_process_demo_launch.py @@ -49,6 +49,7 @@ {'name': 'rgb_camera.profile', 'default': '0,0,0', 'description': 'color image width'}, {'name': 'enable_color', 'default': 'true', 'description': 'enable color stream'}, {'name': 'enable_depth', 'default': 'false', 'description': 'enable depth stream'}, + {'name': 'enable_infra', 'default': 'false', 'description': 'enable infra stream'}, {'name': 'enable_infra1', 'default': 'false', 'description': 'enable infra1 stream'}, {'name': 'enable_infra2', 'default': 'false', 'description': 'enable infra2 stream'}, {'name': 'enable_gyro', 'default': 'false', 'description': "enable gyro stream"}, @@ -56,6 +57,7 @@ {'name': 'intra_process_comms', 'default': 'true', 'description': "enable intra-process communication"}, {'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': 'topic_name', 'default': '/color/image_raw', 'description': 'topic to which latency calculated'}, ] def declare_configurable_parameters(parameters): diff --git a/realsense2_camera/tools/frame_latency/frame_latency.cpp b/realsense2_camera/tools/frame_latency/frame_latency.cpp index 022e733e49..59d37484a1 100644 --- a/realsense2_camera/tools/frame_latency/frame_latency.cpp +++ b/realsense2_camera/tools/frame_latency/frame_latency.cpp @@ -28,6 +28,7 @@ FrameLatencyNode::FrameLatencyNode( const std::string & node_name, { } +std::string topic_name = "/color/image_raw"; FrameLatencyNode::FrameLatencyNode( const rclcpp::NodeOptions & node_options ) : Node( "frame_latency", "/", node_options ) @@ -36,16 +37,20 @@ FrameLatencyNode::FrameLatencyNode( const rclcpp::NodeOptions & node_options ) ROS_INFO_STREAM( "frame_latency node is UP!" ); ROS_INFO_STREAM( "Intra-Process is " << ( this->get_node_options().use_intra_process_comms() ? "ON" : "OFF" ) ); + + topic_name = this->declare_parameter("topic_name", topic_name); + + ROS_INFO_STREAM( "Subscribing to Topic: " << topic_name); + // Create a subscription on the input topic. _sub = this->create_subscription< sensor_msgs::msg::Image >( - "/color/image_raw", // TODO Currently color only, we can declare and accept the required - // streams as ros parameters + topic_name, rclcpp::QoS( rclcpp::QoSInitialization::from_rmw( rmw_qos_profile_default ), rmw_qos_profile_default ), [&, this]( const sensor_msgs::msg::Image::SharedPtr msg ) { rclcpp::Time curr_time = this->get_clock()->now(); auto latency = ( curr_time - msg->header.stamp ).seconds(); - ROS_INFO_STREAM( "Got msg with address 0x" + ROS_INFO_STREAM( "Got msg with "<< msg->header.frame_id <<" frame id at address 0x" << std::hex << reinterpret_cast< std::uintptr_t >( msg.get() ) << std::dec << " with latency of " << latency << " [sec]" ); } ); From 180a7111e31b25317c6beaf1eb6aef1282d3f4ad Mon Sep 17 00:00:00 2001 From: "(=YG=) Hyunseok Yang" Date: Thu, 24 Aug 2023 12:00:58 +0900 Subject: [PATCH 11/76] Apply camera name in topics --- realsense2_camera/src/named_filter.cpp | 18 +++++------ realsense2_camera/src/rs_node_setup.cpp | 40 ++++++++++++------------- 2 files changed, 29 insertions(+), 29 deletions(-) diff --git a/realsense2_camera/src/named_filter.cpp b/realsense2_camera/src/named_filter.cpp index 729770b605..31fd80a8cb 100644 --- a/realsense2_camera/src/named_filter.cpp +++ b/realsense2_camera/src/named_filter.cpp @@ -44,7 +44,7 @@ void NamedFilter::clearParameters() { auto name = _parameters_names.back(); _params.getParameters()->removeParam(name); - _parameters_names.pop_back(); + _parameters_names.pop_back(); } } @@ -117,12 +117,12 @@ void PointcloudFilter::setParameters() }); } -void PointcloudFilter::setPublisher() -{ +void PointcloudFilter::setPublisher() +{ std::lock_guard lock_guard(_mutex_publisher); if ((_is_enabled) && (!_pointcloud_publisher)) { - _pointcloud_publisher = _node.create_publisher("depth/color/points", + _pointcloud_publisher = _node.create_publisher("~/depth/color/points", rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_string_to_qos(_pointcloud_qos)), qos_string_to_qos(_pointcloud_qos))); } @@ -156,8 +156,8 @@ void PointcloudFilter::Publish(rs2::points pc, const rclcpp::Time& t, const rs2: if (use_texture) { std::set available_formats{ rs2_format::RS2_FORMAT_RGB8, rs2_format::RS2_FORMAT_Y8 }; - - texture_frame_itr = std::find_if(frameset.begin(), frameset.end(), [&texture_source_id, &available_formats] (rs2::frame f) + + texture_frame_itr = std::find_if(frameset.begin(), frameset.end(), [&texture_source_id, &available_formats] (rs2::frame f) {return (rs2_stream(f.get_profile().stream_type()) == texture_source_id) && (available_formats.find(f.get_profile().format()) != available_formats.end()); }); if (texture_frame_itr == frameset.end()) @@ -181,7 +181,7 @@ void PointcloudFilter::Publish(rs2::points pc, const rclcpp::Time& t, const rs2: sensor_msgs::msg::PointCloud2::UniquePtr msg_pointcloud = std::make_unique(); sensor_msgs::PointCloud2Modifier modifier(*msg_pointcloud); - modifier.setPointCloud2FieldsByString(1, "xyz"); + modifier.setPointCloud2FieldsByString(1, "xyz"); modifier.resize(pc.size()); if (_ordered_pc) { @@ -266,7 +266,7 @@ void PointcloudFilter::Publish(rs2::points pc, const rclcpp::Time& t, const rs2: *iter_x = vertex->x; *iter_y = vertex->y; *iter_z = vertex->z; - + ++iter_x; ++iter_y; ++iter_z; ++valid_count; } @@ -289,7 +289,7 @@ void PointcloudFilter::Publish(rs2::points pc, const rclcpp::Time& t, const rs2: } -AlignDepthFilter::AlignDepthFilter(std::shared_ptr filter, +AlignDepthFilter::AlignDepthFilter(std::shared_ptr filter, std::function update_align_depth_func, std::shared_ptr parameters, rclcpp::Logger logger, bool is_enabled): NamedFilter(filter, parameters, logger, is_enabled, false) diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index fcdb673e34..4de40a2a41 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -36,7 +36,7 @@ void BaseRealSenseNode::setup() void BaseRealSenseNode::setupFiltersPublishers() { - _synced_imu_publisher = std::make_shared(_node.create_publisher("imu", 5)); + _synced_imu_publisher = std::make_shared(_node.create_publisher("~/imu", 5)); } void BaseRealSenseNode::monitoringProfileChanges() @@ -108,12 +108,12 @@ void BaseRealSenseNode::setAvailableSensors() ROS_INFO_STREAM("Device Product ID: 0x" << pid); ROS_INFO_STREAM("Sync Mode: " << ((_sync_frames)?"On":"Off")); - + std::function frame_callback_function = [this](rs2::frame frame){ bool is_filter(_filters.end() != find_if(_filters.begin(), _filters.end(), [](std::shared_ptr f){return (f->is_enabled()); })); if (_sync_frames || is_filter) this->_asyncer.invoke(frame); - else + else frame_callback(frame); }; @@ -141,7 +141,7 @@ void BaseRealSenseNode::setAvailableSensors() { const std::string module_name(rs2_to_ros(sensor.get_info(RS2_CAMERA_INFO_NAME))); std::unique_ptr rosSensor; - if (sensor.is() || + if (sensor.is() || sensor.is() || sensor.is()) { @@ -225,8 +225,8 @@ void BaseRealSenseNode::startPublishers(const std::vector& profi if (sensor.rs2::sensor::is()) rectified_image = true; - image_raw << stream_name << "/image_" << ((rectified_image)?"rect_":"") << "raw"; - camera_info << stream_name << "/camera_info"; + image_raw << "~/" << stream_name << "/image_" << ((rectified_image)?"rect_":"") << "raw"; + camera_info << "~/" << stream_name << "/camera_info"; // We can use 2 types of publishers: // Native RCL publisher that support intra-process zero-copy comunication @@ -247,8 +247,8 @@ void BaseRealSenseNode::startPublishers(const std::vector& profi if (_align_depth_filter->is_enabled() && (sip != DEPTH) && sip.second < 2) { std::stringstream aligned_image_raw, aligned_camera_info; - aligned_image_raw << "aligned_depth_to_" << stream_name << "/image_raw"; - aligned_camera_info << "aligned_depth_to_" << stream_name << "/camera_info"; + aligned_image_raw << "~/" << "aligned_depth_to_" << stream_name << "/image_raw"; + aligned_camera_info << "~/" << "aligned_depth_to_" << stream_name << "/camera_info"; std::string aligned_stream_name = "aligned_depth_to_" + stream_name; @@ -271,11 +271,11 @@ void BaseRealSenseNode::startPublishers(const std::vector& profi else if (profile.is()) { std::stringstream data_topic_name, info_topic_name; - data_topic_name << stream_name << "/sample"; + data_topic_name << "~/" << stream_name << "/sample"; _imu_publishers[sip] = _node.create_publisher(data_topic_name.str(), rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos), qos)); // Publish Intrinsics: - info_topic_name << stream_name << "/imu_info"; + info_topic_name << "~/" << stream_name << "/imu_info"; _imu_info_publishers[sip] = _node.create_publisher(info_topic_name.str(), rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(info_qos), info_qos)); IMUInfo info_msg = getImuInfo(profile); @@ -284,14 +284,14 @@ void BaseRealSenseNode::startPublishers(const std::vector& profi else if (profile.is()) { std::stringstream data_topic_name, info_topic_name; - data_topic_name << stream_name << "/sample"; + data_topic_name << "~/" << stream_name << "/sample"; _odom_publisher = _node.create_publisher(data_topic_name.str(), rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos), qos)); } - std::string topic_metadata(stream_name + "/metadata"); - _metadata_publishers[sip] = _node.create_publisher(topic_metadata, + std::string topic_metadata("~/" + stream_name + "/metadata"); + _metadata_publishers[sip] = _node.create_publisher(topic_metadata, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(info_qos), info_qos)); - + if (!((rs2::stream_profile)profile==(rs2::stream_profile)_base_profile)) { @@ -299,9 +299,9 @@ void BaseRealSenseNode::startPublishers(const std::vector& profi rclcpp::PublisherOptionsWithAllocator> options; options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; rmw_qos_profile_t extrinsics_qos = rmw_qos_profile_latched; - - std::string topic_extrinsics("extrinsics/" + create_graph_resource_name(ros_stream_to_string(_base_profile.stream_type()) + "_to_" + stream_name)); - _extrinsics_publishers[sip] = _node.create_publisher(topic_extrinsics, + + std::string topic_extrinsics("~/extrinsics/" + create_graph_resource_name(ros_stream_to_string(_base_profile.stream_type()) + "_to_" + stream_name)); + _extrinsics_publishers[sip] = _node.create_publisher(topic_extrinsics, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(extrinsics_qos), extrinsics_qos), std::move(options)); } } @@ -316,7 +316,7 @@ void BaseRealSenseNode::startRGBDPublisherIfNeeded() { rmw_qos_profile_t qos = _use_intra_process ? qos_string_to_qos(DEFAULT_QOS) : qos_string_to_qos(IMAGE_QOS); - _rgbd_publisher = _node.create_publisher("rgbd", + _rgbd_publisher = _node.create_publisher("~/rgbd", rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos), qos)); } else { @@ -327,7 +327,7 @@ void BaseRealSenseNode::startRGBDPublisherIfNeeded() } void BaseRealSenseNode::updateSensors() -{ +{ std::lock_guard lock_guard(_update_sensor_mutex); try{ for(auto&& sensor : _available_ros_sensors) @@ -337,7 +337,7 @@ void BaseRealSenseNode::updateSensors() std::vector wanted_profiles; bool is_profile_changed(sensor->getUpdatedProfiles(wanted_profiles)); - bool is_video_sensor = (sensor->is() || sensor->is() || sensor->is()); + bool is_video_sensor = (sensor->is() || sensor->is() || sensor->is()); // do all updates if profile has been changed, or if the align depth filter status has been changed // and we are on a video sensor. TODO: explore better options to monitor and update changes From af3cd227a49b0a71892e5a659640c5adcf966997 Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Mon, 28 Aug 2023 20:48:49 +0530 Subject: [PATCH 12/76] Added the changes from https://github.com/IntelRealSense/realsense-ros/pull/2857 and adapted the tests accordingly --- realsense2_camera/src/named_filter.cpp | 18 ++++----- realsense2_camera/src/rs_node_setup.cpp | 40 +++++++++---------- .../test_align_depth.py | 8 ++-- .../rosbag/test_rosbag_all_topics_test.py | 19 ++++----- .../test/rosbag/test_rosbag_basic_tests.py | 7 ++-- .../rosbag/test_rosbag_dec_point_tests.py | 9 +++-- .../test/rosbag/test_rosbag_depth_tests.py | 11 ++--- .../test/rosbag/test_rosbag_imu_test.py | 9 +++-- .../templates/test_integration_template.py | 9 +++-- .../templates/test_parameterized_template.py | 3 +- .../test/utils/pytest_rs_utils.py | 24 ++++++----- 11 files changed, 84 insertions(+), 73 deletions(-) diff --git a/realsense2_camera/src/named_filter.cpp b/realsense2_camera/src/named_filter.cpp index 729770b605..31fd80a8cb 100644 --- a/realsense2_camera/src/named_filter.cpp +++ b/realsense2_camera/src/named_filter.cpp @@ -44,7 +44,7 @@ void NamedFilter::clearParameters() { auto name = _parameters_names.back(); _params.getParameters()->removeParam(name); - _parameters_names.pop_back(); + _parameters_names.pop_back(); } } @@ -117,12 +117,12 @@ void PointcloudFilter::setParameters() }); } -void PointcloudFilter::setPublisher() -{ +void PointcloudFilter::setPublisher() +{ std::lock_guard lock_guard(_mutex_publisher); if ((_is_enabled) && (!_pointcloud_publisher)) { - _pointcloud_publisher = _node.create_publisher("depth/color/points", + _pointcloud_publisher = _node.create_publisher("~/depth/color/points", rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_string_to_qos(_pointcloud_qos)), qos_string_to_qos(_pointcloud_qos))); } @@ -156,8 +156,8 @@ void PointcloudFilter::Publish(rs2::points pc, const rclcpp::Time& t, const rs2: if (use_texture) { std::set available_formats{ rs2_format::RS2_FORMAT_RGB8, rs2_format::RS2_FORMAT_Y8 }; - - texture_frame_itr = std::find_if(frameset.begin(), frameset.end(), [&texture_source_id, &available_formats] (rs2::frame f) + + texture_frame_itr = std::find_if(frameset.begin(), frameset.end(), [&texture_source_id, &available_formats] (rs2::frame f) {return (rs2_stream(f.get_profile().stream_type()) == texture_source_id) && (available_formats.find(f.get_profile().format()) != available_formats.end()); }); if (texture_frame_itr == frameset.end()) @@ -181,7 +181,7 @@ void PointcloudFilter::Publish(rs2::points pc, const rclcpp::Time& t, const rs2: sensor_msgs::msg::PointCloud2::UniquePtr msg_pointcloud = std::make_unique(); sensor_msgs::PointCloud2Modifier modifier(*msg_pointcloud); - modifier.setPointCloud2FieldsByString(1, "xyz"); + modifier.setPointCloud2FieldsByString(1, "xyz"); modifier.resize(pc.size()); if (_ordered_pc) { @@ -266,7 +266,7 @@ void PointcloudFilter::Publish(rs2::points pc, const rclcpp::Time& t, const rs2: *iter_x = vertex->x; *iter_y = vertex->y; *iter_z = vertex->z; - + ++iter_x; ++iter_y; ++iter_z; ++valid_count; } @@ -289,7 +289,7 @@ void PointcloudFilter::Publish(rs2::points pc, const rclcpp::Time& t, const rs2: } -AlignDepthFilter::AlignDepthFilter(std::shared_ptr filter, +AlignDepthFilter::AlignDepthFilter(std::shared_ptr filter, std::function update_align_depth_func, std::shared_ptr parameters, rclcpp::Logger logger, bool is_enabled): NamedFilter(filter, parameters, logger, is_enabled, false) diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index fcdb673e34..4de40a2a41 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -36,7 +36,7 @@ void BaseRealSenseNode::setup() void BaseRealSenseNode::setupFiltersPublishers() { - _synced_imu_publisher = std::make_shared(_node.create_publisher("imu", 5)); + _synced_imu_publisher = std::make_shared(_node.create_publisher("~/imu", 5)); } void BaseRealSenseNode::monitoringProfileChanges() @@ -108,12 +108,12 @@ void BaseRealSenseNode::setAvailableSensors() ROS_INFO_STREAM("Device Product ID: 0x" << pid); ROS_INFO_STREAM("Sync Mode: " << ((_sync_frames)?"On":"Off")); - + std::function frame_callback_function = [this](rs2::frame frame){ bool is_filter(_filters.end() != find_if(_filters.begin(), _filters.end(), [](std::shared_ptr f){return (f->is_enabled()); })); if (_sync_frames || is_filter) this->_asyncer.invoke(frame); - else + else frame_callback(frame); }; @@ -141,7 +141,7 @@ void BaseRealSenseNode::setAvailableSensors() { const std::string module_name(rs2_to_ros(sensor.get_info(RS2_CAMERA_INFO_NAME))); std::unique_ptr rosSensor; - if (sensor.is() || + if (sensor.is() || sensor.is() || sensor.is()) { @@ -225,8 +225,8 @@ void BaseRealSenseNode::startPublishers(const std::vector& profi if (sensor.rs2::sensor::is()) rectified_image = true; - image_raw << stream_name << "/image_" << ((rectified_image)?"rect_":"") << "raw"; - camera_info << stream_name << "/camera_info"; + image_raw << "~/" << stream_name << "/image_" << ((rectified_image)?"rect_":"") << "raw"; + camera_info << "~/" << stream_name << "/camera_info"; // We can use 2 types of publishers: // Native RCL publisher that support intra-process zero-copy comunication @@ -247,8 +247,8 @@ void BaseRealSenseNode::startPublishers(const std::vector& profi if (_align_depth_filter->is_enabled() && (sip != DEPTH) && sip.second < 2) { std::stringstream aligned_image_raw, aligned_camera_info; - aligned_image_raw << "aligned_depth_to_" << stream_name << "/image_raw"; - aligned_camera_info << "aligned_depth_to_" << stream_name << "/camera_info"; + aligned_image_raw << "~/" << "aligned_depth_to_" << stream_name << "/image_raw"; + aligned_camera_info << "~/" << "aligned_depth_to_" << stream_name << "/camera_info"; std::string aligned_stream_name = "aligned_depth_to_" + stream_name; @@ -271,11 +271,11 @@ void BaseRealSenseNode::startPublishers(const std::vector& profi else if (profile.is()) { std::stringstream data_topic_name, info_topic_name; - data_topic_name << stream_name << "/sample"; + data_topic_name << "~/" << stream_name << "/sample"; _imu_publishers[sip] = _node.create_publisher(data_topic_name.str(), rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos), qos)); // Publish Intrinsics: - info_topic_name << stream_name << "/imu_info"; + info_topic_name << "~/" << stream_name << "/imu_info"; _imu_info_publishers[sip] = _node.create_publisher(info_topic_name.str(), rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(info_qos), info_qos)); IMUInfo info_msg = getImuInfo(profile); @@ -284,14 +284,14 @@ void BaseRealSenseNode::startPublishers(const std::vector& profi else if (profile.is()) { std::stringstream data_topic_name, info_topic_name; - data_topic_name << stream_name << "/sample"; + data_topic_name << "~/" << stream_name << "/sample"; _odom_publisher = _node.create_publisher(data_topic_name.str(), rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos), qos)); } - std::string topic_metadata(stream_name + "/metadata"); - _metadata_publishers[sip] = _node.create_publisher(topic_metadata, + std::string topic_metadata("~/" + stream_name + "/metadata"); + _metadata_publishers[sip] = _node.create_publisher(topic_metadata, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(info_qos), info_qos)); - + if (!((rs2::stream_profile)profile==(rs2::stream_profile)_base_profile)) { @@ -299,9 +299,9 @@ void BaseRealSenseNode::startPublishers(const std::vector& profi rclcpp::PublisherOptionsWithAllocator> options; options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; rmw_qos_profile_t extrinsics_qos = rmw_qos_profile_latched; - - std::string topic_extrinsics("extrinsics/" + create_graph_resource_name(ros_stream_to_string(_base_profile.stream_type()) + "_to_" + stream_name)); - _extrinsics_publishers[sip] = _node.create_publisher(topic_extrinsics, + + std::string topic_extrinsics("~/extrinsics/" + create_graph_resource_name(ros_stream_to_string(_base_profile.stream_type()) + "_to_" + stream_name)); + _extrinsics_publishers[sip] = _node.create_publisher(topic_extrinsics, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(extrinsics_qos), extrinsics_qos), std::move(options)); } } @@ -316,7 +316,7 @@ void BaseRealSenseNode::startRGBDPublisherIfNeeded() { rmw_qos_profile_t qos = _use_intra_process ? qos_string_to_qos(DEFAULT_QOS) : qos_string_to_qos(IMAGE_QOS); - _rgbd_publisher = _node.create_publisher("rgbd", + _rgbd_publisher = _node.create_publisher("~/rgbd", rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos), qos)); } else { @@ -327,7 +327,7 @@ void BaseRealSenseNode::startRGBDPublisherIfNeeded() } void BaseRealSenseNode::updateSensors() -{ +{ std::lock_guard lock_guard(_update_sensor_mutex); try{ for(auto&& sensor : _available_ros_sensors) @@ -337,7 +337,7 @@ void BaseRealSenseNode::updateSensors() std::vector wanted_profiles; bool is_profile_changed(sensor->getUpdatedProfiles(wanted_profiles)); - bool is_video_sensor = (sensor->is() || sensor->is() || sensor->is()); + bool is_video_sensor = (sensor->is() || sensor->is() || sensor->is()); // do all updates if profile has been changed, or if the align depth filter status has been changed // and we are on a video sensor. TODO: explore better options to monitor and update changes diff --git a/realsense2_camera/test/post_processing_filters/test_align_depth.py b/realsense2_camera/test/post_processing_filters/test_align_depth.py index 2a5b4b1017..8ac3c23e2b 100644 --- a/realsense2_camera/test/post_processing_filters/test_align_depth.py +++ b/realsense2_camera/test/post_processing_filters/test_align_depth.py @@ -32,7 +32,7 @@ import pytest_rs_utils from pytest_rs_utils import launch_descr_with_yaml from pytest_rs_utils import get_rosbag_file_path - +from pytest_rs_utils import get_node_heirarchy ''' This test imitates the ros2 launch rs_launch.py realsense2_camera with the given parameters below @@ -66,11 +66,11 @@ class TestBasicAlignDepthEnable(pytest_rs_utils.RsTestBaseClass): def test_align_depth_on(self, launch_descr_with_yaml): params = launch_descr_with_yaml[1] themes = [ - {'topic': '/'+params['camera_name']+'/color/image_raw', 'msg_type': msg_Image, + {'topic': get_node_heirarchy(params)+'/color/image_raw', 'msg_type': msg_Image, 'expected_data_chunks': 1, 'width': 640, 'height': 480}, - {'topic': '/'+params['camera_name']+'/depth/image_rect_raw', 'msg_type': msg_Image, + {'topic': get_node_heirarchy(params)+'/depth/image_rect_raw', 'msg_type': msg_Image, 'expected_data_chunks': 1, 'width': 1280, 'height': 720}, - {'topic': '/'+params['camera_name']+'/aligned_depth_to_color/image_raw', 'msg_type': msg_Image, + {'topic': get_node_heirarchy(params)+'/aligned_depth_to_color/image_raw', 'msg_type': msg_Image, 'expected_data_chunks': 1, 'width': 640, 'height': 480} ] try: diff --git a/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py b/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py index 98a63008cc..34abbc3039 100644 --- a/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py +++ b/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py @@ -39,6 +39,7 @@ from pytest_rs_utils import delayed_launch_descr_with_parameters from pytest_rs_utils import get_rosbag_file_path +from pytest_rs_utils import get_node_heirarchy test_params_all_topics = {"rosbag_filename":get_rosbag_file_path("outdoors_1color.bag"), @@ -77,18 +78,18 @@ def test_all_topics(self,delayed_launch_descr_with_parameters): data = pytest_rs_utils.ImageColorGetData(params["rosbag_filename"]) themes = [ { - 'topic':'/'+params['camera_name']+'/extrinsics/depth_to_color', + 'topic':get_node_heirarchy(params)+'/extrinsics/depth_to_color', 'msg_type':msg_Extrinsics, 'expected_data_chunks':1, 'data':depth_to_color_extrinsics_data }, { - 'topic':'/'+params['camera_name']+'/extrinsics/depth_to_infra1', + 'topic':get_node_heirarchy(params)+'/extrinsics/depth_to_infra1', 'msg_type':msg_Extrinsics, 'expected_data_chunks':1, 'data':depth_to_infra_extrinsics_data }, - {'topic':'/'+params['camera_name']+'/color/image_raw', + {'topic':get_node_heirarchy(params)+'/color/image_raw', 'msg_type':msg_Image, 'expected_data_chunks':1, 'data':data @@ -145,19 +146,19 @@ def test_metadata_topics(self,delayed_launch_descr_with_parameters): themes = [ { - 'topic':'/'+params['camera_name']+'/color/metadata', + 'topic':get_node_heirarchy(params)+'/color/metadata', 'msg_type':msg_Metadata, 'expected_data_chunks':1, #'data':color_metadata }, { - 'topic':'/'+params['camera_name']+'/depth/metadata', + 'topic':get_node_heirarchy(params)+'/depth/metadata', 'msg_type':msg_Metadata, 'expected_data_chunks':1, #'data':depth_metadata }, { - 'topic':'/'+params['camera_name']+'/infra1/metadata', + 'topic':get_node_heirarchy(params)+'/infra1/metadata', 'msg_type':msg_Metadata, 'expected_data_chunks':1, #'data':infra1_metadata @@ -260,19 +261,19 @@ def test_camera_info_topics(self,delayed_launch_descr_with_parameters): themes = [ { - 'topic':'/'+params['camera_name']+'/color/camera_info', + 'topic':get_node_heirarchy(params)+'/color/camera_info', 'msg_type':CameraInfo, 'expected_data_chunks':1, 'data':color_data }, { - 'topic':'/'+params['camera_name']+'/depth/camera_info', + 'topic':get_node_heirarchy(params)+'/depth/camera_info', 'msg_type':CameraInfo, 'expected_data_chunks':1, 'data':depth_data }, { - 'topic':'/'+params['camera_name']+'/infra1/camera_info', + 'topic':get_node_heirarchy(params)+'/infra1/camera_info', 'msg_type':CameraInfo, 'expected_data_chunks':1, 'data':infra1_data diff --git a/realsense2_camera/test/rosbag/test_rosbag_basic_tests.py b/realsense2_camera/test/rosbag/test_rosbag_basic_tests.py index dfcad8f5fc..614936a973 100644 --- a/realsense2_camera/test/rosbag/test_rosbag_basic_tests.py +++ b/realsense2_camera/test/rosbag/test_rosbag_basic_tests.py @@ -32,6 +32,7 @@ from pytest_rs_utils import launch_descr_with_parameters from pytest_rs_utils import delayed_launch_descr_with_parameters from pytest_rs_utils import get_rosbag_file_path +from pytest_rs_utils import get_node_heirarchy test_params = {"rosbag_filename":get_rosbag_file_path("outdoors_1color.bag"), 'camera_name': 'Vis2_Cam', @@ -54,7 +55,7 @@ def test_vis_2(self,delayed_launch_descr_with_parameters): params = delayed_launch_descr_with_parameters[1] data = pytest_rs_utils.ImageColorGetData(params["rosbag_filename"]) themes = [ - {'topic':'/'+params['camera_name']+'/color/image_raw', + {'topic':get_node_heirarchy(params)+'/color/image_raw', 'msg_type':msg_Image, 'expected_data_chunks':1, 'data':data @@ -96,7 +97,7 @@ def test_depth_w_cloud_1(self,launch_descr_with_parameters): params = launch_descr_with_parameters[1] data = pytest_rs_utils.ImageDepthGetData(params["rosbag_filename"]) themes = [ - {'topic':'/'+params['camera_name']+'/depth/image_rect_raw', + {'topic':get_node_heirarchy(params)+'/depth/image_rect_raw', 'msg_type':msg_Image, 'expected_data_chunks':1, 'data':data @@ -137,7 +138,7 @@ def test_depth_avg_1(self,launch_descr_with_parameters): params = launch_descr_with_parameters[1] data = pytest_rs_utils.ImageDepthGetData(params["rosbag_filename"]) themes = [ - {'topic':'/'+params['camera_name']+'/depth/image_rect_raw', + {'topic':get_node_heirarchy(params)+'/depth/image_rect_raw', 'msg_type':msg_Image, 'expected_data_chunks':1, 'data':data diff --git a/realsense2_camera/test/rosbag/test_rosbag_dec_point_tests.py b/realsense2_camera/test/rosbag/test_rosbag_dec_point_tests.py index 39a366f776..688ca6c8d8 100644 --- a/realsense2_camera/test/rosbag/test_rosbag_dec_point_tests.py +++ b/realsense2_camera/test/rosbag/test_rosbag_dec_point_tests.py @@ -32,6 +32,7 @@ from pytest_rs_utils import launch_descr_with_parameters from pytest_rs_utils import delayed_launch_descr_with_parameters from pytest_rs_utils import get_rosbag_file_path +from pytest_rs_utils import get_node_heirarchy test_params_depth_avg_decimation_1 = {"rosbag_filename":get_rosbag_file_path("outdoors_1color.bag"), @@ -56,7 +57,7 @@ def test_depth_avg_decimation_1(self,launch_descr_with_parameters): params = launch_descr_with_parameters[1] data = pytest_rs_utils.ImageDepthGetData_decimation(params["rosbag_filename"]) themes = [ - {'topic':'/'+params['camera_name']+'/depth/image_rect_raw', + {'topic':get_node_heirarchy(params)+'/depth/image_rect_raw', 'msg_type':msg_Image, 'expected_data_chunks':1, 'data':data @@ -97,7 +98,7 @@ def test_depth_avg_1(self,launch_descr_with_parameters): params = launch_descr_with_parameters[1] data = pytest_rs_utils.ImageDepthGetData(params["rosbag_filename"]) themes = [ - {'topic':'/'+params['camera_name']+'/depth/image_rect_raw', + {'topic':get_node_heirarchy(params)+'/depth/image_rect_raw', 'msg_type':msg_Image, 'expected_data_chunks':1, 'data':data @@ -139,7 +140,7 @@ def test_depth_avg_decimation_1(self,launch_descr_with_parameters): params = launch_descr_with_parameters[1] data = pytest_rs_utils.ImageDepthGetData_decimation(params["rosbag_filename"]) themes = [ - {'topic':'/'+params['camera_name']+'/depth/image_rect_raw', + {'topic':get_node_heirarchy(params)+'/depth/image_rect_raw', 'msg_type':msg_Image, 'expected_data_chunks':1, 'data':data @@ -181,7 +182,7 @@ def test_points_cloud_1(self,delayed_launch_descr_with_parameters): params = delayed_launch_descr_with_parameters[1] self.rosbag = params["rosbag_filename"] themes = [ - {'topic':'/'+params['camera_name']+'/depth/color/points', + {'topic':get_node_heirarchy(params)+'/depth/color/points', 'msg_type':msg_PointCloud2, 'expected_data_chunks':1, #'data':data diff --git a/realsense2_camera/test/rosbag/test_rosbag_depth_tests.py b/realsense2_camera/test/rosbag/test_rosbag_depth_tests.py index 80b9c334d8..abbfe28cab 100644 --- a/realsense2_camera/test/rosbag/test_rosbag_depth_tests.py +++ b/realsense2_camera/test/rosbag/test_rosbag_depth_tests.py @@ -32,6 +32,7 @@ from pytest_rs_utils import launch_descr_with_parameters from pytest_rs_utils import delayed_launch_descr_with_parameters from pytest_rs_utils import get_rosbag_file_path +from pytest_rs_utils import get_node_heirarchy test_params_depth_points_cloud_1 = {"rosbag_filename":get_rosbag_file_path("outdoors_1color.bag"), @@ -72,12 +73,12 @@ def test_depth_points_cloud_1(self,delayed_launch_descr_with_parameters): 'avg': [np.array([ 1.28251814, -0.15839984, 4.82235184, 80, 160, 240])], 'epsilon': [0.04, 5]} themes = [ - {'topic':'/'+params['camera_name']+'/depth/color/points', + {'topic':get_node_heirarchy(params)+'/depth/color/points', 'msg_type':msg_PointCloud2, 'expected_data_chunks':1, 'data':data1 }, - {'topic':'/'+params['camera_name']+'/depth/image_rect_raw', + {'topic':get_node_heirarchy(params)+'/depth/image_rect_raw', 'msg_type':msg_Image, 'expected_data_chunks':1, 'data':data2 @@ -127,7 +128,7 @@ def test_static_tf_1(self,delayed_launch_descr_with_parameters): ('camera_depth_frame', 'camera_color_frame'): ([-0.00010158783697988838, 0.014841210097074509, -0.00022671300393994898], [-0.0008337442995980382, 0.0010442184284329414, -0.0009920650627464056, 0.9999986290931702]), ('camera_infra1_frame', 'camera_color_frame'): ([-0.00010158783697988838, 0.014841210097074509, -0.00022671300393994898], [-0.0008337442995980382, 0.0010442184284329414, -0.0009920650627464056, 0.9999986290931702])} themes = [ - {'topic':'/'+params['camera_name']+'/color/image_raw', + {'topic':get_node_heirarchy(params)+'/color/image_raw', 'msg_type':msg_Image, 'expected_data_chunks':1, 'data':data, @@ -204,7 +205,7 @@ def test_align_depth_color_1(self,delayed_launch_descr_with_parameters): params = delayed_launch_descr_with_parameters[1] data = pytest_rs_utils.ImageDepthInColorShapeGetData(params["rosbag_filename"]) themes = [ - {'topic':'/'+params['camera_name']+'/aligned_depth_to_color/image_raw', + {'topic':get_node_heirarchy(params)+'/aligned_depth_to_color/image_raw', 'msg_type':msg_Image, 'expected_data_chunks':1, 'data':data @@ -260,7 +261,7 @@ def test_align_depth_infra_1(self,delayed_launch_descr_with_parameters): self.rosbag = params["rosbag_filename"] #data = pytest_rs_utils.ImageDepthInColorShapeGetData(params["rosbag_filename"]) themes = [ - {'topic':'/'+params['camera_name']+'/aligned_depth_to_infra1/image_raw', + {'topic':get_node_heirarchy(params)+'/aligned_depth_to_infra1/image_raw', 'msg_type':msg_Image, 'expected_data_chunks':1, #'data':data diff --git a/realsense2_camera/test/rosbag/test_rosbag_imu_test.py b/realsense2_camera/test/rosbag/test_rosbag_imu_test.py index 3d74f5e0e5..4beb7fe5a7 100644 --- a/realsense2_camera/test/rosbag/test_rosbag_imu_test.py +++ b/realsense2_camera/test/rosbag/test_rosbag_imu_test.py @@ -38,6 +38,7 @@ from pytest_rs_utils import delayed_launch_descr_with_parameters from pytest_rs_utils import get_rosbag_file_path +from pytest_rs_utils import get_node_heirarchy test_params_accel = {"rosbag_filename":get_rosbag_file_path("D435i_Depth_and_IMU_Stands_still.bag"), @@ -63,7 +64,7 @@ def test_accel_up_1(self,delayed_launch_descr_with_parameters): params = delayed_launch_descr_with_parameters[1] data = pytest_rs_utils.AccelGetDataDeviceStandStraight(params["rosbag_filename"]) themes = [ - {'topic':'/'+params['camera_name']+'/accel/sample', + {'topic':get_node_heirarchy(params)+'/accel/sample', 'msg_type':msg_Imu, 'expected_data_chunks':1, 'data':data @@ -107,19 +108,19 @@ def test_imu_topics(self,delayed_launch_descr_with_parameters): params = delayed_launch_descr_with_parameters[1] self.rosbag = params["rosbag_filename"] themes = [{ - 'topic':'/'+params['camera_name']+'/imu', + 'topic':get_node_heirarchy(params)+'/imu', 'msg_type':msg_Imu, 'expected_data_chunks':1, #'data':depth_to_color_data }, { - 'topic':'/'+params['camera_name']+'/gyro/sample', + 'topic':get_node_heirarchy(params)+'/gyro/sample', 'msg_type':msg_Imu, 'expected_data_chunks':1, #'data':depth_to_color_data }, { - 'topic':'/'+params['camera_name']+'/accel/sample', + 'topic':get_node_heirarchy(params)+'/accel/sample', 'msg_type':msg_Imu, 'expected_data_chunks':1, #'data':depth_to_color_data diff --git a/realsense2_camera/test/templates/test_integration_template.py b/realsense2_camera/test/templates/test_integration_template.py index 3a40959608..73eb42e3b6 100644 --- a/realsense2_camera/test/templates/test_integration_template.py +++ b/realsense2_camera/test/templates/test_integration_template.py @@ -32,6 +32,7 @@ import pytest_rs_utils from pytest_rs_utils import launch_descr_with_yaml from pytest_rs_utils import get_rosbag_file_path +from pytest_rs_utils import get_node_heirarchy ''' This is a testcase simiar to the integration_fn testcase, the only difference is that @@ -57,7 +58,7 @@ def test_using_function(launch_context,launch_descr_with_yaml): # by now, the camera would have started start = time.time() timeout = 4.0 - camera_name = '/'+params['camera_name']+'/'+params['camera_name'] + camera_name = get_node_heirarchy(params)+'/'+params['camera_name'] while (time.time() - start) < timeout: service_list = subprocess.check_output(['ros2', 'node', 'list']).decode("utf-8") is_node_up = camera_name in service_list @@ -104,7 +105,7 @@ def test_camera_1(self, launch_descr_with_yaml): params = launch_descr_with_yaml[1] themes = [ #{'topic':'/camera/color/image_raw','msg_type':msg_Image,'expected_data_chunks':1}, - {'topic':'/'+params['camera_name']+'/depth/image_rect_raw','msg_type':msg_Image,'expected_data_chunks':1} + {'topic':get_node_heirarchy(params)+'/depth/image_rect_raw','msg_type':msg_Image,'expected_data_chunks':1} ] try: ''' @@ -134,14 +135,14 @@ class TestCamera2(pytest_rs_utils.RsTestBaseClass): def test_camera_2(self,launch_descr_with_yaml): params = launch_descr_with_yaml[1] themes = [ - {'topic':'/'+params['camera_name']+'/depth/image_rect_raw', + {'topic':get_node_heirarchy(params)+'/depth/image_rect_raw', 'msg_type':msg_Image, 'store_raw_data':True, 'expected_data_chunks':1, 'frame_id':params['camera_name']+'_depth_optical_frame', 'height':720, 'width':1280}, - {'topic':'/'+params['camera_name']+'/color/image_raw', + {'topic':get_node_heirarchy(params)+'/color/image_raw', 'msg_type':msg_Image, 'store_raw_data':True, 'expected_data_chunks':1, diff --git a/realsense2_camera/test/templates/test_parameterized_template.py b/realsense2_camera/test/templates/test_parameterized_template.py index 4875038bea..a316caa1dd 100644 --- a/realsense2_camera/test/templates/test_parameterized_template.py +++ b/realsense2_camera/test/templates/test_parameterized_template.py @@ -32,6 +32,7 @@ import pytest_rs_utils from pytest_rs_utils import launch_descr_with_parameters from pytest_rs_utils import get_rosbag_file_path +from pytest_rs_utils import get_node_heirarchy @@ -61,7 +62,7 @@ class TestCamera2(pytest_rs_utils.RsTestBaseClass): def test_node_start(self, launch_descr_with_parameters): params = launch_descr_with_parameters[1] themes = [ - {'topic':'/'+params['camera_name']+'/depth/image_rect_raw', + {'topic':get_node_heirarchy(params)+'/depth/image_rect_raw', 'msg_type':msg_Image, 'store_raw_data':True, 'expected_data_chunks':1, diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index 0fd4563381..63ede24d57 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/realsense2_camera/test/utils/pytest_rs_utils.py @@ -459,13 +459,14 @@ def get_params_string_for_launch(params): camera with a temporary yaml file to hold the parameters. ''' -def get_rs_node_description(name, params): +def get_rs_node_description(params): import tempfile import yaml tmp_yaml = tempfile.NamedTemporaryFile(prefix='launch_rs_',delete=False) params = convert_params(params) ros_params = {"ros__parameters":params} - camera_params = {name+"/"+name: ros_params} + + camera_params = {params['camera_namespace'] +"/"+params['camera_name']: ros_params} with open(tmp_yaml.name, 'w') as f: yaml.dump(camera_params, f) @@ -477,8 +478,8 @@ def get_rs_node_description(name, params): package='realsense2_camera', #namespace=LaunchConfiguration("camera_name"), #name=LaunchConfiguration("camera_name"), - namespace=params["camera_name"], - name=name, + namespace=params["camera_namespace"], + name=params["camera_name"], #prefix=['xterm -e gdb --args'], executable='realsense2_camera_node', parameters=[tmp_yaml.name], @@ -487,6 +488,9 @@ def get_rs_node_description(name, params): emulate_tty=True, ) +def get_node_heirarchy(params): + return "/"+params['camera_namespace'] +"/"+params['camera_name'] + ''' This function returns a launch description with three rs nodes that use the same rosbag file. Test developer can use this as a reference and @@ -501,11 +505,11 @@ def launch_descr_with_yaml(request): params[key] = value if 'camera_name' not in changed_params: params['camera_name'] = 'camera_with_yaml' - first_node = get_rs_node_description(params['camera_name'], params) + first_node = get_rs_node_description(params) return LaunchDescription([ first_node, launch_pytest.actions.ReadyToTest(), - ]),request.param + ]),params ''' This function returns a launch description with a single rs node instance built based on the parameter @@ -519,11 +523,11 @@ def launch_descr_with_parameters(request): params[key] = value if 'camera_name' not in changed_params: params['camera_name'] = 'camera_with_params' - first_node = get_rs_node_description(params['camera_name'], params) + first_node = get_rs_node_description(params) return LaunchDescription([ first_node, launch_pytest.actions.ReadyToTest(), - ]),request.param + ]),params ''' This function returns a launch description with a single rs node instance built based on the parameter @@ -543,11 +547,11 @@ def delayed_launch_descr_with_parameters(request): period = 2.0 if 'delay_ms' in changed_params.keys(): period = changed_params['delay_ms']/1000 - first_node = get_rs_node_description(params['camera_name'], params) + first_node = get_rs_node_description(params) return LaunchDescription([launch.actions.TimerAction( actions = [ first_node,], period=period) - ]),request.param + ]),params ''' This is that holds the test node that listens to a subscription created by a test. From 3fc9f1d15895157763c9be01627288cac4da19ce Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Mon, 28 Aug 2023 21:43:41 +0530 Subject: [PATCH 13/76] Updated readme --- README.md | 48 ++++++++++++++++++++++++------------------------ 1 file changed, 24 insertions(+), 24 deletions(-) diff --git a/README.md b/README.md index ec670e6614..ae9f0638b9 100644 --- a/README.md +++ b/README.md @@ -190,9 +190,9 @@ ### Available Parameters: - For the entire list of parameters type `ros2 param list`. - For reading a parameter value use `ros2 param get ` - - For example: `ros2 param get /camera/camera depth_module.emitter_on_off` + - For example: `ros2 param get /camera/camera depth_module.emitter_enabled` - For setting a new value for a parameter use `ros2 param set ` - - For example: `ros2 param set /camera/camera depth_module.emitter_on_off true` + - For example: `ros2 param set /camera/camera depth_module.emitter_enabled true` #### Parameters that can be modified during runtime: - All of the filters and sensors inner parameters. @@ -379,17 +379,17 @@ translation: The published topics differ according to the device and parameters. After running the above command with D435i attached, the following list of topics will be available (This is a partial list. For full one type `ros2 topic list`): -- /camera/aligned_depth_to_color/camera_info -- /camera/aligned_depth_to_color/image_raw -- /camera/color/camera_info -- /camera/color/image_raw -- /camera/color/metadata -- /camera/depth/camera_info -- /camera/depth/color/points -- /camera/depth/image_rect_raw -- /camera/depth/metadata -- /camera/extrinsics/depth_to_color -- /camera/imu +- /camera/camera/aligned_depth_to_color/camera_info +- /camera/camera/aligned_depth_to_color/image_raw +- /camera/camera/color/camera_info +- /camera/camera/color/image_raw +- /camera/camera/color/metadata +- /camera/camera/depth/camera_info +- /camera/camera/depth/color/points +- /camera/camera/depth/image_rect_raw +- /camera/camera/depth/metadata +- /camera/camera/extrinsics/depth_to_color +- /camera/camera/imu - /diagnostics - /parameter_events - /rosout @@ -406,14 +406,14 @@ ros2 param set /camera/camera enable_gyro true ``` Enabling stream adds matching topics. For instance, enabling the gyro and accel streams adds the following topics: -- /camera/accel/imu_info -- /camera/accel/metadata -- /camera/accel/sample -- /camera/extrinsics/depth_to_accel -- /camera/extrinsics/depth_to_gyro -- /camera/gyro/imu_info -- /camera/gyro/metadata -- /camera/gyro/sample +- /camera/camera/accel/imu_info +- /camera/camera/accel/metadata +- /camera/camera/accel/sample +- /camera/camera/extrinsics/depth_to_accel +- /camera/camera/extrinsics/depth_to_gyro +- /camera/camera/gyro/imu_info +- /camera/camera/gyro/metadata +- /camera/camera/gyro/sample
@@ -445,7 +445,7 @@ ros2 launch realsense2_camera rs_launch.py enable_rgbd:=true enable_sync:=true a The metadata messages store the camera's available metadata in a *json* format. To learn more, a dedicated script for echoing a metadata topic in runtime is attached. For instance, use the following command to echo the camera/depth/metadata topic: ``` -python3 src/realsense-ros/realsense2_camera/scripts/echo_metadada.py /camera/depth/metadata +python3 src/realsense-ros/realsense2_camera/scripts/echo_metadada.py /camera/camera/depth/metadata ```
@@ -455,10 +455,10 @@ python3 src/realsense-ros/realsense2_camera/scripts/echo_metadada.py /camera/dep

The following post processing filters are available: - - ```align_depth```: If enabled, will publish the depth image aligned to the color image on the topic `/camera/aligned_depth_to_color/image_raw`. + - ```align_depth```: If enabled, will publish the depth image aligned to the color image on the topic `/camera/camera/aligned_depth_to_color/image_raw`. - The pointcloud, if created, will be based on the aligned depth image. - ```colorizer```: will color the depth image. On the depth topic an RGB image will be published, instead of the 16bit depth values . - - ```pointcloud```: will add a pointcloud topic `/camera/depth/color/points`. + - ```pointcloud```: will add a pointcloud topic `/camera/camera/depth/color/points`. * The texture of the pointcloud can be modified using the `pointcloud.stream_filter` parameter.
* The depth FOV and the texture FOV are not similar. By default, pointcloud is limited to the section of depth containing the texture. You can have a full depth to pointcloud, coloring the regions beyond the texture with zeros, by setting `pointcloud.allow_no_texture_points` to true. * pointcloud is of an unordered format by default. This can be changed by setting `pointcloud.ordered_pc` to true. From ec52d3b7c5f17136fe4e27c08168a716ea75bb03 Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Mon, 28 Aug 2023 21:58:12 +0530 Subject: [PATCH 14/76] modified service call device_info also --- README.md | 4 ++-- realsense2_camera/src/rs_node_setup.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index ae9f0638b9..c944d59d94 100644 --- a/README.md +++ b/README.md @@ -192,7 +192,7 @@ - For reading a parameter value use `ros2 param get ` - For example: `ros2 param get /camera/camera depth_module.emitter_enabled` - For setting a new value for a parameter use `ros2 param set ` - - For example: `ros2 param set /camera/camera depth_module.emitter_enabled true` + - For example: `ros2 param set /camera/camera depth_module.emitter_enabled 1` #### Parameters that can be modified during runtime: - All of the filters and sensors inner parameters. @@ -487,7 +487,7 @@ Each of the above filters have it's own parameters, following the naming convent Available services -- device_info : retrieve information about the device - serial_number, firmware_version etc. Type `ros2 interface show realsense2_camera_msgs/srv/DeviceInfo` for the full list. Call example: `ros2 service call /camera/device_info realsense2_camera_msgs/srv/DeviceInfo` +- device_info : retrieve information about the device - serial_number, firmware_version etc. Type `ros2 interface show realsense2_camera_msgs/srv/DeviceInfo` for the full list. Call example: `ros2 service call /camera/camera/device_info realsense2_camera_msgs/srv/DeviceInfo`
diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index 4de40a2a41..09740d7a1b 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -404,7 +404,7 @@ void BaseRealSenseNode::updateSensors() void BaseRealSenseNode::publishServices() { _device_info_srv = _node.create_service( - "device_info", + "~/device_info", [&](const realsense2_camera_msgs::srv::DeviceInfo::Request::SharedPtr req, realsense2_camera_msgs::srv::DeviceInfo::Response::SharedPtr res) {getDeviceInfo(req, res);}); From bd385e999c18f2d5a91c82daa858758b02455cb9 Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Mon, 28 Aug 2023 22:09:03 +0530 Subject: [PATCH 15/76] minor change in readme --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index c944d59d94..d672e1d364 100644 --- a/README.md +++ b/README.md @@ -349,7 +349,7 @@ The `/diagnostics` topic includes information regarding the device temperatures ![d435i](https://user-images.githubusercontent.com/99127997/230220297-e392f0fc-63bf-4bab-8001-af1ddf0ed00e.png) ``` -administrator@perclnx466 ~/ros2_humble $ ros2 topic echo /camera/extrinsics/depth_to_color +administrator@perclnx466 ~/ros2_humble $ ros2 topic echo /camera/camera/extrinsics/depth_to_color rotation: - 0.9999583959579468 - 0.008895332925021648 From 5d0695abdfb47f1c6550caaf92bcef0ea32acc1d Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Tue, 29 Aug 2023 08:07:19 +0000 Subject: [PATCH 16/76] fix readme and nodefactory for ros2 run --- realsense2_camera/src/realsense_node_factory.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index 410aacc78f..46b1d13ce1 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -31,7 +31,7 @@ using namespace realsense2_camera; constexpr auto realsense_ros_camera_version = REALSENSE_ROS_EMBEDDED_VERSION_STR; RealSenseNodeFactory::RealSenseNodeFactory(const rclcpp::NodeOptions & node_options) : - Node("camera", "/", node_options), + Node("camera", "/camera", node_options), _logger(this->get_logger()) { init(); From 30a70e1e344ea0b97205e8ee5fdcb1f4cb96a964 Mon Sep 17 00:00:00 2001 From: Samer Khshiboun <99127997+SamerKhshiboun@users.noreply.github.com> Date: Tue, 29 Aug 2023 11:38:51 +0300 Subject: [PATCH 17/76] Update README.md --- README.md | 69 +++++++++++++++++++++++++ realsense2_camera/src/rs_node_setup.cpp | 6 +++ 2 files changed, 75 insertions(+) diff --git a/README.md b/README.md index d672e1d364..de23e1721c 100644 --- a/README.md +++ b/README.md @@ -27,6 +27,7 @@ * [Installation](#installation) * [Usage](#usage) * [Starting the camera node](#start-camera-node) + * [Camera name and namespace](#camera-name-and-namespace) * [Parameters](#parameters) * [ROS2-vs-Optical Coordination Systems](#coordination) * [TF from coordinate A to coordinate B](#tfs) @@ -183,6 +184,74 @@
+

+ Camera Name And Camera Namespace +

+ +### Usage +User can set the camera name and camera namespace, to distinguish between cameras and platforms, which helps identifying the right nodes and topics to work with. + +### Example +- If user have multiple cameras (might be of the same model) and multiple robots then user can choose to launch/run his nodes on this way. +- For the first robot and first camera he will run/launch it with these parameters: + - camera_namespace: + - robot1 + - camera_name + - D455_1 + + - With ros2 launch (via command line or by editing these two parameters in the launch file): + + ```ros2 launch realsense2_camera rs_launch.py camera_namespace:=robot1 camera_name:=D455_1``` + + - With ros2 run (using remapping mechanisim [Reference](https://docs.ros.org/en/foxy/How-To-Guides/Node-arguments.html)): + + ```ros2 run realsense2_camera realsense2_camera_node --ros-args -r __node:=D455_1 -r __ns:=robot1``` + + - Result + ``` + > ros2 node list + /robot1/D455_1 + + > ros2 topic list + /robot1/D455_1/color/camera_info + /robot1/D455_1/color/image_raw + /robot1/D455_1/color/metadata + /robot1/D455_1/depth/camera_info + /robot1/D455_1/depth/image_rect_raw + /robot1/D455_1/depth/metadata + /robot1/D455_1/extrinsics/depth_to_color + /robot1/D455_1/imu + + > ros2 service list + /robot1/D455_1/device_info + ``` + +### Default behavior if non of these parameters are given: + - camera_namespace:=camera + - camera_name:=camera + +``` +> ros2 node list +/camera/camera + +> ros2 topic list +/camera/camera/color/camera_info +/camera/camera/color/image_raw +/camera/camera/color/metadata +/camera/camera/depth/camera_info +/camera/camera/depth/image_rect_raw +/camera/camera/depth/metadata +/camera/camera/extrinsics/depth_to_color +/camera/camera/imu + +> ros2 service list +/camera/camera/device_info +``` + + +
+ +

Parameters

diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index 09740d7a1b..05cb8049a6 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -225,6 +225,8 @@ void BaseRealSenseNode::startPublishers(const std::vector& profi if (sensor.rs2::sensor::is()) rectified_image = true; + // adding "~/" to the topic name will add node namespace and node name to the topic + // see "Private Namespace Substitution Character" section on https://design.ros2.org/articles/topic_and_service_names.html image_raw << "~/" << stream_name << "/image_" << ((rectified_image)?"rect_":"") << "raw"; camera_info << "~/" << stream_name << "/camera_info"; @@ -316,6 +318,8 @@ void BaseRealSenseNode::startRGBDPublisherIfNeeded() { rmw_qos_profile_t qos = _use_intra_process ? qos_string_to_qos(DEFAULT_QOS) : qos_string_to_qos(IMAGE_QOS); + // adding "~/" to the topic name will add node namespace and node name to the topic + // see "Private Namespace Substitution Character" section on https://design.ros2.org/articles/topic_and_service_names.html _rgbd_publisher = _node.create_publisher("~/rgbd", rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos), qos)); } @@ -403,6 +407,8 @@ void BaseRealSenseNode::updateSensors() void BaseRealSenseNode::publishServices() { + // adding "~/" to the service name will add node namespace and node name to the service + // see "Private Namespace Substitution Character" section on https://design.ros2.org/articles/topic_and_service_names.html _device_info_srv = _node.create_service( "~/device_info", [&](const realsense2_camera_msgs::srv::DeviceInfo::Request::SharedPtr req, From 327e60255e814ce5bd6e1b1669c165f84089c050 Mon Sep 17 00:00:00 2001 From: Samer Khshiboun <99127997+SamerKhshiboun@users.noreply.github.com> Date: Tue, 29 Aug 2023 14:48:02 +0300 Subject: [PATCH 18/76] Update README.md --- README.md | 1 - 1 file changed, 1 deletion(-) diff --git a/README.md b/README.md index d672e1d364..4b981efac1 100644 --- a/README.md +++ b/README.md @@ -268,7 +268,6 @@ - On occasions the device was not closed properly and due to firmware issues needs to reset. - If set to true, the device will reset prior to usage. - For example: `initial_reset:=true` -- ****_frame_id**, ****_optical_frame_id**, **aligned_depth_to_**_frame_id**: Specify the different frame_id for the different frames. Especially important when using multiple cameras. - **base_frame_id**: defines the frame_id all static transformations refers to. - **odom_frame_id**: defines the origin coordinate system in ROS convention (X-Forward, Y-Left, Z-Up). pose topic defines the pose relative to that system. From 66ba4ee573eeef1b1fbd880a8b94c1650df8f55c Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Wed, 30 Aug 2023 20:42:53 +0530 Subject: [PATCH 19/76] Added live camera tests --- realsense2_camera/CMakeLists.txt | 35 +++ realsense2_camera/package.xml | 3 +- .../test_camera_all_profile_tests.py | 264 +++++++++++++++++ .../live_camera/test_camera_failing_tests.py | 268 ++++++++++++++++++ .../test/live_camera/test_camera_fps_tests.py | 102 +++++++ .../test/live_camera/test_camera_imu_tests.py | 126 ++++++++ .../test/live_camera/test_d415_basic_tests.py | 150 ++++++++++ .../test/live_camera/test_d455_basic_tests.py | 93 ++++++ .../test/utils/pytest_live_camera_utils.py | 190 +++++++++++++ .../test/utils/pytest_rs_utils.py | 163 +++++++++-- 10 files changed, 1371 insertions(+), 23 deletions(-) create mode 100644 realsense2_camera/test/live_camera/test_camera_all_profile_tests.py create mode 100644 realsense2_camera/test/live_camera/test_camera_failing_tests.py create mode 100644 realsense2_camera/test/live_camera/test_camera_fps_tests.py create mode 100644 realsense2_camera/test/live_camera/test_camera_imu_tests.py create mode 100644 realsense2_camera/test/live_camera/test_d415_basic_tests.py create mode 100644 realsense2_camera/test/live_camera/test_d455_basic_tests.py create mode 100644 realsense2_camera/test/utils/pytest_live_camera_utils.py diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 4eeda43bdf..bb2d19a47d 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -314,6 +314,41 @@ if(BUILD_TESTING) ) endforeach() endforeach() + + unset(_pytest_folders) + + set(rs_query_cmd "rs-enumerate-devices -s") + execute_process(COMMAND bash -c ${rs_query_cmd} + WORKING_DIRECTORY ${PROJECT_SOURCE_DIR} + RESULT_VARIABLE rs_result + OUTPUT_VARIABLE RS_DEVICE_INFO) + message(STATUS "rs_device_info:") + message(STATUS "${RS_DEVICE_INFO}") + if(RS_DEVICE_INFO MATCHES "D455") + message(STATUS "D455 device found") + set(_pytest_live_folders + test/live_camera + ) + elseif(RS_DEVICE_INFO MATCHES "D415") + message(STATUS "D415 device found") + set(_pytest_live_folders + test/live_camera + ) + endif() + + foreach(test_folder ${_pytest_live_folders}) + file(GLOB files "${test_folder}/test_*.py") + foreach(file ${files}) + + get_filename_component(_test_name ${file} NAME_WE) + ament_add_pytest_test(${_test_name} ${file} + APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR}:${CMAKE_SOURCE_DIR}/test/utils:${CMAKE_SOURCE_DIR}/launch:${CMAKE_SOURCE_DIR}/scripts + TIMEOUT 500 + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + ) + endforeach() + endforeach() + endif() # Ament exports diff --git a/realsense2_camera/package.xml b/realsense2_camera/package.xml index faedde2dd2..3afb91e596 100644 --- a/realsense2_camera/package.xml +++ b/realsense2_camera/package.xml @@ -38,7 +38,8 @@ sensor_msgs_py python3-requests tf2_ros_py - + ros2topic + launch_ros ros_environment diff --git a/realsense2_camera/test/live_camera/test_camera_all_profile_tests.py b/realsense2_camera/test/live_camera/test_camera_all_profile_tests.py new file mode 100644 index 0000000000..274ee67625 --- /dev/null +++ b/realsense2_camera/test/live_camera/test_camera_all_profile_tests.py @@ -0,0 +1,264 @@ +# 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. + + +import os +import sys +import itertools + + +import pytest +import rclpy + +from sensor_msgs.msg import Image as msg_Image +from sensor_msgs.msg import Imu as msg_Imu +from sensor_msgs.msg import PointCloud2 as msg_PointCloud2 + +import numpy as np + +sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils")) +import pytest_rs_utils +from pytest_rs_utils import launch_descr_with_parameters +from pytest_rs_utils import delayed_launch_descr_with_parameters +from pytest_rs_utils import get_rosbag_file_path +from pytest_rs_utils import get_node_heirarchy +import pytest_live_camera_utils +from rclpy.parameter import Parameter +from rcl_interfaces.msg import ParameterValue +from rcl_interfaces.srv import SetParameters, GetParameters, ListParameters +from pytest_live_camera_utils import debug_print +def check_if_skip_test(profile, format): + ''' + if profile == 'Color': + if "BGRA8" == format: + return True + if "RGBA8" == format: + return True + if "Y8" == format: + return True + elif profile == 'Depth': + if "Z16" == format: + return True + el + if profile == 'Infrared': + if "Y8" == format: + return True + if "Y16" == format: + return True + if "BGRA8" == format: + return True + if "RGBA8" == format: + return True + if "Y10BPACK" == format: + return True + if "UYVY" == format: + return True + if "BGR8" == format: + return True + if "RGB8" == format: + return True + if "RAW10" == format: + return True + elif profile == 'Infrared1': + if "Y8" ==format: + return True + if "Y16" ==format: + return True + if "Y10BPACK" == format: + return True + if "UYVY" ==format: + return True + if "BGR8" ==format: + return True + if "RGB8" ==format: + return True + if "RAW10" ==format: + return True + if profile == 'Infrared2': + if "Y8" == format: + return True + if "Y16" == format: + return True + if "Y10BPACK" == format: + return True + if "UYVY" == format: + return True + if "BGR8" == format: + return True + if "RGB8" == format: + return True + if "RAW10" == format: + return True + ''' + if profile == 'Infrared': + if "Y8" == format: + return True + if "Y16" == format: + return True + if profile == 'Infrared1': + if "Y8" ==format: + return True + if "Y16" ==format: + return True + if profile == 'Infrared2': + if "Y8" == format: + return True + if "Y16" == format: + return True + return False + + +test_params_all_profiles_d455 = { + 'camera_name': 'D455', + 'device_type': 'D455', + } +test_params_all_profiles_d415 = { + 'camera_name': 'D415', + 'device_type': 'D415', + } +test_params_all_profiles_d435 = { + 'camera_name': 'D435', + 'device_type': 'D435', + } + + +''' +This test was implemented as a template to set the parameters and run the test. +This directory is not added to the CMakeLists.txt so as to avoid the colcon failure in the +machines that don't have the D455 connected. +1. Only a subset of parameter types are implemented in py_rs_utils, it has to be extended for others +2. After setting the param, rclpy.spin_once may be needed.Test passes even without this though. +''' +@pytest.mark.parametrize("launch_descr_with_parameters", [ + pytest.param(test_params_all_profiles_d455, marks=pytest.mark.d455), + pytest.param(test_params_all_profiles_d415, marks=pytest.mark.d415), + pytest.param(test_params_all_profiles_d435, marks=pytest.mark.d435),] + ,indirect=True) +@pytest.mark.launch(fixture=launch_descr_with_parameters) +class TestLiveCamera_Change_Resolution(pytest_rs_utils.RsTestBaseClass): + def test_LiveCamera_Change_Resolution(self,launch_descr_with_parameters): + skipped_tests = [] + failed_tests = [] + num_passed = 0 + num_failed = 0 + params = launch_descr_with_parameters[1] + themes = [{'topic':get_node_heirarchy(params)+'/color/image_raw', 'msg_type':msg_Image,'expected_data_chunks':1}] + config = pytest_live_camera_utils.get_profile_config(get_node_heirarchy(params)) + try: + ''' + initialize, run and check the data + ''' + self.init_test("RsTest"+params['camera_name']) + self.spin_for_time(wait_time=1.0) + cap = pytest_live_camera_utils.get_camera_capabilities(params['device_type']) + if cap == None: + debug_print("Device not found? : " + params['device_type']) + return + self.create_param_ifs(get_node_heirarchy(params)) + config["Color"]["default_profile1"],config["Color"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["color_profile"], "Color") + config["Depth"]["default_profile1"],config["Depth"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["depth_profile"], "Depth") + config["Infrared"]["default_profile1"],config["Infrared"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["depth_profile"], "Infrared") + config["Infrared1"]["default_profile1"],config["Infrared1"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["depth_profile"], "Infrared1") + config["Infrared2"]["default_profile1"],config["Infrared2"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["depth_profile"], "Infrared2") + for key in cap["color_profile"]: + profile_type = key[0] + profile = key[1] + format = key[2] + if check_if_skip_test(profile_type, format): + skipped_tests.append(" ".join(key)) + continue + print("Testing " + " ".join(key)) + themes[0]['topic'] = config[profile_type]['topic'] + themes[0]['width'] = int(profile.split('x')[0]) + themes[0]['height'] = int(profile.split('x')[1]) + if themes[0]['width'] == int(config[profile_type]["default_profile2"].split('x')[0]): + self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile1"]) + else: + self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile2"]) + self.set_bool_param(config[profile_type]["param"], True) + self.disable_all_params() + self.spin_for_time(wait_time=0.2) + self.set_string_param(config[profile_type]["profile"], profile) + self.set_string_param(config[profile_type]["format"], format) + self.set_bool_param(config[profile_type]["param"], True) + try: + ret = self.run_test(themes) + assert ret[0], ret[1] + assert self.process_data(themes), " ".join(key) + " failed" + num_passed += 1 + except Exception as e: + exc_type, exc_obj, exc_tb = sys.exc_info() + fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1] + print("Test failed") + print(e) + print(exc_type, fname, exc_tb.tb_lineno) + num_failed += 1 + failed_tests.append(" ".join(key)) + debug_print("Color tests completed") + for key in cap["depth_profile"]: + profile_type = key[0] + profile = key[1] + format = key[2] + if check_if_skip_test(profile_type, format): + skipped_tests.append(" ".join(key)) + continue + print("Testing " + " ".join(key)) + + themes[0]['topic'] = config[profile_type]['topic'] + themes[0]['width'] = int(profile.split('x')[0]) + themes[0]['height'] = int(profile.split('x')[1]) + if themes[0]['width'] == int(config[profile_type]["default_profile2"].split('x')[0]): + self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile1"]) + else: + self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile2"]) + self.set_bool_param(config[profile_type]["param"], True) + + + self.disable_all_params() + self.spin_for_time(wait_time=0.2) + self.set_string_param(config[profile_type]["profile"], profile) + self.set_string_param(config[profile_type]["format"], format) + self.set_bool_param(config[profile_type]["param"], True) + try: + ret = self.run_test(themes) + assert ret[0], ret[1] + assert self.process_data(themes), " ".join(key) + " failed" + num_passed += 1 + except Exception as e: + print("Test failed") + print(e) + num_failed += 1 + failed_tests.append(" ".join(key)) + debug_print("Depth tests completed") + finally: + #this step is important because the test will fail next time + pytest_rs_utils.kill_realsense2_camera_node() + self.shutdown() + print("Tests passed " + str(num_passed)) + print("Tests skipped " + str(len(skipped_tests))) + if len(skipped_tests): + debug_print("\nSkipped tests:" + params['device_type']) + debug_print("\n".join(skipped_tests)) + print("Tests failed " + str(num_failed)) + if len(failed_tests): + print("\nFailed tests:" + params['device_type']) + print("\n".join(failed_tests)) + + def disable_all_params(self): + self.set_bool_param('enable_color', False) + self.set_bool_param('enable_depth', False) + self.set_bool_param('enable_infra', False) + self.set_bool_param('enable_infra1', False) + self.set_bool_param('enable_infra2', False) + diff --git a/realsense2_camera/test/live_camera/test_camera_failing_tests.py b/realsense2_camera/test/live_camera/test_camera_failing_tests.py new file mode 100644 index 0000000000..3bdc87b19b --- /dev/null +++ b/realsense2_camera/test/live_camera/test_camera_failing_tests.py @@ -0,0 +1,268 @@ +# 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. + + +import os +import sys +import itertools + + +import pytest +import rclpy + +from sensor_msgs.msg import Image as msg_Image +from sensor_msgs.msg import Imu as msg_Imu +from sensor_msgs.msg import PointCloud2 as msg_PointCloud2 + +import numpy as np + +sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils")) +import pytest_rs_utils +from pytest_rs_utils import launch_descr_with_parameters +from pytest_rs_utils import delayed_launch_descr_with_parameters +from pytest_rs_utils import get_rosbag_file_path +from pytest_rs_utils import get_node_heirarchy +import pytest_live_camera_utils +from rclpy.parameter import Parameter +from rcl_interfaces.msg import ParameterValue +from rcl_interfaces.srv import SetParameters, GetParameters, ListParameters +from pytest_live_camera_utils import debug_print + +test_params_all_profiles_d455 = { + 'camera_name': 'D455', + 'device_type': 'D455', + } +test_params_all_profiles_d415 = { + 'camera_name': 'D415', + 'device_type': 'D415', + } +''' +This test was implemented as a template to set the parameters and run the test. +This directory is not added to the CMakeLists.txt so as to avoid the colcon failure in the +machines that don't have the D455 connected. +1. Only a subset of parameter types are implemented in py_rs_utils, it has to be extended for others +2. After setting the param, rclpy.spin_once may be needed.Test passes even without this though. +''' +@pytest.mark.parametrize("launch_descr_with_parameters", [test_params_all_profiles_d415],indirect=True) +@pytest.mark.launch(fixture=launch_descr_with_parameters) +class TestLiveCamera_Change_Resolution(pytest_rs_utils.RsTestBaseClass): + def test_LiveCamera_Change_Resolution(self,launch_descr_with_parameters): + passed_tests = [] + failed_tests = [] + num_passed = 0 + num_failed = 0 + wait_time_s = 1.2 + cap = {} + ''' + need two configurations with different profiles(height & width) for each profile, + this is to ensure the test sets a different profile first, before testing the + actual profile to be tested. + ''' + cap['color_profile'] = [ + ['Color', '1920x1080x30','RGB8'], + ['Color', '1280x720x30','RGB8'], + ] + cap['depth_profile'] = [ + ['Infrared1', '1920x1080x25', 'Y8'], + ['Infrared1', '1920x1080x25', 'Y16'], + ['Infrared1', '1920x1080x15', 'Y16'], + ['Infrared', '848x100x100', 'BGRA8'], + ['Infrared', '640x480x60', 'BGRA8'], + ['Infrared', '640x480x60', 'RGBA8'], + ['Infrared', '640x480x60', 'RGB8'], + ['Infrared', '640x360x60', 'RGBA8'], + ['Infrared', '640x360x60', 'RGB8'], + ['Infrared', '640x360x30', 'UYVY'], + ['Infrared', '480x270x15', 'RGB8'], + ['Infrared', '424x240x60', 'BGRA8'], + ['Infrared', '424x240x30', 'UYVY'], + ['Infrared1', '1920x1080x15', 'Y8'], + ['Infrared1', '1280x720x30', 'Y8'], + ['Infrared1', '1280x720x15', 'Y8'], + ['Infrared1', '1280x720x6', 'Y8'], + ['Infrared1', '960x540x25', 'Y16'], + ['Infrared1', '960x540x15', 'Y16'], + ['Infrared1', '848x480x90', 'Y8'], + ['Infrared1', '848x480x60', 'Y8'], + ['Infrared1', '848x480x30', 'Y8'], + ['Infrared1', '848x480x15', 'Y8'], + ['Infrared1', '848x480x6', 'Y8'], + ['Infrared1', '848x100x100', 'Y8'], + ['Infrared1', '640x480x90', 'Y8'], + ['Infrared1', '640x480x60', 'Y8'], + ['Infrared1', '640x480x30', 'Y8'], + ['Infrared1', '640x480x15', 'Y8'], + ['Infrared1', '640x480x6', 'Y8'], + ['Infrared1', '640x360x90', 'Y8'], + ['Infrared1', '640x360x60', 'Y8'], + ['Infrared1', '640x360x30', 'Y8'], + ['Infrared1', '640x360x15', 'Y8'], + ['Infrared1', '640x360x6', 'Y8'], + ['Infrared1', '480x270x90', 'Y8'], + ['Infrared1', '480x270x60', 'Y8'], + ['Infrared1', '480x270x30', 'Y8'], + ['Infrared1', '480x270x15', 'Y8'], + ['Infrared1', '480x270x6', 'Y8'], + ['Infrared1', '424x240x90', 'Y8'], + ['Infrared1', '424x240x60', 'Y8'], + ['Infrared1', '424x240x30', 'Y8'], + ['Infrared1', '424x240x15', 'Y8'], + ['Infrared1', '424x240x6', 'Y8'], + ['Infrared2', '1920x1080x25', 'Y16'], + ['Infrared2', '1920x1080x25', 'Y8'], + ['Infrared2', '1920x1080x15', 'Y16'], + ['Infrared2', '1920x1080x15', 'Y8'], + ['Infrared2', '1280x720x30', 'Y8'], + ['Infrared2', '1280x720x15', 'Y8'], + ['Infrared2', '1280x720x6', 'Y8'], + ['Infrared2', '960x540x25', 'Y16'], + ['Infrared2', '960x540x15', 'Y16'], + ['Infrared2', '848x480x90', 'Y8'], + ['Infrared2', '848x480x60', 'Y8'], + ['Infrared2', '848x480x30', 'Y8'], + ['Infrared2', '848x480x15', 'Y8'], + ['Infrared2', '848x480x6', 'Y8'], + ['Infrared2', '848x100x100', 'Y8'], + ['Infrared2', '640x480x90', 'Y8'], + ['Infrared2', '640x480x60', 'Y8'], + ['Infrared2', '640x480x30', 'Y8'], + ['Infrared2', '640x480x15', 'Y8'], + ['Infrared2', '640x480x6', 'Y8'], + ['Infrared2', '640x360x90', 'Y8'], + ['Infrared2', '640x360x60', 'Y8'], + ['Infrared2', '640x360x30', 'Y8'], + ['Infrared2', '640x360x15', 'Y8'], + ['Infrared2', '640x360x6', 'Y8'], + ['Infrared2', '480x270x90', 'Y8'], + ['Infrared2', '480x270x60', 'Y8'], + ['Infrared2', '480x270x30', 'Y8'], + ['Infrared2', '480x270x15', 'Y8'], + ['Infrared2', '480x270x6', 'Y8'], + ['Infrared2', '424x240x90', 'Y8'], + ['Infrared2', '424x240x60', 'Y8'], + ['Infrared2', '424x240x30', 'Y8'], + ['Infrared2', '424x240x15', 'Y8'], + ['Infrared2', '424x240x6', 'Y8'], + ['Depth', '640x360x30', 'Z16'], + ['Depth', '480x270x30', 'Z16'], + ['Depth', '424x240x30', 'Z16'], + ] + params = launch_descr_with_parameters[1] + themes = [{'topic':get_node_heirarchy(params)+'/color/image_raw', 'msg_type':msg_Image,'expected_data_chunks':1,'initial_reset':True}] + config = pytest_live_camera_utils.get_profile_config(get_node_heirarchy(params)) + try: + ''' + initialize, run and check the data + ''' + serial_no = None + if 'serial_no' in params: + serial_no = params['serial_no'] + self.init_test("RsTest"+params['camera_name']) + self.spin_for_time(wait_time=1.0) + if cap == None: + debug_print("Device not found? : " + params['device_type']) + return + self.create_param_ifs(get_node_heirarchy(params)) + config["Color"]["default_profile1"],config["Color"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["color_profile"], "Color") + config["Depth"]["default_profile1"],config["Depth"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["depth_profile"], "Depth") + config["Infrared"]["default_profile1"],config["Infrared"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["depth_profile"], "Infrared") + config["Infrared1"]["default_profile1"],config["Infrared1"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["depth_profile"], "Infrared1") + config["Infrared2"]["default_profile1"],config["Infrared2"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["depth_profile"], "Infrared2") + for key in cap["color_profile"]: + profile_type = key[0] + profile = key[1] + format = key[2] + print("Testing " + " ".join(key)) + themes[0]['topic'] = config[profile_type]['topic'] + themes[0]['width'] = int(profile.split('x')[0]) + themes[0]['height'] = int(profile.split('x')[1]) + if themes[0]['width'] == int(config[profile_type]["default_profile2"].split('x')[0]): + self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile1"]) + else: + self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile2"]) + self.set_bool_param(config[profile_type]["param"], True) + self.disable_all_params() + self.spin_for_time(wait_time=wait_time_s) + self.set_string_param(config[profile_type]["profile"], profile) + self.set_string_param(config[profile_type]["format"], format) + self.set_bool_param(config[profile_type]["param"], True) + self.spin_for_time(wait_time=wait_time_s) + try: + ret = self.run_test(themes) + assert ret[0], ret[1] + assert self.process_data(themes), " ".join(key) + " failed" + num_passed += 1 + passed_tests.append(" ".join(key)) + except Exception as e: + exc_type, exc_obj, exc_tb = sys.exc_info() + fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1] + print("Test failed") + print(e) + print(exc_type, fname, exc_tb.tb_lineno) + num_failed += 1 + failed_tests.append(" ".join(key)) + debug_print("Color tests completed") + for key in cap["depth_profile"]: + profile_type = key[0] + profile = key[1] + format = key[2] + print("Testing " + " ".join(key)) + + themes[0]['topic'] = config[profile_type]['topic'] + themes[0]['width'] = int(profile.split('x')[0]) + themes[0]['height'] = int(profile.split('x')[1]) + if themes[0]['width'] == int(config[profile_type]["default_profile2"].split('x')[0]): + self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile1"]) + else: + self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile2"]) + self.set_bool_param(config[profile_type]["param"], True) + + + self.disable_all_params() + self.spin_for_time(wait_time=wait_time_s) + self.set_string_param(config[profile_type]["profile"], profile) + self.set_string_param(config[profile_type]["format"], format) + self.set_bool_param(config[profile_type]["param"], True) + self.spin_for_time(wait_time=wait_time_s) + try: + ret = self.run_test(themes) + assert ret[0], ret[1] + assert self.process_data(themes), " ".join(key) + " failed" + num_passed += 1 + passed_tests.append(" ".join(key)) + except Exception as e: + print("Test failed") + print(e) + num_failed += 1 + failed_tests.append(" ".join(key)) + debug_print("Depth tests completed") + finally: + #this step is important because the test will fail next time + pytest_rs_utils.kill_realsense2_camera_node() + self.shutdown() + print("Tests passed " + str(num_passed)) + if len(passed_tests): + debug_print("\nPassed tests:" + params['device_type']) + debug_print("\n".join(passed_tests)) + print("Tests failed " + str(num_failed)) + if len(failed_tests): + print("\nFailed tests:" + params['device_type']) + print("\n".join(failed_tests)) + + def disable_all_params(self): + self.set_bool_param('enable_color', False) + self.set_bool_param('enable_depth', False) + self.set_bool_param('enable_infra', False) + self.set_bool_param('enable_infra1', False) + self.set_bool_param('enable_infra2', False) \ No newline at end of file diff --git a/realsense2_camera/test/live_camera/test_camera_fps_tests.py b/realsense2_camera/test/live_camera/test_camera_fps_tests.py new file mode 100644 index 0000000000..cff5cadbb4 --- /dev/null +++ b/realsense2_camera/test/live_camera/test_camera_fps_tests.py @@ -0,0 +1,102 @@ +# 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. + + +import os +import sys +import itertools + + +import pytest +import rclpy + +from sensor_msgs.msg import Image as msg_Image +from sensor_msgs.msg import Imu as msg_Imu +from sensor_msgs.msg import PointCloud2 as msg_PointCloud2 + +import numpy as np + +sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils")) +import pytest_rs_utils +from pytest_rs_utils import launch_descr_with_parameters + +from pytest_rs_utils import get_rosbag_file_path +from pytest_rs_utils import get_node_heirarchy + +from rclpy.parameter import Parameter +from rcl_interfaces.msg import ParameterValue +from rcl_interfaces.srv import SetParameters, GetParameters, ListParameters + +test_params_depth_avg_1 = { + 'camera_name': 'D455', + 'device_type': 'D455', + } +''' +The test was implemented to check the fps of Depth and Color frames. The RosTopicHz had to be +modified to make it work, see py_rs_utils for more details. +To check the fps, a value 'expected_fps_in_hz' has to be added to the corresponding theme +''' +@pytest.mark.parametrize("launch_descr_with_parameters", [test_params_depth_avg_1],indirect=True) +@pytest.mark.launch(fixture=launch_descr_with_parameters) +class TestCamera_TestFPS(pytest_rs_utils.RsTestBaseClass): + def test_camera_test_fps(self,launch_descr_with_parameters): + params = launch_descr_with_parameters[1] + try: + ''' + initialize, run and check the data + ''' + print("Starting camera test...") + self.init_test("RsTest"+params['camera_name']) + self.wait_for_node(params['camera_name']) + self.create_param_ifs(get_node_heirarchy(params)) + #assert self.set_bool_param('enable_color', False) + + themes = [ + {'topic':get_node_heirarchy(params)+'/depth/image_rect_raw', + 'msg_type':msg_Image, + 'expected_data_chunks':100, + } + ] + profiles = ['640x480x5','640x480x15', '640x480x30', '640x480x90'] + for profile in profiles: + print("Testing profile: ", profile) + themes[0]['expected_fps_in_hz'] = float(profile.split('x')[2]) + assert self.set_string_param('depth_module.profile', profile) + assert self.set_bool_param('enable_depth', True) + ret = self.run_test(themes, timeout=25.0) + assert ret[0], ret[1] + assert self.process_data(themes) + + themes = [ + {'topic':get_node_heirarchy(params)+'/color/image_raw', + 'msg_type':msg_Image, + 'expected_data_chunks':100, + } + ] + profiles = ['640x480x5','640x480x15', '640x480x30', '1280x720x30'] + for profile in profiles: + print("Testing profile: ", profile) + themes[0]['expected_fps_in_hz'] = float(profile.split('x')[2]) + assert self.set_string_param('rgb_camera.profile', profile) + assert self.set_bool_param('enable_color', True) + ret = self.run_test(themes, timeout=25.0) + assert ret[0], ret[1] + assert self.process_data(themes) + + finally: + #this step is important because the test will fail next time + pytest_rs_utils.kill_realsense2_camera_node() + self.shutdown() + + diff --git a/realsense2_camera/test/live_camera/test_camera_imu_tests.py b/realsense2_camera/test/live_camera/test_camera_imu_tests.py new file mode 100644 index 0000000000..fa173e0a8e --- /dev/null +++ b/realsense2_camera/test/live_camera/test_camera_imu_tests.py @@ -0,0 +1,126 @@ +# 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. + + +import os +import sys +import itertools + + +import pytest +import rclpy + +from sensor_msgs.msg import Image as msg_Image +from sensor_msgs.msg import Imu as msg_Imu +from sensor_msgs.msg import PointCloud2 as msg_PointCloud2 + +import numpy as np + +sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils")) +import pytest_rs_utils +from pytest_rs_utils import launch_descr_with_parameters +from pytest_rs_utils import delayed_launch_descr_with_parameters +from pytest_rs_utils import get_rosbag_file_path +from pytest_rs_utils import get_node_heirarchy + +import pytest_live_camera_utils +from rclpy.parameter import Parameter +from rcl_interfaces.msg import ParameterValue +from rcl_interfaces.srv import SetParameters, GetParameters, ListParameters +from pytest_live_camera_utils import debug_print + +test_params_all_profiles_d455 = { + 'camera_name': 'D455', + 'device_type': 'D455', + 'enable_accel':True, + 'enable_gyro':True, + 'unite_imu_method':1, + } + +@pytest.mark.parametrize("launch_descr_with_parameters", [ + pytest.param(test_params_all_profiles_d455, marks=pytest.mark.d455) + ],indirect=True) +@pytest.mark.launch(fixture=launch_descr_with_parameters) +class TestLiveCamera_TestMotionSensor(pytest_rs_utils.RsTestBaseClass): + def test_LiveCamera_check_motion_sensor(self,launch_descr_with_parameters): + params = launch_descr_with_parameters[1] + themes = [{'topic':get_node_heirarchy(params)+'/imu', 'msg_type':msg_Imu,'expected_data_chunks':1}, + {'topic':get_node_heirarchy(params)+'/gyro/sample', 'msg_type':msg_Imu,'expected_data_chunks':1}, + {'topic':get_node_heirarchy(params)+'/accel/sample', 'msg_type':msg_Imu,'expected_data_chunks':1}] + IMU_TOPIC = 0 + GYRO_TOPIC = 1 + ACCEL_TOPIC = 2 + if params['unite_imu_method'] == '0': + themes[IMU_TOPIC]['expected_data_chunks'] = 0 + try: + ''' + initialize, run and check the data + ''' + msg = "Test with the default params " + self.init_test("RsTest"+params['camera_name']) + self.create_param_ifs(get_node_heirarchy(params)) + print(msg) + ret = self.run_test(themes) + assert ret[0], msg + str(ret[1]) + assert self.process_data(themes), msg + " failed" + + + msg = "Test with the accel false " + self.set_bool_param('enable_accel', False) + self.set_bool_param('enable_gyro', True) + ''' + This step fails if the next line is commented + ''' + self.set_integer_param('unite_imu_method', 0) #this shouldn't matter because the unite_imu_method cannot be changed + themes[ACCEL_TOPIC]['expected_data_chunks'] = 0 + print(msg) + ret = self.run_test(themes) + assert ret[0], msg + str(ret[1]) + assert self.process_data(themes), msg + " failed" + ''' + Test fails + ''' + + msg = "Test with the gyro false " + self.set_bool_param('enable_accel', True) + self.set_bool_param('enable_gyro', False) + self.set_integer_param('unite_imu_method', 0) #this shouldn't matter because the unite_imu_method cannot be changed + themes[IMU_TOPIC]['expected_data_chunks'] = 1 + themes[ACCEL_TOPIC]['expected_data_chunks'] = 1 + themes[GYRO_TOPIC]['expected_data_chunks'] = 0 + print(msg) + ret = self.run_test(themes) + assert ret[0], msg + str(ret[1]) + assert self.process_data(themes), msg + " failed" + + ''' + Test fails, both gyro and accel data is available, not imu + ''' + msg = "Test with both gyro and accel false " + self.set_bool_param('enable_accel', False) + self.set_bool_param('enable_gyro', False) + self.set_integer_param('unite_imu_method', 1) #this shouldn't matter because the unite_imu_method cannot be changed + + themes[ACCEL_TOPIC]['expected_data_chunks'] = 0 + themes[GYRO_TOPIC]['expected_data_chunks'] = 0 + themes[IMU_TOPIC]['expected_data_chunks'] = 1 #Seems that imu data is available even if gyro and accel is disabled + print(msg) + ret = self.run_test(themes, initial_wait_time=1.0) #wait_time added as test doesn't have to wait for any data + assert ret[0], msg + " failed" + assert self.process_data(themes), msg +" failed" + + finally: + #this step is important because the test will fail next time + pytest_rs_utils.kill_realsense2_camera_node() + self.shutdown() diff --git a/realsense2_camera/test/live_camera/test_d415_basic_tests.py b/realsense2_camera/test/live_camera/test_d415_basic_tests.py new file mode 100644 index 0000000000..8b88a30e67 --- /dev/null +++ b/realsense2_camera/test/live_camera/test_d415_basic_tests.py @@ -0,0 +1,150 @@ +# 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. + + +import os +import sys +import itertools + + +import pytest +import rclpy + +from sensor_msgs.msg import Image as msg_Image +from sensor_msgs.msg import Imu as msg_Imu +from sensor_msgs.msg import PointCloud2 as msg_PointCloud2 + +import numpy as np + +sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils")) +import pytest_rs_utils +from pytest_rs_utils import launch_descr_with_parameters +from pytest_rs_utils import get_rosbag_file_path +from pytest_rs_utils import get_node_heirarchy +import pytest_live_camera_utils + +from rclpy.parameter import Parameter +from rcl_interfaces.msg import ParameterValue +from rcl_interfaces.srv import SetParameters, GetParameters, ListParameters + +test_params_depth_avg_1 = { + 'camera_name': 'D415', + 'device_type': 'D415', + } +''' +This test was implemented as a template to set the parameters and run the test. +This directory is not added to the CMakeLists.txt so as to avoid the colcon failure in the +machines that don't have the D415 connected. +1. Only a subset of parameter types are implemented in py_rs_utils, it has to be extended for others +2. After setting the param, rclpy.spin_once may be needed.Test passes even without this though. +''' +@pytest.mark.parametrize("launch_descr_with_parameters", [test_params_depth_avg_1],indirect=True) +@pytest.mark.launch(fixture=launch_descr_with_parameters) +class TestD415_Change_Resolution(pytest_rs_utils.RsTestBaseClass): + def test_D415_Change_Resolution(self,launch_descr_with_parameters): + params = launch_descr_with_parameters[1] + failed_tests = [] + num_passed = 0 + num_failed = 0 + themes = [{'topic':get_node_heirarchy(params)+'/color/image_raw', 'msg_type':msg_Image,'expected_data_chunks':1}] + config = pytest_live_camera_utils.get_profile_config(get_node_heirarchy(params)) + config["Color"]["default_profile1"] = "640x480x6" + config["Color"]["default_profile2"] = "1280x720x6" + config["Depth"]["default_profile1"] = "640x480x6" + config["Depth"]["default_profile2"] = "1280x720x6" + config["Infrared"]["default_profile1"] = "640x480x6" + config["Infrared"]["default_profile2"] = "1280x720x6" + config["Infrared1"]["default_profile1"] = "640x480x6" + config["Infrared1"]["default_profile2"] = "1280x720x6" + config["Infrared2"]["default_profile1"] = "640x480x6" + config["Infrared2"]["default_profile2"] = "1280x720x6" + + cap = [ + #['Infrared1', '1920x1080x25', 'Y8'], + #['Infrared1', '1920x1080x15', 'Y16'], + ['Infrared', '848x100x100', 'BGRA8'], + ['Infrared', '848x480x60', 'RGBA8'], + ['Infrared', '640x480x60', 'RGBA8'], + ['Infrared', '640x480x60', 'BGR8'], + ['Infrared', '640x360x60', 'BGRA8'], + ['Infrared', '640x360x60', 'BGR8'], + ['Infrared', '640x360x30', 'UYVY'], + ['Infrared', '480x270x60', 'BGRA8'], + ['Infrared', '480x270x60', 'RGB8'], + ['Infrared', '424x240x60', 'BGRA8'], + + ] + try: + ''' + initialize, run and check the data + ''' + self.init_test("RsTest"+params['camera_name']) + self.spin_for_time(wait_time=1.0) + self.create_param_ifs(get_node_heirarchy(params)) + + for key in cap: + profile_type = key[0] + profile = key[1] + format = key[2] + print("Testing " + " ".join(key)) + themes[0]['topic'] = config[profile_type]['topic'] + themes[0]['width'] = int(profile.split('x')[0]) + themes[0]['height'] = int(profile.split('x')[1]) + #''' + if themes[0]['width'] == int(config[profile_type]["default_profile2"].split('x')[0]): + self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile1"]) + else: + self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile2"]) + self.set_bool_param(config[profile_type]["param"], True) + self.disable_all_params() + #self.set_string_param("depth_profile", "640x480x6") + #self.set_bool_param("enable_depth", True) + #''' + self.spin_for_time(wait_time=0.2) + self.set_string_param(config[profile_type]["profile"], profile) + self.set_string_param(config[profile_type]["format"], format) + self.set_bool_param(config[profile_type]["param"], True) + try: + ret = self.run_test(themes, timeout=5.0) + assert ret[0], ret[1] + assert self.process_data(themes), " ".join(key) + " failed" + num_passed += 1 + except Exception as e: + exc_type, exc_obj, exc_tb = sys.exc_info() + fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1] + print("Test failed") + print(e) + print(exc_type, fname, exc_tb.tb_lineno) + num_failed += 1 + failed_tests.append(" ".join(key)) + finally: + #this step is important because the test will fail next time + pytest_rs_utils.kill_realsense2_camera_node() + self.shutdown() + if num_failed != 0: + print("Failed tests:") + print("\n".join(failed_tests)) + assert False, " Tests failed" + + + def disable_all_params(self): + ''' + self.set_bool_param('enable_color', False) + self.set_bool_param('enable_depth', False) + self.set_bool_param('enable_infra', False) + self.set_bool_param('enable_infra1', False) + self.set_bool_param('enable_infra2', False) + ''' + pass + diff --git a/realsense2_camera/test/live_camera/test_d455_basic_tests.py b/realsense2_camera/test/live_camera/test_d455_basic_tests.py new file mode 100644 index 0000000000..fcf4561714 --- /dev/null +++ b/realsense2_camera/test/live_camera/test_d455_basic_tests.py @@ -0,0 +1,93 @@ +# 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. + + +import os +import sys +import itertools + + +import pytest +import rclpy + +from sensor_msgs.msg import Image as msg_Image +from sensor_msgs.msg import Imu as msg_Imu +from sensor_msgs.msg import PointCloud2 as msg_PointCloud2 + +import numpy as np + +sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils")) +import pytest_rs_utils +from pytest_rs_utils import launch_descr_with_parameters + +from pytest_rs_utils import get_rosbag_file_path +from pytest_rs_utils import get_node_heirarchy + +from rclpy.parameter import Parameter +from rcl_interfaces.msg import ParameterValue +from rcl_interfaces.srv import SetParameters, GetParameters, ListParameters + +test_params_depth_avg_1 = { + 'camera_name': 'D455', + 'device_type': 'D455', + } +''' +This test was implemented as a template to set the parameters and run the test. +This directory is not added to the CMakeLists.txt so as to avoid the colcon failure in the +machines that don't have the D455 connected. +1. Only a subset of parameter types are implemented in py_rs_utils, it has to be extended for others +2. After setting the param, rclpy.spin_once may be needed.Test passes even without this though. +''' +@pytest.mark.parametrize("launch_descr_with_parameters", [test_params_depth_avg_1],indirect=True) +@pytest.mark.launch(fixture=launch_descr_with_parameters) +class TestD455_Change_Resolution(pytest_rs_utils.RsTestBaseClass): + def test_D455_Change_Resolution(self,launch_descr_with_parameters): + params = launch_descr_with_parameters[1] + themes = [ + {'topic':get_node_heirarchy(params)+'/color/image_raw', + 'msg_type':msg_Image, + 'expected_data_chunks':1, + 'width':640, + 'height':480, + #'data':data + } + ] + try: + ''' + initialize, run and check the data + ''' + print("Starting camera test...") + self.init_test("RsTest"+params['camera_name']) + self.wait_for_node(params['camera_name']) + self.create_param_ifs(get_node_heirarchy(params)) + #assert self.set_bool_param('enable_color', False) + assert self.set_string_param('rgb_camera.profile', '640x480x30') + assert self.set_bool_param('enable_color', True) + ret = self.run_test(themes) + assert ret[0], ret[1] + assert self.process_data(themes) + self.set_string_param('rgb_camera.profile', '1280x800x5') + self.set_bool_param('enable_color', True) + themes[0]['width'] = 1280 + themes[0]['height'] = 800 + + ret = self.run_test(themes) + assert ret[0], ret[1] + assert self.process_data(themes) + finally: + #this step is important because the test will fail next time + pytest_rs_utils.kill_realsense2_camera_node() + self.shutdown() + + diff --git a/realsense2_camera/test/utils/pytest_live_camera_utils.py b/realsense2_camera/test/utils/pytest_live_camera_utils.py new file mode 100644 index 0000000000..3a5ab6f600 --- /dev/null +++ b/realsense2_camera/test/utils/pytest_live_camera_utils.py @@ -0,0 +1,190 @@ +# 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. +import os +import sys +import time +import ctypes +import struct +import requests +import json + +from pytest_rs_utils import debug_print + + +def get_profile_config(camera_name): + config = { + "Color":{"profile":"rgb_camera.profile", "format":'rgb_camera.color_format', "param":"enable_color", "topic":camera_name+'/color/image_raw',}, + "Depth":{"profile":"depth_module.profile", "format":'depth_module.depth_format', "param":"enable_depth", 'topic':camera_name+'/depth/image_rect_raw'}, + "Infrared":{"profile":"depth_module.profile", "format":'depth_module.infra_format', "param":"enable_infra", 'topic':camera_name+'/infra/image_rect_raw'}, + "Infrared1":{"profile":"depth_module.profile", "format":'depth_module.infra1_format',"param":"enable_infra1", 'topic':camera_name+'/infra/image_rect_raw'}, + "Infrared2":{"profile":"depth_module.profile", "format":'depth_module.infra2_format',"param":"enable_infra2", 'topic':camera_name+'/infra/image_rect_raw'}, + } + return config + + +def get_default_profiles(cap, profile): + profile1 = "" + profile2 = "" + for profiles in cap: + if profiles[0] == profile and int(profiles[1].split('x')[0]) != 640: + profile1 = profiles[1] + break + for profiles in cap: + if profiles[0] == profile and int(profiles[1].split('x')[0]) != int(profile1.split('x')[0]): + profile2 = profiles[1] + break + debug_print(profile + " default profile1:" + profile1) + debug_print(profile + " default profile2:" + profile2) + return profile1,profile2 + +def get_camera_capabilities_short(device_type, serial_no=None): + short_data = os.popen("rs-enumerate-devices -s").read().splitlines() + print(serial_no) + for line in short_data: + print(line) + if device_type in line: + if serial_no is None or serial_no == "" : + print(device_type+ " found in " + line) + return + if serial_no in line: + print(device_type + " with serial_no " + serial_no +" found in " + line) + return + print(device_type + " not found") + + +device_info_string = "Device info:" + +def get_device_info_location(long_data, index=0): + for line_no in range(index, len(long_data)): + if device_info_string in long_data[line_no]: + return line_no + return len(long_data) + +stream_profile_string = "Stream Profiles supported by" +def get_stream_profile_location(long_data, start_index, end_index, profile_string): + for line_no in range(start_index, end_index): + if stream_profile_string in long_data[line_no]: + if profile_string in long_data[line_no]: + return line_no + return None + +def get_depth_profiles(long_data, start_index, end_index): + cap = [] + for line_no in range(start_index, end_index): + if len(long_data[line_no]) == 0: + break + debug_print("depth profile processing:" + long_data[line_no]) + depth_profile = long_data[line_no].split() + if len(depth_profile) == 5: + profile = depth_profile[0] + value = depth_profile[1]+"x"+depth_profile[3] + format = depth_profile[4] + elif len(depth_profile) == 6: + profile = depth_profile[0]+depth_profile[1] + value = depth_profile[2]+"x"+depth_profile[4] + format = depth_profile[5] + else: + assert false, "Seems that the depth profile info format printed by rs-enumerate-devices changed" + value = value[:-2] + debug_print("depth profile added: " + profile, value, format) + cap.append([profile, value, format]) + debug_print(cap) + return cap + + +def get_color_profiles(long_data, start_index, end_index): + cap = [] + for line_no in range(start_index, end_index): + if len(long_data[line_no]) == 0: + break + debug_print("color profile processing:" + long_data[line_no]) + color_profile = long_data[line_no].split() + if len(color_profile) == 5: + profile = color_profile[0] + value = color_profile[1]+"x"+color_profile[3] + format = color_profile[4] + else: + assert false, "Seems that the color profile info format printed by rs-enumerate-devices changed" + value = value[:-2] + debug_print("color profile added: " + profile, value, format) + cap.append([profile, value, format]) + debug_print(cap) + return cap + +NAME_LINE_INDEX = 1 +NAME_LINE_NAME_OFFSET = 4 +SERIAL_NO_LINE_INDEX = 2 +SERIAL_NO_VALUE_OFFSET = 3 +def parse_device_info(long_data, start_index, end_index, device_type, serial_no): + #after device_info, the next line should have the name and device type + capability = {} + debug_print("Searching for data between lines ", str(start_index) + " and " + str(end_index)) + name_line = long_data[start_index+NAME_LINE_INDEX].split() + if name_line[0] != "Name": + assert False, "rs-enumerate-devices output format changed" + if name_line[4] != device_type: + debug_print("device not matching:" + name_line[NAME_LINE_NAME_OFFSET]) + return None + debug_print("device matched:" + name_line[NAME_LINE_NAME_OFFSET]) + if serial_no != None: + #next line after nameline should have the serial_no + serial_no_line = long_data[start_index+SERIAL_NO_LINE_INDEX].split() + if serial_no_line[0] != "Serial": + assert False, "rs-enumerate-devices output format changed" + if serial_no_line[SERIAL_NO_VALUE_OFFSET] != serial_no: + debug_print("serial_no not matching:" + serial_no_line[SERIAL_NO_VALUE_OFFSET]) + return None + debug_print("serial_no matched:" + serial_no_line[SERIAL_NO_VALUE_OFFSET]) + else: + serial_no = long_data[start_index+SERIAL_NO_LINE_INDEX].split()[SERIAL_NO_VALUE_OFFSET] + + capability["device_type"] = device_type + capability["serial_no"] = serial_no + depth_profile_index = get_stream_profile_location(long_data, start_index, end_index, "Stereo Module") + if depth_profile_index != None: + capability["depth_profile"] = get_depth_profiles(long_data, depth_profile_index+3, end_index) + rgb_profile_index = get_stream_profile_location(long_data, start_index, end_index, "RGB Camera") + if rgb_profile_index != None: + capability["color_profile"] = get_color_profiles(long_data, rgb_profile_index+3, end_index) + return capability + +def get_camera_capabilities(device_type, serial_no=None): + long_data = os.popen("rs-enumerate-devices").read().splitlines() + debug_print(serial_no) + index = 0 + while index < len(long_data): + index = get_device_info_location(long_data, index) + if index == len(long_data): + return + else: + debug_print("DeviceInfo found at: " + str(index)) + start_index = index + index += 1 + index = get_device_info_location(long_data, index) + capability = parse_device_info(long_data, start_index, index, device_type, serial_no) + if capability != None: + return capability + return None + +if __name__ == '__main__': + device_type = 'D455' + serial_no = None + if len(sys.argv) > 1: + device_type = sys.argv[1] + if len(sys.argv) > 2: + serial_no = sys.argv[2] + + cap = get_camera_capabilities(device_type, serial_no) + print("Capabilities:") + print(cap) \ No newline at end of file diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index 63ede24d57..d78eeb9ef8 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/realsense2_camera/test/utils/pytest_rs_utils.py @@ -15,6 +15,7 @@ import sys import time from collections import deque +import functools import pytest import numpy as np @@ -28,10 +29,31 @@ from rclpy import qos from rclpy.node import Node from rclpy.utilities import ok +from ros2topic.verb.bw import ROSTopicBandwidth +from ros2topic.verb.hz import ROSTopicHz +from ros2topic.api import get_msg_class import ctypes import struct import requests +import math + +#from rclpy.parameter import Parameter +from rcl_interfaces.msg import Parameter +from rcl_interfaces.msg import ParameterValue +from rcl_interfaces.srv import SetParameters, GetParameters, ListParameters +''' +humble doesn't have the SetParametersResult and SetParameters_Response imported using +__init__.py. The below two lines can be used for iron and hopefully succeeding ROS2 versions +''' +#from rcl_interfaces.msg import SetParametersResult +#from rcl_interfaces.srv import SetParameters_Response +from rcl_interfaces.msg._set_parameters_result import SetParametersResult +from rcl_interfaces.srv._set_parameters import SetParameters_Response + +from rcl_interfaces.msg import ParameterType +from rcl_interfaces.msg import ParameterValue + from sensor_msgs.msg import Image as msg_Image @@ -54,6 +76,11 @@ import tempfile import os import requests + +def debug_print(*args): + if(True): + print(*args) + class RosbagManager(object): def __new__(cls): if not hasattr(cls, 'instance'): @@ -82,12 +109,13 @@ def init(self): def get_rosbag_path(self, filename): if filename in self.rosbag_files: return self.rosbag_location + "/" + filename -rosbagMgr = RosbagManager() + def get_rosbag_file_path(filename): + rosbagMgr = RosbagManager() path = rosbagMgr.get_rosbag_path(filename) assert path, "No rosbag file found :"+filename return path - +get_rosbag_file_path.rosbagMgr = None def CameraInfoGetData(rec_filename, topic): data = importRosbag(rec_filename, importTopics=[topic], log='ERROR', disable_bar=True)[topic] @@ -552,6 +580,16 @@ def delayed_launch_descr_with_parameters(request): actions = [ first_node,], period=period) ]),params +class HzWrapper(ROSTopicHz): + def _callback_hz(self, m, topic=None): + if self.get_last_printed_tn(topic=topic) == 0: + self.set_last_printed_tn(self.get_msg_tn(topic=topic), topic=topic) + return self.callback_hz(m, topic) + def restart_topic(self, topic): + self._last_printed_tn[topic] = 0 + self._times[topic].clear() + self._msg_tn[topic] = 0 + self._msg_t0[topic] = -1 ''' This is that holds the test node that listens to a subscription created by a test. @@ -564,6 +602,7 @@ def __init__(self, name='test_node'): self.data = {} self.tfBuffer = None self.frame_counter = {} + self._ros_topic_hz = HzWrapper(super(), 10000, use_wtime=False) def wait_for_node(self, node_name, timeout=8.0): start = time.time() @@ -576,10 +615,21 @@ def wait_for_node(self, node_name, timeout=8.0): return True, "" time.sleep(timeout/5) return False, "Timed out waiting for "+ str(timeout)+ "seconds" - def create_subscription(self, msg_type, topic , data_type, store_raw_data): - super().create_subscription(msg_type, topic , self.rsCallback(topic,msg_type, store_raw_data), data_type) + def reset_data(self, topic): self.data[topic] = deque() self.frame_counter[topic] = 0 + self._ros_topic_hz.restart_topic(topic) + + + def create_subscription(self, msg_type, topic , data_type, store_raw_data, measure_hz): + self.reset_data(topic) + super().create_subscription(msg_type, topic , self.rsCallback(topic,msg_type, store_raw_data), data_type) + #hz measurements are not working + if measure_hz == True: + msg_class = get_msg_class(super(), topic, blocking=True, include_hidden_topics=True) + super().create_subscription(msg_class,topic,functools.partial(self._ros_topic_hz._callback_hz, topic=topic),data_type) + self._ros_topic_hz.set_last_printed_tn(0, topic=topic) + if (os.getenv('ROS_DISTRO') != "dashing") and (self.tfBuffer == None): self.tfBuffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tfBuffer, super()) @@ -592,14 +642,14 @@ def pop_first_chunk(self, topic): def image_msg_to_numpy(self, data): fmtString = data.encoding - if fmtString in ['mono8', '8UC1', 'bgr8', 'rgb8', 'bgra8', 'rgba8']: + if fmtString in ['mono8', '8UC1', 'bgr8', 'rgb8', 'bgra8', 'rgba8', 'yuv422_yuy2', 'yuv422']: img = np.frombuffer(data.data, np.uint8) elif fmtString in ['mono16', '16UC1', '16SC1']: img = np.frombuffer(data.data, np.uint16) elif fmtString == '32FC1': img = np.frombuffer(data.data, np.float32) else: - print('image format not supported:' + fmtString) + print('py_rs_utils.image_msg_to_numpy:image format not supported:' + fmtString) return None depth = data.step / (data.width * img.dtype.itemsize) @@ -613,9 +663,9 @@ def image_msg_to_numpy(self, data): The processing of data is taken from the existing testcase in scripts rs2_test ''' def rsCallback(self, topic, msg_type, store_raw_data): - print("RSCallback") + debug_print("RSCallback") def _rsCallback(data): - print('Got the callback for ' + topic) + debug_print('Got the callback for ' + topic) #print(data.header) self.flag = True if store_raw_data == True: @@ -707,23 +757,71 @@ def init_test(self,name='RsTestNode'): self.flag = False self.node = RsTestNode(name) self.subscribed_topics = [] - def create_subscription(self, msg_type, topic, data_type, store_raw_data=False): + + def wait_for_node(self, node_name, timeout=8.0): + self.node.wait_for_node(node_name, timeout) + def create_subscription(self, msg_type, topic, data_type, store_raw_data=False, measure_hz=False): if not topic in self.subscribed_topics: - self.node.create_subscription(msg_type, topic, data_type, store_raw_data) + self.node.create_subscription(msg_type, topic, data_type, store_raw_data, measure_hz) self.subscribed_topics.append(topic) + else: + self.node.reset_data(topic) + + - def spin_for_data(self,themes): + def create_param_ifs(self, camera_name): + + self.set_param_if = self.node.create_client(SetParameters, camera_name + '/set_parameters') + self.get_param_if = self.node.create_client(GetParameters, camera_name + '/get_parameters') + while not self.get_param_if.wait_for_service(timeout_sec=1.0): + print('service not available, waiting again...') + while not self.set_param_if.wait_for_service(timeout_sec=1.0): + print('service not available, waiting again...') + + def send_param(self, req): + future = self.set_param_if.call_async(req) + while rclpy.ok(): + rclpy.spin_once(self.node) + if future.done(): + try: + response = future.result() + if response.results[0].successful: + return True + except Exception as e: + print("exception raised:") + print(e) + pass + return False + + def set_string_param(self, param_name, param_value): + req = SetParameters.Request() + new_param_value = ParameterValue(type=ParameterType.PARAMETER_STRING, string_value=param_value) + req.parameters = [Parameter(name=param_name, value=new_param_value)] + return self.send_param(req) + + def set_bool_param(self, param_name, param_value): + req = SetParameters.Request() + new_param_value = ParameterValue(type=ParameterType.PARAMETER_BOOL, bool_value=param_value) + req.parameters = [Parameter(name=param_name, value=new_param_value)] + return self.send_param(req) + + def set_integer_param(self, param_name, param_value): + req = SetParameters.Request() + new_param_value = ParameterValue(type=ParameterType.PARAMETER_INTEGER, integer_value=param_value) + req.parameters = [Parameter(name=param_name, value=new_param_value)] + return self.send_param(req) + + def spin_for_data(self,themes, timeout=5.0): start = time.time() ''' timeout value varies depending upon the system, it needs to be more if the access is over the network ''' - timeout = 25.0 print('Waiting for topic... ' ) flag = False while (time.time() - start) < timeout: rclpy.spin_once(self.node, timeout_sec=1) - print('Spun once... ' ) + debug_print('Spun once... ' ) all_found = True for theme in themes: if theme['expected_data_chunks'] > int(self.node.get_num_chunks(theme['topic'])): @@ -739,21 +837,28 @@ def spin_for_data(self,themes): def spin_for_time(self,wait_time): start = time.time() - print('Waiting for topic... ' ) + print('Waiting for time... ' ) flag = False - while time.time() - start < wait_time: - rclpy.spin_once(self.node) - print('Spun once... ' ) + while (time.time() - start) < wait_time: + print('Spun for time once... ' ) + rclpy.spin_once(self.node, timeout_sec=wait_time) - def run_test(self, themes): + def run_test(self, themes, initial_wait_time=0.0, timeout=5.0): try: for theme in themes: store_raw_data = False if 'store_raw_data' in theme: store_raw_data = theme['store_raw_data'] - self.create_subscription(theme['msg_type'], theme['topic'] , qos.qos_profile_sensor_data,store_raw_data) + if 'expected_fps_in_hz' in theme: + measure_hz = True + else: + measure_hz = False + + self.create_subscription(theme['msg_type'], theme['topic'] , qos.qos_profile_sensor_data,store_raw_data, measure_hz) print('subscription created for ' + theme['topic']) - self.flag = self.spin_for_data(themes) + if initial_wait_time != 0.0: + self.spin_for_time(initial_wait_time) + self.flag = self.spin_for_data(themes, timeout) except Exception as e: exc_type, exc_obj, exc_tb = sys.exc_info() fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1] @@ -773,11 +878,25 @@ def run_test(self, themes): ''' def process_data(self, themes): for theme in themes: + if theme['expected_data_chunks'] == 0: + assert self.node.get_num_chunks(theme['topic']) == 0, "Received data, when not expected for topic:" + theme['topic'] + continue #no more checks needed if data is not available + + if 'expected_fps_in_hz' in theme: + hz = self.node._ros_topic_hz.get_hz(theme['topic']) + if hz == None: + print("Couldn't measure fps, no of data frames expected are enough for the measurement?") + else: + speed= 1e9*hz[0] + msg = "FPS in Hz of topic " + theme['topic'] + " is " + str(speed) + ". Expected is " + str(theme['expected_fps_in_hz']) + print(msg) + if (abs(theme['expected_fps_in_hz']-speed) > theme['expected_fps_in_hz']/10): + assert False,msg data = self.node.pop_first_chunk(theme['topic']) if 'width' in theme: - assert theme['width'] == data['shape'][0][1] # (get from numpy image the width) + assert theme['width'] == data['shape'][0][1], "Width not matched. Expected:" + str(theme['width']) + " & got: " + str(data['shape'][0][1]) # (get from numpy image the width) if 'height' in theme: - assert theme['height'] == data['shape'][0][0] # (get from numpy image the height) + assert theme['height'] == data['shape'][0][0], "Height not matched. Expected:" + str(theme['height']) + " & got: " + str(data['shape'][0][0]) # (get from numpy image the height) if 'data' not in theme: print('No data to compare for ' + theme['topic']) #print(data) From 6922d4db5b56d4d8b6039d8a670aa734091a7904 Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Thu, 31 Aug 2023 17:47:07 +0530 Subject: [PATCH 20/76] Updated readme and imu tests --- realsense2_camera/test/README.md | 20 +++++++-- .../test/live_camera/test_camera_imu_tests.py | 41 ++++++++----------- .../test/utils/pytest_rs_utils.py | 8 +++- 3 files changed, 39 insertions(+), 30 deletions(-) diff --git a/realsense2_camera/test/README.md b/realsense2_camera/test/README.md index 0be7600c24..a026a36b2e 100644 --- a/realsense2_camera/test/README.md +++ b/realsense2_camera/test/README.md @@ -146,7 +146,21 @@ The below command finds the test with the name test_static_tf_1 in realsense2_ca pytest-3 -s -k test_static_tf_1 realsense2_camera/test/ -### Points to be noted while writing pytests -The tests that are in one file are nromally run in parallel. So if there are multiple tests in one file, the system capacity can influence the test execution. It's recomended to have 3-4 tests in file, more than that can affect the test results due to delays. - +## Points to be noted while writing pytests +The tests that are in one file are normally run in parallel. So if there are multiple tests in one file, the system capacity can influence the test execution. It's recomended to have 3-4 tests in file, more than that can affect the test results due to delays. +### Passing/changing parameters +The parameters passed while creating the node can be initialized individually for each test, please see the test_parameterized_template example for reference. The default values are taken from rs_launch.py and the passed parameters are used for overriding the default values. The parameters that can be dynamically modified can be changed using the param interface provided. However, the function create_param_ifs has to be called to create this interface. Please see the test_d455_basic_tests.py for reference. There are specific functions to change the string, integer and bool parameters, the utils can be extended if any more types are needed. +### Difference in setting the bool parameters +There is a difference between setting the default values of bool parameters and setting them dynamically. +The bool test params are assinged withn quotes as below. + test_params_all_profiles_d455 = { + 'camera_name': 'D455', + 'device_type': 'D455', + 'enable_accel':"True", + 'enable_gyro':"True", + 'unite_imu_method':1, + } + +However the function that implements the setting of bool parameter dynamically takes the python bool datatype. For example: + self.set_bool_param('enable_accel', False) diff --git a/realsense2_camera/test/live_camera/test_camera_imu_tests.py b/realsense2_camera/test/live_camera/test_camera_imu_tests.py index fa173e0a8e..c712cd1ba7 100644 --- a/realsense2_camera/test/live_camera/test_camera_imu_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_imu_tests.py @@ -43,8 +43,8 @@ test_params_all_profiles_d455 = { 'camera_name': 'D455', 'device_type': 'D455', - 'enable_accel':True, - 'enable_gyro':True, + 'enable_accel':"True", + 'enable_gyro':"True", 'unite_imu_method':1, } @@ -64,62 +64,53 @@ def test_LiveCamera_check_motion_sensor(self,launch_descr_with_parameters): if params['unite_imu_method'] == '0': themes[IMU_TOPIC]['expected_data_chunks'] = 0 try: - ''' - initialize, run and check the data - ''' - msg = "Test with the default params " + #initialize self.init_test("RsTest"+params['camera_name']) self.create_param_ifs(get_node_heirarchy(params)) + + + #run with default params and check the data + msg = "Test with the default params " print(msg) - ret = self.run_test(themes) + ret = self.run_test(themes, timeout=50) assert ret[0], msg + str(ret[1]) assert self.process_data(themes), msg + " failed" - msg = "Test with the accel false " self.set_bool_param('enable_accel', False) self.set_bool_param('enable_gyro', True) - ''' - This step fails if the next line is commented - ''' - self.set_integer_param('unite_imu_method', 0) #this shouldn't matter because the unite_imu_method cannot be changed themes[ACCEL_TOPIC]['expected_data_chunks'] = 0 + #uncomment once RSDEV-550 is closed + #themes[IMU_TOPIC]['expected_data_chunks'] = 0 print(msg) ret = self.run_test(themes) assert ret[0], msg + str(ret[1]) assert self.process_data(themes), msg + " failed" - ''' - Test fails - ''' msg = "Test with the gyro false " self.set_bool_param('enable_accel', True) self.set_bool_param('enable_gyro', False) - self.set_integer_param('unite_imu_method', 0) #this shouldn't matter because the unite_imu_method cannot be changed - themes[IMU_TOPIC]['expected_data_chunks'] = 1 + themes[IMU_TOPIC]['expected_data_chunks'] = 0 themes[ACCEL_TOPIC]['expected_data_chunks'] = 1 themes[GYRO_TOPIC]['expected_data_chunks'] = 0 print(msg) + self.spin_for_time(wait_time=1.0) ret = self.run_test(themes) assert ret[0], msg + str(ret[1]) assert self.process_data(themes), msg + " failed" - ''' - Test fails, both gyro and accel data is available, not imu - ''' msg = "Test with both gyro and accel false " self.set_bool_param('enable_accel', False) self.set_bool_param('enable_gyro', False) - self.set_integer_param('unite_imu_method', 1) #this shouldn't matter because the unite_imu_method cannot be changed - + self.set_integer_param('unite_imu_method', 1) + self.spin_for_time(wait_time=1.0) themes[ACCEL_TOPIC]['expected_data_chunks'] = 0 themes[GYRO_TOPIC]['expected_data_chunks'] = 0 - themes[IMU_TOPIC]['expected_data_chunks'] = 1 #Seems that imu data is available even if gyro and accel is disabled + themes[IMU_TOPIC]['expected_data_chunks'] = 0 print(msg) - ret = self.run_test(themes, initial_wait_time=1.0) #wait_time added as test doesn't have to wait for any data + ret = self.run_test(themes, initial_wait_time=1.0) assert ret[0], msg + " failed" assert self.process_data(themes), msg +" failed" - finally: #this step is important because the test will fail next time pytest_rs_utils.kill_realsense2_camera_node() diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index d78eeb9ef8..e693eefc64 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/realsense2_camera/test/utils/pytest_rs_utils.py @@ -819,9 +819,12 @@ def spin_for_data(self,themes, timeout=5.0): ''' print('Waiting for topic... ' ) flag = False + data_not_expected = [i for i in themes if (i["expected_data_chunks"]) == 0] while (time.time() - start) < timeout: rclpy.spin_once(self.node, timeout_sec=1) debug_print('Spun once... ' ) + if data_not_expected == True: + continue all_found = True for theme in themes: if theme['expected_data_chunks'] > int(self.node.get_num_chunks(theme['topic'])): @@ -831,8 +834,9 @@ def spin_for_data(self,themes, timeout=5.0): flag =True break else: - print("Timed out waiting for", timeout, "seconds" ) - return False, "run_test timedout" + if data_not_expected == False: + print("Timed out waiting for", timeout, "seconds" ) + return False, "run_test timedout" return flag,"" def spin_for_time(self,wait_time): From a20eb25394a4ff705a0e976749400fb9efadd9da Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Fri, 1 Sep 2023 18:46:04 +0530 Subject: [PATCH 21/76] static_tf fix --- .../test/rosbag/test_rosbag_depth_tests.py | 43 ++++++++----------- .../test/utils/pytest_rs_utils.py | 34 +++++++++++---- 2 files changed, 44 insertions(+), 33 deletions(-) diff --git a/realsense2_camera/test/rosbag/test_rosbag_depth_tests.py b/realsense2_camera/test/rosbag/test_rosbag_depth_tests.py index abbfe28cab..1b628e07f6 100644 --- a/realsense2_camera/test/rosbag/test_rosbag_depth_tests.py +++ b/realsense2_camera/test/rosbag/test_rosbag_depth_tests.py @@ -100,12 +100,10 @@ def process_data(self, themes): test_params_static_tf_1 = {"rosbag_filename":get_rosbag_file_path("outdoors_1color.bag"), - 'camera_name': 'Static_tf_1', + 'camera_name': 'Static_tf1', 'color_width': '0', 'color_height': '0', - 'depth_width': '0', - 'depth_height': '0', - 'infra_width': '0', + "static_tf":True, 'infra_height': '0', 'enable_infra1':'true', 'enable_infra2':'true' @@ -119,43 +117,38 @@ def process_data(self, themes): @pytest.mark.launch(fixture=delayed_launch_descr_with_parameters) class TestStaticTf1(pytest_rs_utils.RsTestBaseClass): def test_static_tf_1(self,delayed_launch_descr_with_parameters): - params = delayed_launch_descr_with_parameters[1] - self.rosbag = params["rosbag_filename"] - data = {('camera_link', 'camera_color_frame'): ([-0.00010158783697988838, 0.014841210097074509, -0.00022671300393994898], [-0.0008337442995980382, 0.0010442184284329414, -0.0009920650627464056, 0.9999986290931702]), - ('camera_link', 'camera_depth_frame'): ([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]), - ('camera_link', 'camera_infra1_frame'): ([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]), - ('camera_depth_frame', 'camera_infra1_frame'): ([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]), - ('camera_depth_frame', 'camera_color_frame'): ([-0.00010158783697988838, 0.014841210097074509, -0.00022671300393994898], [-0.0008337442995980382, 0.0010442184284329414, -0.0009920650627464056, 0.9999986290931702]), - ('camera_infra1_frame', 'camera_color_frame'): ([-0.00010158783697988838, 0.014841210097074509, -0.00022671300393994898], [-0.0008337442995980382, 0.0010442184284329414, -0.0009920650627464056, 0.9999986290931702])} + self.params = delayed_launch_descr_with_parameters[1] + self.rosbag = self.params["rosbag_filename"] themes = [ - {'topic':get_node_heirarchy(params)+'/color/image_raw', + {'topic':get_node_heirarchy(self.params)+'/color/image_raw', 'msg_type':msg_Image, 'expected_data_chunks':1, - 'data':data, + 'static_tf':True, } ] try: ''' initialize, run and check the data ''' - self.init_test("RsTest"+params['camera_name']) + self.init_test("RsTest"+self.params['camera_name']) ret = self.run_test(themes) assert ret[0], ret[1] assert self.process_data(themes) finally: self.shutdown() def process_data(self, themes): - #print ('Gathering static transforms') - frame_ids = ['camera_link', 'camera_depth_frame', 'camera_infra1_frame', 'camera_infra2_frame', 'camera_color_frame', 'camera_fisheye_frame', 'camera_pose'] + expected_data = {(self.params['camera_name']+'_link', self.params['camera_name']+'_color_frame'): ([-0.00010158783697988838, 0.014841210097074509, -0.00022671300393994898], [-0.0008337442995980382, 0.0010442184284329414, -0.0009920650627464056, 0.9999986290931702]), + (self.params['camera_name']+'_link', self.params['camera_name']+'_depth_frame'): ([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]), + (self.params['camera_name']+'_link', self.params['camera_name']+'_infra1_frame'): ([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]), + (self.params['camera_name']+'_depth_frame', self.params['camera_name']+'_infra1_frame'): ([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]), + (self.params['camera_name']+'_depth_frame', self.params['camera_name']+'_color_frame'): ([-0.00010158783697988838, 0.014841210097074509, -0.00022671300393994898], [-0.0008337442995980382, 0.0010442184284329414, -0.0009920650627464056, 0.9999986290931702]), + (self.params['camera_name']+'_infra1_frame', self.params['camera_name']+'_color_frame'): ([-0.00010158783697988838, 0.014841210097074509, -0.00022671300393994898], [-0.0008337442995980382, 0.0010442184284329414, -0.0009920650627464056, 0.9999986290931702])} + frame_ids = [self.params['camera_name']+'_link', self.params['camera_name']+'_depth_frame', self.params['camera_name']+'_infra1_frame', self.params['camera_name']+'_infra2_frame', self.params['camera_name']+'_color_frame', self.params['camera_name']+'_fisheye_frame', self.params['camera_name']+'_pose'] coupled_frame_ids = [xx for xx in itertools.combinations(frame_ids, 2)] - res = {} - for couple in coupled_frame_ids: - from_id, to_id = couple - if (self.node.tfBuffer.can_transform(from_id, to_id, rclpy.time.Time(), rclpy.time.Duration(nanoseconds=3e6))): - res[couple] = self.node.tfBuffer.lookup_transform(from_id, to_id, rclpy.time.Time(), rclpy.time.Duration(nanoseconds=1e6)).transform - else: - res[couple] = None - return pytest_rs_utils.staticTFTest(res, themes[0]["data"]) + tfs_data = self.get_tfs(coupled_frame_ids) + ret = pytest_rs_utils.staticTFTest(tfs_data, expected_data) + assert ret[0], ret[1] + return ret[0] test_params_non_existing_rosbag = {"rosbag_filename":"non_existent.bag", diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index e693eefc64..a30f2fb7a2 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/realsense2_camera/test/utils/pytest_rs_utils.py @@ -16,8 +16,11 @@ import time from collections import deque import functools +import itertools + import pytest + import numpy as np from launch import LaunchDescription @@ -620,8 +623,7 @@ def reset_data(self, topic): self.frame_counter[topic] = 0 self._ros_topic_hz.restart_topic(topic) - - def create_subscription(self, msg_type, topic , data_type, store_raw_data, measure_hz): + def create_subscription(self, msg_type, topic , data_type, store_raw_data, measure_hz, static_tf): self.reset_data(topic) super().create_subscription(msg_type, topic , self.rsCallback(topic,msg_type, store_raw_data), data_type) #hz measurements are not working @@ -630,9 +632,18 @@ def create_subscription(self, msg_type, topic , data_type, store_raw_data, measu super().create_subscription(msg_class,topic,functools.partial(self._ros_topic_hz._callback_hz, topic=topic),data_type) self._ros_topic_hz.set_last_printed_tn(0, topic=topic) - if (os.getenv('ROS_DISTRO') != "dashing") and (self.tfBuffer == None): + if (static_tf == True) and (self.tfBuffer == None): self.tfBuffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tfBuffer, super()) + def get_tfs(self, coupled_frame_ids): + res = dict() + for couple in coupled_frame_ids: + from_id, to_id = couple + if (self.tfBuffer.can_transform(from_id, to_id, rclpy.time.Time(), rclpy.time.Duration(nanoseconds=3e6))): + res[couple] = self.tfBuffer.lookup_transform(from_id, to_id, rclpy.time.Time(), rclpy.time.Duration(nanoseconds=1e6)).transform + else: + res[couple] = None + return res def get_num_chunks(self,topic): return len(self.data[topic]) @@ -760,9 +771,9 @@ def init_test(self,name='RsTestNode'): def wait_for_node(self, node_name, timeout=8.0): self.node.wait_for_node(node_name, timeout) - def create_subscription(self, msg_type, topic, data_type, store_raw_data=False, measure_hz=False): + def create_subscription(self, msg_type, topic, data_type, store_raw_data=False, measure_hz=False, static_tf=False): if not topic in self.subscribed_topics: - self.node.create_subscription(msg_type, topic, data_type, store_raw_data, measure_hz) + self.node.create_subscription(msg_type, topic, data_type, store_raw_data, measure_hz, static_tf) self.subscribed_topics.append(topic) else: self.node.reset_data(topic) @@ -770,7 +781,6 @@ def create_subscription(self, msg_type, topic, data_type, store_raw_data=False, def create_param_ifs(self, camera_name): - self.set_param_if = self.node.create_client(SetParameters, camera_name + '/set_parameters') self.get_param_if = self.node.create_client(GetParameters, camera_name + '/get_parameters') while not self.get_param_if.wait_for_service(timeout_sec=1.0): @@ -849,6 +859,7 @@ def spin_for_time(self,wait_time): def run_test(self, themes, initial_wait_time=0.0, timeout=5.0): try: + static_tf_found = False for theme in themes: store_raw_data = False if 'store_raw_data' in theme: @@ -857,12 +868,17 @@ def run_test(self, themes, initial_wait_time=0.0, timeout=5.0): measure_hz = True else: measure_hz = False + if 'static_tf' in theme: + static_tf = True + static_tf_found = True + else: + static_tf = False - self.create_subscription(theme['msg_type'], theme['topic'] , qos.qos_profile_sensor_data,store_raw_data, measure_hz) + self.create_subscription(theme['msg_type'], theme['topic'] , qos.qos_profile_sensor_data,store_raw_data, measure_hz, static_tf) print('subscription created for ' + theme['topic']) if initial_wait_time != 0.0: self.spin_for_time(initial_wait_time) - self.flag = self.spin_for_data(themes, timeout) + self.flag = self.spin_for_data(themes, timeout) except Exception as e: exc_type, exc_obj, exc_tb = sys.exc_info() fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1] @@ -875,6 +891,8 @@ def run_test(self, themes, initial_wait_time=0.0, timeout=5.0): self.flag =False,e return self.flag + def get_tfs(self, coupled_frame_ids): + return self.node.get_tfs(coupled_frame_ids) ''' Please override and use your own process_data if the default check is not suitable. Please also store_raw_data parameter in the themes as True, if you want the From c80e47117cccf88d4e7043eb075468163b8d0952 Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Tue, 5 Sep 2023 19:30:17 +0530 Subject: [PATCH 22/76] Added tf and tf_static tests --- .../test/live_camera/test_camera_fps_tests.py | 4 +- .../test/live_camera/test_camera_tf_tests.py | 219 ++++++++++++++++++ .../test/utils/pytest_live_camera_utils.py | 17 ++ .../test/utils/pytest_rs_utils.py | 44 ++-- 4 files changed, 269 insertions(+), 15 deletions(-) create mode 100644 realsense2_camera/test/live_camera/test_camera_tf_tests.py diff --git a/realsense2_camera/test/live_camera/test_camera_fps_tests.py b/realsense2_camera/test/live_camera/test_camera_fps_tests.py index cff5cadbb4..291e4eaf8c 100644 --- a/realsense2_camera/test/live_camera/test_camera_fps_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_fps_tests.py @@ -38,7 +38,7 @@ from rcl_interfaces.msg import ParameterValue from rcl_interfaces.srv import SetParameters, GetParameters, ListParameters -test_params_depth_avg_1 = { +test_params_test_fps = { 'camera_name': 'D455', 'device_type': 'D455', } @@ -47,7 +47,7 @@ modified to make it work, see py_rs_utils for more details. To check the fps, a value 'expected_fps_in_hz' has to be added to the corresponding theme ''' -@pytest.mark.parametrize("launch_descr_with_parameters", [test_params_depth_avg_1],indirect=True) +@pytest.mark.parametrize("launch_descr_with_parameters", [test_params_test_fps],indirect=True) @pytest.mark.launch(fixture=launch_descr_with_parameters) class TestCamera_TestFPS(pytest_rs_utils.RsTestBaseClass): def test_camera_test_fps(self,launch_descr_with_parameters): diff --git a/realsense2_camera/test/live_camera/test_camera_tf_tests.py b/realsense2_camera/test/live_camera/test_camera_tf_tests.py new file mode 100644 index 0000000000..10c72c8b09 --- /dev/null +++ b/realsense2_camera/test/live_camera/test_camera_tf_tests.py @@ -0,0 +1,219 @@ +# 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. + + +import os +import sys +import itertools + + +import pytest +import rclpy + +from rclpy.qos import DurabilityPolicy +from rclpy.qos import HistoryPolicy +from rclpy.qos import QoSProfile +import tf2_ros + + +from sensor_msgs.msg import Image as msg_Image +from sensor_msgs.msg import Imu as msg_Imu +from sensor_msgs.msg import PointCloud2 as msg_PointCloud2 +from tf2_msgs.msg import TFMessage as msg_TFMessage + +import numpy as np + +sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils")) +import pytest_rs_utils +from pytest_rs_utils import launch_descr_with_parameters + +import pytest_live_camera_utils +from pytest_rs_utils import get_rosbag_file_path +from pytest_rs_utils import get_node_heirarchy + +from rclpy.parameter import Parameter +from rcl_interfaces.msg import ParameterValue +from rcl_interfaces.srv import SetParameters, GetParameters, ListParameters + +''' +The test was implemented to check the tf_static before and after infra1 was enabled. There shouldn't be any +related data before enabling. +''' +test_params_tf_static_change_d455 = { + 'camera_name': 'D455', + 'device_type': 'D455', + 'enable_infra1': 'false', + 'enable_infra2': 'true', + 'enable_accel': 'true', + 'enable_gyro': 'true', + } +test_params_tf_static_change_d435 = { + 'camera_name': 'D435', + 'device_type': 'D435', + 'enable_infra1': 'false', + 'enable_infra2': 'true', + 'enable_accel': 'true', + 'enable_gyro': 'true', + } + +test_params_tf_static_change_d415 = { + 'camera_name': 'D415', + 'device_type': 'D415', + 'enable_infra1': 'false', + 'enable_infra2': 'true', + 'enable_accel': 'true', + 'enable_gyro': 'true', + } +@pytest.mark.parametrize("launch_descr_with_parameters", [ + pytest.param(test_params_tf_static_change_d455, marks=pytest.mark.d455), + pytest.param(test_params_tf_static_change_d435, marks=pytest.mark.d435), + pytest.param(test_params_tf_static_change_d415, marks=pytest.mark.d415), + ],indirect=True) +@pytest.mark.launch(fixture=launch_descr_with_parameters) +class TestCamera_TestTF_Static_change(pytest_rs_utils.RsTestBaseClass): + def test_camera_test_tf_static_change(self,launch_descr_with_parameters): + self.params = launch_descr_with_parameters[1] + if pytest_live_camera_utils.check_if_camera_connected(self.params['device_type']) == False: + print("Device not found? : " + self.params['device_type']) + return + themes = [ + {'topic':'/tf_static', + 'msg_type':msg_TFMessage, + 'expected_data_chunks':1, + 'qos' : QoSProfile(depth=100,durability=DurabilityPolicy.TRANSIENT_LOCAL,history=HistoryPolicy.KEEP_LAST,)#static + } + ] + try: + ''' + initialize, run and check the data + ''' + self.init_test("RsTest"+self.params['camera_name']) + self.wait_for_node(self.params['camera_name']) + self.create_param_ifs(get_node_heirarchy(self.params)) + ret = self.run_test(themes, timeout=10) + assert ret[0], ret[1] + frame_ids = [self.params['camera_name']+'_link', + self.params['camera_name']+'_depth_frame', + self.params['camera_name']+'_infra1_frame', + self.params['camera_name']+'_infra2_frame', + self.params['camera_name']+'_color_frame'] + if self.params['device_type'] == 'D455': + frame_ids.append(self.params['camera_name']+'_gyro_frame') + frame_ids.append(self.params['camera_name']+'_accel_frame') + + ret = self.process_data(themes, False) + assert ret[0], ret[1] + assert self.set_bool_param('enable_infra1', True) + + ret = self.run_test(themes, timeout=10) + assert ret[0], ret[1] + ret = self.process_data(themes, True) + assert ret[0], ret[1] + finally: + self.shutdown() + def process_data(self, themes, enable_infra1): + frame_ids = [self.params['camera_name']+'_link', + self.params['camera_name']+'_depth_frame', + self.params['camera_name']+'_infra2_frame', + self.params['camera_name']+'_color_frame'] + if self.params['device_type'] == 'D455': + frame_ids.append(self.params['camera_name']+'_gyro_frame') + frame_ids.append(self.params['camera_name']+'_accel_frame') + if enable_infra1: + frame_ids.append(self.params['camera_name']+'_infra1_frame') + + data = self.node.pop_first_chunk('/tf_static') + ret = self.check_transform_data(data, frame_ids, True) + return ret + + +test_params_tf_d455 = { + 'camera_name': 'D455', + 'device_type': 'D455', + 'publish_tf': 'true', + 'tf_publish_rate': '1.1', + } + +test_params_tf_d435 = { + 'camera_name': 'D435', + 'device_type': 'D435', + 'publish_tf': 'true', + 'tf_publish_rate': '1.1', + } + +test_params_tf_d415 = { + 'camera_name': 'D415', + 'device_type': 'D415', + 'publish_tf': 'true', + 'tf_publish_rate': '1.1', + } +@pytest.mark.parametrize("launch_descr_with_parameters", [ + pytest.param(test_params_tf_d455, marks=pytest.mark.d455), + pytest.param(test_params_tf_d435, marks=pytest.mark.d415), + pytest.param(test_params_tf_d415, marks=pytest.mark.d415), + ],indirect=True) +@pytest.mark.launch(fixture=launch_descr_with_parameters) +class TestCamera_TestTF_DYN(pytest_rs_utils.RsTestBaseClass): + def test_camera_test_tf_dyn(self,launch_descr_with_parameters): + self.params = launch_descr_with_parameters[1] + if pytest_live_camera_utils.check_if_camera_connected(self.params['device_type']) == False: + print("Device not found? : " + self.params['device_type']) + return + themes = [ + {'topic':'/tf', + 'msg_type':msg_TFMessage, + 'expected_data_chunks':3, + 'qos' : QoSProfile(depth=100,durability=DurabilityPolicy.VOLATILE,history=HistoryPolicy.KEEP_LAST,) + #'qos' : QoSProfile(depth=100,durability=DurabilityPolicy.TRANSIENT_LOCAL,history=HistoryPolicy.KEEP_LAST,)#static + }, + {'topic':'/tf_static', + 'msg_type':msg_TFMessage, + 'expected_data_chunks':1, + #'qos' : QoSProfile(depth=100,durability=DurabilityPolicy.VOLATILE,history=HistoryPolicy.KEEP_LAST,) + 'qos' : QoSProfile(depth=100,durability=DurabilityPolicy.TRANSIENT_LOCAL,history=HistoryPolicy.KEEP_LAST,)#static + } + ] + try: + ''' + initialize, run and check the data + ''' + self.init_test("RsTest"+self.params['camera_name']) + self.wait_for_node(self.params['camera_name']) + self.create_param_ifs(get_node_heirarchy(self.params)) + ret = self.run_test(themes, timeout=10) + assert ret[0], ret[1] + ret = self.process_data(themes, False) + assert ret[0], ret[1] + finally: + self.shutdown() + + def process_data(self, themes, enable_infra1): + frame_ids = [self.params['camera_name']+'_link', + self.params['camera_name']+'_depth_frame', + self.params['camera_name']+'_color_frame'] + data = self.node.pop_first_chunk('/tf_static') + ret = self.check_transform_data(data, frame_ids, True) + assert ret[0], ret[1] + data = self.node.pop_first_chunk('/tf') + ret = self.check_transform_data(data, frame_ids) + assert ret[0], ret[1] + data = self.node.pop_first_chunk('/tf') + ret = self.check_transform_data(data, frame_ids) + assert ret[0], ret[1] + data = self.node.pop_first_chunk('/tf') + ret = self.check_transform_data(data, frame_ids) + assert ret[0], ret[1] + #return True, "" + + return True, "" diff --git a/realsense2_camera/test/utils/pytest_live_camera_utils.py b/realsense2_camera/test/utils/pytest_live_camera_utils.py index 3a5ab6f600..36ade782d5 100644 --- a/realsense2_camera/test/utils/pytest_live_camera_utils.py +++ b/realsense2_camera/test/utils/pytest_live_camera_utils.py @@ -177,6 +177,23 @@ def get_camera_capabilities(device_type, serial_no=None): return capability return None +def check_if_camera_connected(device_type, serial_no=None): + long_data = os.popen("rs-enumerate-devices -s").read().splitlines() + debug_print(serial_no) + index = 0 + for index in range(len(long_data)): + name_line = long_data[index].split() + if name_line[0] != "Intel": + continue + if name_line[2] != device_type: + continue + if serial_no == None: + return True + if serial_no == name_line[3]: + return True + + return False + if __name__ == '__main__': device_type = 'D455' serial_no = None diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index a30f2fb7a2..05e6847e13 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/realsense2_camera/test/utils/pytest_rs_utils.py @@ -81,7 +81,7 @@ import requests def debug_print(*args): - if(True): + if(False): print(*args) class RosbagManager(object): @@ -623,7 +623,7 @@ def reset_data(self, topic): self.frame_counter[topic] = 0 self._ros_topic_hz.restart_topic(topic) - def create_subscription(self, msg_type, topic , data_type, store_raw_data, measure_hz, static_tf): + def create_subscription(self, msg_type, topic , data_type, store_raw_data, measure_hz): self.reset_data(topic) super().create_subscription(msg_type, topic , self.rsCallback(topic,msg_type, store_raw_data), data_type) #hz measurements are not working @@ -632,7 +632,7 @@ def create_subscription(self, msg_type, topic , data_type, store_raw_data, measu super().create_subscription(msg_class,topic,functools.partial(self._ros_topic_hz._callback_hz, topic=topic),data_type) self._ros_topic_hz.set_last_printed_tn(0, topic=topic) - if (static_tf == True) and (self.tfBuffer == None): + if self.tfBuffer == None: self.tfBuffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tfBuffer, super()) def get_tfs(self, coupled_frame_ids): @@ -771,9 +771,9 @@ def init_test(self,name='RsTestNode'): def wait_for_node(self, node_name, timeout=8.0): self.node.wait_for_node(node_name, timeout) - def create_subscription(self, msg_type, topic, data_type, store_raw_data=False, measure_hz=False, static_tf=False): + def create_subscription(self, msg_type, topic, data_type, store_raw_data=False, measure_hz=False): if not topic in self.subscribed_topics: - self.node.create_subscription(msg_type, topic, data_type, store_raw_data, measure_hz, static_tf) + self.node.create_subscription(msg_type, topic, data_type, store_raw_data, measure_hz) self.subscribed_topics.append(topic) else: self.node.reset_data(topic) @@ -859,7 +859,6 @@ def spin_for_time(self,wait_time): def run_test(self, themes, initial_wait_time=0.0, timeout=5.0): try: - static_tf_found = False for theme in themes: store_raw_data = False if 'store_raw_data' in theme: @@ -868,13 +867,10 @@ def run_test(self, themes, initial_wait_time=0.0, timeout=5.0): measure_hz = True else: measure_hz = False - if 'static_tf' in theme: - static_tf = True - static_tf_found = True - else: - static_tf = False - - self.create_subscription(theme['msg_type'], theme['topic'] , qos.qos_profile_sensor_data,store_raw_data, measure_hz, static_tf) + qos_type = qos.qos_profile_sensor_data + if 'qos' in theme: + qos_type = theme['qos'] + self.create_subscription(theme['msg_type'], theme['topic'] , qos_type,store_raw_data, measure_hz) print('subscription created for ' + theme['topic']) if initial_wait_time != 0.0: self.spin_for_time(initial_wait_time) @@ -893,6 +889,28 @@ def run_test(self, themes, initial_wait_time=0.0, timeout=5.0): return self.flag def get_tfs(self, coupled_frame_ids): return self.node.get_tfs(coupled_frame_ids) + + + def check_transform_data(self, data, frame_ids, is_static=False): + coupled_frame_ids = [xx for xx in itertools.combinations(frame_ids, 2)] + tfBuffer = tf2_ros.Buffer() + for transform in data.transforms: + if is_static: + tfBuffer.set_transform_static(transform, "default_authority") + else: + tfBuffer.set_transform(transform, "default_authority") + res = dict() + for couple in coupled_frame_ids: + from_id, to_id = couple + if (tfBuffer.can_transform(from_id, to_id, rclpy.time.Time(), rclpy.time.Duration(nanoseconds=3e6))): + res[couple] = tfBuffer.lookup_transform(from_id, to_id, rclpy.time.Time(), rclpy.time.Duration(nanoseconds=1e6)).transform + else: + res[couple] = None + for couple in coupled_frame_ids: + if res[couple] == None: + return False, str(couple) + ": didn't get any tf data" + return True,"" + ''' Please override and use your own process_data if the default check is not suitable. Please also store_raw_data parameter in the themes as True, if you want the From adc75d74c3cf9ed878708e3e058ee8b5b09e0d8c Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Tue, 5 Sep 2023 21:29:05 +0530 Subject: [PATCH 23/76] Added pointcloud tests for live camera --- .../test_camera_point_cloud_tests.py | 114 ++++++++++++++++++ .../test/live_camera/test_camera_tf_tests.py | 28 ++--- .../test/utils/pytest_rs_utils.py | 7 +- 3 files changed, 133 insertions(+), 16 deletions(-) create mode 100644 realsense2_camera/test/live_camera/test_camera_point_cloud_tests.py diff --git a/realsense2_camera/test/live_camera/test_camera_point_cloud_tests.py b/realsense2_camera/test/live_camera/test_camera_point_cloud_tests.py new file mode 100644 index 0000000000..14def86ca9 --- /dev/null +++ b/realsense2_camera/test/live_camera/test_camera_point_cloud_tests.py @@ -0,0 +1,114 @@ +# 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. + + +import os +import sys +import itertools + + +import pytest +import rclpy + +from rclpy.qos import DurabilityPolicy +from rclpy.qos import HistoryPolicy +from rclpy.qos import QoSProfile +import tf2_ros + + +from sensor_msgs.msg import Image as msg_Image +from sensor_msgs.msg import Imu as msg_Imu +from sensor_msgs.msg import PointCloud2 as msg_PointCloud2 +from tf2_msgs.msg import TFMessage as msg_TFMessage + +import numpy as np + +sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils")) +import pytest_rs_utils +from pytest_rs_utils import launch_descr_with_parameters + +import pytest_live_camera_utils +from pytest_rs_utils import get_rosbag_file_path +from pytest_rs_utils import get_node_heirarchy + +from rclpy.parameter import Parameter +from rcl_interfaces.msg import ParameterValue +from rcl_interfaces.srv import SetParameters, GetParameters, ListParameters + +''' +The test was implemented to check the tf_static before and after infra1 was enabled. There shouldn't be any +related data before enabling. +''' +test_params_points_cloud_d455 = { + 'camera_name': 'D455', + 'device_type': 'D455', + 'pointcloud.enable': 'true' + } +test_params_points_cloud_d435 = { + 'camera_name': 'D435', + 'device_type': 'D435', + 'pointcloud.enable': 'true' + } + +test_params_points_cloud_d415 = { + 'camera_name': 'D415', + 'device_type': 'D415', + 'enable_color':'true', + 'enable_depth':'true', + 'depth_module.profile':'848x480x30', + 'pointcloud.enable': 'true' + } + +''' +This test was ported from rs2_test.py +the command used to run is "python3 realsense2_camera/scripts/rs2_test.py points_cloud_1" +''' +@pytest.mark.parametrize("launch_descr_with_parameters", [ + pytest.param(test_params_points_cloud_d455, marks=pytest.mark.d455), + pytest.param(test_params_points_cloud_d435, marks=pytest.mark.d435), + pytest.param(test_params_points_cloud_d415, marks=pytest.mark.d415), + ],indirect=True) +@pytest.mark.launch(fixture=launch_descr_with_parameters) +class TestCamera_TestPointCloud(pytest_rs_utils.RsTestBaseClass): + def test_camera_test_point_cloud(self,launch_descr_with_parameters): + self.params = launch_descr_with_parameters[1] + if pytest_live_camera_utils.check_if_camera_connected(self.params['device_type']) == False: + print("Device not found? : " + self.params['device_type']) + return + themes = [ + { + 'topic':get_node_heirarchy(self.params)+'/depth/color/points', + 'msg_type':msg_PointCloud2, + 'expected_data_chunks':5, + }, + ] + try: + ''' + initialize, run and check the data + ''' + self.init_test("RsTest"+self.params['camera_name']) + self.wait_for_node(self.params['camera_name']) + self.create_param_ifs(get_node_heirarchy(self.params)) + ret = self.run_test(themes, timeout=10) + assert ret[0], ret[1] + ret = self.process_data(themes, False) + assert ret[0], ret[1] + finally: + self.shutdown() + def process_data(self, themes, enable_infra1): + for count in range(self.node.get_num_chunks(get_node_heirarchy(self.params)+'/depth/color/points')): + data = self.node.pop_first_chunk(get_node_heirarchy(self.params)+'/depth/color/points') + print(data)#the frame counter starts with zero, jumps to 2 and continues. To be checked + return True,"" + diff --git a/realsense2_camera/test/live_camera/test_camera_tf_tests.py b/realsense2_camera/test/live_camera/test_camera_tf_tests.py index 10c72c8b09..4e8250f063 100644 --- a/realsense2_camera/test/live_camera/test_camera_tf_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_tf_tests.py @@ -103,15 +103,7 @@ def test_camera_test_tf_static_change(self,launch_descr_with_parameters): self.create_param_ifs(get_node_heirarchy(self.params)) ret = self.run_test(themes, timeout=10) assert ret[0], ret[1] - frame_ids = [self.params['camera_name']+'_link', - self.params['camera_name']+'_depth_frame', - self.params['camera_name']+'_infra1_frame', - self.params['camera_name']+'_infra2_frame', - self.params['camera_name']+'_color_frame'] - if self.params['device_type'] == 'D455': - frame_ids.append(self.params['camera_name']+'_gyro_frame') - frame_ids.append(self.params['camera_name']+'_accel_frame') - + ret = self.process_data(themes, False) assert ret[0], ret[1] assert self.set_bool_param('enable_infra1', True) @@ -130,12 +122,20 @@ def process_data(self, themes, enable_infra1): if self.params['device_type'] == 'D455': frame_ids.append(self.params['camera_name']+'_gyro_frame') frame_ids.append(self.params['camera_name']+'_accel_frame') - if enable_infra1: - frame_ids.append(self.params['camera_name']+'_infra1_frame') - + frame_ids.append(self.params['camera_name']+'_infra1_frame') data = self.node.pop_first_chunk('/tf_static') - ret = self.check_transform_data(data, frame_ids, True) - return ret + coupled_frame_ids = [xx for xx in itertools.combinations(frame_ids, 2)] + res = self.get_transform_data(data, coupled_frame_ids, is_static=True) + for couple in coupled_frame_ids: + if self.params['camera_name']+'_infra1_frame' in couple: + if enable_infra1 == True and res[couple] != None: + continue + if enable_infra1 == False and res[couple] == None: + continue + return False, str(couple) + ": tf_data not as expected" + if res[couple] == None: + return False, str(couple) + ": didn't get any tf data" + return True,"" test_params_tf_d455 = { diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index 05e6847e13..c07b135065 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/realsense2_camera/test/utils/pytest_rs_utils.py @@ -891,8 +891,7 @@ def get_tfs(self, coupled_frame_ids): return self.node.get_tfs(coupled_frame_ids) - def check_transform_data(self, data, frame_ids, is_static=False): - coupled_frame_ids = [xx for xx in itertools.combinations(frame_ids, 2)] + def get_transform_data(self, data, coupled_frame_ids, is_static=False): tfBuffer = tf2_ros.Buffer() for transform in data.transforms: if is_static: @@ -906,6 +905,10 @@ def check_transform_data(self, data, frame_ids, is_static=False): res[couple] = tfBuffer.lookup_transform(from_id, to_id, rclpy.time.Time(), rclpy.time.Duration(nanoseconds=1e6)).transform else: res[couple] = None + return res + def check_transform_data(self, data, frame_ids, is_static=False): + coupled_frame_ids = [xx for xx in itertools.combinations(frame_ids, 2)] + res = self.get_transform_data(data, coupled_frame_ids, is_static) for couple in coupled_frame_ids: if res[couple] == None: return False, str(couple) + ": didn't get any tf data" From 6fdfaa0c07562290e4643384c520afc3e3487d01 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Thu, 7 Sep 2023 06:02:21 +0530 Subject: [PATCH 24/76] updated readme --- README.md | 28 ++++++++++++++++------------ realsense2_camera/src/parameters.cpp | 2 ++ 2 files changed, 18 insertions(+), 12 deletions(-) diff --git a/README.md b/README.md index e308b3b80b..58506045cc 100644 --- a/README.md +++ b/README.md @@ -304,6 +304,22 @@ User can set the camera name and camera namespace, to distinguish between camera - This param also depends on **publish_tf** param - If **publish_tf:=false**, then no TFs will be published, even if **tf_publish_rate** is >0.0 Hz - If **publish_tf:=true** and **tf_publish_rate** set to >0.0 Hz, then dynamic TFs will be published at the specified rate +- **unite_imu_method**: + - For the D400 cameras with built in IMU components, below 2 unrelated streams (each with it's own frequency) will be created: + - *gyro* - which shows angular velocity + - *accel* - which shows linear acceleration. + - Both streams will publish data to its corresponding topics: + - '/camera/camera/gyro/sample' & '/camera/camera/accel/sample' + - Though both topics are of same message type 'sensor_msgs::Imu', only their relevant fields are filled out. + - A new topic called **imu** will be created, when both *accel* and *gyro* streams are enabled and the param *unite_imu_method* set to > 0. + - Data from both accel and gyro are combined and published to this topic + - All the fields of the Imu message are filled out. + - It will be published at the rate of the gyro. + - `unite_imu_method` param supports below values: + - 0 -> **none**: no imu topic + - 1 -> **copy**: Every gyro message will be attached by the last accel message. + - 2 -> **linear_interpolation**: Every gyro message will be attached by an accel message which is interpolated to gyro's timestamp. + - Note: When the param *unite_imu_method* is dynamically updated, re-enable either gyro or accel stream for the change to take effect. #### Parameters that cannot be changed in runtime: - **serial_no**: @@ -339,18 +355,6 @@ User can set the camera name and camera namespace, to distinguish between camera - For example: `initial_reset:=true` - **base_frame_id**: defines the frame_id all static transformations refers to. - **odom_frame_id**: defines the origin coordinate system in ROS convention (X-Forward, Y-Left, Z-Up). pose topic defines the pose relative to that system. - -- **unite_imu_method**: - - D400 cameras have built in IMU components which produce 2 unrelated streams, each with it's own frequency: - - *gyro* - which shows angular velocity - - *accel* which shows linear acceleration. - - By default, 2 corresponding topics are available, each with only the relevant fields of the message sensor_msgs::Imu are filled out. - - Setting *unite_imu_method* creates a new topic, *imu*, that replaces the default *gyro* and *accel* topics. - - The *imu* topic is published at the rate of the gyro. - - All the fields of the Imu message under the *imu* topic are filled out. - - `unite_imu_method` parameter supported values are [0-2] meaning: [0 -> None, 1 -> Copy, 2 -> Linear_ interpolation] when: - - **linear_interpolation**: Every gyro message is attached by the an accel message interpolated to the gyro's timestamp. - - **copy**: Every gyro message is attached by the last accel message. - **clip_distance**: - Remove from the depth image all values above a given value (meters). Disable by giving negative value (default) - For example: `clip_distance:=1.5` diff --git a/realsense2_camera/src/parameters.cpp b/realsense2_camera/src/parameters.cpp index 16965f27cf..c5d1abef3a 100644 --- a/realsense2_camera/src/parameters.cpp +++ b/realsense2_camera/src/parameters.cpp @@ -135,6 +135,8 @@ void BaseRealSenseNode::setDynamicParams() [this](const rclcpp::Parameter& parameter) { _imu_sync_method = imu_sync_method(parameter.get_value()); + ROS_WARN("For the 'unite_imu_method' param update to take effect, " + "re-enable either gyro or accel stream."); }, crnt_descriptor); _parameters_names.push_back(param_name); } From 41ae997c9ac6da139725a789679329aec156e36d Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Thu, 7 Sep 2023 06:41:51 +0530 Subject: [PATCH 25/76] updated QoS of IMU topic --- realsense2_camera/src/rs_node_setup.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index 51aa070273..6e05d9ad85 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -311,7 +311,10 @@ void BaseRealSenseNode::startPublishers(const std::vector& profi } if (_is_accel_enabled && _is_gyro_enabled && (_imu_sync_method > imu_sync_method::NONE)) { - _synced_imu_publisher = std::make_shared(_node.create_publisher("~/imu", 5)); + rmw_qos_profile_t qos = _use_intra_process ? qos_string_to_qos(DEFAULT_QOS) : qos_string_to_qos(HID_QOS); + + _synced_imu_publisher = std::make_shared(_node.create_publisher("~/imu", + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos), qos))); } } From ee244efdfa99a5a14db4349c86caf992b209ac58 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Thu, 7 Sep 2023 03:09:16 +0530 Subject: [PATCH 26/76] Fix: Pointcloud topic's frame_id --- realsense2_camera/src/base_realsense_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 35a88d1b14..38c5ab39f6 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -814,7 +814,7 @@ void BaseRealSenseNode::SetBaseStream() void BaseRealSenseNode::publishPointCloud(rs2::points pc, const rclcpp::Time& t, const rs2::frameset& frameset) { - std::string frame_id = (_align_depth_filter->is_enabled() ? OPTICAL_FRAME_ID(COLOR) : OPTICAL_FRAME_ID(DEPTH)); + std::string frame_id = OPTICAL_FRAME_ID(DEPTH); _pc_filter->Publish(pc, t, frameset, frame_id); } From 163444451afb2c7aadff78885ff3bc3d00809a46 Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Fri, 8 Sep 2023 16:58:47 +0530 Subject: [PATCH 27/76] Add aligned tests --- realsense2_camera/test/README.md | 11 + .../live_camera/test_camera_aligned_tests.py | 274 ++++++++++++++++++ .../test_camera_all_profile_tests.py | 1 + .../rosbag/test_rosbag_all_topics_test.py | 1 - .../test/utils/pytest_live_camera_utils.py | 28 +- 5 files changed, 300 insertions(+), 15 deletions(-) create mode 100644 realsense2_camera/test/live_camera/test_camera_aligned_tests.py diff --git a/realsense2_camera/test/README.md b/realsense2_camera/test/README.md index a026a36b2e..9624848955 100644 --- a/realsense2_camera/test/README.md +++ b/realsense2_camera/test/README.md @@ -146,6 +146,17 @@ The below command finds the test with the name test_static_tf_1 in realsense2_ca pytest-3 -s -k test_static_tf_1 realsense2_camera/test/ +### Marking tests as regression tests +Some of the tests, especially the live tests with multiple runs, for e.g., all profile tests (test_camera_all_profile_tests.py) take a long time. Such tests are marked are skipped with condition so that "colcon test" skips it. +If a user wants to add a test to this conditional skip, user can add by adding a marker as below. + +@pytest.mark.skipif (os.getenv('RS_ROS_REGRESSION', "not found") == "not found",reason="Regression is not enabled, define RS_ROS_REGRESSION") + +### Running skipped regression tests +1. Set the environment variable RS_ROS_REGRESSION as 1 and run the "colcon test" +2. pytest example: + RS_ROS_REGRESSION=1 PYTHONPATH=$PYTHONPATH:$PWD/realsense2_camera/test/utils:$PWD/realsense2_camera//launch:$PWD/realsense2_camera//scripts pytest-3 -s realsense2_camera/test/live_camera/test_camera_aligned_tests.py -k test_camera_align_depth_color_all -m d415 + ## Points to be noted while writing pytests The tests that are in one file are normally run in parallel. So if there are multiple tests in one file, the system capacity can influence the test execution. It's recomended to have 3-4 tests in file, more than that can affect the test results due to delays. ### Passing/changing parameters diff --git a/realsense2_camera/test/live_camera/test_camera_aligned_tests.py b/realsense2_camera/test/live_camera/test_camera_aligned_tests.py new file mode 100644 index 0000000000..d489b5d546 --- /dev/null +++ b/realsense2_camera/test/live_camera/test_camera_aligned_tests.py @@ -0,0 +1,274 @@ +# 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. + + +import os +import sys +import itertools + + +import pytest +import rclpy + +from sensor_msgs.msg import Image as msg_Image +from sensor_msgs.msg import Imu as msg_Imu +from sensor_msgs.msg import PointCloud2 as msg_PointCloud2 + +import numpy as np + +sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils")) +import pytest_rs_utils +from pytest_rs_utils import launch_descr_with_parameters +from pytest_rs_utils import delayed_launch_descr_with_parameters +from pytest_rs_utils import get_rosbag_file_path +from pytest_rs_utils import get_node_heirarchy +import pytest_live_camera_utils +from rclpy.parameter import Parameter +from rcl_interfaces.msg import ParameterValue +from rcl_interfaces.srv import SetParameters, GetParameters, ListParameters +from pytest_live_camera_utils import debug_print + + + +test_params_align_depth_color_d455 = { + 'camera_name': 'D455', + 'device_type': 'D455', + 'enable_color':'true', + 'enable_depth':'true', + 'depth_module.profile':'848x480x30', + 'rgb_camera.profile':'640x480x30', + 'align_depth.enable':'true' + } +test_params_align_depth_color_d415 = { + 'camera_name': 'D415', + 'device_type': 'D415', + 'enable_color':'true', + 'enable_depth':'true', + 'depth_module.profile':'848x480x30', + 'rgb_camera.profile':'640x480x30', + 'align_depth.enable':'true' + } +''' +This test was implemented as a template to set the parameters and run the test. +This directory is not added to the CMakeLists.txt so as to avoid the colcon failure in the +machines that don't have the D455 connected. +1. Only a subset of parameter types are implemented in py_rs_utils, it has to be extended for others +2. After setting the param, rclpy.spin_once may be needed.Test passes even without this though. +''' +@pytest.mark.parametrize("launch_descr_with_parameters",[ + pytest.param(test_params_align_depth_color_d455, marks=pytest.mark.d455), + pytest.param(test_params_align_depth_color_d415, marks=pytest.mark.d415), + #pytest.param(test_params_align_depth_color_d435, marks=pytest.mark.d435), + ] + ,indirect=True) +@pytest.mark.launch(fixture=launch_descr_with_parameters) +class TestCamera_AlignDepthColor(pytest_rs_utils.RsTestBaseClass): + def test_camera_align_depth_color(self,launch_descr_with_parameters): + params = launch_descr_with_parameters[1] + themes = [ + {'topic':get_node_heirarchy(params)+'/color/image_raw', + 'msg_type':msg_Image, + 'expected_data_chunks':1, + 'width':640, + 'height':480, + }, + {'topic':get_node_heirarchy(params)+'/depth/image_rect_raw', + 'msg_type':msg_Image, + 'expected_data_chunks':1, + 'width':848, + 'height':480, + }, + {'topic':get_node_heirarchy(params)+'/aligned_depth_to_color/image_raw', + 'msg_type':msg_Image, + 'expected_data_chunks':1, + 'width':640, + 'height':480, + }, + ] + try: + ''' + initialize, run and check the data + ''' + print("Starting camera test...") + self.init_test("RsTest"+params['camera_name']) + self.wait_for_node(params['camera_name']) + self.create_param_ifs(get_node_heirarchy(params)) + ret = self.run_test(themes) + assert ret[0], ret[1] + assert self.process_data(themes) + self.set_string_param('rgb_camera.profile', '1280x720x30') + self.set_bool_param('enable_color', True) + themes[0]['width'] = 1280 + themes[0]['height'] = 720 + themes[2]['width'] = 1280 + themes[2]['height'] = 720 + + ret = self.run_test(themes) + assert ret[0], ret[1] + assert self.process_data(themes) + finally: + #this step is important because the test will fail next time + pytest_rs_utils.kill_realsense2_camera_node() + self.shutdown() + + + +test_params_all_profiles_d455 = { + 'camera_name': 'D455', + 'device_type': 'D455', + 'enable_color':'true', + 'enable_depth':'true', + 'depth_module.profile':'848x480x30', + 'rgb_camera.profile':'640x480x30', + 'align_depth.enable':'true' + } +test_params_all_profiles_d415 = { + 'camera_name': 'D415', + 'device_type': 'D415', + 'enable_color':'true', + 'enable_depth':'true', + 'depth_module.profile':'848x480x30', + 'rgb_camera.profile':'640x480x30', + 'align_depth.enable':'true' + } +test_params_all_profiles_d435 = { + 'camera_name': 'D435', + 'device_type': 'D435', + 'enable_color':'true', + 'enable_depth':'true', + 'depth_module.profile':'848x480x30', + 'rgb_camera.profile':'640x480x30', + 'align_depth.enable':'true' + } + + +''' +This test was implemented as a template to set the parameters and run the test. +This directory is not added to the CMakeLists.txt so as to avoid the colcon failure in the +machines that don't have the D455 connected. +1. Only a subset of parameter types are implemented in py_rs_utils, it has to be extended for others +2. After setting the param, rclpy.spin_once may be needed.Test passes even without this though. +''' +@pytest.mark.skipif (os.getenv('RS_ROS_REGRESSION', "not found") == "not found",reason="Regression is not enabled, define RS_ROS_REGRESSION") +@pytest.mark.parametrize("launch_descr_with_parameters", [ + pytest.param(test_params_all_profiles_d455, marks=pytest.mark.d455), + pytest.param(test_params_all_profiles_d415, marks=pytest.mark.d415), + pytest.param(test_params_all_profiles_d435, marks=pytest.mark.d435),] + ,indirect=True) +@pytest.mark.launch(fixture=launch_descr_with_parameters) +class TestCamera_AlignDepthColor(pytest_rs_utils.RsTestBaseClass): + def test_camera_align_depth_color_all(self,launch_descr_with_parameters): + skipped_tests = [] + failed_tests = [] + num_passed = 0 + num_failed = 0 + params = launch_descr_with_parameters[1] + themes = [ + {'topic':get_node_heirarchy(params)+'/color/image_raw', + 'msg_type':msg_Image, + 'expected_data_chunks':1, + 'width':640, + 'height':480, + }, + {'topic':get_node_heirarchy(params)+'/depth/image_rect_raw', + 'msg_type':msg_Image, + 'expected_data_chunks':1, + 'width':848, + 'height':480, + }, + {'topic':get_node_heirarchy(params)+'/aligned_depth_to_color/image_raw', + 'msg_type':msg_Image, + 'expected_data_chunks':1, + 'width':640, + 'height':480, + }, + ] + try: + ''' + initialize, run and check the data + ''' + self.init_test("RsTest"+params['camera_name']) + self.spin_for_time(wait_time=1.0) + cap = pytest_live_camera_utils.get_camera_capabilities(params['device_type']) + if cap == None: + debug_print("Device not found? : " + params['device_type']) + return + self.create_param_ifs(get_node_heirarchy(params)) + color_profiles = set([i[1] for i in cap["color_profile"] if i[2] == "RGB8"]) + depth_profiles = set([i[1] for i in cap["depth_profile"] if i[0] == "Depth"]) + for color_profile in color_profiles: + for depth_profile in depth_profiles: + if depth_profile == color_profile: + continue + print("Testing the alignment of Depth:", depth_profile, " and Color:", color_profile) + self.set_bool_param('enable_color', False) + self.set_bool_param('enable_color', False) + self.set_bool_param('align_depth.enable', False) + + themes[0]['width'] = themes[2]['width'] = int(color_profile.split('x')[0]) + themes[0]['height'] = themes[2]['height'] = int(color_profile.split('x')[1]) + themes[1]['width'] = int(depth_profile.split('x')[0]) + themes[1]['height'] = int(depth_profile.split('x')[1]) + dfps = int(depth_profile.split('x')[2]) + cfps = int(color_profile.split('x')[2]) + if dfps > cfps: + fps = cfps + else: + fps = dfps + timeout=100.0/fps + #for the changes to take effect + self.spin_for_time(wait_time=timeout/20) + self.set_string_param('rgb_camera.profile', color_profile) + self.set_string_param('depth_module.profile', depth_profile) + self.set_bool_param('enable_color', True) + self.set_bool_param('enable_color', True) + self.set_bool_param('align_depth.enable', True) + #for the changes to take effect + self.spin_for_time(wait_time=timeout/20) + try: + ret = self.run_test(themes, timeout=timeout) + assert ret[0], ret[1] + assert self.process_data(themes), "".join(str(depth_profile) + " " + str(color_profile)) + " failed" + num_passed += 1 + except Exception as e: + exc_type, exc_obj, exc_tb = sys.exc_info() + fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1] + print("Test failed") + print("Tested the alignment of Depth:", depth_profile, " and Color:", color_profile, " with timeout ", timeout) + print(e) + print(exc_type, fname, exc_tb.tb_lineno) + num_failed += 1 + failed_tests.append("".join(str(depth_profile) + " " + str(color_profile))) + + finally: + #this step is important because the test will fail next time + pytest_rs_utils.kill_realsense2_camera_node() + self.shutdown() + print("Tests passed " + str(num_passed)) + print("Tests skipped " + str(len(skipped_tests))) + if len(skipped_tests): + debug_print("\nSkipped tests:" + params['device_type']) + debug_print("\n".join(skipped_tests)) + print("Tests failed " + str(num_failed)) + if len(failed_tests): + print("\nFailed tests:" + params['device_type']) + print("\n".join(failed_tests)) + + def disable_all_params(self): + self.set_bool_param('enable_color', False) + self.set_bool_param('enable_depth', False) + self.set_bool_param('enable_infra', False) + self.set_bool_param('enable_infra1', False) + self.set_bool_param('enable_infra2', False) + diff --git a/realsense2_camera/test/live_camera/test_camera_all_profile_tests.py b/realsense2_camera/test/live_camera/test_camera_all_profile_tests.py index 274ee67625..b337e6f8d5 100644 --- a/realsense2_camera/test/live_camera/test_camera_all_profile_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_all_profile_tests.py @@ -140,6 +140,7 @@ def check_if_skip_test(profile, format): 1. Only a subset of parameter types are implemented in py_rs_utils, it has to be extended for others 2. After setting the param, rclpy.spin_once may be needed.Test passes even without this though. ''' +@pytest.mark.skipif (os.getenv('RS_ROS_REGRESSION', "not found") == "not found",reason="Regression is not enabled, define RS_ROS_REGRESSION") @pytest.mark.parametrize("launch_descr_with_parameters", [ pytest.param(test_params_all_profiles_d455, marks=pytest.mark.d455), pytest.param(test_params_all_profiles_d415, marks=pytest.mark.d415), diff --git a/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py b/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py index 34abbc3039..22c4e1caee 100644 --- a/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py +++ b/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py @@ -57,7 +57,6 @@ ''' To test all topics published ''' -@pytest.mark.skip @pytest.mark.rosbag @pytest.mark.parametrize("delayed_launch_descr_with_parameters", [test_params_all_topics],indirect=True) @pytest.mark.launch(fixture=delayed_launch_descr_with_parameters) diff --git a/realsense2_camera/test/utils/pytest_live_camera_utils.py b/realsense2_camera/test/utils/pytest_live_camera_utils.py index 36ade782d5..9b5bfeac70 100644 --- a/realsense2_camera/test/utils/pytest_live_camera_utils.py +++ b/realsense2_camera/test/utils/pytest_live_camera_utils.py @@ -48,19 +48,6 @@ def get_default_profiles(cap, profile): debug_print(profile + " default profile2:" + profile2) return profile1,profile2 -def get_camera_capabilities_short(device_type, serial_no=None): - short_data = os.popen("rs-enumerate-devices -s").read().splitlines() - print(serial_no) - for line in short_data: - print(line) - if device_type in line: - if serial_no is None or serial_no == "" : - print(device_type+ " found in " + line) - return - if serial_no in line: - print(device_type + " with serial_no " + serial_no +" found in " + line) - return - print(device_type + " not found") device_info_string = "Device info:" @@ -177,6 +164,20 @@ def get_camera_capabilities(device_type, serial_no=None): return capability return None +def get_camera_capabilities_short(device_type, serial_no=None): + short_data = os.popen("rs-enumerate-devices -s").read().splitlines() + print(serial_no) + for line in short_data: + print(line) + if device_type in line: + if serial_no is None or serial_no == "" : + print(device_type+ " found in " + line) + return + if serial_no in line: + print(device_type + " with serial_no " + serial_no +" found in " + line) + return + print(device_type + " not found") + def check_if_camera_connected(device_type, serial_no=None): long_data = os.popen("rs-enumerate-devices -s").read().splitlines() debug_print(serial_no) @@ -201,7 +202,6 @@ def check_if_camera_connected(device_type, serial_no=None): device_type = sys.argv[1] if len(sys.argv) > 2: serial_no = sys.argv[2] - cap = get_camera_capabilities(device_type, serial_no) print("Capabilities:") print(cap) \ No newline at end of file From af646fb609de96219351fb37b55c3323c0720e9d Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Fri, 8 Sep 2023 17:36:35 +0530 Subject: [PATCH 28/76] Modified the imu test to remove workaround for RS550 --- realsense2_camera/test/live_camera/test_camera_imu_tests.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/realsense2_camera/test/live_camera/test_camera_imu_tests.py b/realsense2_camera/test/live_camera/test_camera_imu_tests.py index c712cd1ba7..e2defb6b52 100644 --- a/realsense2_camera/test/live_camera/test_camera_imu_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_imu_tests.py @@ -80,8 +80,7 @@ def test_LiveCamera_check_motion_sensor(self,launch_descr_with_parameters): self.set_bool_param('enable_accel', False) self.set_bool_param('enable_gyro', True) themes[ACCEL_TOPIC]['expected_data_chunks'] = 0 - #uncomment once RSDEV-550 is closed - #themes[IMU_TOPIC]['expected_data_chunks'] = 0 + themes[IMU_TOPIC]['expected_data_chunks'] = 0 print(msg) ret = self.run_test(themes) assert ret[0], msg + str(ret[1]) From 139acdea099dac495ccc1f86677e9cef0ea982a5 Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Fri, 8 Sep 2023 19:28:55 +0530 Subject: [PATCH 29/76] removed failing_test file, was covered in all_profile_tests anyway --- realsense2_camera/CMakeLists.txt | 7 +- .../live_camera/test_camera_failing_tests.py | 268 ------------------ 2 files changed, 1 insertion(+), 274 deletions(-) delete mode 100644 realsense2_camera/test/live_camera/test_camera_failing_tests.py diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index d8eaf08348..75572d377f 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -330,16 +330,11 @@ if(BUILD_TESTING) OUTPUT_VARIABLE RS_DEVICE_INFO) message(STATUS "rs_device_info:") message(STATUS "${RS_DEVICE_INFO}") - if(RS_DEVICE_INFO MATCHES "D455") + if((RS_DEVICE_INFO MATCHES "D455") OR (RS_DEVICE_INFO MATCHES "D415") OR (RS_DEVICE_INFO MATCHES "D435")) message(STATUS "D455 device found") set(_pytest_live_folders test/live_camera ) - elseif(RS_DEVICE_INFO MATCHES "D415") - message(STATUS "D415 device found") - set(_pytest_live_folders - test/live_camera - ) endif() foreach(test_folder ${_pytest_live_folders}) diff --git a/realsense2_camera/test/live_camera/test_camera_failing_tests.py b/realsense2_camera/test/live_camera/test_camera_failing_tests.py deleted file mode 100644 index 3bdc87b19b..0000000000 --- a/realsense2_camera/test/live_camera/test_camera_failing_tests.py +++ /dev/null @@ -1,268 +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. - - -import os -import sys -import itertools - - -import pytest -import rclpy - -from sensor_msgs.msg import Image as msg_Image -from sensor_msgs.msg import Imu as msg_Imu -from sensor_msgs.msg import PointCloud2 as msg_PointCloud2 - -import numpy as np - -sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils")) -import pytest_rs_utils -from pytest_rs_utils import launch_descr_with_parameters -from pytest_rs_utils import delayed_launch_descr_with_parameters -from pytest_rs_utils import get_rosbag_file_path -from pytest_rs_utils import get_node_heirarchy -import pytest_live_camera_utils -from rclpy.parameter import Parameter -from rcl_interfaces.msg import ParameterValue -from rcl_interfaces.srv import SetParameters, GetParameters, ListParameters -from pytest_live_camera_utils import debug_print - -test_params_all_profiles_d455 = { - 'camera_name': 'D455', - 'device_type': 'D455', - } -test_params_all_profiles_d415 = { - 'camera_name': 'D415', - 'device_type': 'D415', - } -''' -This test was implemented as a template to set the parameters and run the test. -This directory is not added to the CMakeLists.txt so as to avoid the colcon failure in the -machines that don't have the D455 connected. -1. Only a subset of parameter types are implemented in py_rs_utils, it has to be extended for others -2. After setting the param, rclpy.spin_once may be needed.Test passes even without this though. -''' -@pytest.mark.parametrize("launch_descr_with_parameters", [test_params_all_profiles_d415],indirect=True) -@pytest.mark.launch(fixture=launch_descr_with_parameters) -class TestLiveCamera_Change_Resolution(pytest_rs_utils.RsTestBaseClass): - def test_LiveCamera_Change_Resolution(self,launch_descr_with_parameters): - passed_tests = [] - failed_tests = [] - num_passed = 0 - num_failed = 0 - wait_time_s = 1.2 - cap = {} - ''' - need two configurations with different profiles(height & width) for each profile, - this is to ensure the test sets a different profile first, before testing the - actual profile to be tested. - ''' - cap['color_profile'] = [ - ['Color', '1920x1080x30','RGB8'], - ['Color', '1280x720x30','RGB8'], - ] - cap['depth_profile'] = [ - ['Infrared1', '1920x1080x25', 'Y8'], - ['Infrared1', '1920x1080x25', 'Y16'], - ['Infrared1', '1920x1080x15', 'Y16'], - ['Infrared', '848x100x100', 'BGRA8'], - ['Infrared', '640x480x60', 'BGRA8'], - ['Infrared', '640x480x60', 'RGBA8'], - ['Infrared', '640x480x60', 'RGB8'], - ['Infrared', '640x360x60', 'RGBA8'], - ['Infrared', '640x360x60', 'RGB8'], - ['Infrared', '640x360x30', 'UYVY'], - ['Infrared', '480x270x15', 'RGB8'], - ['Infrared', '424x240x60', 'BGRA8'], - ['Infrared', '424x240x30', 'UYVY'], - ['Infrared1', '1920x1080x15', 'Y8'], - ['Infrared1', '1280x720x30', 'Y8'], - ['Infrared1', '1280x720x15', 'Y8'], - ['Infrared1', '1280x720x6', 'Y8'], - ['Infrared1', '960x540x25', 'Y16'], - ['Infrared1', '960x540x15', 'Y16'], - ['Infrared1', '848x480x90', 'Y8'], - ['Infrared1', '848x480x60', 'Y8'], - ['Infrared1', '848x480x30', 'Y8'], - ['Infrared1', '848x480x15', 'Y8'], - ['Infrared1', '848x480x6', 'Y8'], - ['Infrared1', '848x100x100', 'Y8'], - ['Infrared1', '640x480x90', 'Y8'], - ['Infrared1', '640x480x60', 'Y8'], - ['Infrared1', '640x480x30', 'Y8'], - ['Infrared1', '640x480x15', 'Y8'], - ['Infrared1', '640x480x6', 'Y8'], - ['Infrared1', '640x360x90', 'Y8'], - ['Infrared1', '640x360x60', 'Y8'], - ['Infrared1', '640x360x30', 'Y8'], - ['Infrared1', '640x360x15', 'Y8'], - ['Infrared1', '640x360x6', 'Y8'], - ['Infrared1', '480x270x90', 'Y8'], - ['Infrared1', '480x270x60', 'Y8'], - ['Infrared1', '480x270x30', 'Y8'], - ['Infrared1', '480x270x15', 'Y8'], - ['Infrared1', '480x270x6', 'Y8'], - ['Infrared1', '424x240x90', 'Y8'], - ['Infrared1', '424x240x60', 'Y8'], - ['Infrared1', '424x240x30', 'Y8'], - ['Infrared1', '424x240x15', 'Y8'], - ['Infrared1', '424x240x6', 'Y8'], - ['Infrared2', '1920x1080x25', 'Y16'], - ['Infrared2', '1920x1080x25', 'Y8'], - ['Infrared2', '1920x1080x15', 'Y16'], - ['Infrared2', '1920x1080x15', 'Y8'], - ['Infrared2', '1280x720x30', 'Y8'], - ['Infrared2', '1280x720x15', 'Y8'], - ['Infrared2', '1280x720x6', 'Y8'], - ['Infrared2', '960x540x25', 'Y16'], - ['Infrared2', '960x540x15', 'Y16'], - ['Infrared2', '848x480x90', 'Y8'], - ['Infrared2', '848x480x60', 'Y8'], - ['Infrared2', '848x480x30', 'Y8'], - ['Infrared2', '848x480x15', 'Y8'], - ['Infrared2', '848x480x6', 'Y8'], - ['Infrared2', '848x100x100', 'Y8'], - ['Infrared2', '640x480x90', 'Y8'], - ['Infrared2', '640x480x60', 'Y8'], - ['Infrared2', '640x480x30', 'Y8'], - ['Infrared2', '640x480x15', 'Y8'], - ['Infrared2', '640x480x6', 'Y8'], - ['Infrared2', '640x360x90', 'Y8'], - ['Infrared2', '640x360x60', 'Y8'], - ['Infrared2', '640x360x30', 'Y8'], - ['Infrared2', '640x360x15', 'Y8'], - ['Infrared2', '640x360x6', 'Y8'], - ['Infrared2', '480x270x90', 'Y8'], - ['Infrared2', '480x270x60', 'Y8'], - ['Infrared2', '480x270x30', 'Y8'], - ['Infrared2', '480x270x15', 'Y8'], - ['Infrared2', '480x270x6', 'Y8'], - ['Infrared2', '424x240x90', 'Y8'], - ['Infrared2', '424x240x60', 'Y8'], - ['Infrared2', '424x240x30', 'Y8'], - ['Infrared2', '424x240x15', 'Y8'], - ['Infrared2', '424x240x6', 'Y8'], - ['Depth', '640x360x30', 'Z16'], - ['Depth', '480x270x30', 'Z16'], - ['Depth', '424x240x30', 'Z16'], - ] - params = launch_descr_with_parameters[1] - themes = [{'topic':get_node_heirarchy(params)+'/color/image_raw', 'msg_type':msg_Image,'expected_data_chunks':1,'initial_reset':True}] - config = pytest_live_camera_utils.get_profile_config(get_node_heirarchy(params)) - try: - ''' - initialize, run and check the data - ''' - serial_no = None - if 'serial_no' in params: - serial_no = params['serial_no'] - self.init_test("RsTest"+params['camera_name']) - self.spin_for_time(wait_time=1.0) - if cap == None: - debug_print("Device not found? : " + params['device_type']) - return - self.create_param_ifs(get_node_heirarchy(params)) - config["Color"]["default_profile1"],config["Color"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["color_profile"], "Color") - config["Depth"]["default_profile1"],config["Depth"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["depth_profile"], "Depth") - config["Infrared"]["default_profile1"],config["Infrared"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["depth_profile"], "Infrared") - config["Infrared1"]["default_profile1"],config["Infrared1"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["depth_profile"], "Infrared1") - config["Infrared2"]["default_profile1"],config["Infrared2"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["depth_profile"], "Infrared2") - for key in cap["color_profile"]: - profile_type = key[0] - profile = key[1] - format = key[2] - print("Testing " + " ".join(key)) - themes[0]['topic'] = config[profile_type]['topic'] - themes[0]['width'] = int(profile.split('x')[0]) - themes[0]['height'] = int(profile.split('x')[1]) - if themes[0]['width'] == int(config[profile_type]["default_profile2"].split('x')[0]): - self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile1"]) - else: - self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile2"]) - self.set_bool_param(config[profile_type]["param"], True) - self.disable_all_params() - self.spin_for_time(wait_time=wait_time_s) - self.set_string_param(config[profile_type]["profile"], profile) - self.set_string_param(config[profile_type]["format"], format) - self.set_bool_param(config[profile_type]["param"], True) - self.spin_for_time(wait_time=wait_time_s) - try: - ret = self.run_test(themes) - assert ret[0], ret[1] - assert self.process_data(themes), " ".join(key) + " failed" - num_passed += 1 - passed_tests.append(" ".join(key)) - except Exception as e: - exc_type, exc_obj, exc_tb = sys.exc_info() - fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1] - print("Test failed") - print(e) - print(exc_type, fname, exc_tb.tb_lineno) - num_failed += 1 - failed_tests.append(" ".join(key)) - debug_print("Color tests completed") - for key in cap["depth_profile"]: - profile_type = key[0] - profile = key[1] - format = key[2] - print("Testing " + " ".join(key)) - - themes[0]['topic'] = config[profile_type]['topic'] - themes[0]['width'] = int(profile.split('x')[0]) - themes[0]['height'] = int(profile.split('x')[1]) - if themes[0]['width'] == int(config[profile_type]["default_profile2"].split('x')[0]): - self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile1"]) - else: - self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile2"]) - self.set_bool_param(config[profile_type]["param"], True) - - - self.disable_all_params() - self.spin_for_time(wait_time=wait_time_s) - self.set_string_param(config[profile_type]["profile"], profile) - self.set_string_param(config[profile_type]["format"], format) - self.set_bool_param(config[profile_type]["param"], True) - self.spin_for_time(wait_time=wait_time_s) - try: - ret = self.run_test(themes) - assert ret[0], ret[1] - assert self.process_data(themes), " ".join(key) + " failed" - num_passed += 1 - passed_tests.append(" ".join(key)) - except Exception as e: - print("Test failed") - print(e) - num_failed += 1 - failed_tests.append(" ".join(key)) - debug_print("Depth tests completed") - finally: - #this step is important because the test will fail next time - pytest_rs_utils.kill_realsense2_camera_node() - self.shutdown() - print("Tests passed " + str(num_passed)) - if len(passed_tests): - debug_print("\nPassed tests:" + params['device_type']) - debug_print("\n".join(passed_tests)) - print("Tests failed " + str(num_failed)) - if len(failed_tests): - print("\nFailed tests:" + params['device_type']) - print("\n".join(failed_tests)) - - def disable_all_params(self): - self.set_bool_param('enable_color', False) - self.set_bool_param('enable_depth', False) - self.set_bool_param('enable_infra', False) - self.set_bool_param('enable_infra1', False) - self.set_bool_param('enable_infra2', False) \ No newline at end of file From 4412cdc1fc50e6d5a0b6d0cf20cf4852a6aa3c9e Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Mon, 11 Sep 2023 10:59:49 +0000 Subject: [PATCH 30/76] rebase from ros2-development --- README.md | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/README.md b/README.md index 6f8275983d..75822f5048 100644 --- a/README.md +++ b/README.md @@ -354,17 +354,6 @@ User can set the camera name and camera namespace, to distinguish between camera - If set to true, the device will reset prior to usage. - For example: `initial_reset:=true` - **base_frame_id**: defines the frame_id all static transformations refers to. -- **unite_imu_method**: - - D400 cameras have built in IMU components which produce 2 unrelated streams, each with it's own frequency: - - *gyro* - which shows angular velocity - - *accel* which shows linear acceleration. - - By default, 2 corresponding topics are available, each with only the relevant fields of the message sensor_msgs::Imu are filled out. - - Setting *unite_imu_method* creates a new topic, *imu*, that replaces the default *gyro* and *accel* topics. - - The *imu* topic is published at the rate of the gyro. - - All the fields of the Imu message under the *imu* topic are filled out. - - `unite_imu_method` parameter supported values are [0-2] meaning: [0 -> None, 1 -> Copy, 2 -> Linear_ interpolation] when: - - **linear_interpolation**: Every gyro message is attached by the an accel message interpolated to the gyro's timestamp. - - **copy**: Every gyro message is attached by the last accel message. - **clip_distance**: - Remove from the depth image all values above a given value (meters). Disable by giving negative value (default) - For example: `clip_distance:=1.5` From 348dec3bfd2acc195aa188545b4b0eb188a183c7 Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Mon, 11 Sep 2023 17:28:12 +0530 Subject: [PATCH 31/76] All topics may need more time in CI --- realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py b/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py index 22c4e1caee..0e77f552ee 100644 --- a/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py +++ b/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py @@ -52,7 +52,7 @@ 'infra_height': '0', 'enable_infra1':'true', 'enable_infra2':'true', - #'align_depth.enable':'true', + 'align_depth.enable':'true', } ''' To test all topics published @@ -99,7 +99,7 @@ def test_all_topics(self,delayed_launch_descr_with_parameters): initialize, run and check the data ''' self.init_test("RsTest"+params['camera_name']) - ret = self.run_test(themes) + ret = self.run_test(themes, timeout=25.0) assert ret[0], ret[1] time.sleep(0.5) assert self.process_data(themes), "Data check failed, probably the rosbag file changed?" From 0446d7c7c0886d9d3a2905c232e5ceb466dbc19b Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Mon, 11 Sep 2023 17:51:57 +0530 Subject: [PATCH 32/76] All topics testing --- realsense2_camera/test/utils/pytest_rs_utils.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index c07b135065..291ea61b16 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/realsense2_camera/test/utils/pytest_rs_utils.py @@ -822,7 +822,6 @@ def set_integer_param(self, param_name, param_value): return self.send_param(req) def spin_for_data(self,themes, timeout=5.0): - start = time.time() ''' timeout value varies depending upon the system, it needs to be more if the access is over the network @@ -830,6 +829,7 @@ def spin_for_data(self,themes, timeout=5.0): print('Waiting for topic... ' ) flag = False data_not_expected = [i for i in themes if (i["expected_data_chunks"]) == 0] + start = time.time() while (time.time() - start) < timeout: rclpy.spin_once(self.node, timeout_sec=1) debug_print('Spun once... ' ) @@ -847,7 +847,9 @@ def spin_for_data(self,themes, timeout=5.0): if data_not_expected == False: print("Timed out waiting for", timeout, "seconds" ) return False, "run_test timedout" - return flag,"" + if flag: + return flag,"" + return flag,"Unexpected error in spin_for_data" def spin_for_time(self,wait_time): start = time.time() @@ -885,8 +887,8 @@ def run_test(self, themes, initial_wait_time=0.0, timeout=5.0): else: print(e) self.flag =False,e - return self.flag + def get_tfs(self, coupled_frame_ids): return self.node.get_tfs(coupled_frame_ids) From 5f7ec208e111358a70b6703d40b93866ae7f508f Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Mon, 11 Sep 2023 18:20:44 +0530 Subject: [PATCH 33/76] All topics testing1 --- realsense2_camera/test/utils/pytest_rs_utils.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index 291ea61b16..2d12903ed9 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/realsense2_camera/test/utils/pytest_rs_utils.py @@ -830,6 +830,7 @@ def spin_for_data(self,themes, timeout=5.0): flag = False data_not_expected = [i for i in themes if (i["expected_data_chunks"]) == 0] start = time.time() + msg = "Unexpected error in spin_for_data" while (time.time() - start) < timeout: rclpy.spin_once(self.node, timeout_sec=1) debug_print('Spun once... ' ) @@ -847,9 +848,10 @@ def spin_for_data(self,themes, timeout=5.0): if data_not_expected == False: print("Timed out waiting for", timeout, "seconds" ) return False, "run_test timedout" + flag = True if flag: return flag,"" - return flag,"Unexpected error in spin_for_data" + return flag,msg def spin_for_time(self,wait_time): start = time.time() From 426776b8d1525620997e3730fc04e040d5b45ce8 Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Mon, 11 Sep 2023 18:41:32 +0530 Subject: [PATCH 34/76] All topics testing2 --- realsense2_camera/test/utils/pytest_rs_utils.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index 2d12903ed9..8326ce5592 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/realsense2_camera/test/utils/pytest_rs_utils.py @@ -830,7 +830,7 @@ def spin_for_data(self,themes, timeout=5.0): flag = False data_not_expected = [i for i in themes if (i["expected_data_chunks"]) == 0] start = time.time() - msg = "Unexpected error in spin_for_data" + msg = "" while (time.time() - start) < timeout: rclpy.spin_once(self.node, timeout_sec=1) debug_print('Spun once... ' ) @@ -848,9 +848,11 @@ def spin_for_data(self,themes, timeout=5.0): if data_not_expected == False: print("Timed out waiting for", timeout, "seconds" ) return False, "run_test timedout" - flag = True - if flag: - return flag,"" + if flag ==False: + for theme in themes: + if theme['expected_data_chunks'] > int(self.node.get_num_chunks(theme['topic'])): + msg = "Data expected, but not received for: " + theme['topic'] + break return flag,msg def spin_for_time(self,wait_time): From 015bd4a181f05c81c2c32e8ab837c8f6a70e7e3a Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Mon, 11 Sep 2023 20:28:31 +0530 Subject: [PATCH 35/76] All topics testing3 --- .../rosbag/test_rosbag_all_topics_test.py | 8 +------- .../test/utils/pytest_rs_utils.py | 20 ++++++++++--------- 2 files changed, 12 insertions(+), 16 deletions(-) diff --git a/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py b/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py index 0e77f552ee..e462e91b57 100644 --- a/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py +++ b/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py @@ -44,15 +44,9 @@ test_params_all_topics = {"rosbag_filename":get_rosbag_file_path("outdoors_1color.bag"), 'camera_name': 'AllTopics', - 'color_width': '0', - 'color_height': '0', - 'depth_width': '0', - 'depth_height': '0', - 'infra_width': '0', - 'infra_height': '0', 'enable_infra1':'true', 'enable_infra2':'true', - 'align_depth.enable':'true', + #'align_depth.enable':'true', } ''' To test all topics published diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index 8326ce5592..810007f4ba 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/realsense2_camera/test/utils/pytest_rs_utils.py @@ -676,7 +676,7 @@ def image_msg_to_numpy(self, data): def rsCallback(self, topic, msg_type, store_raw_data): debug_print("RSCallback") def _rsCallback(data): - debug_print('Got the callback for ' + topic) + print('Got the callback for ' + topic) #print(data.header) self.flag = True if store_raw_data == True: @@ -828,7 +828,11 @@ def spin_for_data(self,themes, timeout=5.0): ''' print('Waiting for topic... ' ) flag = False - data_not_expected = [i for i in themes if (i["expected_data_chunks"]) == 0] + data_not_expected1 = [i for i in themes if (i["expected_data_chunks"]) == 0] + if data_not_expected1 == []: + data_not_expected = False + else: + data_not_expected = True start = time.time() msg = "" while (time.time() - start) < timeout: @@ -846,13 +850,11 @@ def spin_for_data(self,themes, timeout=5.0): break else: if data_not_expected == False: - print("Timed out waiting for", timeout, "seconds" ) - return False, "run_test timedout" - if flag ==False: - for theme in themes: - if theme['expected_data_chunks'] > int(self.node.get_num_chunks(theme['topic'])): - msg = "Data expected, but not received for: " + theme['topic'] - break + msg = "Timedout: Data expected, but not received for: " + for theme in themes: + if theme['expected_data_chunks'] > int(self.node.get_num_chunks(theme['topic'])): + msg += " " + theme['topic'] + return False, msg return flag,msg def spin_for_time(self,wait_time): From 7235d999b999c2844f01c88b696ed0a802dae3b1 Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Mon, 11 Sep 2023 20:48:13 +0530 Subject: [PATCH 36/76] All topics testing4 --- realsense2_camera/test/utils/pytest_rs_utils.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index 810007f4ba..578d27b469 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/realsense2_camera/test/utils/pytest_rs_utils.py @@ -515,7 +515,7 @@ def get_rs_node_description(params): executable='realsense2_camera_node', parameters=[tmp_yaml.name], output='screen', - arguments=['--ros-args', '--log-level', "info"], + arguments=['--ros-args', '--log-level', "debug"], emulate_tty=True, ) From e2cd2410d5cf4538e23eb3f5c9045d74766b9abe Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Mon, 11 Sep 2023 22:28:30 +0530 Subject: [PATCH 37/76] All topics added to regression only, excluded from CI --- .../test/rosbag/test_rosbag_all_topics_test.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py b/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py index e462e91b57..3c5bdb83ac 100644 --- a/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py +++ b/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py @@ -51,7 +51,15 @@ ''' To test all topics published ''' +''' +To test all topics published + +The test doesn't work in CI due to sync. The unlike the live camera, rosbag node publishes the extrinsics only once, +not every time the subscription is created. The CI due to limited resource can start the ros node much earlier than +the testcase, hence missing the published data by the test +''' @pytest.mark.rosbag +@pytest.mark.skipif (os.getenv('RS_ROS_REGRESSION', "not found") == "not found",reason="The test doesn't work in CI") @pytest.mark.parametrize("delayed_launch_descr_with_parameters", [test_params_all_topics],indirect=True) @pytest.mark.launch(fixture=delayed_launch_descr_with_parameters) class TestAllTopics(pytest_rs_utils.RsTestBaseClass): From b958ca1c225378119b3efca72182f658fd1642ab Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Tue, 12 Sep 2023 03:01:28 +0530 Subject: [PATCH 38/76] getting camera_name dynamically in ROS2 wrapper --- realsense2_camera/src/base_realsense_node.cpp | 3 +++ realsense2_camera/src/parameters.cpp | 3 --- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index cae3f6c43f..d9cfb06529 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -125,6 +125,9 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node, ROS_INFO("Intra-Process communication enabled"); } + // Get node name + _camera_name = _node.get_name(); + initializeFormatsMaps(); _monitor_options = {RS2_OPTION_ASIC_TEMPERATURE, RS2_OPTION_PROJECTOR_TEMPERATURE}; } diff --git a/realsense2_camera/src/parameters.cpp b/realsense2_camera/src/parameters.cpp index c5d1abef3a..e007f60515 100644 --- a/realsense2_camera/src/parameters.cpp +++ b/realsense2_camera/src/parameters.cpp @@ -23,9 +23,6 @@ void BaseRealSenseNode::getParameters() ROS_INFO("getParameters..."); std::string param_name; - param_name = std::string("camera_name"); - _camera_name = _parameters->setParam(param_name, "camera"); - _parameters_names.push_back(param_name); param_name = std::string("publish_tf"); _publish_tf = _parameters->setParam(param_name, PUBLISH_TF); From aef295123f95737564da626c4e0b4e9bdb5f441e Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Thu, 14 Sep 2023 17:17:03 +0530 Subject: [PATCH 39/76] updated tests for the failures in CI --- .../live_camera/test_camera_aligned_tests.py | 13 +++-- .../test/live_camera/test_camera_fps_tests.py | 30 ++++++++++-- .../test/live_camera/test_camera_imu_tests.py | 1 + .../rosbag/test_rosbag_all_topics_test.py | 4 +- .../test/rosbag/test_rosbag_depth_tests.py | 19 +++++--- .../test/utils/pytest_rs_utils.py | 48 ++++++++++++------- 6 files changed, 80 insertions(+), 35 deletions(-) diff --git a/realsense2_camera/test/live_camera/test_camera_aligned_tests.py b/realsense2_camera/test/live_camera/test_camera_aligned_tests.py index d489b5d546..0c17e76e47 100644 --- a/realsense2_camera/test/live_camera/test_camera_aligned_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_aligned_tests.py @@ -66,6 +66,7 @@ 1. Only a subset of parameter types are implemented in py_rs_utils, it has to be extended for others 2. After setting the param, rclpy.spin_once may be needed.Test passes even without this though. ''' + @pytest.mark.parametrize("launch_descr_with_parameters",[ pytest.param(test_params_align_depth_color_d455, marks=pytest.mark.d455), pytest.param(test_params_align_depth_color_d415, marks=pytest.mark.d415), @@ -76,6 +77,9 @@ class TestCamera_AlignDepthColor(pytest_rs_utils.RsTestBaseClass): def test_camera_align_depth_color(self,launch_descr_with_parameters): params = launch_descr_with_parameters[1] + if pytest_live_camera_utils.check_if_camera_connected(params['device_type']) == False: + print("Device not found? : " + params['device_type']) + return themes = [ {'topic':get_node_heirarchy(params)+'/color/image_raw', 'msg_type':msg_Image, @@ -122,8 +126,6 @@ def test_camera_align_depth_color(self,launch_descr_with_parameters): pytest_rs_utils.kill_realsense2_camera_node() self.shutdown() - - test_params_all_profiles_d455 = { 'camera_name': 'D455', 'device_type': 'D455', @@ -167,13 +169,16 @@ def test_camera_align_depth_color(self,launch_descr_with_parameters): pytest.param(test_params_all_profiles_d435, marks=pytest.mark.d435),] ,indirect=True) @pytest.mark.launch(fixture=launch_descr_with_parameters) -class TestCamera_AlignDepthColor(pytest_rs_utils.RsTestBaseClass): - def test_camera_align_depth_color_all(self,launch_descr_with_parameters): +class TestCamera_AllAlignDepthColor(pytest_rs_utils.RsTestBaseClass): + def test_camera_all_align_depth_color(self,launch_descr_with_parameters): skipped_tests = [] failed_tests = [] num_passed = 0 num_failed = 0 params = launch_descr_with_parameters[1] + if pytest_live_camera_utils.check_if_camera_connected(params['device_type']) == False: + print("Device not found? : " + params['device_type']) + return themes = [ {'topic':get_node_heirarchy(params)+'/color/image_raw', 'msg_type':msg_Image, diff --git a/realsense2_camera/test/live_camera/test_camera_fps_tests.py b/realsense2_camera/test/live_camera/test_camera_fps_tests.py index 291e4eaf8c..31b2d31187 100644 --- a/realsense2_camera/test/live_camera/test_camera_fps_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_fps_tests.py @@ -38,16 +38,35 @@ from rcl_interfaces.msg import ParameterValue from rcl_interfaces.srv import SetParameters, GetParameters, ListParameters -test_params_test_fps = { +test_params_test_fps_d455 = { 'camera_name': 'D455', 'device_type': 'D455', + + } +test_params_test_fps_d415 = { + 'camera_name': 'D415', + 'device_type': 'D415', + } +test_profiles ={ + 'D455':{ + 'depth_test_profiles':['640x480x5','640x480x15', '640x480x30', '640x480x90'], + 'color_test_profiles':['640x480x5','640x480x15', '640x480x30', '1280x720x30'] + }, + 'D415':{ + 'depth_test_profiles':['640x480x6','640x480x15', '640x480x30', '640x480x90'], + 'color_test_profiles':['640x480x6','640x480x15', '640x480x30', '1280x720x30'] } +} ''' The test was implemented to check the fps of Depth and Color frames. The RosTopicHz had to be modified to make it work, see py_rs_utils for more details. To check the fps, a value 'expected_fps_in_hz' has to be added to the corresponding theme ''' -@pytest.mark.parametrize("launch_descr_with_parameters", [test_params_test_fps],indirect=True) +@pytest.mark.parametrize("launch_descr_with_parameters", [ + pytest.param(test_params_test_fps_d455, marks=pytest.mark.d455), + pytest.param(test_params_test_fps_d415, marks=pytest.mark.d415), + #pytest.param(test_params_test_fps_d435, marks=pytest.mark.d435), + ],indirect=True) @pytest.mark.launch(fixture=launch_descr_with_parameters) class TestCamera_TestFPS(pytest_rs_utils.RsTestBaseClass): def test_camera_test_fps(self,launch_descr_with_parameters): @@ -68,12 +87,14 @@ def test_camera_test_fps(self,launch_descr_with_parameters): 'expected_data_chunks':100, } ] - profiles = ['640x480x5','640x480x15', '640x480x30', '640x480x90'] + profiles = test_profiles[params['camera_name']]['depth_test_profiles'] + assert self.set_bool_param('enable_color', False) for profile in profiles: print("Testing profile: ", profile) themes[0]['expected_fps_in_hz'] = float(profile.split('x')[2]) assert self.set_string_param('depth_module.profile', profile) assert self.set_bool_param('enable_depth', True) + self.spin_for_time(0.5) ret = self.run_test(themes, timeout=25.0) assert ret[0], ret[1] assert self.process_data(themes) @@ -84,7 +105,8 @@ def test_camera_test_fps(self,launch_descr_with_parameters): 'expected_data_chunks':100, } ] - profiles = ['640x480x5','640x480x15', '640x480x30', '1280x720x30'] + assert self.set_bool_param('enable_depth', False) + profiles = test_profiles[params['camera_name']]['color_test_profiles'] for profile in profiles: print("Testing profile: ", profile) themes[0]['expected_fps_in_hz'] = float(profile.split('x')[2]) diff --git a/realsense2_camera/test/live_camera/test_camera_imu_tests.py b/realsense2_camera/test/live_camera/test_camera_imu_tests.py index e2defb6b52..317833e331 100644 --- a/realsense2_camera/test/live_camera/test_camera_imu_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_imu_tests.py @@ -77,6 +77,7 @@ def test_LiveCamera_check_motion_sensor(self,launch_descr_with_parameters): assert self.process_data(themes), msg + " failed" msg = "Test with the accel false " + self.set_integer_param('unite_imu_method', 0) self.set_bool_param('enable_accel', False) self.set_bool_param('enable_gyro', True) themes[ACCEL_TOPIC]['expected_data_chunks'] = 0 diff --git a/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py b/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py index 3c5bdb83ac..9cda836f3a 100644 --- a/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py +++ b/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py @@ -46,7 +46,7 @@ 'camera_name': 'AllTopics', 'enable_infra1':'true', 'enable_infra2':'true', - #'align_depth.enable':'true', + 'align_depth.enable':'true', } ''' To test all topics published @@ -101,7 +101,7 @@ def test_all_topics(self,delayed_launch_descr_with_parameters): initialize, run and check the data ''' self.init_test("RsTest"+params['camera_name']) - ret = self.run_test(themes, timeout=25.0) + ret = self.run_test(themes) assert ret[0], ret[1] time.sleep(0.5) assert self.process_data(themes), "Data check failed, probably the rosbag file changed?" diff --git a/realsense2_camera/test/rosbag/test_rosbag_depth_tests.py b/realsense2_camera/test/rosbag/test_rosbag_depth_tests.py index 1b628e07f6..51d574da85 100644 --- a/realsense2_camera/test/rosbag/test_rosbag_depth_tests.py +++ b/realsense2_camera/test/rosbag/test_rosbag_depth_tests.py @@ -20,10 +20,14 @@ import pytest import rclpy +from rclpy.qos import DurabilityPolicy +from rclpy.qos import HistoryPolicy +from rclpy.qos import QoSProfile from sensor_msgs.msg import Image as msg_Image from sensor_msgs.msg import Imu as msg_Imu from sensor_msgs.msg import PointCloud2 as msg_PointCloud2 +from tf2_msgs.msg import TFMessage as msg_TFMessage import numpy as np @@ -78,12 +82,6 @@ def test_depth_points_cloud_1(self,delayed_launch_descr_with_parameters): 'expected_data_chunks':1, 'data':data1 }, - {'topic':get_node_heirarchy(params)+'/depth/image_rect_raw', - 'msg_type':msg_Image, - 'expected_data_chunks':1, - 'data':data2 - } - ] try: ''' @@ -124,6 +122,11 @@ def test_static_tf_1(self,delayed_launch_descr_with_parameters): 'msg_type':msg_Image, 'expected_data_chunks':1, 'static_tf':True, + }, + {'topic':'/tf_static', + 'msg_type':msg_TFMessage, + 'expected_data_chunks':1, + 'qos' : QoSProfile(depth=100,durability=DurabilityPolicy.TRANSIENT_LOCAL,history=HistoryPolicy.KEEP_LAST,)#static } ] try: @@ -145,7 +148,9 @@ def process_data(self, themes): (self.params['camera_name']+'_infra1_frame', self.params['camera_name']+'_color_frame'): ([-0.00010158783697988838, 0.014841210097074509, -0.00022671300393994898], [-0.0008337442995980382, 0.0010442184284329414, -0.0009920650627464056, 0.9999986290931702])} frame_ids = [self.params['camera_name']+'_link', self.params['camera_name']+'_depth_frame', self.params['camera_name']+'_infra1_frame', self.params['camera_name']+'_infra2_frame', self.params['camera_name']+'_color_frame', self.params['camera_name']+'_fisheye_frame', self.params['camera_name']+'_pose'] coupled_frame_ids = [xx for xx in itertools.combinations(frame_ids, 2)] - tfs_data = self.get_tfs(coupled_frame_ids) + data = self.node.pop_first_chunk('/tf_static') + coupled_frame_ids = [xx for xx in itertools.combinations(frame_ids, 2)] + tfs_data = self.get_transform_data(data, coupled_frame_ids, is_static=True) ret = pytest_rs_utils.staticTFTest(tfs_data, expected_data) assert ret[0], ret[1] return ret[0] diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index 578d27b469..db9c09c3c0 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/realsense2_camera/test/utils/pytest_rs_utils.py @@ -17,6 +17,7 @@ from collections import deque import functools import itertools +import subprocess import pytest @@ -443,6 +444,17 @@ def kill_realsense2_camera_node(): os.system(cmd) pass +''' +function gets all the topics for a camera node +''' + +def get_all_topics(camera_name=None): + cmd = 'ros2 topic list' + if camera_name!=None: + cmd += '| grep ' + camera_name + direct_output = os.popen(cmd).read() + return direct_output + ''' get the default parameters from the launch script so that the test doesn't have to get updated for each change to the parameter or default values @@ -511,11 +523,11 @@ def get_rs_node_description(params): #name=LaunchConfiguration("camera_name"), namespace=params["camera_namespace"], name=params["camera_name"], - #prefix=['xterm -e gdb --args'], + #prefix=['xterm -e gdb -ex=run --args'], executable='realsense2_camera_node', parameters=[tmp_yaml.name], output='screen', - arguments=['--ros-args', '--log-level', "debug"], + arguments=['--ros-args', '--log-level', "info"], emulate_tty=True, ) @@ -635,16 +647,6 @@ def create_subscription(self, msg_type, topic , data_type, store_raw_data, measu if self.tfBuffer == None: self.tfBuffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tfBuffer, super()) - def get_tfs(self, coupled_frame_ids): - res = dict() - for couple in coupled_frame_ids: - from_id, to_id = couple - if (self.tfBuffer.can_transform(from_id, to_id, rclpy.time.Time(), rclpy.time.Duration(nanoseconds=3e6))): - res[couple] = self.tfBuffer.lookup_transform(from_id, to_id, rclpy.time.Time(), rclpy.time.Duration(nanoseconds=1e6)).transform - else: - res[couple] = None - return res - def get_num_chunks(self,topic): return len(self.data[topic]) @@ -676,7 +678,10 @@ def image_msg_to_numpy(self, data): def rsCallback(self, topic, msg_type, store_raw_data): debug_print("RSCallback") def _rsCallback(data): - print('Got the callback for ' + topic) + ''' + enabling prints in callback reduces the fps in some cases + ''' + debug_print('Got the callback for ' + topic) #print(data.header) self.flag = True if store_raw_data == True: @@ -854,7 +859,9 @@ def spin_for_data(self,themes, timeout=5.0): for theme in themes: if theme['expected_data_chunks'] > int(self.node.get_num_chunks(theme['topic'])): msg += " " + theme['topic'] + msg += " Nodes available: " + str(self.node.get_node_names()) return False, msg + flag = True return flag,msg def spin_for_time(self,wait_time): @@ -865,7 +872,7 @@ def spin_for_time(self,wait_time): print('Spun for time once... ' ) rclpy.spin_once(self.node, timeout_sec=wait_time) - def run_test(self, themes, initial_wait_time=0.0, timeout=5.0): + def run_test(self, themes, initial_wait_time=0.0, timeout=0): try: for theme in themes: store_raw_data = False @@ -880,6 +887,15 @@ def run_test(self, themes, initial_wait_time=0.0, timeout=5.0): qos_type = theme['qos'] self.create_subscription(theme['msg_type'], theme['topic'] , qos_type,store_raw_data, measure_hz) print('subscription created for ' + theme['topic']) + ''' + change the default based on whether data is expected or not + ''' + if timeout == 0: + timeout = 5.0 + data_not_expected1 = [i for i in themes if (i["expected_data_chunks"]) == 0] + if data_not_expected1 == []: + timeout = 50.0 #high value due to resource constraints in CI + if initial_wait_time != 0.0: self.spin_for_time(initial_wait_time) self.flag = self.spin_for_data(themes, timeout) @@ -895,10 +911,6 @@ def run_test(self, themes, initial_wait_time=0.0, timeout=5.0): self.flag =False,e return self.flag - def get_tfs(self, coupled_frame_ids): - return self.node.get_tfs(coupled_frame_ids) - - def get_transform_data(self, data, coupled_frame_ids, is_static=False): tfBuffer = tf2_ros.Buffer() for transform in data.transforms: From e861b350930f313eaae657ef73d1711921a82738 Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Thu, 14 Sep 2023 17:40:12 +0530 Subject: [PATCH 40/76] correct the d435 marker --- realsense2_camera/test/live_camera/test_camera_tf_tests.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/realsense2_camera/test/live_camera/test_camera_tf_tests.py b/realsense2_camera/test/live_camera/test_camera_tf_tests.py index 4e8250f063..a31f74e77c 100644 --- a/realsense2_camera/test/live_camera/test_camera_tf_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_tf_tests.py @@ -160,7 +160,7 @@ def process_data(self, themes, enable_infra1): } @pytest.mark.parametrize("launch_descr_with_parameters", [ pytest.param(test_params_tf_d455, marks=pytest.mark.d455), - pytest.param(test_params_tf_d435, marks=pytest.mark.d415), + pytest.param(test_params_tf_d435, marks=pytest.mark.d435), pytest.param(test_params_tf_d415, marks=pytest.mark.d415), ],indirect=True) @pytest.mark.launch(fixture=launch_descr_with_parameters) From de0100539e7e8cf5fc9bf4899e7fdfb47c002dc9 Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Fri, 15 Sep 2023 16:23:33 +0530 Subject: [PATCH 41/76] added markers for d415 d455 specific tests --- realsense2_camera/test/README.md | 2 +- realsense2_camera/test/live_camera/test_d415_basic_tests.py | 1 + realsense2_camera/test/live_camera/test_d455_basic_tests.py | 1 + 3 files changed, 3 insertions(+), 1 deletion(-) diff --git a/realsense2_camera/test/README.md b/realsense2_camera/test/README.md index 9624848955..ea3a043a92 100644 --- a/realsense2_camera/test/README.md +++ b/realsense2_camera/test/README.md @@ -158,7 +158,7 @@ If a user wants to add a test to this conditional skip, user can add by adding a RS_ROS_REGRESSION=1 PYTHONPATH=$PYTHONPATH:$PWD/realsense2_camera/test/utils:$PWD/realsense2_camera//launch:$PWD/realsense2_camera//scripts pytest-3 -s realsense2_camera/test/live_camera/test_camera_aligned_tests.py -k test_camera_align_depth_color_all -m d415 ## Points to be noted while writing pytests -The tests that are in one file are normally run in parallel. So if there are multiple tests in one file, the system capacity can influence the test execution. It's recomended to have 3-4 tests in file, more than that can affect the test results due to delays. +The tests that are in one file are normally run in parallel, there could also be changes in the pytest plugin. So if there are multiple tests in one file, the system capacity can influence the test execution. It's recomended to have 3-4 tests in file, more than that can affect the test results due to delays. ### Passing/changing parameters The parameters passed while creating the node can be initialized individually for each test, please see the test_parameterized_template example for reference. The default values are taken from rs_launch.py and the passed parameters are used for overriding the default values. The parameters that can be dynamically modified can be changed using the param interface provided. However, the function create_param_ifs has to be called to create this interface. Please see the test_d455_basic_tests.py for reference. There are specific functions to change the string, integer and bool parameters, the utils can be extended if any more types are needed. ### Difference in setting the bool parameters diff --git a/realsense2_camera/test/live_camera/test_d415_basic_tests.py b/realsense2_camera/test/live_camera/test_d415_basic_tests.py index 8b88a30e67..d93bbe5b57 100644 --- a/realsense2_camera/test/live_camera/test_d415_basic_tests.py +++ b/realsense2_camera/test/live_camera/test_d415_basic_tests.py @@ -49,6 +49,7 @@ 1. Only a subset of parameter types are implemented in py_rs_utils, it has to be extended for others 2. After setting the param, rclpy.spin_once may be needed.Test passes even without this though. ''' +@pytest.mark.d415 @pytest.mark.parametrize("launch_descr_with_parameters", [test_params_depth_avg_1],indirect=True) @pytest.mark.launch(fixture=launch_descr_with_parameters) class TestD415_Change_Resolution(pytest_rs_utils.RsTestBaseClass): diff --git a/realsense2_camera/test/live_camera/test_d455_basic_tests.py b/realsense2_camera/test/live_camera/test_d455_basic_tests.py index fcf4561714..0d2163a3e3 100644 --- a/realsense2_camera/test/live_camera/test_d455_basic_tests.py +++ b/realsense2_camera/test/live_camera/test_d455_basic_tests.py @@ -49,6 +49,7 @@ 1. Only a subset of parameter types are implemented in py_rs_utils, it has to be extended for others 2. After setting the param, rclpy.spin_once may be needed.Test passes even without this though. ''' +@pytest.mark.d455 @pytest.mark.parametrize("launch_descr_with_parameters", [test_params_depth_avg_1],indirect=True) @pytest.mark.launch(fixture=launch_descr_with_parameters) class TestD455_Change_Resolution(pytest_rs_utils.RsTestBaseClass): From 241961e4e3523db9893241a89df200753e914ed1 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Tue, 19 Sep 2023 05:00:57 +0530 Subject: [PATCH 42/76] updated examples and readme --- .../examples/align_depth/README.md | 12 ++ .../examples/dual_camera/README.md | 123 ++++++++++++++++++ .../dual_camera/rs_dual_camera_launch.py | 6 +- .../{ => rviz}/dual_camera_pointcloud.rviz | 4 +- .../examples/launch_from_rosbag/README.md | 17 +++ .../rs_launch_from_rosbag.py | 2 +- .../launch_params_from_file/README.md | 33 +++++ .../{ => config}/config.yaml | 3 +- .../rs_launch_get_params_from_yaml.py | 2 +- .../examples/pointcloud/README.md | 17 +++ .../pointcloud/rs_d455_pointcloud_launch.py | 2 +- .../pointcloud/rs_pointcloud_launch.py | 2 +- .../pointcloud/{ => rviz}/pointcloud.rviz | 86 ++++++------ .../{ => rviz}/urdf_pointcloud.rviz | 16 +-- realsense2_camera/launch/default.rviz | 86 ++++++++---- 15 files changed, 327 insertions(+), 84 deletions(-) create mode 100644 realsense2_camera/examples/align_depth/README.md create mode 100644 realsense2_camera/examples/dual_camera/README.md rename realsense2_camera/examples/dual_camera/{ => rviz}/dual_camera_pointcloud.rviz (98%) create mode 100644 realsense2_camera/examples/launch_from_rosbag/README.md create mode 100644 realsense2_camera/examples/launch_params_from_file/README.md rename realsense2_camera/examples/launch_params_from_file/{ => config}/config.yaml (74%) create mode 100644 realsense2_camera/examples/pointcloud/README.md rename realsense2_camera/examples/pointcloud/{ => rviz}/pointcloud.rviz (69%) rename realsense2_camera/examples/pointcloud/{ => rviz}/urdf_pointcloud.rviz (92%) diff --git a/realsense2_camera/examples/align_depth/README.md b/realsense2_camera/examples/align_depth/README.md new file mode 100644 index 0000000000..ee869ac70c --- /dev/null +++ b/realsense2_camera/examples/align_depth/README.md @@ -0,0 +1,12 @@ +# Align Depth to Color +This example shows how to start the camera node and align depth stream to color stream. +``` +ros2 launch realsense2_camera rs_align_depth_launch.py +``` + +The aligned image will be published to the topic "/aligned_depth_to_color/image_raw" + +Also, align depth to color can enabled by following cmd: +``` +ros2 launch realsense2_camera rs_launch.py align_depth.enable:=true" +``` \ No newline at end of file diff --git a/realsense2_camera/examples/dual_camera/README.md b/realsense2_camera/examples/dual_camera/README.md new file mode 100644 index 0000000000..03a7cfa08f --- /dev/null +++ b/realsense2_camera/examples/dual_camera/README.md @@ -0,0 +1,123 @@ +# Launching Dual RS ROS2 nodes +The following example lanches two RS ROS2 nodes. +``` +ros2 launch realsense2_camera rs_dual_camera_launch.py serial_no1:= serial_no2:= +``` + +## Example: +Let's say the serial numbers of two RS cameras are 207322251310 and 234422060144. +``` +ros2 launch realsense2_camera rs_dual_camera_launch.py serial_no1:="'207322251310'" serial_no2:="'234422060144'" +``` +or +``` +ros2 launch realsense2_camera rs_dual_camera_launch.py serial_no1:=_207322251310 serial_no2:=_234422060144 +``` + +## How to know the serial number? +Method 1: Using the rs-enumerate-devices tool +``` +rs-enumerate-devices | grep "Serial Number" +``` + +Method 2: Connect single camera and run +``` +ros2 launch realsense2_camera rs_launch.py +``` +and look for the serial number in the log printed to screen under "[INFO][...] Device Serial No:". + +# Using Multiple RS camera by launching each in differnet terminals +Make sure you set a different name and namespace for each camera. + +Terminal 1: +``` +ros2 launch realsense2_camera rs_launch.py serial_no:="'207322251310'" camera_name:='camera1' camera_namespace:='camera1' +``` +Terminal 2: +``` +ros2 launch realsense2_camera rs_launch.py serial_no:="'234422060144'" camera_name:='camera2' camera_namespace:='camera2' +``` + +# Multiple cameras showing a semi-unified pointcloud +The D430 series of RealSense cameras use stereo based algorithm to calculate depth. This mean, a couple of cameras can operate on the same scene. For the purpose of this demonstration, let's say 2 cameras can be coupled to look at the same scene from 2 different points of view. See image: + +![multi_cameras](https://user-images.githubusercontent.com/127019120/268692789-1b3d5d8b-a41f-4a97-995d-81d44b4bcacb.jpg) + +The schematic settings could be described as: +X--------------------------------->cam_2 +|    (70 cm) +| +| +| (60 cm) +| +| +/ +cam_1 + +The cameras have no data regarding their relative position. ThatÂ’s up to a third party program to set. To simplify things, the coordinate system of cam_1 can be considered as the refernce coordinate system for the whole scene. + +The estimated translation of cam_2 from cam_1 is 70(cm) on X-axis and 60(cm) on Y-axis. Also, the estimated yaw angle of cam_2 relative to cam_1 as 90(degrees) clockwise. These are the initial parameters to be set for setting the transformation between the 2 cameras as follows: + +``` +ros2 launch realsense2_camera rs_dual_camera_launch.py serial_no1:=_207322251310 serial_no2:=_234422060144 tf.translation.x:=0.7 tf.translation.y:=0.6 tf.translation.z:=0.0 tf.rotation.yaw:=-90.0 tf.rotation.pitch:=0.0 tf.rotation.roll:=0.0 +``` + +If the unified pointcloud result is not good, follow the below steps to fine-tune the calibaration. + +## Visualizing the pointclouds and fine-tune the camera calibration +Launch 2 cameras in separate terminals: + +**Terminal 1:** +``` +ros2 launch realsense2_camera rs_launch.py serial_no:="'207322251310'" camera_name:='camera1' camera_namespace:='camera1' +``` +**Terminal 2:** +``` +ros2 launch realsense2_camera rs_launch.py serial_no:="'234422060144'" camera_name:='camera2' camera_namespace:='camera2' +``` +**Terminal 3:** +``` +rviz2 +``` +Open rviz and set '“Fixed Frame'” to “camera1_link” +Add Pointcloud2-> By topic -> /camera1/camera1/depth/color/points +Add Pointcloud2 -> By topic -> /camera2/camera2/depth/color/points + +**Terminal 4:** +Run the 'set_cams_transforms.py' tool. It can be used to fine-tune the calibaration. +``` +python src/realsense-ros/realsense2_camera/scripts/set_cams_transforms.py camera1_link camera2_link 0.7 0.6 0 -90 0 0 +``` + +**Instructions printed by the tool:** +``` +Using default file /home/user_name/ros2_ws/src/realsense-ros/realsense2_camera/scripts/_set_cams_info_file.txt + +Use given initial values. + +Press the following keys to change mode: x, y, z, (a)zimuth, (p)itch, (r)oll + +For each mode, press 6 to increase by step and 4 to decrease + +Press + to multiply step by 2 or - to divide + +Press Q to quit +``` + +Note that the tool prints the path of the current configuration file. It saves its last configuration automatically, all the time, to be used on the next run. + +After a lot of fiddling around, unified pointcloud looked better with the following calibaration: +``` +x = 0.75 +y = 0.575 +z = 0 +azimuth = -91.25 +pitch = 0.75 +roll = 0 +``` + +Now, use the above results in the launch file: +``` +ros2 launch realsense2_camera rs_dual_camera_launch.py serial_no1:=_207322251310 serial_no2:=_234422060144 tf.translation.x:=0.75 tf.translation.y:=0.575 tf.translation.z:=0.0 tf.rotation.yaw:=-91.25 tf.rotation.pitch:=0.75 tf.rotation.roll:=0.0 +``` + diff --git a/realsense2_camera/examples/dual_camera/rs_dual_camera_launch.py b/realsense2_camera/examples/dual_camera/rs_dual_camera_launch.py index 4a4db4a275..81643d2c7e 100644 --- a/realsense2_camera/examples/dual_camera/rs_dual_camera_launch.py +++ b/realsense2_camera/examples/dual_camera/rs_dual_camera_launch.py @@ -19,7 +19,7 @@ # 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.. +# ros2 launch realsense2_camera rs_dual_camera_launch.py serial_no1:= serial_no2:= """Launch realsense2_camera node.""" import copy @@ -68,7 +68,7 @@ def duplicate_params(general_params, posix): return local_params def launch_static_transform_publisher_node(context : LaunchContext): - # dummy static transformation from camera1 to camera2 + # Static transformation from camera1 to camera2 node = launch_ros.actions.Node( name = "my_static_transform_publisher", package = "tf2_ros", @@ -104,6 +104,6 @@ def generate_launch_description(): namespace='', executable='rviz2', name='rviz2', - arguments=['-d', [ThisLaunchFileDir(), '/dual_camera_pointcloud.rviz']] + arguments=['-d', [ThisLaunchFileDir(), '/rviz/dual_camera_pointcloud.rviz']] ) ]) diff --git a/realsense2_camera/examples/dual_camera/dual_camera_pointcloud.rviz b/realsense2_camera/examples/dual_camera/rviz/dual_camera_pointcloud.rviz similarity index 98% rename from realsense2_camera/examples/dual_camera/dual_camera_pointcloud.rviz rename to realsense2_camera/examples/dual_camera/rviz/dual_camera_pointcloud.rviz index 3469eda9d8..1a4c0caa11 100644 --- a/realsense2_camera/examples/dual_camera/dual_camera_pointcloud.rviz +++ b/realsense2_camera/examples/dual_camera/rviz/dual_camera_pointcloud.rviz @@ -74,7 +74,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /camera1/depth/color/points + Value: /camera1/camera1/depth/color/points Use Fixed Frame: true Use rainbow: true Value: true @@ -108,7 +108,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /camera2/depth/color/points + Value: /camera2/camera2/depth/color/points Use Fixed Frame: true Use rainbow: true Value: true diff --git a/realsense2_camera/examples/launch_from_rosbag/README.md b/realsense2_camera/examples/launch_from_rosbag/README.md new file mode 100644 index 0000000000..5bfbb017f5 --- /dev/null +++ b/realsense2_camera/examples/launch_from_rosbag/README.md @@ -0,0 +1,17 @@ +# Launching RS ROS2 node from rosbag File +The following example allows streaming a rosbag file, saved by Intel RealSense Viewer, instead of streaming live with a camera. It can be used for testing and repetition of the same sequence. +``` +ros2 launch realsense2_camera rs_launch_from_rosbag.py +``` +By default, the 'rs_launch_from_rosbag.py' launch file uses the "/rosbag/D435i_Depth_and_IMU_Stands_still.bag" rosbag file. + +User can also provide a different rosbag file through cmd line as follows: +``` +ros2 launch realsense2_camera rs_launch_from_rosbag.py rosbag_filename:="/full/path/to/rosbag/file" +``` +or +``` +ros2 launch realsense2_camera rs_launch.py rosbag_filename:="/full/path/to/rosbag/file" +``` + +Check-out [sample-recordings](https://github.com/IntelRealSense/librealsense/blob/master/doc/sample-data.md) for a few recorded samples. \ No newline at end of file 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 index ad9b76667b..a0d9620cad 100644 --- a/realsense2_camera/examples/launch_from_rosbag/rs_launch_from_rosbag.py +++ b/realsense2_camera/examples/launch_from_rosbag/rs_launch_from_rosbag.py @@ -37,7 +37,7 @@ {'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'}, + {'name': 'rosbag_filename', 'default': [ThisLaunchFileDir(), "/rosbag/D435i_Depth_and_IMU_Stands_still.bag"], 'description': 'A realsense bagfile to run from as a device'}, ] def set_configurable_parameters(local_params): diff --git a/realsense2_camera/examples/launch_params_from_file/README.md b/realsense2_camera/examples/launch_params_from_file/README.md new file mode 100644 index 0000000000..ce9980aa5b --- /dev/null +++ b/realsense2_camera/examples/launch_params_from_file/README.md @@ -0,0 +1,33 @@ +# Get RS ROS2 node params from YAML file +The following example gets the RS ROS2 node params from YAML file. +``` +ros2 launch realsense2_camera rs_launch_get_params_from_yaml.py +``` + +By default, 'rs_launch_get_params_from_yaml.py' launch file uses the "/config/config.yaml" YAML file. + +User can provide a different YAML file through cmd line as follows: +``` +ros2 launch realsense2_camera rs_launch_get_params_from_yaml.py config_file:="/full/path/to/config/file" +``` +or +``` +ros2 launch realsense2_camera rs_launch.py config_file:="/full/path/to/config/file" +``` + +## Syntax for defining params in YAML file +``` +param1: value +param2: value +``` + +Example: +``` +enable_color: true +rgb_camera.profile: 1280x720x15 +enable_depth: true +align_depth.enable: true +enable_sync: true +publish_tf: true +tf_publish_rate: 1.0 +``` \ No newline at end of file diff --git a/realsense2_camera/examples/launch_params_from_file/config.yaml b/realsense2_camera/examples/launch_params_from_file/config/config.yaml similarity index 74% rename from realsense2_camera/examples/launch_params_from_file/config.yaml rename to realsense2_camera/examples/launch_params_from_file/config/config.yaml index 8b8bcc1709..d15a19a0fe 100644 --- a/realsense2_camera/examples/launch_params_from_file/config.yaml +++ b/realsense2_camera/examples/launch_params_from_file/config/config.yaml @@ -3,4 +3,5 @@ rgb_camera.profile: 1280x720x15 enable_depth: true align_depth.enable: true enable_sync: true - +publish_tf: true +tf_publish_rate: 1.0 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 index a6b5582039..c0ebd7bfbd 100644 --- 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 @@ -34,7 +34,7 @@ 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'}, + {'name': 'config_file', 'default': [ThisLaunchFileDir(), "/config/config.yaml"], 'description': 'yaml config file'}, ] def set_configurable_parameters(local_params): diff --git a/realsense2_camera/examples/pointcloud/README.md b/realsense2_camera/examples/pointcloud/README.md new file mode 100644 index 0000000000..1285e0a843 --- /dev/null +++ b/realsense2_camera/examples/pointcloud/README.md @@ -0,0 +1,17 @@ +# PointCloud Visualization +The following example starts the camera and simultaneously opens RViz GUI to visualize the published pointcloud. +``` +ros2 launch realsense2_camera rs_pointcloud_launch.py +``` + +Alternatively, start the camera terminal 1: +``` +ros2 launch realsense2_camera rs_launch.py pointcloud.enable:=true +``` +and in terminal 2 open rviz to visualize pointcloud. + +# PointCloud with different coordinate systems +This example opens rviz and shows the camera model with different coordinate systems and the pointcloud, so it presents the pointcloud and the camera together. +``` +ros2 launch realsense2_camera rs_d455_pointcloud_launch.py +``` \ No newline at end of file diff --git a/realsense2_camera/examples/pointcloud/rs_d455_pointcloud_launch.py b/realsense2_camera/examples/pointcloud/rs_d455_pointcloud_launch.py index 4f013b1c87..85ee0ff48d 100644 --- a/realsense2_camera/examples/pointcloud/rs_d455_pointcloud_launch.py +++ b/realsense2_camera/examples/pointcloud/rs_d455_pointcloud_launch.py @@ -77,7 +77,7 @@ def generate_launch_description(): namespace='', executable='rviz2', name='rviz2', - arguments=['-d', [ThisLaunchFileDir(), '/urdf_pointcloud.rviz']], + arguments=['-d', [ThisLaunchFileDir(), '/rviz/urdf_pointcloud.rviz']], output='screen', parameters=[{'use_sim_time': False}] ), diff --git a/realsense2_camera/examples/pointcloud/rs_pointcloud_launch.py b/realsense2_camera/examples/pointcloud/rs_pointcloud_launch.py index f0a5b541e0..f0acfe80b2 100644 --- a/realsense2_camera/examples/pointcloud/rs_pointcloud_launch.py +++ b/realsense2_camera/examples/pointcloud/rs_pointcloud_launch.py @@ -57,6 +57,6 @@ def generate_launch_description(): namespace='', executable='rviz2', name='rviz2', - arguments=['-d', [ThisLaunchFileDir(), '/pointcloud.rviz']] + arguments=['-d', [ThisLaunchFileDir(), '/rviz/pointcloud.rviz']] ) ]) diff --git a/realsense2_camera/examples/pointcloud/pointcloud.rviz b/realsense2_camera/examples/pointcloud/rviz/pointcloud.rviz similarity index 69% rename from realsense2_camera/examples/pointcloud/pointcloud.rviz rename to realsense2_camera/examples/pointcloud/rviz/pointcloud.rviz index 055431f228..bb8955146f 100644 --- a/realsense2_camera/examples/pointcloud/pointcloud.rviz +++ b/realsense2_camera/examples/pointcloud/rviz/pointcloud.rviz @@ -8,14 +8,12 @@ Panels: - /Status1 - /Grid1 - /PointCloud21 - - /Image1 Splitter Ratio: 0.5 - Tree Height: 222 + Tree Height: 382 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties Expanded: - - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 @@ -65,13 +63,17 @@ Visualization Manager: 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 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera/camera/depth/color/points Use Fixed Frame: true Use rainbow: true Value: true @@ -82,9 +84,12 @@ Visualization Manager: Min Value: 0 Name: Image Normalize Range: true - Queue Size: 10 - Topic: /camera/color/image_raw - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera/camera/color/image_raw Value: true - Class: rviz_default_plugins/Image Enabled: true @@ -93,31 +98,12 @@ Visualization Manager: 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 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera/camera/depth/image_rect_raw Value: true Enabled: true Global Options: @@ -132,12 +118,30 @@ Visualization Manager: - Class: rviz_default_plugins/Measure Line color: 128; 128; 0 - Class: rviz_default_plugins/SetInitialPose - Topic: /initialpose + 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: /move_base_simple/goal + 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: /clicked_point + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point Transformation: Current: Class: rviz_default_plugins/TF @@ -168,18 +172,18 @@ Visualization Manager: Window Geometry: Displays: collapsed: false - Height: 1025 + Height: 1016 Hide Left Dock: false Hide Right Dock: true Image: collapsed: false - QMainWindow State: 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 + QMainWindow State: 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 Selection: collapsed: false Tool Properties: collapsed: false Views: collapsed: true - Width: 1853 - X: 67 + Width: 1846 + X: 74 Y: 27 diff --git a/realsense2_camera/examples/pointcloud/urdf_pointcloud.rviz b/realsense2_camera/examples/pointcloud/rviz/urdf_pointcloud.rviz similarity index 92% rename from realsense2_camera/examples/pointcloud/urdf_pointcloud.rviz rename to realsense2_camera/examples/pointcloud/rviz/urdf_pointcloud.rviz index 500e60114e..294917ad48 100644 --- a/realsense2_camera/examples/pointcloud/urdf_pointcloud.rviz +++ b/realsense2_camera/examples/pointcloud/rviz/urdf_pointcloud.rviz @@ -8,12 +8,12 @@ Panels: - /Status1 - /RobotModel1 - /RobotModel1/Description Topic1 - - /TF1 - /TF1/Frames1 + - /PointCloud21 - /Image1 - /Image2 - Splitter Ratio: 0.6360294222831726 - Tree Height: 362 + Splitter Ratio: 0.36195287108421326 + Tree Height: 308 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -245,7 +245,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /camera/depth/color/points + Value: /camera/camera/depth/color/points Use Fixed Frame: true Use rainbow: true Value: true @@ -261,7 +261,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /camera/color/image_raw + Value: /camera/camera/color/image_raw Value: true - Class: rviz_default_plugins/Image Enabled: true @@ -275,7 +275,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /camera/depth/image_rect_raw + Value: /camera/camera/depth/image_rect_raw Value: true Enabled: true Global Options: @@ -323,7 +323,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 3.9530911445617676 + Distance: 4.639365196228027 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -351,7 +351,7 @@ Window Geometry: Hide Right Dock: false Image: collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000001560000039efc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001f5000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000238000000b00000002800fffffffb0000000a0049006d00610067006501000002ee000000ed0000002800ffffff000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004c50000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000002540000039efc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001bf000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000132000000b00000000000000000fb0000000a0049006d00610067006501000001e8000000ed0000000000000000fb0000000a0049006d0061006700650100000202000000e90000002800fffffffb0000000a0049006d00610067006501000002f1000000ea0000002800ffffff000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000003c70000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Tool Properties: diff --git a/realsense2_camera/launch/default.rviz b/realsense2_camera/launch/default.rviz index 055431f228..c4374772c6 100644 --- a/realsense2_camera/launch/default.rviz +++ b/realsense2_camera/launch/default.rviz @@ -9,13 +9,15 @@ Panels: - /Grid1 - /PointCloud21 - /Image1 + - /Image2 + - /Image3 + - /Image4 Splitter Ratio: 0.5 - Tree Height: 222 + Tree Height: 190 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties Expanded: - - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 @@ -65,13 +67,17 @@ Visualization Manager: 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 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera/camera/depth/color/points Use Fixed Frame: true Use rainbow: true Value: true @@ -82,9 +88,12 @@ Visualization Manager: Min Value: 0 Name: Image Normalize Range: true - Queue Size: 10 - Topic: /camera/color/image_raw - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera/camera/color/image_raw Value: true - Class: rviz_default_plugins/Image Enabled: true @@ -93,9 +102,12 @@ Visualization Manager: Min Value: 0 Name: Image Normalize Range: true - Queue Size: 10 - Topic: /camera/depth/image_rect_raw - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera/camera/depth/image_rect_raw Value: true - Class: rviz_default_plugins/Image Enabled: true @@ -104,9 +116,12 @@ Visualization Manager: Min Value: 0 Name: Image Normalize Range: true - Queue Size: 10 - Topic: /camera/infra1/image_rect_raw - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera/camera/infra1/image_rect_raw Value: true - Class: rviz_default_plugins/Image Enabled: true @@ -115,9 +130,12 @@ Visualization Manager: Min Value: 0 Name: Image Normalize Range: true - Queue Size: 10 - Topic: /camera/infra2/image_rect_raw - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera/camera/infra2/image_rect_raw Value: true Enabled: true Global Options: @@ -132,12 +150,30 @@ Visualization Manager: - Class: rviz_default_plugins/Measure Line color: 128; 128; 0 - Class: rviz_default_plugins/SetInitialPose - Topic: /initialpose + 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: /move_base_simple/goal + 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: /clicked_point + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point Transformation: Current: Class: rviz_default_plugins/TF @@ -145,7 +181,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 1.0510121583938599 + Distance: 3.677529811859131 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -168,18 +204,18 @@ Visualization Manager: Window Geometry: Displays: collapsed: false - Height: 1025 + Height: 1016 Hide Left Dock: false Hide Right Dock: true Image: collapsed: false - QMainWindow State: 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 + QMainWindow State: 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 Selection: collapsed: false Tool Properties: collapsed: false Views: collapsed: true - Width: 1853 - X: 67 + Width: 1846 + X: 74 Y: 27 From df411b1c1805527ac97797156f4ace694a83f089 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Tue, 26 Sep 2023 22:47:44 +0530 Subject: [PATCH 43/76] [Frame Latency] Support for all topics --- .../launch/rs_intra_process_demo_launch.py | 51 +++++++++------ .../tools/frame_latency/frame_latency.cpp | 65 +++++++++++++++---- .../tools/frame_latency/frame_latency.h | 27 +++++++- 3 files changed, 108 insertions(+), 35 deletions(-) diff --git a/realsense2_camera/launch/rs_intra_process_demo_launch.py b/realsense2_camera/launch/rs_intra_process_demo_launch.py index 9bdf75111f..a4e54391a0 100644 --- a/realsense2_camera/launch/rs_intra_process_demo_launch.py +++ b/realsense2_camera/launch/rs_intra_process_demo_launch.py @@ -41,24 +41,32 @@ '\nplease make sure you run "colcon build --cmake-args \'-DBUILD_TOOLS=ON\'" command before running this launch file') -configurable_parameters = [{'name': 'camera_name', 'default': 'camera', 'description': 'camera unique name'}, - {'name': 'serial_no', 'default': "''", 'description': 'choose device by serial number'}, - {'name': 'usb_port_id', 'default': "''", 'description': 'choose device by usb port id'}, - {'name': 'device_type', 'default': "''", 'description': 'choose device by type'}, - {'name': 'log_level', 'default': 'info', 'description': 'debug log level [DEBUG|INFO|WARN|ERROR|FATAL]'}, - {'name': 'rgb_camera.profile', 'default': '0,0,0', 'description': 'color image width'}, - {'name': 'enable_color', 'default': 'true', 'description': 'enable color stream'}, - {'name': 'enable_depth', 'default': 'false', 'description': 'enable depth stream'}, - {'name': 'enable_infra', 'default': 'false', 'description': 'enable infra stream'}, - {'name': 'enable_infra1', 'default': 'false', 'description': 'enable infra1 stream'}, - {'name': 'enable_infra2', 'default': 'false', 'description': 'enable infra2 stream'}, - {'name': 'enable_gyro', 'default': 'false', 'description': "enable gyro stream"}, - {'name': 'enable_accel', 'default': 'false', 'description': "enable accel stream"}, - {'name': 'intra_process_comms', 'default': 'true', 'description': "enable intra-process communication"}, - {'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': 'topic_name', 'default': '/color/image_raw', 'description': 'topic to which latency calculated'}, - ] +realsense_node_params = [{'name': 'serial_no', 'default': "''", 'description': 'choose device by serial number'}, + {'name': 'usb_port_id', 'default': "''", 'description': 'choose device by usb port id'}, + {'name': 'device_type', 'default': "''", 'description': 'choose device by type'}, + {'name': 'log_level', 'default': 'info', 'description': 'debug log level [DEBUG|INFO|WARN|ERROR|FATAL]'}, + {'name': 'rgb_camera.profile', 'default': '0,0,0', 'description': 'color image width'}, + {'name': 'enable_color', 'default': 'true', 'description': 'enable color stream'}, + {'name': 'enable_depth', 'default': 'true', 'description': 'enable depth stream'}, + {'name': 'enable_infra', 'default': 'false', 'description': 'enable infra stream'}, + {'name': 'enable_infra1', 'default': 'true', 'description': 'enable infra1 stream'}, + {'name': 'enable_infra2', 'default': 'true', 'description': 'enable infra2 stream'}, + {'name': 'enable_gyro', 'default': 'true', 'description': "enable gyro stream"}, + {'name': 'enable_accel', 'default': 'true', 'description': "enable accel stream"}, + {'name': 'unite_imu_method', 'default': "1", 'description': '[0-None, 1-copy, 2-linear_interpolation]'}, + {'name': 'intra_process_comms', 'default': 'true', 'description': "enable intra-process communication"}, + {'name': 'enable_sync', 'default': 'true', 'description': "'enable sync mode'"}, + {'name': 'pointcloud.enable', 'default': 'true', 'description': ''}, + {'name': 'enable_rgbd', 'default': 'true', 'description': "'enable rgbd topic'"}, + {'name': 'align_depth.enable', 'default': 'true', 'description': "'enable align depth filter'"}, + {'name': 'publish_tf', 'default': 'true', 'description': '[bool] enable/disable publishing static & dynamic TF'}, + {'name': 'tf_publish_rate', 'default': '1.0', 'description': '[double] rate in HZ for publishing dynamic TF'}, + ] + +frame_latency_node_params = [{'name': 'topic_name', 'default': '/camera/color/image_raw', 'description': 'topic to which latency calculated'}, + {'name': 'topic_type', 'default': 'image', 'description': 'topic type [image|points|imu|metadata|camera_info|rgbd|imu_info|tf]'}, + ] + def declare_configurable_parameters(parameters): return [DeclareLaunchArgument(param['name'], default_value=param['default'], description=param['description']) for param in parameters] @@ -70,7 +78,8 @@ def set_configurable_parameters(parameters): def generate_launch_description(): - return LaunchDescription(declare_configurable_parameters(configurable_parameters) + [ + return LaunchDescription(declare_configurable_parameters(realsense_node_params) + + declare_configurable_parameters(frame_latency_node_params) +[ ComposableNodeContainer( name='my_container', namespace='', @@ -82,14 +91,14 @@ def generate_launch_description(): namespace='', plugin='realsense2_camera::' + rs_node_class, name="camera", - parameters=[set_configurable_parameters(configurable_parameters)], + parameters=[set_configurable_parameters(realsense_node_params)], extra_arguments=[{'use_intra_process_comms': LaunchConfiguration("intra_process_comms")}]) , ComposableNode( package='realsense2_camera', namespace='', plugin='rs2_ros::tools::frame_latency::' + rs_latency_tool_class, name='frame_latency', - parameters=[set_configurable_parameters(configurable_parameters)], + parameters=[set_configurable_parameters(frame_latency_node_params)], extra_arguments=[{'use_intra_process_comms': LaunchConfiguration("intra_process_comms")}]) , ], output='screen', diff --git a/realsense2_camera/tools/frame_latency/frame_latency.cpp b/realsense2_camera/tools/frame_latency/frame_latency.cpp index 59d37484a1..149da670ed 100644 --- a/realsense2_camera/tools/frame_latency/frame_latency.cpp +++ b/realsense2_camera/tools/frame_latency/frame_latency.cpp @@ -28,7 +28,39 @@ FrameLatencyNode::FrameLatencyNode( const std::string & node_name, { } -std::string topic_name = "/color/image_raw"; +std::string topic_name = "/camera/color/image_raw"; +std::string topic_type = "image"; + +template +void FrameLatencyNode::createListener(std::string topicName, const rmw_qos_profile_t qos_profile) +{ + _sub = this->create_subscription( + topicName, + rclcpp::QoS( rclcpp::QoSInitialization::from_rmw( qos_profile ), + qos_profile ), + [&, this]( const std::shared_ptr< MsgType> msg ) { + rclcpp::Time curr_time = this->get_clock()->now(); + auto latency = ( curr_time - msg->header.stamp ).seconds(); + ROS_INFO_STREAM( "Got msg with "<< msg->header.frame_id <<" frame id at address 0x" + << std::hex << reinterpret_cast< std::uintptr_t >( msg.get() ) + << std::dec << " with latency of " << latency << " [sec]" ); + } ); +} + +void FrameLatencyNode::createTFListener(std::string topicName, const rmw_qos_profile_t qos_profile) +{ + _sub = this->create_subscription( + topicName, + rclcpp::QoS( rclcpp::QoSInitialization::from_rmw( qos_profile ), + qos_profile ), + [&, this]( const std::shared_ptr msg ) { + rclcpp::Time curr_time = this->get_clock()->now(); + auto latency = ( curr_time - msg->transforms.back().header.stamp ).seconds(); + ROS_INFO_STREAM( "Got msg with "<< msg->transforms.back().header.frame_id <<" frame id at address 0x" + << std::hex << reinterpret_cast< std::uintptr_t >( msg.get() ) + << std::dec << " with latency of " << latency << " [sec]" ); + } ); +} FrameLatencyNode::FrameLatencyNode( const rclcpp::NodeOptions & node_options ) : Node( "frame_latency", "/", node_options ) @@ -39,21 +71,28 @@ FrameLatencyNode::FrameLatencyNode( const rclcpp::NodeOptions & node_options ) << ( this->get_node_options().use_intra_process_comms() ? "ON" : "OFF" ) ); topic_name = this->declare_parameter("topic_name", topic_name); + topic_type = this->declare_parameter("topic_type", topic_type); ROS_INFO_STREAM( "Subscribing to Topic: " << topic_name); - // Create a subscription on the input topic. - _sub = this->create_subscription< sensor_msgs::msg::Image >( - topic_name, - rclcpp::QoS( rclcpp::QoSInitialization::from_rmw( rmw_qos_profile_default ), - rmw_qos_profile_default ), - [&, this]( const sensor_msgs::msg::Image::SharedPtr msg ) { - rclcpp::Time curr_time = this->get_clock()->now(); - auto latency = ( curr_time - msg->header.stamp ).seconds(); - ROS_INFO_STREAM( "Got msg with "<< msg->header.frame_id <<" frame id at address 0x" - << std::hex << reinterpret_cast< std::uintptr_t >( msg.get() ) - << std::dec << " with latency of " << latency << " [sec]" ); - } ); + if (topic_type == "image") + createListener(topic_name, rmw_qos_profile_default); + else if (topic_type == "points") + createListener(topic_name, rmw_qos_profile_default); + else if (topic_type == "imu") + createListener(topic_name, rmw_qos_profile_sensor_data); + else if (topic_type == "metadata") + createListener(topic_name, rmw_qos_profile_default); + else if (topic_type == "camera_info") + createListener(topic_name, rmw_qos_profile_default); + else if (topic_type == "rgbd") + createListener(topic_name, rmw_qos_profile_default); + else if (topic_type == "imu_info") + createListener(topic_name, rmw_qos_profile_default); + else if (topic_type == "tf") + createTFListener(topic_name, rmw_qos_profile_default); + else + ROS_ERROR_STREAM("Specified message type '" << topic_type << "' is not supported"); } #include "rclcpp_components/register_node_macro.hpp" diff --git a/realsense2_camera/tools/frame_latency/frame_latency.h b/realsense2_camera/tools/frame_latency/frame_latency.h index 653c648578..aad8c81c0e 100644 --- a/realsense2_camera/tools/frame_latency/frame_latency.h +++ b/realsense2_camera/tools/frame_latency/frame_latency.h @@ -16,6 +16,25 @@ #include #include "sensor_msgs/msg/image.hpp" +#include "sensor_msgs/msg/imu.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" + +#include +#include +#include "realsense2_camera_msgs/msg/imu_info.hpp" +#include "realsense2_camera_msgs/msg/extrinsics.hpp" +#include "realsense2_camera_msgs/msg/metadata.hpp" +#include "realsense2_camera_msgs/msg/rgbd.hpp" +#include "realsense2_camera_msgs/srv/device_info.hpp" +#include +#include + +#include +#include +#include +#include +#include + namespace rs2_ros { namespace tools { @@ -31,8 +50,14 @@ class FrameLatencyNode : public rclcpp::Node const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions().use_intra_process_comms( true ) ); + template + void createListener(std::string topicName, const rmw_qos_profile_t qos_profile); + + void createTFListener(std::string topicName, const rmw_qos_profile_t qos_profile); + private: - rclcpp::Subscription< sensor_msgs::msg::Image >::SharedPtr _sub; + std::shared_ptr _sub; + rclcpp::Logger _logger; }; } // namespace frame_latency From 9679a9be23bd60528194c272fc82a5feb0cedecc Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Wed, 27 Sep 2023 21:44:01 +0530 Subject: [PATCH 44/76] [Frame latency Tool] Added description --- .../tools/frame_latency/frame_latency.cpp | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/realsense2_camera/tools/frame_latency/frame_latency.cpp b/realsense2_camera/tools/frame_latency/frame_latency.cpp index 149da670ed..7f4ebb0da7 100644 --- a/realsense2_camera/tools/frame_latency/frame_latency.cpp +++ b/realsense2_camera/tools/frame_latency/frame_latency.cpp @@ -12,11 +12,25 @@ // See the License for the specific language governing permissions and // limitations under the License. +// DESCRIPTION: # +// ------------ # +// This tool created a node which can be used to calulate the specified topic's latency. +// Input parameters: +// - topic_name : +// - topic to which latency need to be calculated +// - topic_type : +// - Message type of the topic. +// - Valid inputs: {'image','points','imu','metadata','camera_info','rgbd','imu_info','tf'} +// Note: +// - This tool doesn't support calulating latency for extrinsic topics. +// Because, those topics doesn't have timestamp in it and this tool uses +// that timestamp as an input to calculate the latency. +// + #include #include #include #include -// Node which receives sensor_msgs/Image messages and prints the image latency. using namespace rs2_ros::tools::frame_latency; From d2549b1a533239c66bf4bf6241f6aa2d1d5fd556 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Wed, 4 Oct 2023 22:55:50 +0530 Subject: [PATCH 45/76] Revert 'Updating _camera_name with RS node's name' --- realsense2_camera/src/base_realsense_node.cpp | 3 --- realsense2_camera/src/parameters.cpp | 4 ++++ 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 368d2f10c1..9870d3ac59 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -121,9 +121,6 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node, ROS_INFO("Intra-Process communication enabled"); } - // Get node name - _camera_name = _node.get_name(); - initializeFormatsMaps(); _monitor_options = {RS2_OPTION_ASIC_TEMPERATURE, RS2_OPTION_PROJECTOR_TEMPERATURE}; } diff --git a/realsense2_camera/src/parameters.cpp b/realsense2_camera/src/parameters.cpp index 14accf94f0..72523cb801 100644 --- a/realsense2_camera/src/parameters.cpp +++ b/realsense2_camera/src/parameters.cpp @@ -24,6 +24,10 @@ void BaseRealSenseNode::getParameters() std::string param_name; + param_name = std::string("camera_name"); + _camera_name = _parameters->setParam(param_name, "camera"); + _parameters_names.push_back(param_name); + param_name = std::string("publish_tf"); _publish_tf = _parameters->setParam(param_name, PUBLISH_TF); _parameters_names.push_back(param_name); From 7762d8d889ae8ee49d1ba66da849c96baf972dcb Mon Sep 17 00:00:00 2001 From: Samer Khshiboun <99127997+SamerKhshiboun@users.noreply.github.com> Date: Thu, 5 Oct 2023 00:03:18 +0300 Subject: [PATCH 46/76] Update README.md --- realsense2_camera/examples/align_depth/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/realsense2_camera/examples/align_depth/README.md b/realsense2_camera/examples/align_depth/README.md index ee869ac70c..3b8f13b826 100644 --- a/realsense2_camera/examples/align_depth/README.md +++ b/realsense2_camera/examples/align_depth/README.md @@ -8,5 +8,5 @@ The aligned image will be published to the topic "/aligned_depth_to_color/image_ Also, align depth to color can enabled by following cmd: ``` -ros2 launch realsense2_camera rs_launch.py align_depth.enable:=true" -``` \ No newline at end of file +ros2 launch realsense2_camera rs_launch.py align_depth.enable:=true +``` From b66a252bba2e6da9051202fb6a4748136151e8ca Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Wed, 30 Aug 2023 20:42:53 +0530 Subject: [PATCH 47/76] Added live camera tests --- realsense2_camera/CMakeLists.txt | 35 +++ realsense2_camera/package.xml | 3 +- .../test_camera_all_profile_tests.py | 264 +++++++++++++++++ .../live_camera/test_camera_failing_tests.py | 268 ++++++++++++++++++ .../test/live_camera/test_camera_fps_tests.py | 102 +++++++ .../test/live_camera/test_camera_imu_tests.py | 126 ++++++++ .../test/live_camera/test_d415_basic_tests.py | 150 ++++++++++ .../test/live_camera/test_d455_basic_tests.py | 93 ++++++ .../test/utils/pytest_live_camera_utils.py | 190 +++++++++++++ .../test/utils/pytest_rs_utils.py | 165 +++++++++-- 10 files changed, 1372 insertions(+), 24 deletions(-) create mode 100644 realsense2_camera/test/live_camera/test_camera_all_profile_tests.py create mode 100644 realsense2_camera/test/live_camera/test_camera_failing_tests.py create mode 100644 realsense2_camera/test/live_camera/test_camera_fps_tests.py create mode 100644 realsense2_camera/test/live_camera/test_camera_imu_tests.py create mode 100644 realsense2_camera/test/live_camera/test_d415_basic_tests.py create mode 100644 realsense2_camera/test/live_camera/test_d455_basic_tests.py create mode 100644 realsense2_camera/test/utils/pytest_live_camera_utils.py diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index fd06cb4868..b4da686f8f 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -304,6 +304,41 @@ if(BUILD_TESTING) ) endforeach() endforeach() + + unset(_pytest_folders) + + set(rs_query_cmd "rs-enumerate-devices -s") + execute_process(COMMAND bash -c ${rs_query_cmd} + WORKING_DIRECTORY ${PROJECT_SOURCE_DIR} + RESULT_VARIABLE rs_result + OUTPUT_VARIABLE RS_DEVICE_INFO) + message(STATUS "rs_device_info:") + message(STATUS "${RS_DEVICE_INFO}") + if(RS_DEVICE_INFO MATCHES "D455") + message(STATUS "D455 device found") + set(_pytest_live_folders + test/live_camera + ) + elseif(RS_DEVICE_INFO MATCHES "D415") + message(STATUS "D415 device found") + set(_pytest_live_folders + test/live_camera + ) + endif() + + foreach(test_folder ${_pytest_live_folders}) + file(GLOB files "${test_folder}/test_*.py") + foreach(file ${files}) + + get_filename_component(_test_name ${file} NAME_WE) + ament_add_pytest_test(${_test_name} ${file} + APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR}:${CMAKE_SOURCE_DIR}/test/utils:${CMAKE_SOURCE_DIR}/launch:${CMAKE_SOURCE_DIR}/scripts + TIMEOUT 500 + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + ) + endforeach() + endforeach() + endif() # Ament exports diff --git a/realsense2_camera/package.xml b/realsense2_camera/package.xml index ab4d1d763b..6db46cff68 100644 --- a/realsense2_camera/package.xml +++ b/realsense2_camera/package.xml @@ -38,7 +38,8 @@ sensor_msgs_py python3-requests tf2_ros_py - + ros2topic + launch_ros ros_environment diff --git a/realsense2_camera/test/live_camera/test_camera_all_profile_tests.py b/realsense2_camera/test/live_camera/test_camera_all_profile_tests.py new file mode 100644 index 0000000000..274ee67625 --- /dev/null +++ b/realsense2_camera/test/live_camera/test_camera_all_profile_tests.py @@ -0,0 +1,264 @@ +# 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. + + +import os +import sys +import itertools + + +import pytest +import rclpy + +from sensor_msgs.msg import Image as msg_Image +from sensor_msgs.msg import Imu as msg_Imu +from sensor_msgs.msg import PointCloud2 as msg_PointCloud2 + +import numpy as np + +sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils")) +import pytest_rs_utils +from pytest_rs_utils import launch_descr_with_parameters +from pytest_rs_utils import delayed_launch_descr_with_parameters +from pytest_rs_utils import get_rosbag_file_path +from pytest_rs_utils import get_node_heirarchy +import pytest_live_camera_utils +from rclpy.parameter import Parameter +from rcl_interfaces.msg import ParameterValue +from rcl_interfaces.srv import SetParameters, GetParameters, ListParameters +from pytest_live_camera_utils import debug_print +def check_if_skip_test(profile, format): + ''' + if profile == 'Color': + if "BGRA8" == format: + return True + if "RGBA8" == format: + return True + if "Y8" == format: + return True + elif profile == 'Depth': + if "Z16" == format: + return True + el + if profile == 'Infrared': + if "Y8" == format: + return True + if "Y16" == format: + return True + if "BGRA8" == format: + return True + if "RGBA8" == format: + return True + if "Y10BPACK" == format: + return True + if "UYVY" == format: + return True + if "BGR8" == format: + return True + if "RGB8" == format: + return True + if "RAW10" == format: + return True + elif profile == 'Infrared1': + if "Y8" ==format: + return True + if "Y16" ==format: + return True + if "Y10BPACK" == format: + return True + if "UYVY" ==format: + return True + if "BGR8" ==format: + return True + if "RGB8" ==format: + return True + if "RAW10" ==format: + return True + if profile == 'Infrared2': + if "Y8" == format: + return True + if "Y16" == format: + return True + if "Y10BPACK" == format: + return True + if "UYVY" == format: + return True + if "BGR8" == format: + return True + if "RGB8" == format: + return True + if "RAW10" == format: + return True + ''' + if profile == 'Infrared': + if "Y8" == format: + return True + if "Y16" == format: + return True + if profile == 'Infrared1': + if "Y8" ==format: + return True + if "Y16" ==format: + return True + if profile == 'Infrared2': + if "Y8" == format: + return True + if "Y16" == format: + return True + return False + + +test_params_all_profiles_d455 = { + 'camera_name': 'D455', + 'device_type': 'D455', + } +test_params_all_profiles_d415 = { + 'camera_name': 'D415', + 'device_type': 'D415', + } +test_params_all_profiles_d435 = { + 'camera_name': 'D435', + 'device_type': 'D435', + } + + +''' +This test was implemented as a template to set the parameters and run the test. +This directory is not added to the CMakeLists.txt so as to avoid the colcon failure in the +machines that don't have the D455 connected. +1. Only a subset of parameter types are implemented in py_rs_utils, it has to be extended for others +2. After setting the param, rclpy.spin_once may be needed.Test passes even without this though. +''' +@pytest.mark.parametrize("launch_descr_with_parameters", [ + pytest.param(test_params_all_profiles_d455, marks=pytest.mark.d455), + pytest.param(test_params_all_profiles_d415, marks=pytest.mark.d415), + pytest.param(test_params_all_profiles_d435, marks=pytest.mark.d435),] + ,indirect=True) +@pytest.mark.launch(fixture=launch_descr_with_parameters) +class TestLiveCamera_Change_Resolution(pytest_rs_utils.RsTestBaseClass): + def test_LiveCamera_Change_Resolution(self,launch_descr_with_parameters): + skipped_tests = [] + failed_tests = [] + num_passed = 0 + num_failed = 0 + params = launch_descr_with_parameters[1] + themes = [{'topic':get_node_heirarchy(params)+'/color/image_raw', 'msg_type':msg_Image,'expected_data_chunks':1}] + config = pytest_live_camera_utils.get_profile_config(get_node_heirarchy(params)) + try: + ''' + initialize, run and check the data + ''' + self.init_test("RsTest"+params['camera_name']) + self.spin_for_time(wait_time=1.0) + cap = pytest_live_camera_utils.get_camera_capabilities(params['device_type']) + if cap == None: + debug_print("Device not found? : " + params['device_type']) + return + self.create_param_ifs(get_node_heirarchy(params)) + config["Color"]["default_profile1"],config["Color"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["color_profile"], "Color") + config["Depth"]["default_profile1"],config["Depth"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["depth_profile"], "Depth") + config["Infrared"]["default_profile1"],config["Infrared"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["depth_profile"], "Infrared") + config["Infrared1"]["default_profile1"],config["Infrared1"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["depth_profile"], "Infrared1") + config["Infrared2"]["default_profile1"],config["Infrared2"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["depth_profile"], "Infrared2") + for key in cap["color_profile"]: + profile_type = key[0] + profile = key[1] + format = key[2] + if check_if_skip_test(profile_type, format): + skipped_tests.append(" ".join(key)) + continue + print("Testing " + " ".join(key)) + themes[0]['topic'] = config[profile_type]['topic'] + themes[0]['width'] = int(profile.split('x')[0]) + themes[0]['height'] = int(profile.split('x')[1]) + if themes[0]['width'] == int(config[profile_type]["default_profile2"].split('x')[0]): + self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile1"]) + else: + self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile2"]) + self.set_bool_param(config[profile_type]["param"], True) + self.disable_all_params() + self.spin_for_time(wait_time=0.2) + self.set_string_param(config[profile_type]["profile"], profile) + self.set_string_param(config[profile_type]["format"], format) + self.set_bool_param(config[profile_type]["param"], True) + try: + ret = self.run_test(themes) + assert ret[0], ret[1] + assert self.process_data(themes), " ".join(key) + " failed" + num_passed += 1 + except Exception as e: + exc_type, exc_obj, exc_tb = sys.exc_info() + fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1] + print("Test failed") + print(e) + print(exc_type, fname, exc_tb.tb_lineno) + num_failed += 1 + failed_tests.append(" ".join(key)) + debug_print("Color tests completed") + for key in cap["depth_profile"]: + profile_type = key[0] + profile = key[1] + format = key[2] + if check_if_skip_test(profile_type, format): + skipped_tests.append(" ".join(key)) + continue + print("Testing " + " ".join(key)) + + themes[0]['topic'] = config[profile_type]['topic'] + themes[0]['width'] = int(profile.split('x')[0]) + themes[0]['height'] = int(profile.split('x')[1]) + if themes[0]['width'] == int(config[profile_type]["default_profile2"].split('x')[0]): + self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile1"]) + else: + self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile2"]) + self.set_bool_param(config[profile_type]["param"], True) + + + self.disable_all_params() + self.spin_for_time(wait_time=0.2) + self.set_string_param(config[profile_type]["profile"], profile) + self.set_string_param(config[profile_type]["format"], format) + self.set_bool_param(config[profile_type]["param"], True) + try: + ret = self.run_test(themes) + assert ret[0], ret[1] + assert self.process_data(themes), " ".join(key) + " failed" + num_passed += 1 + except Exception as e: + print("Test failed") + print(e) + num_failed += 1 + failed_tests.append(" ".join(key)) + debug_print("Depth tests completed") + finally: + #this step is important because the test will fail next time + pytest_rs_utils.kill_realsense2_camera_node() + self.shutdown() + print("Tests passed " + str(num_passed)) + print("Tests skipped " + str(len(skipped_tests))) + if len(skipped_tests): + debug_print("\nSkipped tests:" + params['device_type']) + debug_print("\n".join(skipped_tests)) + print("Tests failed " + str(num_failed)) + if len(failed_tests): + print("\nFailed tests:" + params['device_type']) + print("\n".join(failed_tests)) + + def disable_all_params(self): + self.set_bool_param('enable_color', False) + self.set_bool_param('enable_depth', False) + self.set_bool_param('enable_infra', False) + self.set_bool_param('enable_infra1', False) + self.set_bool_param('enable_infra2', False) + diff --git a/realsense2_camera/test/live_camera/test_camera_failing_tests.py b/realsense2_camera/test/live_camera/test_camera_failing_tests.py new file mode 100644 index 0000000000..3bdc87b19b --- /dev/null +++ b/realsense2_camera/test/live_camera/test_camera_failing_tests.py @@ -0,0 +1,268 @@ +# 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. + + +import os +import sys +import itertools + + +import pytest +import rclpy + +from sensor_msgs.msg import Image as msg_Image +from sensor_msgs.msg import Imu as msg_Imu +from sensor_msgs.msg import PointCloud2 as msg_PointCloud2 + +import numpy as np + +sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils")) +import pytest_rs_utils +from pytest_rs_utils import launch_descr_with_parameters +from pytest_rs_utils import delayed_launch_descr_with_parameters +from pytest_rs_utils import get_rosbag_file_path +from pytest_rs_utils import get_node_heirarchy +import pytest_live_camera_utils +from rclpy.parameter import Parameter +from rcl_interfaces.msg import ParameterValue +from rcl_interfaces.srv import SetParameters, GetParameters, ListParameters +from pytest_live_camera_utils import debug_print + +test_params_all_profiles_d455 = { + 'camera_name': 'D455', + 'device_type': 'D455', + } +test_params_all_profiles_d415 = { + 'camera_name': 'D415', + 'device_type': 'D415', + } +''' +This test was implemented as a template to set the parameters and run the test. +This directory is not added to the CMakeLists.txt so as to avoid the colcon failure in the +machines that don't have the D455 connected. +1. Only a subset of parameter types are implemented in py_rs_utils, it has to be extended for others +2. After setting the param, rclpy.spin_once may be needed.Test passes even without this though. +''' +@pytest.mark.parametrize("launch_descr_with_parameters", [test_params_all_profiles_d415],indirect=True) +@pytest.mark.launch(fixture=launch_descr_with_parameters) +class TestLiveCamera_Change_Resolution(pytest_rs_utils.RsTestBaseClass): + def test_LiveCamera_Change_Resolution(self,launch_descr_with_parameters): + passed_tests = [] + failed_tests = [] + num_passed = 0 + num_failed = 0 + wait_time_s = 1.2 + cap = {} + ''' + need two configurations with different profiles(height & width) for each profile, + this is to ensure the test sets a different profile first, before testing the + actual profile to be tested. + ''' + cap['color_profile'] = [ + ['Color', '1920x1080x30','RGB8'], + ['Color', '1280x720x30','RGB8'], + ] + cap['depth_profile'] = [ + ['Infrared1', '1920x1080x25', 'Y8'], + ['Infrared1', '1920x1080x25', 'Y16'], + ['Infrared1', '1920x1080x15', 'Y16'], + ['Infrared', '848x100x100', 'BGRA8'], + ['Infrared', '640x480x60', 'BGRA8'], + ['Infrared', '640x480x60', 'RGBA8'], + ['Infrared', '640x480x60', 'RGB8'], + ['Infrared', '640x360x60', 'RGBA8'], + ['Infrared', '640x360x60', 'RGB8'], + ['Infrared', '640x360x30', 'UYVY'], + ['Infrared', '480x270x15', 'RGB8'], + ['Infrared', '424x240x60', 'BGRA8'], + ['Infrared', '424x240x30', 'UYVY'], + ['Infrared1', '1920x1080x15', 'Y8'], + ['Infrared1', '1280x720x30', 'Y8'], + ['Infrared1', '1280x720x15', 'Y8'], + ['Infrared1', '1280x720x6', 'Y8'], + ['Infrared1', '960x540x25', 'Y16'], + ['Infrared1', '960x540x15', 'Y16'], + ['Infrared1', '848x480x90', 'Y8'], + ['Infrared1', '848x480x60', 'Y8'], + ['Infrared1', '848x480x30', 'Y8'], + ['Infrared1', '848x480x15', 'Y8'], + ['Infrared1', '848x480x6', 'Y8'], + ['Infrared1', '848x100x100', 'Y8'], + ['Infrared1', '640x480x90', 'Y8'], + ['Infrared1', '640x480x60', 'Y8'], + ['Infrared1', '640x480x30', 'Y8'], + ['Infrared1', '640x480x15', 'Y8'], + ['Infrared1', '640x480x6', 'Y8'], + ['Infrared1', '640x360x90', 'Y8'], + ['Infrared1', '640x360x60', 'Y8'], + ['Infrared1', '640x360x30', 'Y8'], + ['Infrared1', '640x360x15', 'Y8'], + ['Infrared1', '640x360x6', 'Y8'], + ['Infrared1', '480x270x90', 'Y8'], + ['Infrared1', '480x270x60', 'Y8'], + ['Infrared1', '480x270x30', 'Y8'], + ['Infrared1', '480x270x15', 'Y8'], + ['Infrared1', '480x270x6', 'Y8'], + ['Infrared1', '424x240x90', 'Y8'], + ['Infrared1', '424x240x60', 'Y8'], + ['Infrared1', '424x240x30', 'Y8'], + ['Infrared1', '424x240x15', 'Y8'], + ['Infrared1', '424x240x6', 'Y8'], + ['Infrared2', '1920x1080x25', 'Y16'], + ['Infrared2', '1920x1080x25', 'Y8'], + ['Infrared2', '1920x1080x15', 'Y16'], + ['Infrared2', '1920x1080x15', 'Y8'], + ['Infrared2', '1280x720x30', 'Y8'], + ['Infrared2', '1280x720x15', 'Y8'], + ['Infrared2', '1280x720x6', 'Y8'], + ['Infrared2', '960x540x25', 'Y16'], + ['Infrared2', '960x540x15', 'Y16'], + ['Infrared2', '848x480x90', 'Y8'], + ['Infrared2', '848x480x60', 'Y8'], + ['Infrared2', '848x480x30', 'Y8'], + ['Infrared2', '848x480x15', 'Y8'], + ['Infrared2', '848x480x6', 'Y8'], + ['Infrared2', '848x100x100', 'Y8'], + ['Infrared2', '640x480x90', 'Y8'], + ['Infrared2', '640x480x60', 'Y8'], + ['Infrared2', '640x480x30', 'Y8'], + ['Infrared2', '640x480x15', 'Y8'], + ['Infrared2', '640x480x6', 'Y8'], + ['Infrared2', '640x360x90', 'Y8'], + ['Infrared2', '640x360x60', 'Y8'], + ['Infrared2', '640x360x30', 'Y8'], + ['Infrared2', '640x360x15', 'Y8'], + ['Infrared2', '640x360x6', 'Y8'], + ['Infrared2', '480x270x90', 'Y8'], + ['Infrared2', '480x270x60', 'Y8'], + ['Infrared2', '480x270x30', 'Y8'], + ['Infrared2', '480x270x15', 'Y8'], + ['Infrared2', '480x270x6', 'Y8'], + ['Infrared2', '424x240x90', 'Y8'], + ['Infrared2', '424x240x60', 'Y8'], + ['Infrared2', '424x240x30', 'Y8'], + ['Infrared2', '424x240x15', 'Y8'], + ['Infrared2', '424x240x6', 'Y8'], + ['Depth', '640x360x30', 'Z16'], + ['Depth', '480x270x30', 'Z16'], + ['Depth', '424x240x30', 'Z16'], + ] + params = launch_descr_with_parameters[1] + themes = [{'topic':get_node_heirarchy(params)+'/color/image_raw', 'msg_type':msg_Image,'expected_data_chunks':1,'initial_reset':True}] + config = pytest_live_camera_utils.get_profile_config(get_node_heirarchy(params)) + try: + ''' + initialize, run and check the data + ''' + serial_no = None + if 'serial_no' in params: + serial_no = params['serial_no'] + self.init_test("RsTest"+params['camera_name']) + self.spin_for_time(wait_time=1.0) + if cap == None: + debug_print("Device not found? : " + params['device_type']) + return + self.create_param_ifs(get_node_heirarchy(params)) + config["Color"]["default_profile1"],config["Color"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["color_profile"], "Color") + config["Depth"]["default_profile1"],config["Depth"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["depth_profile"], "Depth") + config["Infrared"]["default_profile1"],config["Infrared"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["depth_profile"], "Infrared") + config["Infrared1"]["default_profile1"],config["Infrared1"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["depth_profile"], "Infrared1") + config["Infrared2"]["default_profile1"],config["Infrared2"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["depth_profile"], "Infrared2") + for key in cap["color_profile"]: + profile_type = key[0] + profile = key[1] + format = key[2] + print("Testing " + " ".join(key)) + themes[0]['topic'] = config[profile_type]['topic'] + themes[0]['width'] = int(profile.split('x')[0]) + themes[0]['height'] = int(profile.split('x')[1]) + if themes[0]['width'] == int(config[profile_type]["default_profile2"].split('x')[0]): + self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile1"]) + else: + self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile2"]) + self.set_bool_param(config[profile_type]["param"], True) + self.disable_all_params() + self.spin_for_time(wait_time=wait_time_s) + self.set_string_param(config[profile_type]["profile"], profile) + self.set_string_param(config[profile_type]["format"], format) + self.set_bool_param(config[profile_type]["param"], True) + self.spin_for_time(wait_time=wait_time_s) + try: + ret = self.run_test(themes) + assert ret[0], ret[1] + assert self.process_data(themes), " ".join(key) + " failed" + num_passed += 1 + passed_tests.append(" ".join(key)) + except Exception as e: + exc_type, exc_obj, exc_tb = sys.exc_info() + fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1] + print("Test failed") + print(e) + print(exc_type, fname, exc_tb.tb_lineno) + num_failed += 1 + failed_tests.append(" ".join(key)) + debug_print("Color tests completed") + for key in cap["depth_profile"]: + profile_type = key[0] + profile = key[1] + format = key[2] + print("Testing " + " ".join(key)) + + themes[0]['topic'] = config[profile_type]['topic'] + themes[0]['width'] = int(profile.split('x')[0]) + themes[0]['height'] = int(profile.split('x')[1]) + if themes[0]['width'] == int(config[profile_type]["default_profile2"].split('x')[0]): + self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile1"]) + else: + self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile2"]) + self.set_bool_param(config[profile_type]["param"], True) + + + self.disable_all_params() + self.spin_for_time(wait_time=wait_time_s) + self.set_string_param(config[profile_type]["profile"], profile) + self.set_string_param(config[profile_type]["format"], format) + self.set_bool_param(config[profile_type]["param"], True) + self.spin_for_time(wait_time=wait_time_s) + try: + ret = self.run_test(themes) + assert ret[0], ret[1] + assert self.process_data(themes), " ".join(key) + " failed" + num_passed += 1 + passed_tests.append(" ".join(key)) + except Exception as e: + print("Test failed") + print(e) + num_failed += 1 + failed_tests.append(" ".join(key)) + debug_print("Depth tests completed") + finally: + #this step is important because the test will fail next time + pytest_rs_utils.kill_realsense2_camera_node() + self.shutdown() + print("Tests passed " + str(num_passed)) + if len(passed_tests): + debug_print("\nPassed tests:" + params['device_type']) + debug_print("\n".join(passed_tests)) + print("Tests failed " + str(num_failed)) + if len(failed_tests): + print("\nFailed tests:" + params['device_type']) + print("\n".join(failed_tests)) + + def disable_all_params(self): + self.set_bool_param('enable_color', False) + self.set_bool_param('enable_depth', False) + self.set_bool_param('enable_infra', False) + self.set_bool_param('enable_infra1', False) + self.set_bool_param('enable_infra2', False) \ No newline at end of file diff --git a/realsense2_camera/test/live_camera/test_camera_fps_tests.py b/realsense2_camera/test/live_camera/test_camera_fps_tests.py new file mode 100644 index 0000000000..cff5cadbb4 --- /dev/null +++ b/realsense2_camera/test/live_camera/test_camera_fps_tests.py @@ -0,0 +1,102 @@ +# 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. + + +import os +import sys +import itertools + + +import pytest +import rclpy + +from sensor_msgs.msg import Image as msg_Image +from sensor_msgs.msg import Imu as msg_Imu +from sensor_msgs.msg import PointCloud2 as msg_PointCloud2 + +import numpy as np + +sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils")) +import pytest_rs_utils +from pytest_rs_utils import launch_descr_with_parameters + +from pytest_rs_utils import get_rosbag_file_path +from pytest_rs_utils import get_node_heirarchy + +from rclpy.parameter import Parameter +from rcl_interfaces.msg import ParameterValue +from rcl_interfaces.srv import SetParameters, GetParameters, ListParameters + +test_params_depth_avg_1 = { + 'camera_name': 'D455', + 'device_type': 'D455', + } +''' +The test was implemented to check the fps of Depth and Color frames. The RosTopicHz had to be +modified to make it work, see py_rs_utils for more details. +To check the fps, a value 'expected_fps_in_hz' has to be added to the corresponding theme +''' +@pytest.mark.parametrize("launch_descr_with_parameters", [test_params_depth_avg_1],indirect=True) +@pytest.mark.launch(fixture=launch_descr_with_parameters) +class TestCamera_TestFPS(pytest_rs_utils.RsTestBaseClass): + def test_camera_test_fps(self,launch_descr_with_parameters): + params = launch_descr_with_parameters[1] + try: + ''' + initialize, run and check the data + ''' + print("Starting camera test...") + self.init_test("RsTest"+params['camera_name']) + self.wait_for_node(params['camera_name']) + self.create_param_ifs(get_node_heirarchy(params)) + #assert self.set_bool_param('enable_color', False) + + themes = [ + {'topic':get_node_heirarchy(params)+'/depth/image_rect_raw', + 'msg_type':msg_Image, + 'expected_data_chunks':100, + } + ] + profiles = ['640x480x5','640x480x15', '640x480x30', '640x480x90'] + for profile in profiles: + print("Testing profile: ", profile) + themes[0]['expected_fps_in_hz'] = float(profile.split('x')[2]) + assert self.set_string_param('depth_module.profile', profile) + assert self.set_bool_param('enable_depth', True) + ret = self.run_test(themes, timeout=25.0) + assert ret[0], ret[1] + assert self.process_data(themes) + + themes = [ + {'topic':get_node_heirarchy(params)+'/color/image_raw', + 'msg_type':msg_Image, + 'expected_data_chunks':100, + } + ] + profiles = ['640x480x5','640x480x15', '640x480x30', '1280x720x30'] + for profile in profiles: + print("Testing profile: ", profile) + themes[0]['expected_fps_in_hz'] = float(profile.split('x')[2]) + assert self.set_string_param('rgb_camera.profile', profile) + assert self.set_bool_param('enable_color', True) + ret = self.run_test(themes, timeout=25.0) + assert ret[0], ret[1] + assert self.process_data(themes) + + finally: + #this step is important because the test will fail next time + pytest_rs_utils.kill_realsense2_camera_node() + self.shutdown() + + diff --git a/realsense2_camera/test/live_camera/test_camera_imu_tests.py b/realsense2_camera/test/live_camera/test_camera_imu_tests.py new file mode 100644 index 0000000000..fa173e0a8e --- /dev/null +++ b/realsense2_camera/test/live_camera/test_camera_imu_tests.py @@ -0,0 +1,126 @@ +# 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. + + +import os +import sys +import itertools + + +import pytest +import rclpy + +from sensor_msgs.msg import Image as msg_Image +from sensor_msgs.msg import Imu as msg_Imu +from sensor_msgs.msg import PointCloud2 as msg_PointCloud2 + +import numpy as np + +sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils")) +import pytest_rs_utils +from pytest_rs_utils import launch_descr_with_parameters +from pytest_rs_utils import delayed_launch_descr_with_parameters +from pytest_rs_utils import get_rosbag_file_path +from pytest_rs_utils import get_node_heirarchy + +import pytest_live_camera_utils +from rclpy.parameter import Parameter +from rcl_interfaces.msg import ParameterValue +from rcl_interfaces.srv import SetParameters, GetParameters, ListParameters +from pytest_live_camera_utils import debug_print + +test_params_all_profiles_d455 = { + 'camera_name': 'D455', + 'device_type': 'D455', + 'enable_accel':True, + 'enable_gyro':True, + 'unite_imu_method':1, + } + +@pytest.mark.parametrize("launch_descr_with_parameters", [ + pytest.param(test_params_all_profiles_d455, marks=pytest.mark.d455) + ],indirect=True) +@pytest.mark.launch(fixture=launch_descr_with_parameters) +class TestLiveCamera_TestMotionSensor(pytest_rs_utils.RsTestBaseClass): + def test_LiveCamera_check_motion_sensor(self,launch_descr_with_parameters): + params = launch_descr_with_parameters[1] + themes = [{'topic':get_node_heirarchy(params)+'/imu', 'msg_type':msg_Imu,'expected_data_chunks':1}, + {'topic':get_node_heirarchy(params)+'/gyro/sample', 'msg_type':msg_Imu,'expected_data_chunks':1}, + {'topic':get_node_heirarchy(params)+'/accel/sample', 'msg_type':msg_Imu,'expected_data_chunks':1}] + IMU_TOPIC = 0 + GYRO_TOPIC = 1 + ACCEL_TOPIC = 2 + if params['unite_imu_method'] == '0': + themes[IMU_TOPIC]['expected_data_chunks'] = 0 + try: + ''' + initialize, run and check the data + ''' + msg = "Test with the default params " + self.init_test("RsTest"+params['camera_name']) + self.create_param_ifs(get_node_heirarchy(params)) + print(msg) + ret = self.run_test(themes) + assert ret[0], msg + str(ret[1]) + assert self.process_data(themes), msg + " failed" + + + msg = "Test with the accel false " + self.set_bool_param('enable_accel', False) + self.set_bool_param('enable_gyro', True) + ''' + This step fails if the next line is commented + ''' + self.set_integer_param('unite_imu_method', 0) #this shouldn't matter because the unite_imu_method cannot be changed + themes[ACCEL_TOPIC]['expected_data_chunks'] = 0 + print(msg) + ret = self.run_test(themes) + assert ret[0], msg + str(ret[1]) + assert self.process_data(themes), msg + " failed" + ''' + Test fails + ''' + + msg = "Test with the gyro false " + self.set_bool_param('enable_accel', True) + self.set_bool_param('enable_gyro', False) + self.set_integer_param('unite_imu_method', 0) #this shouldn't matter because the unite_imu_method cannot be changed + themes[IMU_TOPIC]['expected_data_chunks'] = 1 + themes[ACCEL_TOPIC]['expected_data_chunks'] = 1 + themes[GYRO_TOPIC]['expected_data_chunks'] = 0 + print(msg) + ret = self.run_test(themes) + assert ret[0], msg + str(ret[1]) + assert self.process_data(themes), msg + " failed" + + ''' + Test fails, both gyro and accel data is available, not imu + ''' + msg = "Test with both gyro and accel false " + self.set_bool_param('enable_accel', False) + self.set_bool_param('enable_gyro', False) + self.set_integer_param('unite_imu_method', 1) #this shouldn't matter because the unite_imu_method cannot be changed + + themes[ACCEL_TOPIC]['expected_data_chunks'] = 0 + themes[GYRO_TOPIC]['expected_data_chunks'] = 0 + themes[IMU_TOPIC]['expected_data_chunks'] = 1 #Seems that imu data is available even if gyro and accel is disabled + print(msg) + ret = self.run_test(themes, initial_wait_time=1.0) #wait_time added as test doesn't have to wait for any data + assert ret[0], msg + " failed" + assert self.process_data(themes), msg +" failed" + + finally: + #this step is important because the test will fail next time + pytest_rs_utils.kill_realsense2_camera_node() + self.shutdown() diff --git a/realsense2_camera/test/live_camera/test_d415_basic_tests.py b/realsense2_camera/test/live_camera/test_d415_basic_tests.py new file mode 100644 index 0000000000..8b88a30e67 --- /dev/null +++ b/realsense2_camera/test/live_camera/test_d415_basic_tests.py @@ -0,0 +1,150 @@ +# 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. + + +import os +import sys +import itertools + + +import pytest +import rclpy + +from sensor_msgs.msg import Image as msg_Image +from sensor_msgs.msg import Imu as msg_Imu +from sensor_msgs.msg import PointCloud2 as msg_PointCloud2 + +import numpy as np + +sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils")) +import pytest_rs_utils +from pytest_rs_utils import launch_descr_with_parameters +from pytest_rs_utils import get_rosbag_file_path +from pytest_rs_utils import get_node_heirarchy +import pytest_live_camera_utils + +from rclpy.parameter import Parameter +from rcl_interfaces.msg import ParameterValue +from rcl_interfaces.srv import SetParameters, GetParameters, ListParameters + +test_params_depth_avg_1 = { + 'camera_name': 'D415', + 'device_type': 'D415', + } +''' +This test was implemented as a template to set the parameters and run the test. +This directory is not added to the CMakeLists.txt so as to avoid the colcon failure in the +machines that don't have the D415 connected. +1. Only a subset of parameter types are implemented in py_rs_utils, it has to be extended for others +2. After setting the param, rclpy.spin_once may be needed.Test passes even without this though. +''' +@pytest.mark.parametrize("launch_descr_with_parameters", [test_params_depth_avg_1],indirect=True) +@pytest.mark.launch(fixture=launch_descr_with_parameters) +class TestD415_Change_Resolution(pytest_rs_utils.RsTestBaseClass): + def test_D415_Change_Resolution(self,launch_descr_with_parameters): + params = launch_descr_with_parameters[1] + failed_tests = [] + num_passed = 0 + num_failed = 0 + themes = [{'topic':get_node_heirarchy(params)+'/color/image_raw', 'msg_type':msg_Image,'expected_data_chunks':1}] + config = pytest_live_camera_utils.get_profile_config(get_node_heirarchy(params)) + config["Color"]["default_profile1"] = "640x480x6" + config["Color"]["default_profile2"] = "1280x720x6" + config["Depth"]["default_profile1"] = "640x480x6" + config["Depth"]["default_profile2"] = "1280x720x6" + config["Infrared"]["default_profile1"] = "640x480x6" + config["Infrared"]["default_profile2"] = "1280x720x6" + config["Infrared1"]["default_profile1"] = "640x480x6" + config["Infrared1"]["default_profile2"] = "1280x720x6" + config["Infrared2"]["default_profile1"] = "640x480x6" + config["Infrared2"]["default_profile2"] = "1280x720x6" + + cap = [ + #['Infrared1', '1920x1080x25', 'Y8'], + #['Infrared1', '1920x1080x15', 'Y16'], + ['Infrared', '848x100x100', 'BGRA8'], + ['Infrared', '848x480x60', 'RGBA8'], + ['Infrared', '640x480x60', 'RGBA8'], + ['Infrared', '640x480x60', 'BGR8'], + ['Infrared', '640x360x60', 'BGRA8'], + ['Infrared', '640x360x60', 'BGR8'], + ['Infrared', '640x360x30', 'UYVY'], + ['Infrared', '480x270x60', 'BGRA8'], + ['Infrared', '480x270x60', 'RGB8'], + ['Infrared', '424x240x60', 'BGRA8'], + + ] + try: + ''' + initialize, run and check the data + ''' + self.init_test("RsTest"+params['camera_name']) + self.spin_for_time(wait_time=1.0) + self.create_param_ifs(get_node_heirarchy(params)) + + for key in cap: + profile_type = key[0] + profile = key[1] + format = key[2] + print("Testing " + " ".join(key)) + themes[0]['topic'] = config[profile_type]['topic'] + themes[0]['width'] = int(profile.split('x')[0]) + themes[0]['height'] = int(profile.split('x')[1]) + #''' + if themes[0]['width'] == int(config[profile_type]["default_profile2"].split('x')[0]): + self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile1"]) + else: + self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile2"]) + self.set_bool_param(config[profile_type]["param"], True) + self.disable_all_params() + #self.set_string_param("depth_profile", "640x480x6") + #self.set_bool_param("enable_depth", True) + #''' + self.spin_for_time(wait_time=0.2) + self.set_string_param(config[profile_type]["profile"], profile) + self.set_string_param(config[profile_type]["format"], format) + self.set_bool_param(config[profile_type]["param"], True) + try: + ret = self.run_test(themes, timeout=5.0) + assert ret[0], ret[1] + assert self.process_data(themes), " ".join(key) + " failed" + num_passed += 1 + except Exception as e: + exc_type, exc_obj, exc_tb = sys.exc_info() + fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1] + print("Test failed") + print(e) + print(exc_type, fname, exc_tb.tb_lineno) + num_failed += 1 + failed_tests.append(" ".join(key)) + finally: + #this step is important because the test will fail next time + pytest_rs_utils.kill_realsense2_camera_node() + self.shutdown() + if num_failed != 0: + print("Failed tests:") + print("\n".join(failed_tests)) + assert False, " Tests failed" + + + def disable_all_params(self): + ''' + self.set_bool_param('enable_color', False) + self.set_bool_param('enable_depth', False) + self.set_bool_param('enable_infra', False) + self.set_bool_param('enable_infra1', False) + self.set_bool_param('enable_infra2', False) + ''' + pass + diff --git a/realsense2_camera/test/live_camera/test_d455_basic_tests.py b/realsense2_camera/test/live_camera/test_d455_basic_tests.py new file mode 100644 index 0000000000..fcf4561714 --- /dev/null +++ b/realsense2_camera/test/live_camera/test_d455_basic_tests.py @@ -0,0 +1,93 @@ +# 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. + + +import os +import sys +import itertools + + +import pytest +import rclpy + +from sensor_msgs.msg import Image as msg_Image +from sensor_msgs.msg import Imu as msg_Imu +from sensor_msgs.msg import PointCloud2 as msg_PointCloud2 + +import numpy as np + +sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils")) +import pytest_rs_utils +from pytest_rs_utils import launch_descr_with_parameters + +from pytest_rs_utils import get_rosbag_file_path +from pytest_rs_utils import get_node_heirarchy + +from rclpy.parameter import Parameter +from rcl_interfaces.msg import ParameterValue +from rcl_interfaces.srv import SetParameters, GetParameters, ListParameters + +test_params_depth_avg_1 = { + 'camera_name': 'D455', + 'device_type': 'D455', + } +''' +This test was implemented as a template to set the parameters and run the test. +This directory is not added to the CMakeLists.txt so as to avoid the colcon failure in the +machines that don't have the D455 connected. +1. Only a subset of parameter types are implemented in py_rs_utils, it has to be extended for others +2. After setting the param, rclpy.spin_once may be needed.Test passes even without this though. +''' +@pytest.mark.parametrize("launch_descr_with_parameters", [test_params_depth_avg_1],indirect=True) +@pytest.mark.launch(fixture=launch_descr_with_parameters) +class TestD455_Change_Resolution(pytest_rs_utils.RsTestBaseClass): + def test_D455_Change_Resolution(self,launch_descr_with_parameters): + params = launch_descr_with_parameters[1] + themes = [ + {'topic':get_node_heirarchy(params)+'/color/image_raw', + 'msg_type':msg_Image, + 'expected_data_chunks':1, + 'width':640, + 'height':480, + #'data':data + } + ] + try: + ''' + initialize, run and check the data + ''' + print("Starting camera test...") + self.init_test("RsTest"+params['camera_name']) + self.wait_for_node(params['camera_name']) + self.create_param_ifs(get_node_heirarchy(params)) + #assert self.set_bool_param('enable_color', False) + assert self.set_string_param('rgb_camera.profile', '640x480x30') + assert self.set_bool_param('enable_color', True) + ret = self.run_test(themes) + assert ret[0], ret[1] + assert self.process_data(themes) + self.set_string_param('rgb_camera.profile', '1280x800x5') + self.set_bool_param('enable_color', True) + themes[0]['width'] = 1280 + themes[0]['height'] = 800 + + ret = self.run_test(themes) + assert ret[0], ret[1] + assert self.process_data(themes) + finally: + #this step is important because the test will fail next time + pytest_rs_utils.kill_realsense2_camera_node() + self.shutdown() + + diff --git a/realsense2_camera/test/utils/pytest_live_camera_utils.py b/realsense2_camera/test/utils/pytest_live_camera_utils.py new file mode 100644 index 0000000000..3a5ab6f600 --- /dev/null +++ b/realsense2_camera/test/utils/pytest_live_camera_utils.py @@ -0,0 +1,190 @@ +# 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. +import os +import sys +import time +import ctypes +import struct +import requests +import json + +from pytest_rs_utils import debug_print + + +def get_profile_config(camera_name): + config = { + "Color":{"profile":"rgb_camera.profile", "format":'rgb_camera.color_format', "param":"enable_color", "topic":camera_name+'/color/image_raw',}, + "Depth":{"profile":"depth_module.profile", "format":'depth_module.depth_format', "param":"enable_depth", 'topic':camera_name+'/depth/image_rect_raw'}, + "Infrared":{"profile":"depth_module.profile", "format":'depth_module.infra_format', "param":"enable_infra", 'topic':camera_name+'/infra/image_rect_raw'}, + "Infrared1":{"profile":"depth_module.profile", "format":'depth_module.infra1_format',"param":"enable_infra1", 'topic':camera_name+'/infra/image_rect_raw'}, + "Infrared2":{"profile":"depth_module.profile", "format":'depth_module.infra2_format',"param":"enable_infra2", 'topic':camera_name+'/infra/image_rect_raw'}, + } + return config + + +def get_default_profiles(cap, profile): + profile1 = "" + profile2 = "" + for profiles in cap: + if profiles[0] == profile and int(profiles[1].split('x')[0]) != 640: + profile1 = profiles[1] + break + for profiles in cap: + if profiles[0] == profile and int(profiles[1].split('x')[0]) != int(profile1.split('x')[0]): + profile2 = profiles[1] + break + debug_print(profile + " default profile1:" + profile1) + debug_print(profile + " default profile2:" + profile2) + return profile1,profile2 + +def get_camera_capabilities_short(device_type, serial_no=None): + short_data = os.popen("rs-enumerate-devices -s").read().splitlines() + print(serial_no) + for line in short_data: + print(line) + if device_type in line: + if serial_no is None or serial_no == "" : + print(device_type+ " found in " + line) + return + if serial_no in line: + print(device_type + " with serial_no " + serial_no +" found in " + line) + return + print(device_type + " not found") + + +device_info_string = "Device info:" + +def get_device_info_location(long_data, index=0): + for line_no in range(index, len(long_data)): + if device_info_string in long_data[line_no]: + return line_no + return len(long_data) + +stream_profile_string = "Stream Profiles supported by" +def get_stream_profile_location(long_data, start_index, end_index, profile_string): + for line_no in range(start_index, end_index): + if stream_profile_string in long_data[line_no]: + if profile_string in long_data[line_no]: + return line_no + return None + +def get_depth_profiles(long_data, start_index, end_index): + cap = [] + for line_no in range(start_index, end_index): + if len(long_data[line_no]) == 0: + break + debug_print("depth profile processing:" + long_data[line_no]) + depth_profile = long_data[line_no].split() + if len(depth_profile) == 5: + profile = depth_profile[0] + value = depth_profile[1]+"x"+depth_profile[3] + format = depth_profile[4] + elif len(depth_profile) == 6: + profile = depth_profile[0]+depth_profile[1] + value = depth_profile[2]+"x"+depth_profile[4] + format = depth_profile[5] + else: + assert false, "Seems that the depth profile info format printed by rs-enumerate-devices changed" + value = value[:-2] + debug_print("depth profile added: " + profile, value, format) + cap.append([profile, value, format]) + debug_print(cap) + return cap + + +def get_color_profiles(long_data, start_index, end_index): + cap = [] + for line_no in range(start_index, end_index): + if len(long_data[line_no]) == 0: + break + debug_print("color profile processing:" + long_data[line_no]) + color_profile = long_data[line_no].split() + if len(color_profile) == 5: + profile = color_profile[0] + value = color_profile[1]+"x"+color_profile[3] + format = color_profile[4] + else: + assert false, "Seems that the color profile info format printed by rs-enumerate-devices changed" + value = value[:-2] + debug_print("color profile added: " + profile, value, format) + cap.append([profile, value, format]) + debug_print(cap) + return cap + +NAME_LINE_INDEX = 1 +NAME_LINE_NAME_OFFSET = 4 +SERIAL_NO_LINE_INDEX = 2 +SERIAL_NO_VALUE_OFFSET = 3 +def parse_device_info(long_data, start_index, end_index, device_type, serial_no): + #after device_info, the next line should have the name and device type + capability = {} + debug_print("Searching for data between lines ", str(start_index) + " and " + str(end_index)) + name_line = long_data[start_index+NAME_LINE_INDEX].split() + if name_line[0] != "Name": + assert False, "rs-enumerate-devices output format changed" + if name_line[4] != device_type: + debug_print("device not matching:" + name_line[NAME_LINE_NAME_OFFSET]) + return None + debug_print("device matched:" + name_line[NAME_LINE_NAME_OFFSET]) + if serial_no != None: + #next line after nameline should have the serial_no + serial_no_line = long_data[start_index+SERIAL_NO_LINE_INDEX].split() + if serial_no_line[0] != "Serial": + assert False, "rs-enumerate-devices output format changed" + if serial_no_line[SERIAL_NO_VALUE_OFFSET] != serial_no: + debug_print("serial_no not matching:" + serial_no_line[SERIAL_NO_VALUE_OFFSET]) + return None + debug_print("serial_no matched:" + serial_no_line[SERIAL_NO_VALUE_OFFSET]) + else: + serial_no = long_data[start_index+SERIAL_NO_LINE_INDEX].split()[SERIAL_NO_VALUE_OFFSET] + + capability["device_type"] = device_type + capability["serial_no"] = serial_no + depth_profile_index = get_stream_profile_location(long_data, start_index, end_index, "Stereo Module") + if depth_profile_index != None: + capability["depth_profile"] = get_depth_profiles(long_data, depth_profile_index+3, end_index) + rgb_profile_index = get_stream_profile_location(long_data, start_index, end_index, "RGB Camera") + if rgb_profile_index != None: + capability["color_profile"] = get_color_profiles(long_data, rgb_profile_index+3, end_index) + return capability + +def get_camera_capabilities(device_type, serial_no=None): + long_data = os.popen("rs-enumerate-devices").read().splitlines() + debug_print(serial_no) + index = 0 + while index < len(long_data): + index = get_device_info_location(long_data, index) + if index == len(long_data): + return + else: + debug_print("DeviceInfo found at: " + str(index)) + start_index = index + index += 1 + index = get_device_info_location(long_data, index) + capability = parse_device_info(long_data, start_index, index, device_type, serial_no) + if capability != None: + return capability + return None + +if __name__ == '__main__': + device_type = 'D455' + serial_no = None + if len(sys.argv) > 1: + device_type = sys.argv[1] + if len(sys.argv) > 2: + serial_no = sys.argv[2] + + cap = get_camera_capabilities(device_type, serial_no) + print("Capabilities:") + print(cap) \ No newline at end of file diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index bdd1bdba1a..6823e6aa12 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/realsense2_camera/test/utils/pytest_rs_utils.py @@ -15,6 +15,7 @@ import sys import time from collections import deque +import functools import pytest import numpy as np @@ -28,10 +29,31 @@ from rclpy import qos from rclpy.node import Node from rclpy.utilities import ok +from ros2topic.verb.bw import ROSTopicBandwidth +from ros2topic.verb.hz import ROSTopicHz +from ros2topic.api import get_msg_class import ctypes import struct import requests +import math + +#from rclpy.parameter import Parameter +from rcl_interfaces.msg import Parameter +from rcl_interfaces.msg import ParameterValue +from rcl_interfaces.srv import SetParameters, GetParameters, ListParameters +''' +humble doesn't have the SetParametersResult and SetParameters_Response imported using +__init__.py. The below two lines can be used for iron and hopefully succeeding ROS2 versions +''' +#from rcl_interfaces.msg import SetParametersResult +#from rcl_interfaces.srv import SetParameters_Response +from rcl_interfaces.msg._set_parameters_result import SetParametersResult +from rcl_interfaces.srv._set_parameters import SetParameters_Response + +from rcl_interfaces.msg import ParameterType +from rcl_interfaces.msg import ParameterValue + from sensor_msgs.msg import Image as msg_Image @@ -54,6 +76,11 @@ import tempfile import os import requests + +def debug_print(*args): + if(True): + print(*args) + class RosbagManager(object): def __new__(cls): if not hasattr(cls, 'instance'): @@ -82,12 +109,13 @@ def init(self): def get_rosbag_path(self, filename): if filename in self.rosbag_files: return self.rosbag_location + "/" + filename -rosbagMgr = RosbagManager() + def get_rosbag_file_path(filename): + rosbagMgr = RosbagManager() path = rosbagMgr.get_rosbag_path(filename) assert path, "No rosbag file found :"+filename return path - +get_rosbag_file_path.rosbagMgr = None def CameraInfoGetData(rec_filename, topic): data = importRosbag(rec_filename, importTopics=[topic], log='ERROR', disable_bar=True)[topic] @@ -552,6 +580,16 @@ def delayed_launch_descr_with_parameters(request): actions = [ first_node,], period=period) ]),params +class HzWrapper(ROSTopicHz): + def _callback_hz(self, m, topic=None): + if self.get_last_printed_tn(topic=topic) == 0: + self.set_last_printed_tn(self.get_msg_tn(topic=topic), topic=topic) + return self.callback_hz(m, topic) + def restart_topic(self, topic): + self._last_printed_tn[topic] = 0 + self._times[topic].clear() + self._msg_tn[topic] = 0 + self._msg_t0[topic] = -1 ''' This is that holds the test node that listens to a subscription created by a test. @@ -564,6 +602,7 @@ def __init__(self, name='test_node'): self.data = {} self.tfBuffer = None self.frame_counter = {} + self._ros_topic_hz = HzWrapper(super(), 10000, use_wtime=False) def wait_for_node(self, node_name, timeout=8.0): start = time.time() @@ -576,11 +615,22 @@ def wait_for_node(self, node_name, timeout=8.0): return True, "" time.sleep(timeout/5) return False, "Timed out waiting for "+ str(timeout)+ "seconds" - def create_subscription(self, msg_type, topic , data_type, store_raw_data): - super().create_subscription(msg_type, topic , self.rsCallback(topic,msg_type, store_raw_data), data_type) + def reset_data(self, topic): self.data[topic] = deque() self.frame_counter[topic] = 0 - if (self.tfBuffer == None): + self._ros_topic_hz.restart_topic(topic) + + + def create_subscription(self, msg_type, topic , data_type, store_raw_data, measure_hz): + self.reset_data(topic) + super().create_subscription(msg_type, topic , self.rsCallback(topic,msg_type, store_raw_data), data_type) + #hz measurements are not working + if measure_hz == True: + msg_class = get_msg_class(super(), topic, blocking=True, include_hidden_topics=True) + super().create_subscription(msg_class,topic,functools.partial(self._ros_topic_hz._callback_hz, topic=topic),data_type) + self._ros_topic_hz.set_last_printed_tn(0, topic=topic) + + if self.tfBuffer == None: self.tfBuffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tfBuffer, super()) @@ -592,14 +642,14 @@ def pop_first_chunk(self, topic): def image_msg_to_numpy(self, data): fmtString = data.encoding - if fmtString in ['mono8', '8UC1', 'bgr8', 'rgb8', 'bgra8', 'rgba8']: + if fmtString in ['mono8', '8UC1', 'bgr8', 'rgb8', 'bgra8', 'rgba8', 'yuv422_yuy2', 'yuv422']: img = np.frombuffer(data.data, np.uint8) elif fmtString in ['mono16', '16UC1', '16SC1']: img = np.frombuffer(data.data, np.uint16) elif fmtString == '32FC1': img = np.frombuffer(data.data, np.float32) else: - print('image format not supported:' + fmtString) + print('py_rs_utils.image_msg_to_numpy:image format not supported:' + fmtString) return None depth = data.step / (data.width * img.dtype.itemsize) @@ -613,9 +663,9 @@ def image_msg_to_numpy(self, data): The processing of data is taken from the existing testcase in scripts rs2_test ''' def rsCallback(self, topic, msg_type, store_raw_data): - print("RSCallback") + debug_print("RSCallback") def _rsCallback(data): - print('Got the callback for ' + topic) + debug_print('Got the callback for ' + topic) #print(data.header) self.flag = True if store_raw_data == True: @@ -707,23 +757,71 @@ def init_test(self,name='RsTestNode'): self.flag = False self.node = RsTestNode(name) self.subscribed_topics = [] - def create_subscription(self, msg_type, topic, data_type, store_raw_data=False): + + def wait_for_node(self, node_name, timeout=8.0): + self.node.wait_for_node(node_name, timeout) + def create_subscription(self, msg_type, topic, data_type, store_raw_data=False, measure_hz=False): if not topic in self.subscribed_topics: - self.node.create_subscription(msg_type, topic, data_type, store_raw_data) + self.node.create_subscription(msg_type, topic, data_type, store_raw_data, measure_hz) self.subscribed_topics.append(topic) + else: + self.node.reset_data(topic) + + - def spin_for_data(self,themes): + def create_param_ifs(self, camera_name): + + self.set_param_if = self.node.create_client(SetParameters, camera_name + '/set_parameters') + self.get_param_if = self.node.create_client(GetParameters, camera_name + '/get_parameters') + while not self.get_param_if.wait_for_service(timeout_sec=1.0): + print('service not available, waiting again...') + while not self.set_param_if.wait_for_service(timeout_sec=1.0): + print('service not available, waiting again...') + + def send_param(self, req): + future = self.set_param_if.call_async(req) + while rclpy.ok(): + rclpy.spin_once(self.node) + if future.done(): + try: + response = future.result() + if response.results[0].successful: + return True + except Exception as e: + print("exception raised:") + print(e) + pass + return False + + def set_string_param(self, param_name, param_value): + req = SetParameters.Request() + new_param_value = ParameterValue(type=ParameterType.PARAMETER_STRING, string_value=param_value) + req.parameters = [Parameter(name=param_name, value=new_param_value)] + return self.send_param(req) + + def set_bool_param(self, param_name, param_value): + req = SetParameters.Request() + new_param_value = ParameterValue(type=ParameterType.PARAMETER_BOOL, bool_value=param_value) + req.parameters = [Parameter(name=param_name, value=new_param_value)] + return self.send_param(req) + + def set_integer_param(self, param_name, param_value): + req = SetParameters.Request() + new_param_value = ParameterValue(type=ParameterType.PARAMETER_INTEGER, integer_value=param_value) + req.parameters = [Parameter(name=param_name, value=new_param_value)] + return self.send_param(req) + + def spin_for_data(self,themes, timeout=5.0): start = time.time() ''' timeout value varies depending upon the system, it needs to be more if the access is over the network ''' - timeout = 25.0 print('Waiting for topic... ' ) flag = False while (time.time() - start) < timeout: rclpy.spin_once(self.node, timeout_sec=1) - print('Spun once... ' ) + debug_print('Spun once... ' ) all_found = True for theme in themes: if theme['expected_data_chunks'] > int(self.node.get_num_chunks(theme['topic'])): @@ -739,21 +837,28 @@ def spin_for_data(self,themes): def spin_for_time(self,wait_time): start = time.time() - print('Waiting for topic... ' ) + print('Waiting for time... ' ) flag = False - while time.time() - start < wait_time: - rclpy.spin_once(self.node) - print('Spun once... ' ) + while (time.time() - start) < wait_time: + print('Spun for time once... ' ) + rclpy.spin_once(self.node, timeout_sec=wait_time) - def run_test(self, themes): + def run_test(self, themes, initial_wait_time=0.0, timeout=5.0): try: for theme in themes: store_raw_data = False if 'store_raw_data' in theme: store_raw_data = theme['store_raw_data'] - self.create_subscription(theme['msg_type'], theme['topic'] , qos.qos_profile_sensor_data,store_raw_data) + if 'expected_fps_in_hz' in theme: + measure_hz = True + else: + measure_hz = False + + self.create_subscription(theme['msg_type'], theme['topic'] , qos.qos_profile_sensor_data,store_raw_data, measure_hz) print('subscription created for ' + theme['topic']) - self.flag = self.spin_for_data(themes) + if initial_wait_time != 0.0: + self.spin_for_time(initial_wait_time) + self.flag = self.spin_for_data(themes, timeout) except Exception as e: exc_type, exc_obj, exc_tb = sys.exc_info() fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1] @@ -773,11 +878,25 @@ def run_test(self, themes): ''' def process_data(self, themes): for theme in themes: + if theme['expected_data_chunks'] == 0: + assert self.node.get_num_chunks(theme['topic']) == 0, "Received data, when not expected for topic:" + theme['topic'] + continue #no more checks needed if data is not available + + if 'expected_fps_in_hz' in theme: + hz = self.node._ros_topic_hz.get_hz(theme['topic']) + if hz == None: + print("Couldn't measure fps, no of data frames expected are enough for the measurement?") + else: + speed= 1e9*hz[0] + msg = "FPS in Hz of topic " + theme['topic'] + " is " + str(speed) + ". Expected is " + str(theme['expected_fps_in_hz']) + print(msg) + if (abs(theme['expected_fps_in_hz']-speed) > theme['expected_fps_in_hz']/10): + assert False,msg data = self.node.pop_first_chunk(theme['topic']) if 'width' in theme: - assert theme['width'] == data['shape'][0][1] # (get from numpy image the width) + assert theme['width'] == data['shape'][0][1], "Width not matched. Expected:" + str(theme['width']) + " & got: " + str(data['shape'][0][1]) # (get from numpy image the width) if 'height' in theme: - assert theme['height'] == data['shape'][0][0] # (get from numpy image the height) + assert theme['height'] == data['shape'][0][0], "Height not matched. Expected:" + str(theme['height']) + " & got: " + str(data['shape'][0][0]) # (get from numpy image the height) if 'data' not in theme: print('No data to compare for ' + theme['topic']) #print(data) From dc12788064c531043d6d2aebeea980a77454f3c8 Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Thu, 31 Aug 2023 17:47:07 +0530 Subject: [PATCH 48/76] Updated readme and imu tests --- realsense2_camera/test/README.md | 20 +++++++-- .../test/live_camera/test_camera_imu_tests.py | 41 ++++++++----------- .../test/utils/pytest_rs_utils.py | 8 +++- 3 files changed, 39 insertions(+), 30 deletions(-) diff --git a/realsense2_camera/test/README.md b/realsense2_camera/test/README.md index 0be7600c24..a026a36b2e 100644 --- a/realsense2_camera/test/README.md +++ b/realsense2_camera/test/README.md @@ -146,7 +146,21 @@ The below command finds the test with the name test_static_tf_1 in realsense2_ca pytest-3 -s -k test_static_tf_1 realsense2_camera/test/ -### Points to be noted while writing pytests -The tests that are in one file are nromally run in parallel. So if there are multiple tests in one file, the system capacity can influence the test execution. It's recomended to have 3-4 tests in file, more than that can affect the test results due to delays. - +## Points to be noted while writing pytests +The tests that are in one file are normally run in parallel. So if there are multiple tests in one file, the system capacity can influence the test execution. It's recomended to have 3-4 tests in file, more than that can affect the test results due to delays. +### Passing/changing parameters +The parameters passed while creating the node can be initialized individually for each test, please see the test_parameterized_template example for reference. The default values are taken from rs_launch.py and the passed parameters are used for overriding the default values. The parameters that can be dynamically modified can be changed using the param interface provided. However, the function create_param_ifs has to be called to create this interface. Please see the test_d455_basic_tests.py for reference. There are specific functions to change the string, integer and bool parameters, the utils can be extended if any more types are needed. +### Difference in setting the bool parameters +There is a difference between setting the default values of bool parameters and setting them dynamically. +The bool test params are assinged withn quotes as below. + test_params_all_profiles_d455 = { + 'camera_name': 'D455', + 'device_type': 'D455', + 'enable_accel':"True", + 'enable_gyro':"True", + 'unite_imu_method':1, + } + +However the function that implements the setting of bool parameter dynamically takes the python bool datatype. For example: + self.set_bool_param('enable_accel', False) diff --git a/realsense2_camera/test/live_camera/test_camera_imu_tests.py b/realsense2_camera/test/live_camera/test_camera_imu_tests.py index fa173e0a8e..c712cd1ba7 100644 --- a/realsense2_camera/test/live_camera/test_camera_imu_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_imu_tests.py @@ -43,8 +43,8 @@ test_params_all_profiles_d455 = { 'camera_name': 'D455', 'device_type': 'D455', - 'enable_accel':True, - 'enable_gyro':True, + 'enable_accel':"True", + 'enable_gyro':"True", 'unite_imu_method':1, } @@ -64,62 +64,53 @@ def test_LiveCamera_check_motion_sensor(self,launch_descr_with_parameters): if params['unite_imu_method'] == '0': themes[IMU_TOPIC]['expected_data_chunks'] = 0 try: - ''' - initialize, run and check the data - ''' - msg = "Test with the default params " + #initialize self.init_test("RsTest"+params['camera_name']) self.create_param_ifs(get_node_heirarchy(params)) + + + #run with default params and check the data + msg = "Test with the default params " print(msg) - ret = self.run_test(themes) + ret = self.run_test(themes, timeout=50) assert ret[0], msg + str(ret[1]) assert self.process_data(themes), msg + " failed" - msg = "Test with the accel false " self.set_bool_param('enable_accel', False) self.set_bool_param('enable_gyro', True) - ''' - This step fails if the next line is commented - ''' - self.set_integer_param('unite_imu_method', 0) #this shouldn't matter because the unite_imu_method cannot be changed themes[ACCEL_TOPIC]['expected_data_chunks'] = 0 + #uncomment once RSDEV-550 is closed + #themes[IMU_TOPIC]['expected_data_chunks'] = 0 print(msg) ret = self.run_test(themes) assert ret[0], msg + str(ret[1]) assert self.process_data(themes), msg + " failed" - ''' - Test fails - ''' msg = "Test with the gyro false " self.set_bool_param('enable_accel', True) self.set_bool_param('enable_gyro', False) - self.set_integer_param('unite_imu_method', 0) #this shouldn't matter because the unite_imu_method cannot be changed - themes[IMU_TOPIC]['expected_data_chunks'] = 1 + themes[IMU_TOPIC]['expected_data_chunks'] = 0 themes[ACCEL_TOPIC]['expected_data_chunks'] = 1 themes[GYRO_TOPIC]['expected_data_chunks'] = 0 print(msg) + self.spin_for_time(wait_time=1.0) ret = self.run_test(themes) assert ret[0], msg + str(ret[1]) assert self.process_data(themes), msg + " failed" - ''' - Test fails, both gyro and accel data is available, not imu - ''' msg = "Test with both gyro and accel false " self.set_bool_param('enable_accel', False) self.set_bool_param('enable_gyro', False) - self.set_integer_param('unite_imu_method', 1) #this shouldn't matter because the unite_imu_method cannot be changed - + self.set_integer_param('unite_imu_method', 1) + self.spin_for_time(wait_time=1.0) themes[ACCEL_TOPIC]['expected_data_chunks'] = 0 themes[GYRO_TOPIC]['expected_data_chunks'] = 0 - themes[IMU_TOPIC]['expected_data_chunks'] = 1 #Seems that imu data is available even if gyro and accel is disabled + themes[IMU_TOPIC]['expected_data_chunks'] = 0 print(msg) - ret = self.run_test(themes, initial_wait_time=1.0) #wait_time added as test doesn't have to wait for any data + ret = self.run_test(themes, initial_wait_time=1.0) assert ret[0], msg + " failed" assert self.process_data(themes), msg +" failed" - finally: #this step is important because the test will fail next time pytest_rs_utils.kill_realsense2_camera_node() diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index 6823e6aa12..f2bd195cb1 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/realsense2_camera/test/utils/pytest_rs_utils.py @@ -819,9 +819,12 @@ def spin_for_data(self,themes, timeout=5.0): ''' print('Waiting for topic... ' ) flag = False + data_not_expected = [i for i in themes if (i["expected_data_chunks"]) == 0] while (time.time() - start) < timeout: rclpy.spin_once(self.node, timeout_sec=1) debug_print('Spun once... ' ) + if data_not_expected == True: + continue all_found = True for theme in themes: if theme['expected_data_chunks'] > int(self.node.get_num_chunks(theme['topic'])): @@ -831,8 +834,9 @@ def spin_for_data(self,themes, timeout=5.0): flag =True break else: - print("Timed out waiting for", timeout, "seconds" ) - return False, "run_test timedout" + if data_not_expected == False: + print("Timed out waiting for", timeout, "seconds" ) + return False, "run_test timedout" return flag,"" def spin_for_time(self,wait_time): From 6d34932332894ddb03166e52f685ffbc50ddff02 Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Fri, 1 Sep 2023 18:46:04 +0530 Subject: [PATCH 49/76] static_tf fix --- .../test/rosbag/test_rosbag_depth_tests.py | 43 ++++++++----------- .../test/utils/pytest_rs_utils.py | 32 +++++++++++--- 2 files changed, 43 insertions(+), 32 deletions(-) diff --git a/realsense2_camera/test/rosbag/test_rosbag_depth_tests.py b/realsense2_camera/test/rosbag/test_rosbag_depth_tests.py index f393f42709..1b628e07f6 100644 --- a/realsense2_camera/test/rosbag/test_rosbag_depth_tests.py +++ b/realsense2_camera/test/rosbag/test_rosbag_depth_tests.py @@ -100,12 +100,10 @@ def process_data(self, themes): test_params_static_tf_1 = {"rosbag_filename":get_rosbag_file_path("outdoors_1color.bag"), - 'camera_name': 'Static_tf_1', + 'camera_name': 'Static_tf1', 'color_width': '0', 'color_height': '0', - 'depth_width': '0', - 'depth_height': '0', - 'infra_width': '0', + "static_tf":True, 'infra_height': '0', 'enable_infra1':'true', 'enable_infra2':'true' @@ -119,43 +117,38 @@ def process_data(self, themes): @pytest.mark.launch(fixture=delayed_launch_descr_with_parameters) class TestStaticTf1(pytest_rs_utils.RsTestBaseClass): def test_static_tf_1(self,delayed_launch_descr_with_parameters): - params = delayed_launch_descr_with_parameters[1] - self.rosbag = params["rosbag_filename"] - data = {('camera_link', 'camera_color_frame'): ([-0.00010158783697988838, 0.014841210097074509, -0.00022671300393994898], [-0.0008337442995980382, 0.0010442184284329414, -0.0009920650627464056, 0.9999986290931702]), - ('camera_link', 'camera_depth_frame'): ([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]), - ('camera_link', 'camera_infra1_frame'): ([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]), - ('camera_depth_frame', 'camera_infra1_frame'): ([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]), - ('camera_depth_frame', 'camera_color_frame'): ([-0.00010158783697988838, 0.014841210097074509, -0.00022671300393994898], [-0.0008337442995980382, 0.0010442184284329414, -0.0009920650627464056, 0.9999986290931702]), - ('camera_infra1_frame', 'camera_color_frame'): ([-0.00010158783697988838, 0.014841210097074509, -0.00022671300393994898], [-0.0008337442995980382, 0.0010442184284329414, -0.0009920650627464056, 0.9999986290931702])} + self.params = delayed_launch_descr_with_parameters[1] + self.rosbag = self.params["rosbag_filename"] themes = [ - {'topic':get_node_heirarchy(params)+'/color/image_raw', + {'topic':get_node_heirarchy(self.params)+'/color/image_raw', 'msg_type':msg_Image, 'expected_data_chunks':1, - 'data':data, + 'static_tf':True, } ] try: ''' initialize, run and check the data ''' - self.init_test("RsTest"+params['camera_name']) + self.init_test("RsTest"+self.params['camera_name']) ret = self.run_test(themes) assert ret[0], ret[1] assert self.process_data(themes) finally: self.shutdown() def process_data(self, themes): - #print ('Gathering static transforms') - frame_ids = ['camera_link', 'camera_depth_frame', 'camera_infra1_frame', 'camera_infra2_frame', 'camera_color_frame'] + expected_data = {(self.params['camera_name']+'_link', self.params['camera_name']+'_color_frame'): ([-0.00010158783697988838, 0.014841210097074509, -0.00022671300393994898], [-0.0008337442995980382, 0.0010442184284329414, -0.0009920650627464056, 0.9999986290931702]), + (self.params['camera_name']+'_link', self.params['camera_name']+'_depth_frame'): ([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]), + (self.params['camera_name']+'_link', self.params['camera_name']+'_infra1_frame'): ([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]), + (self.params['camera_name']+'_depth_frame', self.params['camera_name']+'_infra1_frame'): ([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]), + (self.params['camera_name']+'_depth_frame', self.params['camera_name']+'_color_frame'): ([-0.00010158783697988838, 0.014841210097074509, -0.00022671300393994898], [-0.0008337442995980382, 0.0010442184284329414, -0.0009920650627464056, 0.9999986290931702]), + (self.params['camera_name']+'_infra1_frame', self.params['camera_name']+'_color_frame'): ([-0.00010158783697988838, 0.014841210097074509, -0.00022671300393994898], [-0.0008337442995980382, 0.0010442184284329414, -0.0009920650627464056, 0.9999986290931702])} + frame_ids = [self.params['camera_name']+'_link', self.params['camera_name']+'_depth_frame', self.params['camera_name']+'_infra1_frame', self.params['camera_name']+'_infra2_frame', self.params['camera_name']+'_color_frame', self.params['camera_name']+'_fisheye_frame', self.params['camera_name']+'_pose'] coupled_frame_ids = [xx for xx in itertools.combinations(frame_ids, 2)] - res = {} - for couple in coupled_frame_ids: - from_id, to_id = couple - if (self.node.tfBuffer.can_transform(from_id, to_id, rclpy.time.Time(), rclpy.time.Duration(nanoseconds=3e6))): - res[couple] = self.node.tfBuffer.lookup_transform(from_id, to_id, rclpy.time.Time(), rclpy.time.Duration(nanoseconds=1e6)).transform - else: - res[couple] = None - return pytest_rs_utils.staticTFTest(res, themes[0]["data"]) + tfs_data = self.get_tfs(coupled_frame_ids) + ret = pytest_rs_utils.staticTFTest(tfs_data, expected_data) + assert ret[0], ret[1] + return ret[0] test_params_non_existing_rosbag = {"rosbag_filename":"non_existent.bag", diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index f2bd195cb1..9fdf8412c4 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/realsense2_camera/test/utils/pytest_rs_utils.py @@ -16,8 +16,11 @@ import time from collections import deque import functools +import itertools + import pytest + import numpy as np from launch import LaunchDescription @@ -620,8 +623,7 @@ def reset_data(self, topic): self.frame_counter[topic] = 0 self._ros_topic_hz.restart_topic(topic) - - def create_subscription(self, msg_type, topic , data_type, store_raw_data, measure_hz): + def create_subscription(self, msg_type, topic , data_type, store_raw_data, measure_hz, static_tf): self.reset_data(topic) super().create_subscription(msg_type, topic , self.rsCallback(topic,msg_type, store_raw_data), data_type) #hz measurements are not working @@ -633,6 +635,15 @@ def create_subscription(self, msg_type, topic , data_type, store_raw_data, measu if self.tfBuffer == None: self.tfBuffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tfBuffer, super()) + def get_tfs(self, coupled_frame_ids): + res = dict() + for couple in coupled_frame_ids: + from_id, to_id = couple + if (self.tfBuffer.can_transform(from_id, to_id, rclpy.time.Time(), rclpy.time.Duration(nanoseconds=3e6))): + res[couple] = self.tfBuffer.lookup_transform(from_id, to_id, rclpy.time.Time(), rclpy.time.Duration(nanoseconds=1e6)).transform + else: + res[couple] = None + return res def get_num_chunks(self,topic): return len(self.data[topic]) @@ -760,9 +771,9 @@ def init_test(self,name='RsTestNode'): def wait_for_node(self, node_name, timeout=8.0): self.node.wait_for_node(node_name, timeout) - def create_subscription(self, msg_type, topic, data_type, store_raw_data=False, measure_hz=False): + def create_subscription(self, msg_type, topic, data_type, store_raw_data=False, measure_hz=False, static_tf=False): if not topic in self.subscribed_topics: - self.node.create_subscription(msg_type, topic, data_type, store_raw_data, measure_hz) + self.node.create_subscription(msg_type, topic, data_type, store_raw_data, measure_hz, static_tf) self.subscribed_topics.append(topic) else: self.node.reset_data(topic) @@ -770,7 +781,6 @@ def create_subscription(self, msg_type, topic, data_type, store_raw_data=False, def create_param_ifs(self, camera_name): - self.set_param_if = self.node.create_client(SetParameters, camera_name + '/set_parameters') self.get_param_if = self.node.create_client(GetParameters, camera_name + '/get_parameters') while not self.get_param_if.wait_for_service(timeout_sec=1.0): @@ -849,6 +859,7 @@ def spin_for_time(self,wait_time): def run_test(self, themes, initial_wait_time=0.0, timeout=5.0): try: + static_tf_found = False for theme in themes: store_raw_data = False if 'store_raw_data' in theme: @@ -857,12 +868,17 @@ def run_test(self, themes, initial_wait_time=0.0, timeout=5.0): measure_hz = True else: measure_hz = False + if 'static_tf' in theme: + static_tf = True + static_tf_found = True + else: + static_tf = False - self.create_subscription(theme['msg_type'], theme['topic'] , qos.qos_profile_sensor_data,store_raw_data, measure_hz) + self.create_subscription(theme['msg_type'], theme['topic'] , qos.qos_profile_sensor_data,store_raw_data, measure_hz, static_tf) print('subscription created for ' + theme['topic']) if initial_wait_time != 0.0: self.spin_for_time(initial_wait_time) - self.flag = self.spin_for_data(themes, timeout) + self.flag = self.spin_for_data(themes, timeout) except Exception as e: exc_type, exc_obj, exc_tb = sys.exc_info() fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1] @@ -875,6 +891,8 @@ def run_test(self, themes, initial_wait_time=0.0, timeout=5.0): self.flag =False,e return self.flag + def get_tfs(self, coupled_frame_ids): + return self.node.get_tfs(coupled_frame_ids) ''' Please override and use your own process_data if the default check is not suitable. Please also store_raw_data parameter in the themes as True, if you want the From 295f08dc138feb2490b8975e9855b27fcc73e6f7 Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Tue, 5 Sep 2023 19:30:17 +0530 Subject: [PATCH 50/76] Added tf and tf_static tests --- .../test/live_camera/test_camera_fps_tests.py | 4 +- .../test/live_camera/test_camera_tf_tests.py | 219 ++++++++++++++++++ .../test/utils/pytest_live_camera_utils.py | 17 ++ .../test/utils/pytest_rs_utils.py | 42 +++- 4 files changed, 268 insertions(+), 14 deletions(-) create mode 100644 realsense2_camera/test/live_camera/test_camera_tf_tests.py diff --git a/realsense2_camera/test/live_camera/test_camera_fps_tests.py b/realsense2_camera/test/live_camera/test_camera_fps_tests.py index cff5cadbb4..291e4eaf8c 100644 --- a/realsense2_camera/test/live_camera/test_camera_fps_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_fps_tests.py @@ -38,7 +38,7 @@ from rcl_interfaces.msg import ParameterValue from rcl_interfaces.srv import SetParameters, GetParameters, ListParameters -test_params_depth_avg_1 = { +test_params_test_fps = { 'camera_name': 'D455', 'device_type': 'D455', } @@ -47,7 +47,7 @@ modified to make it work, see py_rs_utils for more details. To check the fps, a value 'expected_fps_in_hz' has to be added to the corresponding theme ''' -@pytest.mark.parametrize("launch_descr_with_parameters", [test_params_depth_avg_1],indirect=True) +@pytest.mark.parametrize("launch_descr_with_parameters", [test_params_test_fps],indirect=True) @pytest.mark.launch(fixture=launch_descr_with_parameters) class TestCamera_TestFPS(pytest_rs_utils.RsTestBaseClass): def test_camera_test_fps(self,launch_descr_with_parameters): diff --git a/realsense2_camera/test/live_camera/test_camera_tf_tests.py b/realsense2_camera/test/live_camera/test_camera_tf_tests.py new file mode 100644 index 0000000000..10c72c8b09 --- /dev/null +++ b/realsense2_camera/test/live_camera/test_camera_tf_tests.py @@ -0,0 +1,219 @@ +# 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. + + +import os +import sys +import itertools + + +import pytest +import rclpy + +from rclpy.qos import DurabilityPolicy +from rclpy.qos import HistoryPolicy +from rclpy.qos import QoSProfile +import tf2_ros + + +from sensor_msgs.msg import Image as msg_Image +from sensor_msgs.msg import Imu as msg_Imu +from sensor_msgs.msg import PointCloud2 as msg_PointCloud2 +from tf2_msgs.msg import TFMessage as msg_TFMessage + +import numpy as np + +sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils")) +import pytest_rs_utils +from pytest_rs_utils import launch_descr_with_parameters + +import pytest_live_camera_utils +from pytest_rs_utils import get_rosbag_file_path +from pytest_rs_utils import get_node_heirarchy + +from rclpy.parameter import Parameter +from rcl_interfaces.msg import ParameterValue +from rcl_interfaces.srv import SetParameters, GetParameters, ListParameters + +''' +The test was implemented to check the tf_static before and after infra1 was enabled. There shouldn't be any +related data before enabling. +''' +test_params_tf_static_change_d455 = { + 'camera_name': 'D455', + 'device_type': 'D455', + 'enable_infra1': 'false', + 'enable_infra2': 'true', + 'enable_accel': 'true', + 'enable_gyro': 'true', + } +test_params_tf_static_change_d435 = { + 'camera_name': 'D435', + 'device_type': 'D435', + 'enable_infra1': 'false', + 'enable_infra2': 'true', + 'enable_accel': 'true', + 'enable_gyro': 'true', + } + +test_params_tf_static_change_d415 = { + 'camera_name': 'D415', + 'device_type': 'D415', + 'enable_infra1': 'false', + 'enable_infra2': 'true', + 'enable_accel': 'true', + 'enable_gyro': 'true', + } +@pytest.mark.parametrize("launch_descr_with_parameters", [ + pytest.param(test_params_tf_static_change_d455, marks=pytest.mark.d455), + pytest.param(test_params_tf_static_change_d435, marks=pytest.mark.d435), + pytest.param(test_params_tf_static_change_d415, marks=pytest.mark.d415), + ],indirect=True) +@pytest.mark.launch(fixture=launch_descr_with_parameters) +class TestCamera_TestTF_Static_change(pytest_rs_utils.RsTestBaseClass): + def test_camera_test_tf_static_change(self,launch_descr_with_parameters): + self.params = launch_descr_with_parameters[1] + if pytest_live_camera_utils.check_if_camera_connected(self.params['device_type']) == False: + print("Device not found? : " + self.params['device_type']) + return + themes = [ + {'topic':'/tf_static', + 'msg_type':msg_TFMessage, + 'expected_data_chunks':1, + 'qos' : QoSProfile(depth=100,durability=DurabilityPolicy.TRANSIENT_LOCAL,history=HistoryPolicy.KEEP_LAST,)#static + } + ] + try: + ''' + initialize, run and check the data + ''' + self.init_test("RsTest"+self.params['camera_name']) + self.wait_for_node(self.params['camera_name']) + self.create_param_ifs(get_node_heirarchy(self.params)) + ret = self.run_test(themes, timeout=10) + assert ret[0], ret[1] + frame_ids = [self.params['camera_name']+'_link', + self.params['camera_name']+'_depth_frame', + self.params['camera_name']+'_infra1_frame', + self.params['camera_name']+'_infra2_frame', + self.params['camera_name']+'_color_frame'] + if self.params['device_type'] == 'D455': + frame_ids.append(self.params['camera_name']+'_gyro_frame') + frame_ids.append(self.params['camera_name']+'_accel_frame') + + ret = self.process_data(themes, False) + assert ret[0], ret[1] + assert self.set_bool_param('enable_infra1', True) + + ret = self.run_test(themes, timeout=10) + assert ret[0], ret[1] + ret = self.process_data(themes, True) + assert ret[0], ret[1] + finally: + self.shutdown() + def process_data(self, themes, enable_infra1): + frame_ids = [self.params['camera_name']+'_link', + self.params['camera_name']+'_depth_frame', + self.params['camera_name']+'_infra2_frame', + self.params['camera_name']+'_color_frame'] + if self.params['device_type'] == 'D455': + frame_ids.append(self.params['camera_name']+'_gyro_frame') + frame_ids.append(self.params['camera_name']+'_accel_frame') + if enable_infra1: + frame_ids.append(self.params['camera_name']+'_infra1_frame') + + data = self.node.pop_first_chunk('/tf_static') + ret = self.check_transform_data(data, frame_ids, True) + return ret + + +test_params_tf_d455 = { + 'camera_name': 'D455', + 'device_type': 'D455', + 'publish_tf': 'true', + 'tf_publish_rate': '1.1', + } + +test_params_tf_d435 = { + 'camera_name': 'D435', + 'device_type': 'D435', + 'publish_tf': 'true', + 'tf_publish_rate': '1.1', + } + +test_params_tf_d415 = { + 'camera_name': 'D415', + 'device_type': 'D415', + 'publish_tf': 'true', + 'tf_publish_rate': '1.1', + } +@pytest.mark.parametrize("launch_descr_with_parameters", [ + pytest.param(test_params_tf_d455, marks=pytest.mark.d455), + pytest.param(test_params_tf_d435, marks=pytest.mark.d415), + pytest.param(test_params_tf_d415, marks=pytest.mark.d415), + ],indirect=True) +@pytest.mark.launch(fixture=launch_descr_with_parameters) +class TestCamera_TestTF_DYN(pytest_rs_utils.RsTestBaseClass): + def test_camera_test_tf_dyn(self,launch_descr_with_parameters): + self.params = launch_descr_with_parameters[1] + if pytest_live_camera_utils.check_if_camera_connected(self.params['device_type']) == False: + print("Device not found? : " + self.params['device_type']) + return + themes = [ + {'topic':'/tf', + 'msg_type':msg_TFMessage, + 'expected_data_chunks':3, + 'qos' : QoSProfile(depth=100,durability=DurabilityPolicy.VOLATILE,history=HistoryPolicy.KEEP_LAST,) + #'qos' : QoSProfile(depth=100,durability=DurabilityPolicy.TRANSIENT_LOCAL,history=HistoryPolicy.KEEP_LAST,)#static + }, + {'topic':'/tf_static', + 'msg_type':msg_TFMessage, + 'expected_data_chunks':1, + #'qos' : QoSProfile(depth=100,durability=DurabilityPolicy.VOLATILE,history=HistoryPolicy.KEEP_LAST,) + 'qos' : QoSProfile(depth=100,durability=DurabilityPolicy.TRANSIENT_LOCAL,history=HistoryPolicy.KEEP_LAST,)#static + } + ] + try: + ''' + initialize, run and check the data + ''' + self.init_test("RsTest"+self.params['camera_name']) + self.wait_for_node(self.params['camera_name']) + self.create_param_ifs(get_node_heirarchy(self.params)) + ret = self.run_test(themes, timeout=10) + assert ret[0], ret[1] + ret = self.process_data(themes, False) + assert ret[0], ret[1] + finally: + self.shutdown() + + def process_data(self, themes, enable_infra1): + frame_ids = [self.params['camera_name']+'_link', + self.params['camera_name']+'_depth_frame', + self.params['camera_name']+'_color_frame'] + data = self.node.pop_first_chunk('/tf_static') + ret = self.check_transform_data(data, frame_ids, True) + assert ret[0], ret[1] + data = self.node.pop_first_chunk('/tf') + ret = self.check_transform_data(data, frame_ids) + assert ret[0], ret[1] + data = self.node.pop_first_chunk('/tf') + ret = self.check_transform_data(data, frame_ids) + assert ret[0], ret[1] + data = self.node.pop_first_chunk('/tf') + ret = self.check_transform_data(data, frame_ids) + assert ret[0], ret[1] + #return True, "" + + return True, "" diff --git a/realsense2_camera/test/utils/pytest_live_camera_utils.py b/realsense2_camera/test/utils/pytest_live_camera_utils.py index 3a5ab6f600..36ade782d5 100644 --- a/realsense2_camera/test/utils/pytest_live_camera_utils.py +++ b/realsense2_camera/test/utils/pytest_live_camera_utils.py @@ -177,6 +177,23 @@ def get_camera_capabilities(device_type, serial_no=None): return capability return None +def check_if_camera_connected(device_type, serial_no=None): + long_data = os.popen("rs-enumerate-devices -s").read().splitlines() + debug_print(serial_no) + index = 0 + for index in range(len(long_data)): + name_line = long_data[index].split() + if name_line[0] != "Intel": + continue + if name_line[2] != device_type: + continue + if serial_no == None: + return True + if serial_no == name_line[3]: + return True + + return False + if __name__ == '__main__': device_type = 'D455' serial_no = None diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index 9fdf8412c4..05e6847e13 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/realsense2_camera/test/utils/pytest_rs_utils.py @@ -81,7 +81,7 @@ import requests def debug_print(*args): - if(True): + if(False): print(*args) class RosbagManager(object): @@ -623,7 +623,7 @@ def reset_data(self, topic): self.frame_counter[topic] = 0 self._ros_topic_hz.restart_topic(topic) - def create_subscription(self, msg_type, topic , data_type, store_raw_data, measure_hz, static_tf): + def create_subscription(self, msg_type, topic , data_type, store_raw_data, measure_hz): self.reset_data(topic) super().create_subscription(msg_type, topic , self.rsCallback(topic,msg_type, store_raw_data), data_type) #hz measurements are not working @@ -771,9 +771,9 @@ def init_test(self,name='RsTestNode'): def wait_for_node(self, node_name, timeout=8.0): self.node.wait_for_node(node_name, timeout) - def create_subscription(self, msg_type, topic, data_type, store_raw_data=False, measure_hz=False, static_tf=False): + def create_subscription(self, msg_type, topic, data_type, store_raw_data=False, measure_hz=False): if not topic in self.subscribed_topics: - self.node.create_subscription(msg_type, topic, data_type, store_raw_data, measure_hz, static_tf) + self.node.create_subscription(msg_type, topic, data_type, store_raw_data, measure_hz) self.subscribed_topics.append(topic) else: self.node.reset_data(topic) @@ -859,7 +859,6 @@ def spin_for_time(self,wait_time): def run_test(self, themes, initial_wait_time=0.0, timeout=5.0): try: - static_tf_found = False for theme in themes: store_raw_data = False if 'store_raw_data' in theme: @@ -868,13 +867,10 @@ def run_test(self, themes, initial_wait_time=0.0, timeout=5.0): measure_hz = True else: measure_hz = False - if 'static_tf' in theme: - static_tf = True - static_tf_found = True - else: - static_tf = False - - self.create_subscription(theme['msg_type'], theme['topic'] , qos.qos_profile_sensor_data,store_raw_data, measure_hz, static_tf) + qos_type = qos.qos_profile_sensor_data + if 'qos' in theme: + qos_type = theme['qos'] + self.create_subscription(theme['msg_type'], theme['topic'] , qos_type,store_raw_data, measure_hz) print('subscription created for ' + theme['topic']) if initial_wait_time != 0.0: self.spin_for_time(initial_wait_time) @@ -893,6 +889,28 @@ def run_test(self, themes, initial_wait_time=0.0, timeout=5.0): return self.flag def get_tfs(self, coupled_frame_ids): return self.node.get_tfs(coupled_frame_ids) + + + def check_transform_data(self, data, frame_ids, is_static=False): + coupled_frame_ids = [xx for xx in itertools.combinations(frame_ids, 2)] + tfBuffer = tf2_ros.Buffer() + for transform in data.transforms: + if is_static: + tfBuffer.set_transform_static(transform, "default_authority") + else: + tfBuffer.set_transform(transform, "default_authority") + res = dict() + for couple in coupled_frame_ids: + from_id, to_id = couple + if (tfBuffer.can_transform(from_id, to_id, rclpy.time.Time(), rclpy.time.Duration(nanoseconds=3e6))): + res[couple] = tfBuffer.lookup_transform(from_id, to_id, rclpy.time.Time(), rclpy.time.Duration(nanoseconds=1e6)).transform + else: + res[couple] = None + for couple in coupled_frame_ids: + if res[couple] == None: + return False, str(couple) + ": didn't get any tf data" + return True,"" + ''' Please override and use your own process_data if the default check is not suitable. Please also store_raw_data parameter in the themes as True, if you want the From 1ff67d45fb721de28bbd3c24f170d75c99254299 Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Tue, 5 Sep 2023 21:29:05 +0530 Subject: [PATCH 51/76] Added pointcloud tests for live camera --- .../test_camera_point_cloud_tests.py | 114 ++++++++++++++++++ .../test/live_camera/test_camera_tf_tests.py | 28 ++--- .../test/utils/pytest_rs_utils.py | 7 +- 3 files changed, 133 insertions(+), 16 deletions(-) create mode 100644 realsense2_camera/test/live_camera/test_camera_point_cloud_tests.py diff --git a/realsense2_camera/test/live_camera/test_camera_point_cloud_tests.py b/realsense2_camera/test/live_camera/test_camera_point_cloud_tests.py new file mode 100644 index 0000000000..14def86ca9 --- /dev/null +++ b/realsense2_camera/test/live_camera/test_camera_point_cloud_tests.py @@ -0,0 +1,114 @@ +# 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. + + +import os +import sys +import itertools + + +import pytest +import rclpy + +from rclpy.qos import DurabilityPolicy +from rclpy.qos import HistoryPolicy +from rclpy.qos import QoSProfile +import tf2_ros + + +from sensor_msgs.msg import Image as msg_Image +from sensor_msgs.msg import Imu as msg_Imu +from sensor_msgs.msg import PointCloud2 as msg_PointCloud2 +from tf2_msgs.msg import TFMessage as msg_TFMessage + +import numpy as np + +sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils")) +import pytest_rs_utils +from pytest_rs_utils import launch_descr_with_parameters + +import pytest_live_camera_utils +from pytest_rs_utils import get_rosbag_file_path +from pytest_rs_utils import get_node_heirarchy + +from rclpy.parameter import Parameter +from rcl_interfaces.msg import ParameterValue +from rcl_interfaces.srv import SetParameters, GetParameters, ListParameters + +''' +The test was implemented to check the tf_static before and after infra1 was enabled. There shouldn't be any +related data before enabling. +''' +test_params_points_cloud_d455 = { + 'camera_name': 'D455', + 'device_type': 'D455', + 'pointcloud.enable': 'true' + } +test_params_points_cloud_d435 = { + 'camera_name': 'D435', + 'device_type': 'D435', + 'pointcloud.enable': 'true' + } + +test_params_points_cloud_d415 = { + 'camera_name': 'D415', + 'device_type': 'D415', + 'enable_color':'true', + 'enable_depth':'true', + 'depth_module.profile':'848x480x30', + 'pointcloud.enable': 'true' + } + +''' +This test was ported from rs2_test.py +the command used to run is "python3 realsense2_camera/scripts/rs2_test.py points_cloud_1" +''' +@pytest.mark.parametrize("launch_descr_with_parameters", [ + pytest.param(test_params_points_cloud_d455, marks=pytest.mark.d455), + pytest.param(test_params_points_cloud_d435, marks=pytest.mark.d435), + pytest.param(test_params_points_cloud_d415, marks=pytest.mark.d415), + ],indirect=True) +@pytest.mark.launch(fixture=launch_descr_with_parameters) +class TestCamera_TestPointCloud(pytest_rs_utils.RsTestBaseClass): + def test_camera_test_point_cloud(self,launch_descr_with_parameters): + self.params = launch_descr_with_parameters[1] + if pytest_live_camera_utils.check_if_camera_connected(self.params['device_type']) == False: + print("Device not found? : " + self.params['device_type']) + return + themes = [ + { + 'topic':get_node_heirarchy(self.params)+'/depth/color/points', + 'msg_type':msg_PointCloud2, + 'expected_data_chunks':5, + }, + ] + try: + ''' + initialize, run and check the data + ''' + self.init_test("RsTest"+self.params['camera_name']) + self.wait_for_node(self.params['camera_name']) + self.create_param_ifs(get_node_heirarchy(self.params)) + ret = self.run_test(themes, timeout=10) + assert ret[0], ret[1] + ret = self.process_data(themes, False) + assert ret[0], ret[1] + finally: + self.shutdown() + def process_data(self, themes, enable_infra1): + for count in range(self.node.get_num_chunks(get_node_heirarchy(self.params)+'/depth/color/points')): + data = self.node.pop_first_chunk(get_node_heirarchy(self.params)+'/depth/color/points') + print(data)#the frame counter starts with zero, jumps to 2 and continues. To be checked + return True,"" + diff --git a/realsense2_camera/test/live_camera/test_camera_tf_tests.py b/realsense2_camera/test/live_camera/test_camera_tf_tests.py index 10c72c8b09..4e8250f063 100644 --- a/realsense2_camera/test/live_camera/test_camera_tf_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_tf_tests.py @@ -103,15 +103,7 @@ def test_camera_test_tf_static_change(self,launch_descr_with_parameters): self.create_param_ifs(get_node_heirarchy(self.params)) ret = self.run_test(themes, timeout=10) assert ret[0], ret[1] - frame_ids = [self.params['camera_name']+'_link', - self.params['camera_name']+'_depth_frame', - self.params['camera_name']+'_infra1_frame', - self.params['camera_name']+'_infra2_frame', - self.params['camera_name']+'_color_frame'] - if self.params['device_type'] == 'D455': - frame_ids.append(self.params['camera_name']+'_gyro_frame') - frame_ids.append(self.params['camera_name']+'_accel_frame') - + ret = self.process_data(themes, False) assert ret[0], ret[1] assert self.set_bool_param('enable_infra1', True) @@ -130,12 +122,20 @@ def process_data(self, themes, enable_infra1): if self.params['device_type'] == 'D455': frame_ids.append(self.params['camera_name']+'_gyro_frame') frame_ids.append(self.params['camera_name']+'_accel_frame') - if enable_infra1: - frame_ids.append(self.params['camera_name']+'_infra1_frame') - + frame_ids.append(self.params['camera_name']+'_infra1_frame') data = self.node.pop_first_chunk('/tf_static') - ret = self.check_transform_data(data, frame_ids, True) - return ret + coupled_frame_ids = [xx for xx in itertools.combinations(frame_ids, 2)] + res = self.get_transform_data(data, coupled_frame_ids, is_static=True) + for couple in coupled_frame_ids: + if self.params['camera_name']+'_infra1_frame' in couple: + if enable_infra1 == True and res[couple] != None: + continue + if enable_infra1 == False and res[couple] == None: + continue + return False, str(couple) + ": tf_data not as expected" + if res[couple] == None: + return False, str(couple) + ": didn't get any tf data" + return True,"" test_params_tf_d455 = { diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index 05e6847e13..c07b135065 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/realsense2_camera/test/utils/pytest_rs_utils.py @@ -891,8 +891,7 @@ def get_tfs(self, coupled_frame_ids): return self.node.get_tfs(coupled_frame_ids) - def check_transform_data(self, data, frame_ids, is_static=False): - coupled_frame_ids = [xx for xx in itertools.combinations(frame_ids, 2)] + def get_transform_data(self, data, coupled_frame_ids, is_static=False): tfBuffer = tf2_ros.Buffer() for transform in data.transforms: if is_static: @@ -906,6 +905,10 @@ def check_transform_data(self, data, frame_ids, is_static=False): res[couple] = tfBuffer.lookup_transform(from_id, to_id, rclpy.time.Time(), rclpy.time.Duration(nanoseconds=1e6)).transform else: res[couple] = None + return res + def check_transform_data(self, data, frame_ids, is_static=False): + coupled_frame_ids = [xx for xx in itertools.combinations(frame_ids, 2)] + res = self.get_transform_data(data, coupled_frame_ids, is_static) for couple in coupled_frame_ids: if res[couple] == None: return False, str(couple) + ": didn't get any tf data" From 4f5c34d6cbe922db6cdfbfa10150a78477bd647e Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Fri, 8 Sep 2023 16:58:47 +0530 Subject: [PATCH 52/76] Add aligned tests --- realsense2_camera/test/README.md | 11 + .../live_camera/test_camera_aligned_tests.py | 274 ++++++++++++++++++ .../test_camera_all_profile_tests.py | 1 + .../rosbag/test_rosbag_all_topics_test.py | 1 - .../test/utils/pytest_live_camera_utils.py | 28 +- 5 files changed, 300 insertions(+), 15 deletions(-) create mode 100644 realsense2_camera/test/live_camera/test_camera_aligned_tests.py diff --git a/realsense2_camera/test/README.md b/realsense2_camera/test/README.md index a026a36b2e..9624848955 100644 --- a/realsense2_camera/test/README.md +++ b/realsense2_camera/test/README.md @@ -146,6 +146,17 @@ The below command finds the test with the name test_static_tf_1 in realsense2_ca pytest-3 -s -k test_static_tf_1 realsense2_camera/test/ +### Marking tests as regression tests +Some of the tests, especially the live tests with multiple runs, for e.g., all profile tests (test_camera_all_profile_tests.py) take a long time. Such tests are marked are skipped with condition so that "colcon test" skips it. +If a user wants to add a test to this conditional skip, user can add by adding a marker as below. + +@pytest.mark.skipif (os.getenv('RS_ROS_REGRESSION', "not found") == "not found",reason="Regression is not enabled, define RS_ROS_REGRESSION") + +### Running skipped regression tests +1. Set the environment variable RS_ROS_REGRESSION as 1 and run the "colcon test" +2. pytest example: + RS_ROS_REGRESSION=1 PYTHONPATH=$PYTHONPATH:$PWD/realsense2_camera/test/utils:$PWD/realsense2_camera//launch:$PWD/realsense2_camera//scripts pytest-3 -s realsense2_camera/test/live_camera/test_camera_aligned_tests.py -k test_camera_align_depth_color_all -m d415 + ## Points to be noted while writing pytests The tests that are in one file are normally run in parallel. So if there are multiple tests in one file, the system capacity can influence the test execution. It's recomended to have 3-4 tests in file, more than that can affect the test results due to delays. ### Passing/changing parameters diff --git a/realsense2_camera/test/live_camera/test_camera_aligned_tests.py b/realsense2_camera/test/live_camera/test_camera_aligned_tests.py new file mode 100644 index 0000000000..d489b5d546 --- /dev/null +++ b/realsense2_camera/test/live_camera/test_camera_aligned_tests.py @@ -0,0 +1,274 @@ +# 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. + + +import os +import sys +import itertools + + +import pytest +import rclpy + +from sensor_msgs.msg import Image as msg_Image +from sensor_msgs.msg import Imu as msg_Imu +from sensor_msgs.msg import PointCloud2 as msg_PointCloud2 + +import numpy as np + +sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils")) +import pytest_rs_utils +from pytest_rs_utils import launch_descr_with_parameters +from pytest_rs_utils import delayed_launch_descr_with_parameters +from pytest_rs_utils import get_rosbag_file_path +from pytest_rs_utils import get_node_heirarchy +import pytest_live_camera_utils +from rclpy.parameter import Parameter +from rcl_interfaces.msg import ParameterValue +from rcl_interfaces.srv import SetParameters, GetParameters, ListParameters +from pytest_live_camera_utils import debug_print + + + +test_params_align_depth_color_d455 = { + 'camera_name': 'D455', + 'device_type': 'D455', + 'enable_color':'true', + 'enable_depth':'true', + 'depth_module.profile':'848x480x30', + 'rgb_camera.profile':'640x480x30', + 'align_depth.enable':'true' + } +test_params_align_depth_color_d415 = { + 'camera_name': 'D415', + 'device_type': 'D415', + 'enable_color':'true', + 'enable_depth':'true', + 'depth_module.profile':'848x480x30', + 'rgb_camera.profile':'640x480x30', + 'align_depth.enable':'true' + } +''' +This test was implemented as a template to set the parameters and run the test. +This directory is not added to the CMakeLists.txt so as to avoid the colcon failure in the +machines that don't have the D455 connected. +1. Only a subset of parameter types are implemented in py_rs_utils, it has to be extended for others +2. After setting the param, rclpy.spin_once may be needed.Test passes even without this though. +''' +@pytest.mark.parametrize("launch_descr_with_parameters",[ + pytest.param(test_params_align_depth_color_d455, marks=pytest.mark.d455), + pytest.param(test_params_align_depth_color_d415, marks=pytest.mark.d415), + #pytest.param(test_params_align_depth_color_d435, marks=pytest.mark.d435), + ] + ,indirect=True) +@pytest.mark.launch(fixture=launch_descr_with_parameters) +class TestCamera_AlignDepthColor(pytest_rs_utils.RsTestBaseClass): + def test_camera_align_depth_color(self,launch_descr_with_parameters): + params = launch_descr_with_parameters[1] + themes = [ + {'topic':get_node_heirarchy(params)+'/color/image_raw', + 'msg_type':msg_Image, + 'expected_data_chunks':1, + 'width':640, + 'height':480, + }, + {'topic':get_node_heirarchy(params)+'/depth/image_rect_raw', + 'msg_type':msg_Image, + 'expected_data_chunks':1, + 'width':848, + 'height':480, + }, + {'topic':get_node_heirarchy(params)+'/aligned_depth_to_color/image_raw', + 'msg_type':msg_Image, + 'expected_data_chunks':1, + 'width':640, + 'height':480, + }, + ] + try: + ''' + initialize, run and check the data + ''' + print("Starting camera test...") + self.init_test("RsTest"+params['camera_name']) + self.wait_for_node(params['camera_name']) + self.create_param_ifs(get_node_heirarchy(params)) + ret = self.run_test(themes) + assert ret[0], ret[1] + assert self.process_data(themes) + self.set_string_param('rgb_camera.profile', '1280x720x30') + self.set_bool_param('enable_color', True) + themes[0]['width'] = 1280 + themes[0]['height'] = 720 + themes[2]['width'] = 1280 + themes[2]['height'] = 720 + + ret = self.run_test(themes) + assert ret[0], ret[1] + assert self.process_data(themes) + finally: + #this step is important because the test will fail next time + pytest_rs_utils.kill_realsense2_camera_node() + self.shutdown() + + + +test_params_all_profiles_d455 = { + 'camera_name': 'D455', + 'device_type': 'D455', + 'enable_color':'true', + 'enable_depth':'true', + 'depth_module.profile':'848x480x30', + 'rgb_camera.profile':'640x480x30', + 'align_depth.enable':'true' + } +test_params_all_profiles_d415 = { + 'camera_name': 'D415', + 'device_type': 'D415', + 'enable_color':'true', + 'enable_depth':'true', + 'depth_module.profile':'848x480x30', + 'rgb_camera.profile':'640x480x30', + 'align_depth.enable':'true' + } +test_params_all_profiles_d435 = { + 'camera_name': 'D435', + 'device_type': 'D435', + 'enable_color':'true', + 'enable_depth':'true', + 'depth_module.profile':'848x480x30', + 'rgb_camera.profile':'640x480x30', + 'align_depth.enable':'true' + } + + +''' +This test was implemented as a template to set the parameters and run the test. +This directory is not added to the CMakeLists.txt so as to avoid the colcon failure in the +machines that don't have the D455 connected. +1. Only a subset of parameter types are implemented in py_rs_utils, it has to be extended for others +2. After setting the param, rclpy.spin_once may be needed.Test passes even without this though. +''' +@pytest.mark.skipif (os.getenv('RS_ROS_REGRESSION', "not found") == "not found",reason="Regression is not enabled, define RS_ROS_REGRESSION") +@pytest.mark.parametrize("launch_descr_with_parameters", [ + pytest.param(test_params_all_profiles_d455, marks=pytest.mark.d455), + pytest.param(test_params_all_profiles_d415, marks=pytest.mark.d415), + pytest.param(test_params_all_profiles_d435, marks=pytest.mark.d435),] + ,indirect=True) +@pytest.mark.launch(fixture=launch_descr_with_parameters) +class TestCamera_AlignDepthColor(pytest_rs_utils.RsTestBaseClass): + def test_camera_align_depth_color_all(self,launch_descr_with_parameters): + skipped_tests = [] + failed_tests = [] + num_passed = 0 + num_failed = 0 + params = launch_descr_with_parameters[1] + themes = [ + {'topic':get_node_heirarchy(params)+'/color/image_raw', + 'msg_type':msg_Image, + 'expected_data_chunks':1, + 'width':640, + 'height':480, + }, + {'topic':get_node_heirarchy(params)+'/depth/image_rect_raw', + 'msg_type':msg_Image, + 'expected_data_chunks':1, + 'width':848, + 'height':480, + }, + {'topic':get_node_heirarchy(params)+'/aligned_depth_to_color/image_raw', + 'msg_type':msg_Image, + 'expected_data_chunks':1, + 'width':640, + 'height':480, + }, + ] + try: + ''' + initialize, run and check the data + ''' + self.init_test("RsTest"+params['camera_name']) + self.spin_for_time(wait_time=1.0) + cap = pytest_live_camera_utils.get_camera_capabilities(params['device_type']) + if cap == None: + debug_print("Device not found? : " + params['device_type']) + return + self.create_param_ifs(get_node_heirarchy(params)) + color_profiles = set([i[1] for i in cap["color_profile"] if i[2] == "RGB8"]) + depth_profiles = set([i[1] for i in cap["depth_profile"] if i[0] == "Depth"]) + for color_profile in color_profiles: + for depth_profile in depth_profiles: + if depth_profile == color_profile: + continue + print("Testing the alignment of Depth:", depth_profile, " and Color:", color_profile) + self.set_bool_param('enable_color', False) + self.set_bool_param('enable_color', False) + self.set_bool_param('align_depth.enable', False) + + themes[0]['width'] = themes[2]['width'] = int(color_profile.split('x')[0]) + themes[0]['height'] = themes[2]['height'] = int(color_profile.split('x')[1]) + themes[1]['width'] = int(depth_profile.split('x')[0]) + themes[1]['height'] = int(depth_profile.split('x')[1]) + dfps = int(depth_profile.split('x')[2]) + cfps = int(color_profile.split('x')[2]) + if dfps > cfps: + fps = cfps + else: + fps = dfps + timeout=100.0/fps + #for the changes to take effect + self.spin_for_time(wait_time=timeout/20) + self.set_string_param('rgb_camera.profile', color_profile) + self.set_string_param('depth_module.profile', depth_profile) + self.set_bool_param('enable_color', True) + self.set_bool_param('enable_color', True) + self.set_bool_param('align_depth.enable', True) + #for the changes to take effect + self.spin_for_time(wait_time=timeout/20) + try: + ret = self.run_test(themes, timeout=timeout) + assert ret[0], ret[1] + assert self.process_data(themes), "".join(str(depth_profile) + " " + str(color_profile)) + " failed" + num_passed += 1 + except Exception as e: + exc_type, exc_obj, exc_tb = sys.exc_info() + fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1] + print("Test failed") + print("Tested the alignment of Depth:", depth_profile, " and Color:", color_profile, " with timeout ", timeout) + print(e) + print(exc_type, fname, exc_tb.tb_lineno) + num_failed += 1 + failed_tests.append("".join(str(depth_profile) + " " + str(color_profile))) + + finally: + #this step is important because the test will fail next time + pytest_rs_utils.kill_realsense2_camera_node() + self.shutdown() + print("Tests passed " + str(num_passed)) + print("Tests skipped " + str(len(skipped_tests))) + if len(skipped_tests): + debug_print("\nSkipped tests:" + params['device_type']) + debug_print("\n".join(skipped_tests)) + print("Tests failed " + str(num_failed)) + if len(failed_tests): + print("\nFailed tests:" + params['device_type']) + print("\n".join(failed_tests)) + + def disable_all_params(self): + self.set_bool_param('enable_color', False) + self.set_bool_param('enable_depth', False) + self.set_bool_param('enable_infra', False) + self.set_bool_param('enable_infra1', False) + self.set_bool_param('enable_infra2', False) + diff --git a/realsense2_camera/test/live_camera/test_camera_all_profile_tests.py b/realsense2_camera/test/live_camera/test_camera_all_profile_tests.py index 274ee67625..b337e6f8d5 100644 --- a/realsense2_camera/test/live_camera/test_camera_all_profile_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_all_profile_tests.py @@ -140,6 +140,7 @@ def check_if_skip_test(profile, format): 1. Only a subset of parameter types are implemented in py_rs_utils, it has to be extended for others 2. After setting the param, rclpy.spin_once may be needed.Test passes even without this though. ''' +@pytest.mark.skipif (os.getenv('RS_ROS_REGRESSION', "not found") == "not found",reason="Regression is not enabled, define RS_ROS_REGRESSION") @pytest.mark.parametrize("launch_descr_with_parameters", [ pytest.param(test_params_all_profiles_d455, marks=pytest.mark.d455), pytest.param(test_params_all_profiles_d415, marks=pytest.mark.d415), diff --git a/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py b/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py index 34abbc3039..22c4e1caee 100644 --- a/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py +++ b/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py @@ -57,7 +57,6 @@ ''' To test all topics published ''' -@pytest.mark.skip @pytest.mark.rosbag @pytest.mark.parametrize("delayed_launch_descr_with_parameters", [test_params_all_topics],indirect=True) @pytest.mark.launch(fixture=delayed_launch_descr_with_parameters) diff --git a/realsense2_camera/test/utils/pytest_live_camera_utils.py b/realsense2_camera/test/utils/pytest_live_camera_utils.py index 36ade782d5..9b5bfeac70 100644 --- a/realsense2_camera/test/utils/pytest_live_camera_utils.py +++ b/realsense2_camera/test/utils/pytest_live_camera_utils.py @@ -48,19 +48,6 @@ def get_default_profiles(cap, profile): debug_print(profile + " default profile2:" + profile2) return profile1,profile2 -def get_camera_capabilities_short(device_type, serial_no=None): - short_data = os.popen("rs-enumerate-devices -s").read().splitlines() - print(serial_no) - for line in short_data: - print(line) - if device_type in line: - if serial_no is None or serial_no == "" : - print(device_type+ " found in " + line) - return - if serial_no in line: - print(device_type + " with serial_no " + serial_no +" found in " + line) - return - print(device_type + " not found") device_info_string = "Device info:" @@ -177,6 +164,20 @@ def get_camera_capabilities(device_type, serial_no=None): return capability return None +def get_camera_capabilities_short(device_type, serial_no=None): + short_data = os.popen("rs-enumerate-devices -s").read().splitlines() + print(serial_no) + for line in short_data: + print(line) + if device_type in line: + if serial_no is None or serial_no == "" : + print(device_type+ " found in " + line) + return + if serial_no in line: + print(device_type + " with serial_no " + serial_no +" found in " + line) + return + print(device_type + " not found") + def check_if_camera_connected(device_type, serial_no=None): long_data = os.popen("rs-enumerate-devices -s").read().splitlines() debug_print(serial_no) @@ -201,7 +202,6 @@ def check_if_camera_connected(device_type, serial_no=None): device_type = sys.argv[1] if len(sys.argv) > 2: serial_no = sys.argv[2] - cap = get_camera_capabilities(device_type, serial_no) print("Capabilities:") print(cap) \ No newline at end of file From 726dbd2aea4163ae63a5a132e8586f3d3aa2881a Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Fri, 8 Sep 2023 17:36:35 +0530 Subject: [PATCH 53/76] Modified the imu test to remove workaround for RS550 --- realsense2_camera/test/live_camera/test_camera_imu_tests.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/realsense2_camera/test/live_camera/test_camera_imu_tests.py b/realsense2_camera/test/live_camera/test_camera_imu_tests.py index c712cd1ba7..e2defb6b52 100644 --- a/realsense2_camera/test/live_camera/test_camera_imu_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_imu_tests.py @@ -80,8 +80,7 @@ def test_LiveCamera_check_motion_sensor(self,launch_descr_with_parameters): self.set_bool_param('enable_accel', False) self.set_bool_param('enable_gyro', True) themes[ACCEL_TOPIC]['expected_data_chunks'] = 0 - #uncomment once RSDEV-550 is closed - #themes[IMU_TOPIC]['expected_data_chunks'] = 0 + themes[IMU_TOPIC]['expected_data_chunks'] = 0 print(msg) ret = self.run_test(themes) assert ret[0], msg + str(ret[1]) From 7a525a704212e448e90c52339a2f9ff778a8a9c2 Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Fri, 8 Sep 2023 19:28:55 +0530 Subject: [PATCH 54/76] removed failing_test file, was covered in all_profile_tests anyway --- realsense2_camera/CMakeLists.txt | 7 +- .../live_camera/test_camera_failing_tests.py | 268 ------------------ 2 files changed, 1 insertion(+), 274 deletions(-) delete mode 100644 realsense2_camera/test/live_camera/test_camera_failing_tests.py diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index b4da686f8f..67361a948c 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -314,16 +314,11 @@ if(BUILD_TESTING) OUTPUT_VARIABLE RS_DEVICE_INFO) message(STATUS "rs_device_info:") message(STATUS "${RS_DEVICE_INFO}") - if(RS_DEVICE_INFO MATCHES "D455") + if((RS_DEVICE_INFO MATCHES "D455") OR (RS_DEVICE_INFO MATCHES "D415") OR (RS_DEVICE_INFO MATCHES "D435")) message(STATUS "D455 device found") set(_pytest_live_folders test/live_camera ) - elseif(RS_DEVICE_INFO MATCHES "D415") - message(STATUS "D415 device found") - set(_pytest_live_folders - test/live_camera - ) endif() foreach(test_folder ${_pytest_live_folders}) diff --git a/realsense2_camera/test/live_camera/test_camera_failing_tests.py b/realsense2_camera/test/live_camera/test_camera_failing_tests.py deleted file mode 100644 index 3bdc87b19b..0000000000 --- a/realsense2_camera/test/live_camera/test_camera_failing_tests.py +++ /dev/null @@ -1,268 +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. - - -import os -import sys -import itertools - - -import pytest -import rclpy - -from sensor_msgs.msg import Image as msg_Image -from sensor_msgs.msg import Imu as msg_Imu -from sensor_msgs.msg import PointCloud2 as msg_PointCloud2 - -import numpy as np - -sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils")) -import pytest_rs_utils -from pytest_rs_utils import launch_descr_with_parameters -from pytest_rs_utils import delayed_launch_descr_with_parameters -from pytest_rs_utils import get_rosbag_file_path -from pytest_rs_utils import get_node_heirarchy -import pytest_live_camera_utils -from rclpy.parameter import Parameter -from rcl_interfaces.msg import ParameterValue -from rcl_interfaces.srv import SetParameters, GetParameters, ListParameters -from pytest_live_camera_utils import debug_print - -test_params_all_profiles_d455 = { - 'camera_name': 'D455', - 'device_type': 'D455', - } -test_params_all_profiles_d415 = { - 'camera_name': 'D415', - 'device_type': 'D415', - } -''' -This test was implemented as a template to set the parameters and run the test. -This directory is not added to the CMakeLists.txt so as to avoid the colcon failure in the -machines that don't have the D455 connected. -1. Only a subset of parameter types are implemented in py_rs_utils, it has to be extended for others -2. After setting the param, rclpy.spin_once may be needed.Test passes even without this though. -''' -@pytest.mark.parametrize("launch_descr_with_parameters", [test_params_all_profiles_d415],indirect=True) -@pytest.mark.launch(fixture=launch_descr_with_parameters) -class TestLiveCamera_Change_Resolution(pytest_rs_utils.RsTestBaseClass): - def test_LiveCamera_Change_Resolution(self,launch_descr_with_parameters): - passed_tests = [] - failed_tests = [] - num_passed = 0 - num_failed = 0 - wait_time_s = 1.2 - cap = {} - ''' - need two configurations with different profiles(height & width) for each profile, - this is to ensure the test sets a different profile first, before testing the - actual profile to be tested. - ''' - cap['color_profile'] = [ - ['Color', '1920x1080x30','RGB8'], - ['Color', '1280x720x30','RGB8'], - ] - cap['depth_profile'] = [ - ['Infrared1', '1920x1080x25', 'Y8'], - ['Infrared1', '1920x1080x25', 'Y16'], - ['Infrared1', '1920x1080x15', 'Y16'], - ['Infrared', '848x100x100', 'BGRA8'], - ['Infrared', '640x480x60', 'BGRA8'], - ['Infrared', '640x480x60', 'RGBA8'], - ['Infrared', '640x480x60', 'RGB8'], - ['Infrared', '640x360x60', 'RGBA8'], - ['Infrared', '640x360x60', 'RGB8'], - ['Infrared', '640x360x30', 'UYVY'], - ['Infrared', '480x270x15', 'RGB8'], - ['Infrared', '424x240x60', 'BGRA8'], - ['Infrared', '424x240x30', 'UYVY'], - ['Infrared1', '1920x1080x15', 'Y8'], - ['Infrared1', '1280x720x30', 'Y8'], - ['Infrared1', '1280x720x15', 'Y8'], - ['Infrared1', '1280x720x6', 'Y8'], - ['Infrared1', '960x540x25', 'Y16'], - ['Infrared1', '960x540x15', 'Y16'], - ['Infrared1', '848x480x90', 'Y8'], - ['Infrared1', '848x480x60', 'Y8'], - ['Infrared1', '848x480x30', 'Y8'], - ['Infrared1', '848x480x15', 'Y8'], - ['Infrared1', '848x480x6', 'Y8'], - ['Infrared1', '848x100x100', 'Y8'], - ['Infrared1', '640x480x90', 'Y8'], - ['Infrared1', '640x480x60', 'Y8'], - ['Infrared1', '640x480x30', 'Y8'], - ['Infrared1', '640x480x15', 'Y8'], - ['Infrared1', '640x480x6', 'Y8'], - ['Infrared1', '640x360x90', 'Y8'], - ['Infrared1', '640x360x60', 'Y8'], - ['Infrared1', '640x360x30', 'Y8'], - ['Infrared1', '640x360x15', 'Y8'], - ['Infrared1', '640x360x6', 'Y8'], - ['Infrared1', '480x270x90', 'Y8'], - ['Infrared1', '480x270x60', 'Y8'], - ['Infrared1', '480x270x30', 'Y8'], - ['Infrared1', '480x270x15', 'Y8'], - ['Infrared1', '480x270x6', 'Y8'], - ['Infrared1', '424x240x90', 'Y8'], - ['Infrared1', '424x240x60', 'Y8'], - ['Infrared1', '424x240x30', 'Y8'], - ['Infrared1', '424x240x15', 'Y8'], - ['Infrared1', '424x240x6', 'Y8'], - ['Infrared2', '1920x1080x25', 'Y16'], - ['Infrared2', '1920x1080x25', 'Y8'], - ['Infrared2', '1920x1080x15', 'Y16'], - ['Infrared2', '1920x1080x15', 'Y8'], - ['Infrared2', '1280x720x30', 'Y8'], - ['Infrared2', '1280x720x15', 'Y8'], - ['Infrared2', '1280x720x6', 'Y8'], - ['Infrared2', '960x540x25', 'Y16'], - ['Infrared2', '960x540x15', 'Y16'], - ['Infrared2', '848x480x90', 'Y8'], - ['Infrared2', '848x480x60', 'Y8'], - ['Infrared2', '848x480x30', 'Y8'], - ['Infrared2', '848x480x15', 'Y8'], - ['Infrared2', '848x480x6', 'Y8'], - ['Infrared2', '848x100x100', 'Y8'], - ['Infrared2', '640x480x90', 'Y8'], - ['Infrared2', '640x480x60', 'Y8'], - ['Infrared2', '640x480x30', 'Y8'], - ['Infrared2', '640x480x15', 'Y8'], - ['Infrared2', '640x480x6', 'Y8'], - ['Infrared2', '640x360x90', 'Y8'], - ['Infrared2', '640x360x60', 'Y8'], - ['Infrared2', '640x360x30', 'Y8'], - ['Infrared2', '640x360x15', 'Y8'], - ['Infrared2', '640x360x6', 'Y8'], - ['Infrared2', '480x270x90', 'Y8'], - ['Infrared2', '480x270x60', 'Y8'], - ['Infrared2', '480x270x30', 'Y8'], - ['Infrared2', '480x270x15', 'Y8'], - ['Infrared2', '480x270x6', 'Y8'], - ['Infrared2', '424x240x90', 'Y8'], - ['Infrared2', '424x240x60', 'Y8'], - ['Infrared2', '424x240x30', 'Y8'], - ['Infrared2', '424x240x15', 'Y8'], - ['Infrared2', '424x240x6', 'Y8'], - ['Depth', '640x360x30', 'Z16'], - ['Depth', '480x270x30', 'Z16'], - ['Depth', '424x240x30', 'Z16'], - ] - params = launch_descr_with_parameters[1] - themes = [{'topic':get_node_heirarchy(params)+'/color/image_raw', 'msg_type':msg_Image,'expected_data_chunks':1,'initial_reset':True}] - config = pytest_live_camera_utils.get_profile_config(get_node_heirarchy(params)) - try: - ''' - initialize, run and check the data - ''' - serial_no = None - if 'serial_no' in params: - serial_no = params['serial_no'] - self.init_test("RsTest"+params['camera_name']) - self.spin_for_time(wait_time=1.0) - if cap == None: - debug_print("Device not found? : " + params['device_type']) - return - self.create_param_ifs(get_node_heirarchy(params)) - config["Color"]["default_profile1"],config["Color"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["color_profile"], "Color") - config["Depth"]["default_profile1"],config["Depth"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["depth_profile"], "Depth") - config["Infrared"]["default_profile1"],config["Infrared"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["depth_profile"], "Infrared") - config["Infrared1"]["default_profile1"],config["Infrared1"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["depth_profile"], "Infrared1") - config["Infrared2"]["default_profile1"],config["Infrared2"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["depth_profile"], "Infrared2") - for key in cap["color_profile"]: - profile_type = key[0] - profile = key[1] - format = key[2] - print("Testing " + " ".join(key)) - themes[0]['topic'] = config[profile_type]['topic'] - themes[0]['width'] = int(profile.split('x')[0]) - themes[0]['height'] = int(profile.split('x')[1]) - if themes[0]['width'] == int(config[profile_type]["default_profile2"].split('x')[0]): - self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile1"]) - else: - self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile2"]) - self.set_bool_param(config[profile_type]["param"], True) - self.disable_all_params() - self.spin_for_time(wait_time=wait_time_s) - self.set_string_param(config[profile_type]["profile"], profile) - self.set_string_param(config[profile_type]["format"], format) - self.set_bool_param(config[profile_type]["param"], True) - self.spin_for_time(wait_time=wait_time_s) - try: - ret = self.run_test(themes) - assert ret[0], ret[1] - assert self.process_data(themes), " ".join(key) + " failed" - num_passed += 1 - passed_tests.append(" ".join(key)) - except Exception as e: - exc_type, exc_obj, exc_tb = sys.exc_info() - fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1] - print("Test failed") - print(e) - print(exc_type, fname, exc_tb.tb_lineno) - num_failed += 1 - failed_tests.append(" ".join(key)) - debug_print("Color tests completed") - for key in cap["depth_profile"]: - profile_type = key[0] - profile = key[1] - format = key[2] - print("Testing " + " ".join(key)) - - themes[0]['topic'] = config[profile_type]['topic'] - themes[0]['width'] = int(profile.split('x')[0]) - themes[0]['height'] = int(profile.split('x')[1]) - if themes[0]['width'] == int(config[profile_type]["default_profile2"].split('x')[0]): - self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile1"]) - else: - self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile2"]) - self.set_bool_param(config[profile_type]["param"], True) - - - self.disable_all_params() - self.spin_for_time(wait_time=wait_time_s) - self.set_string_param(config[profile_type]["profile"], profile) - self.set_string_param(config[profile_type]["format"], format) - self.set_bool_param(config[profile_type]["param"], True) - self.spin_for_time(wait_time=wait_time_s) - try: - ret = self.run_test(themes) - assert ret[0], ret[1] - assert self.process_data(themes), " ".join(key) + " failed" - num_passed += 1 - passed_tests.append(" ".join(key)) - except Exception as e: - print("Test failed") - print(e) - num_failed += 1 - failed_tests.append(" ".join(key)) - debug_print("Depth tests completed") - finally: - #this step is important because the test will fail next time - pytest_rs_utils.kill_realsense2_camera_node() - self.shutdown() - print("Tests passed " + str(num_passed)) - if len(passed_tests): - debug_print("\nPassed tests:" + params['device_type']) - debug_print("\n".join(passed_tests)) - print("Tests failed " + str(num_failed)) - if len(failed_tests): - print("\nFailed tests:" + params['device_type']) - print("\n".join(failed_tests)) - - def disable_all_params(self): - self.set_bool_param('enable_color', False) - self.set_bool_param('enable_depth', False) - self.set_bool_param('enable_infra', False) - self.set_bool_param('enable_infra1', False) - self.set_bool_param('enable_infra2', False) \ No newline at end of file From 2c8a39b2470a91b57d302c349e572605e5ea00a3 Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Mon, 11 Sep 2023 17:28:12 +0530 Subject: [PATCH 55/76] All topics may need more time in CI --- realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py b/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py index 22c4e1caee..0e77f552ee 100644 --- a/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py +++ b/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py @@ -52,7 +52,7 @@ 'infra_height': '0', 'enable_infra1':'true', 'enable_infra2':'true', - #'align_depth.enable':'true', + 'align_depth.enable':'true', } ''' To test all topics published @@ -99,7 +99,7 @@ def test_all_topics(self,delayed_launch_descr_with_parameters): initialize, run and check the data ''' self.init_test("RsTest"+params['camera_name']) - ret = self.run_test(themes) + ret = self.run_test(themes, timeout=25.0) assert ret[0], ret[1] time.sleep(0.5) assert self.process_data(themes), "Data check failed, probably the rosbag file changed?" From 5adccee293eb8f5442516aba1c53517734fbfa03 Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Mon, 11 Sep 2023 17:51:57 +0530 Subject: [PATCH 56/76] All topics testing --- realsense2_camera/test/utils/pytest_rs_utils.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index c07b135065..291ea61b16 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/realsense2_camera/test/utils/pytest_rs_utils.py @@ -822,7 +822,6 @@ def set_integer_param(self, param_name, param_value): return self.send_param(req) def spin_for_data(self,themes, timeout=5.0): - start = time.time() ''' timeout value varies depending upon the system, it needs to be more if the access is over the network @@ -830,6 +829,7 @@ def spin_for_data(self,themes, timeout=5.0): print('Waiting for topic... ' ) flag = False data_not_expected = [i for i in themes if (i["expected_data_chunks"]) == 0] + start = time.time() while (time.time() - start) < timeout: rclpy.spin_once(self.node, timeout_sec=1) debug_print('Spun once... ' ) @@ -847,7 +847,9 @@ def spin_for_data(self,themes, timeout=5.0): if data_not_expected == False: print("Timed out waiting for", timeout, "seconds" ) return False, "run_test timedout" - return flag,"" + if flag: + return flag,"" + return flag,"Unexpected error in spin_for_data" def spin_for_time(self,wait_time): start = time.time() @@ -885,8 +887,8 @@ def run_test(self, themes, initial_wait_time=0.0, timeout=5.0): else: print(e) self.flag =False,e - return self.flag + def get_tfs(self, coupled_frame_ids): return self.node.get_tfs(coupled_frame_ids) From a8225d1dedbd2f8a7847140d0c16ff76d66e7f77 Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Mon, 11 Sep 2023 18:20:44 +0530 Subject: [PATCH 57/76] All topics testing1 --- realsense2_camera/test/utils/pytest_rs_utils.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index 291ea61b16..2d12903ed9 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/realsense2_camera/test/utils/pytest_rs_utils.py @@ -830,6 +830,7 @@ def spin_for_data(self,themes, timeout=5.0): flag = False data_not_expected = [i for i in themes if (i["expected_data_chunks"]) == 0] start = time.time() + msg = "Unexpected error in spin_for_data" while (time.time() - start) < timeout: rclpy.spin_once(self.node, timeout_sec=1) debug_print('Spun once... ' ) @@ -847,9 +848,10 @@ def spin_for_data(self,themes, timeout=5.0): if data_not_expected == False: print("Timed out waiting for", timeout, "seconds" ) return False, "run_test timedout" + flag = True if flag: return flag,"" - return flag,"Unexpected error in spin_for_data" + return flag,msg def spin_for_time(self,wait_time): start = time.time() From be59d2e6a5164f5a4017838398541fd601674e0f Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Mon, 11 Sep 2023 18:41:32 +0530 Subject: [PATCH 58/76] All topics testing2 --- realsense2_camera/test/utils/pytest_rs_utils.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index 2d12903ed9..8326ce5592 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/realsense2_camera/test/utils/pytest_rs_utils.py @@ -830,7 +830,7 @@ def spin_for_data(self,themes, timeout=5.0): flag = False data_not_expected = [i for i in themes if (i["expected_data_chunks"]) == 0] start = time.time() - msg = "Unexpected error in spin_for_data" + msg = "" while (time.time() - start) < timeout: rclpy.spin_once(self.node, timeout_sec=1) debug_print('Spun once... ' ) @@ -848,9 +848,11 @@ def spin_for_data(self,themes, timeout=5.0): if data_not_expected == False: print("Timed out waiting for", timeout, "seconds" ) return False, "run_test timedout" - flag = True - if flag: - return flag,"" + if flag ==False: + for theme in themes: + if theme['expected_data_chunks'] > int(self.node.get_num_chunks(theme['topic'])): + msg = "Data expected, but not received for: " + theme['topic'] + break return flag,msg def spin_for_time(self,wait_time): From ac6301e47945a471fe16e9fd718c880d5c510426 Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Mon, 11 Sep 2023 20:28:31 +0530 Subject: [PATCH 59/76] All topics testing3 --- .../rosbag/test_rosbag_all_topics_test.py | 8 +------- .../test/utils/pytest_rs_utils.py | 20 ++++++++++--------- 2 files changed, 12 insertions(+), 16 deletions(-) diff --git a/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py b/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py index 0e77f552ee..e462e91b57 100644 --- a/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py +++ b/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py @@ -44,15 +44,9 @@ test_params_all_topics = {"rosbag_filename":get_rosbag_file_path("outdoors_1color.bag"), 'camera_name': 'AllTopics', - 'color_width': '0', - 'color_height': '0', - 'depth_width': '0', - 'depth_height': '0', - 'infra_width': '0', - 'infra_height': '0', 'enable_infra1':'true', 'enable_infra2':'true', - 'align_depth.enable':'true', + #'align_depth.enable':'true', } ''' To test all topics published diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index 8326ce5592..810007f4ba 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/realsense2_camera/test/utils/pytest_rs_utils.py @@ -676,7 +676,7 @@ def image_msg_to_numpy(self, data): def rsCallback(self, topic, msg_type, store_raw_data): debug_print("RSCallback") def _rsCallback(data): - debug_print('Got the callback for ' + topic) + print('Got the callback for ' + topic) #print(data.header) self.flag = True if store_raw_data == True: @@ -828,7 +828,11 @@ def spin_for_data(self,themes, timeout=5.0): ''' print('Waiting for topic... ' ) flag = False - data_not_expected = [i for i in themes if (i["expected_data_chunks"]) == 0] + data_not_expected1 = [i for i in themes if (i["expected_data_chunks"]) == 0] + if data_not_expected1 == []: + data_not_expected = False + else: + data_not_expected = True start = time.time() msg = "" while (time.time() - start) < timeout: @@ -846,13 +850,11 @@ def spin_for_data(self,themes, timeout=5.0): break else: if data_not_expected == False: - print("Timed out waiting for", timeout, "seconds" ) - return False, "run_test timedout" - if flag ==False: - for theme in themes: - if theme['expected_data_chunks'] > int(self.node.get_num_chunks(theme['topic'])): - msg = "Data expected, but not received for: " + theme['topic'] - break + msg = "Timedout: Data expected, but not received for: " + for theme in themes: + if theme['expected_data_chunks'] > int(self.node.get_num_chunks(theme['topic'])): + msg += " " + theme['topic'] + return False, msg return flag,msg def spin_for_time(self,wait_time): From 53e39e769d62946f6c5f30cbab24efe77b55a69c Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Mon, 11 Sep 2023 20:48:13 +0530 Subject: [PATCH 60/76] All topics testing4 --- realsense2_camera/test/utils/pytest_rs_utils.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index 810007f4ba..578d27b469 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/realsense2_camera/test/utils/pytest_rs_utils.py @@ -515,7 +515,7 @@ def get_rs_node_description(params): executable='realsense2_camera_node', parameters=[tmp_yaml.name], output='screen', - arguments=['--ros-args', '--log-level', "info"], + arguments=['--ros-args', '--log-level', "debug"], emulate_tty=True, ) From 7777ae7e591d7ea3128bef31af5bfaf5abd28611 Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Mon, 11 Sep 2023 22:28:30 +0530 Subject: [PATCH 61/76] All topics added to regression only, excluded from CI --- .../test/rosbag/test_rosbag_all_topics_test.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py b/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py index e462e91b57..3c5bdb83ac 100644 --- a/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py +++ b/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py @@ -51,7 +51,15 @@ ''' To test all topics published ''' +''' +To test all topics published + +The test doesn't work in CI due to sync. The unlike the live camera, rosbag node publishes the extrinsics only once, +not every time the subscription is created. The CI due to limited resource can start the ros node much earlier than +the testcase, hence missing the published data by the test +''' @pytest.mark.rosbag +@pytest.mark.skipif (os.getenv('RS_ROS_REGRESSION', "not found") == "not found",reason="The test doesn't work in CI") @pytest.mark.parametrize("delayed_launch_descr_with_parameters", [test_params_all_topics],indirect=True) @pytest.mark.launch(fixture=delayed_launch_descr_with_parameters) class TestAllTopics(pytest_rs_utils.RsTestBaseClass): From 31b7a80c497505eb71371317fab082527432fa66 Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Thu, 14 Sep 2023 17:17:03 +0530 Subject: [PATCH 62/76] updated tests for the failures in CI --- .../live_camera/test_camera_aligned_tests.py | 13 +++-- .../test/live_camera/test_camera_fps_tests.py | 30 ++++++++++-- .../test/live_camera/test_camera_imu_tests.py | 1 + .../rosbag/test_rosbag_all_topics_test.py | 4 +- .../test/rosbag/test_rosbag_depth_tests.py | 19 +++++--- .../test/utils/pytest_rs_utils.py | 48 ++++++++++++------- 6 files changed, 80 insertions(+), 35 deletions(-) diff --git a/realsense2_camera/test/live_camera/test_camera_aligned_tests.py b/realsense2_camera/test/live_camera/test_camera_aligned_tests.py index d489b5d546..0c17e76e47 100644 --- a/realsense2_camera/test/live_camera/test_camera_aligned_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_aligned_tests.py @@ -66,6 +66,7 @@ 1. Only a subset of parameter types are implemented in py_rs_utils, it has to be extended for others 2. After setting the param, rclpy.spin_once may be needed.Test passes even without this though. ''' + @pytest.mark.parametrize("launch_descr_with_parameters",[ pytest.param(test_params_align_depth_color_d455, marks=pytest.mark.d455), pytest.param(test_params_align_depth_color_d415, marks=pytest.mark.d415), @@ -76,6 +77,9 @@ class TestCamera_AlignDepthColor(pytest_rs_utils.RsTestBaseClass): def test_camera_align_depth_color(self,launch_descr_with_parameters): params = launch_descr_with_parameters[1] + if pytest_live_camera_utils.check_if_camera_connected(params['device_type']) == False: + print("Device not found? : " + params['device_type']) + return themes = [ {'topic':get_node_heirarchy(params)+'/color/image_raw', 'msg_type':msg_Image, @@ -122,8 +126,6 @@ def test_camera_align_depth_color(self,launch_descr_with_parameters): pytest_rs_utils.kill_realsense2_camera_node() self.shutdown() - - test_params_all_profiles_d455 = { 'camera_name': 'D455', 'device_type': 'D455', @@ -167,13 +169,16 @@ def test_camera_align_depth_color(self,launch_descr_with_parameters): pytest.param(test_params_all_profiles_d435, marks=pytest.mark.d435),] ,indirect=True) @pytest.mark.launch(fixture=launch_descr_with_parameters) -class TestCamera_AlignDepthColor(pytest_rs_utils.RsTestBaseClass): - def test_camera_align_depth_color_all(self,launch_descr_with_parameters): +class TestCamera_AllAlignDepthColor(pytest_rs_utils.RsTestBaseClass): + def test_camera_all_align_depth_color(self,launch_descr_with_parameters): skipped_tests = [] failed_tests = [] num_passed = 0 num_failed = 0 params = launch_descr_with_parameters[1] + if pytest_live_camera_utils.check_if_camera_connected(params['device_type']) == False: + print("Device not found? : " + params['device_type']) + return themes = [ {'topic':get_node_heirarchy(params)+'/color/image_raw', 'msg_type':msg_Image, diff --git a/realsense2_camera/test/live_camera/test_camera_fps_tests.py b/realsense2_camera/test/live_camera/test_camera_fps_tests.py index 291e4eaf8c..31b2d31187 100644 --- a/realsense2_camera/test/live_camera/test_camera_fps_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_fps_tests.py @@ -38,16 +38,35 @@ from rcl_interfaces.msg import ParameterValue from rcl_interfaces.srv import SetParameters, GetParameters, ListParameters -test_params_test_fps = { +test_params_test_fps_d455 = { 'camera_name': 'D455', 'device_type': 'D455', + + } +test_params_test_fps_d415 = { + 'camera_name': 'D415', + 'device_type': 'D415', + } +test_profiles ={ + 'D455':{ + 'depth_test_profiles':['640x480x5','640x480x15', '640x480x30', '640x480x90'], + 'color_test_profiles':['640x480x5','640x480x15', '640x480x30', '1280x720x30'] + }, + 'D415':{ + 'depth_test_profiles':['640x480x6','640x480x15', '640x480x30', '640x480x90'], + 'color_test_profiles':['640x480x6','640x480x15', '640x480x30', '1280x720x30'] } +} ''' The test was implemented to check the fps of Depth and Color frames. The RosTopicHz had to be modified to make it work, see py_rs_utils for more details. To check the fps, a value 'expected_fps_in_hz' has to be added to the corresponding theme ''' -@pytest.mark.parametrize("launch_descr_with_parameters", [test_params_test_fps],indirect=True) +@pytest.mark.parametrize("launch_descr_with_parameters", [ + pytest.param(test_params_test_fps_d455, marks=pytest.mark.d455), + pytest.param(test_params_test_fps_d415, marks=pytest.mark.d415), + #pytest.param(test_params_test_fps_d435, marks=pytest.mark.d435), + ],indirect=True) @pytest.mark.launch(fixture=launch_descr_with_parameters) class TestCamera_TestFPS(pytest_rs_utils.RsTestBaseClass): def test_camera_test_fps(self,launch_descr_with_parameters): @@ -68,12 +87,14 @@ def test_camera_test_fps(self,launch_descr_with_parameters): 'expected_data_chunks':100, } ] - profiles = ['640x480x5','640x480x15', '640x480x30', '640x480x90'] + profiles = test_profiles[params['camera_name']]['depth_test_profiles'] + assert self.set_bool_param('enable_color', False) for profile in profiles: print("Testing profile: ", profile) themes[0]['expected_fps_in_hz'] = float(profile.split('x')[2]) assert self.set_string_param('depth_module.profile', profile) assert self.set_bool_param('enable_depth', True) + self.spin_for_time(0.5) ret = self.run_test(themes, timeout=25.0) assert ret[0], ret[1] assert self.process_data(themes) @@ -84,7 +105,8 @@ def test_camera_test_fps(self,launch_descr_with_parameters): 'expected_data_chunks':100, } ] - profiles = ['640x480x5','640x480x15', '640x480x30', '1280x720x30'] + assert self.set_bool_param('enable_depth', False) + profiles = test_profiles[params['camera_name']]['color_test_profiles'] for profile in profiles: print("Testing profile: ", profile) themes[0]['expected_fps_in_hz'] = float(profile.split('x')[2]) diff --git a/realsense2_camera/test/live_camera/test_camera_imu_tests.py b/realsense2_camera/test/live_camera/test_camera_imu_tests.py index e2defb6b52..317833e331 100644 --- a/realsense2_camera/test/live_camera/test_camera_imu_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_imu_tests.py @@ -77,6 +77,7 @@ def test_LiveCamera_check_motion_sensor(self,launch_descr_with_parameters): assert self.process_data(themes), msg + " failed" msg = "Test with the accel false " + self.set_integer_param('unite_imu_method', 0) self.set_bool_param('enable_accel', False) self.set_bool_param('enable_gyro', True) themes[ACCEL_TOPIC]['expected_data_chunks'] = 0 diff --git a/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py b/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py index 3c5bdb83ac..9cda836f3a 100644 --- a/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py +++ b/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py @@ -46,7 +46,7 @@ 'camera_name': 'AllTopics', 'enable_infra1':'true', 'enable_infra2':'true', - #'align_depth.enable':'true', + 'align_depth.enable':'true', } ''' To test all topics published @@ -101,7 +101,7 @@ def test_all_topics(self,delayed_launch_descr_with_parameters): initialize, run and check the data ''' self.init_test("RsTest"+params['camera_name']) - ret = self.run_test(themes, timeout=25.0) + ret = self.run_test(themes) assert ret[0], ret[1] time.sleep(0.5) assert self.process_data(themes), "Data check failed, probably the rosbag file changed?" diff --git a/realsense2_camera/test/rosbag/test_rosbag_depth_tests.py b/realsense2_camera/test/rosbag/test_rosbag_depth_tests.py index 1b628e07f6..51d574da85 100644 --- a/realsense2_camera/test/rosbag/test_rosbag_depth_tests.py +++ b/realsense2_camera/test/rosbag/test_rosbag_depth_tests.py @@ -20,10 +20,14 @@ import pytest import rclpy +from rclpy.qos import DurabilityPolicy +from rclpy.qos import HistoryPolicy +from rclpy.qos import QoSProfile from sensor_msgs.msg import Image as msg_Image from sensor_msgs.msg import Imu as msg_Imu from sensor_msgs.msg import PointCloud2 as msg_PointCloud2 +from tf2_msgs.msg import TFMessage as msg_TFMessage import numpy as np @@ -78,12 +82,6 @@ def test_depth_points_cloud_1(self,delayed_launch_descr_with_parameters): 'expected_data_chunks':1, 'data':data1 }, - {'topic':get_node_heirarchy(params)+'/depth/image_rect_raw', - 'msg_type':msg_Image, - 'expected_data_chunks':1, - 'data':data2 - } - ] try: ''' @@ -124,6 +122,11 @@ def test_static_tf_1(self,delayed_launch_descr_with_parameters): 'msg_type':msg_Image, 'expected_data_chunks':1, 'static_tf':True, + }, + {'topic':'/tf_static', + 'msg_type':msg_TFMessage, + 'expected_data_chunks':1, + 'qos' : QoSProfile(depth=100,durability=DurabilityPolicy.TRANSIENT_LOCAL,history=HistoryPolicy.KEEP_LAST,)#static } ] try: @@ -145,7 +148,9 @@ def process_data(self, themes): (self.params['camera_name']+'_infra1_frame', self.params['camera_name']+'_color_frame'): ([-0.00010158783697988838, 0.014841210097074509, -0.00022671300393994898], [-0.0008337442995980382, 0.0010442184284329414, -0.0009920650627464056, 0.9999986290931702])} frame_ids = [self.params['camera_name']+'_link', self.params['camera_name']+'_depth_frame', self.params['camera_name']+'_infra1_frame', self.params['camera_name']+'_infra2_frame', self.params['camera_name']+'_color_frame', self.params['camera_name']+'_fisheye_frame', self.params['camera_name']+'_pose'] coupled_frame_ids = [xx for xx in itertools.combinations(frame_ids, 2)] - tfs_data = self.get_tfs(coupled_frame_ids) + data = self.node.pop_first_chunk('/tf_static') + coupled_frame_ids = [xx for xx in itertools.combinations(frame_ids, 2)] + tfs_data = self.get_transform_data(data, coupled_frame_ids, is_static=True) ret = pytest_rs_utils.staticTFTest(tfs_data, expected_data) assert ret[0], ret[1] return ret[0] diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index 578d27b469..db9c09c3c0 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/realsense2_camera/test/utils/pytest_rs_utils.py @@ -17,6 +17,7 @@ from collections import deque import functools import itertools +import subprocess import pytest @@ -443,6 +444,17 @@ def kill_realsense2_camera_node(): os.system(cmd) pass +''' +function gets all the topics for a camera node +''' + +def get_all_topics(camera_name=None): + cmd = 'ros2 topic list' + if camera_name!=None: + cmd += '| grep ' + camera_name + direct_output = os.popen(cmd).read() + return direct_output + ''' get the default parameters from the launch script so that the test doesn't have to get updated for each change to the parameter or default values @@ -511,11 +523,11 @@ def get_rs_node_description(params): #name=LaunchConfiguration("camera_name"), namespace=params["camera_namespace"], name=params["camera_name"], - #prefix=['xterm -e gdb --args'], + #prefix=['xterm -e gdb -ex=run --args'], executable='realsense2_camera_node', parameters=[tmp_yaml.name], output='screen', - arguments=['--ros-args', '--log-level', "debug"], + arguments=['--ros-args', '--log-level', "info"], emulate_tty=True, ) @@ -635,16 +647,6 @@ def create_subscription(self, msg_type, topic , data_type, store_raw_data, measu if self.tfBuffer == None: self.tfBuffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tfBuffer, super()) - def get_tfs(self, coupled_frame_ids): - res = dict() - for couple in coupled_frame_ids: - from_id, to_id = couple - if (self.tfBuffer.can_transform(from_id, to_id, rclpy.time.Time(), rclpy.time.Duration(nanoseconds=3e6))): - res[couple] = self.tfBuffer.lookup_transform(from_id, to_id, rclpy.time.Time(), rclpy.time.Duration(nanoseconds=1e6)).transform - else: - res[couple] = None - return res - def get_num_chunks(self,topic): return len(self.data[topic]) @@ -676,7 +678,10 @@ def image_msg_to_numpy(self, data): def rsCallback(self, topic, msg_type, store_raw_data): debug_print("RSCallback") def _rsCallback(data): - print('Got the callback for ' + topic) + ''' + enabling prints in callback reduces the fps in some cases + ''' + debug_print('Got the callback for ' + topic) #print(data.header) self.flag = True if store_raw_data == True: @@ -854,7 +859,9 @@ def spin_for_data(self,themes, timeout=5.0): for theme in themes: if theme['expected_data_chunks'] > int(self.node.get_num_chunks(theme['topic'])): msg += " " + theme['topic'] + msg += " Nodes available: " + str(self.node.get_node_names()) return False, msg + flag = True return flag,msg def spin_for_time(self,wait_time): @@ -865,7 +872,7 @@ def spin_for_time(self,wait_time): print('Spun for time once... ' ) rclpy.spin_once(self.node, timeout_sec=wait_time) - def run_test(self, themes, initial_wait_time=0.0, timeout=5.0): + def run_test(self, themes, initial_wait_time=0.0, timeout=0): try: for theme in themes: store_raw_data = False @@ -880,6 +887,15 @@ def run_test(self, themes, initial_wait_time=0.0, timeout=5.0): qos_type = theme['qos'] self.create_subscription(theme['msg_type'], theme['topic'] , qos_type,store_raw_data, measure_hz) print('subscription created for ' + theme['topic']) + ''' + change the default based on whether data is expected or not + ''' + if timeout == 0: + timeout = 5.0 + data_not_expected1 = [i for i in themes if (i["expected_data_chunks"]) == 0] + if data_not_expected1 == []: + timeout = 50.0 #high value due to resource constraints in CI + if initial_wait_time != 0.0: self.spin_for_time(initial_wait_time) self.flag = self.spin_for_data(themes, timeout) @@ -895,10 +911,6 @@ def run_test(self, themes, initial_wait_time=0.0, timeout=5.0): self.flag =False,e return self.flag - def get_tfs(self, coupled_frame_ids): - return self.node.get_tfs(coupled_frame_ids) - - def get_transform_data(self, data, coupled_frame_ids, is_static=False): tfBuffer = tf2_ros.Buffer() for transform in data.transforms: From b0d14bf4a39b783cc6feebc239f7be69ff5eca5d Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Thu, 14 Sep 2023 17:40:12 +0530 Subject: [PATCH 63/76] correct the d435 marker --- realsense2_camera/test/live_camera/test_camera_tf_tests.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/realsense2_camera/test/live_camera/test_camera_tf_tests.py b/realsense2_camera/test/live_camera/test_camera_tf_tests.py index 4e8250f063..a31f74e77c 100644 --- a/realsense2_camera/test/live_camera/test_camera_tf_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_tf_tests.py @@ -160,7 +160,7 @@ def process_data(self, themes, enable_infra1): } @pytest.mark.parametrize("launch_descr_with_parameters", [ pytest.param(test_params_tf_d455, marks=pytest.mark.d455), - pytest.param(test_params_tf_d435, marks=pytest.mark.d415), + pytest.param(test_params_tf_d435, marks=pytest.mark.d435), pytest.param(test_params_tf_d415, marks=pytest.mark.d415), ],indirect=True) @pytest.mark.launch(fixture=launch_descr_with_parameters) From 1376bb7e918241bd956dc0725e5e86f7b5ea8221 Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Fri, 15 Sep 2023 16:23:33 +0530 Subject: [PATCH 64/76] added markers for d415 d455 specific tests --- realsense2_camera/test/README.md | 2 +- realsense2_camera/test/live_camera/test_d415_basic_tests.py | 1 + realsense2_camera/test/live_camera/test_d455_basic_tests.py | 1 + 3 files changed, 3 insertions(+), 1 deletion(-) diff --git a/realsense2_camera/test/README.md b/realsense2_camera/test/README.md index 9624848955..ea3a043a92 100644 --- a/realsense2_camera/test/README.md +++ b/realsense2_camera/test/README.md @@ -158,7 +158,7 @@ If a user wants to add a test to this conditional skip, user can add by adding a RS_ROS_REGRESSION=1 PYTHONPATH=$PYTHONPATH:$PWD/realsense2_camera/test/utils:$PWD/realsense2_camera//launch:$PWD/realsense2_camera//scripts pytest-3 -s realsense2_camera/test/live_camera/test_camera_aligned_tests.py -k test_camera_align_depth_color_all -m d415 ## Points to be noted while writing pytests -The tests that are in one file are normally run in parallel. So if there are multiple tests in one file, the system capacity can influence the test execution. It's recomended to have 3-4 tests in file, more than that can affect the test results due to delays. +The tests that are in one file are normally run in parallel, there could also be changes in the pytest plugin. So if there are multiple tests in one file, the system capacity can influence the test execution. It's recomended to have 3-4 tests in file, more than that can affect the test results due to delays. ### Passing/changing parameters The parameters passed while creating the node can be initialized individually for each test, please see the test_parameterized_template example for reference. The default values are taken from rs_launch.py and the passed parameters are used for overriding the default values. The parameters that can be dynamically modified can be changed using the param interface provided. However, the function create_param_ifs has to be called to create this interface. Please see the test_d455_basic_tests.py for reference. There are specific functions to change the string, integer and bool parameters, the utils can be extended if any more types are needed. ### Difference in setting the bool parameters diff --git a/realsense2_camera/test/live_camera/test_d415_basic_tests.py b/realsense2_camera/test/live_camera/test_d415_basic_tests.py index 8b88a30e67..d93bbe5b57 100644 --- a/realsense2_camera/test/live_camera/test_d415_basic_tests.py +++ b/realsense2_camera/test/live_camera/test_d415_basic_tests.py @@ -49,6 +49,7 @@ 1. Only a subset of parameter types are implemented in py_rs_utils, it has to be extended for others 2. After setting the param, rclpy.spin_once may be needed.Test passes even without this though. ''' +@pytest.mark.d415 @pytest.mark.parametrize("launch_descr_with_parameters", [test_params_depth_avg_1],indirect=True) @pytest.mark.launch(fixture=launch_descr_with_parameters) class TestD415_Change_Resolution(pytest_rs_utils.RsTestBaseClass): diff --git a/realsense2_camera/test/live_camera/test_d455_basic_tests.py b/realsense2_camera/test/live_camera/test_d455_basic_tests.py index fcf4561714..0d2163a3e3 100644 --- a/realsense2_camera/test/live_camera/test_d455_basic_tests.py +++ b/realsense2_camera/test/live_camera/test_d455_basic_tests.py @@ -49,6 +49,7 @@ 1. Only a subset of parameter types are implemented in py_rs_utils, it has to be extended for others 2. After setting the param, rclpy.spin_once may be needed.Test passes even without this though. ''' +@pytest.mark.d455 @pytest.mark.parametrize("launch_descr_with_parameters", [test_params_depth_avg_1],indirect=True) @pytest.mark.launch(fixture=launch_descr_with_parameters) class TestD455_Change_Resolution(pytest_rs_utils.RsTestBaseClass): From 8e499f1d5f9841f8d82e98e6739ecfe150671da0 Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Thu, 5 Oct 2023 19:04:39 +0530 Subject: [PATCH 65/76] fixed live camera connection checking --- realsense2_camera/test/live_camera/test_camera_fps_tests.py | 4 ++++ realsense2_camera/test/live_camera/test_camera_imu_tests.py | 3 +++ realsense2_camera/test/live_camera/test_d415_basic_tests.py | 4 ++++ realsense2_camera/test/live_camera/test_d455_basic_tests.py | 5 +++++ 4 files changed, 16 insertions(+) diff --git a/realsense2_camera/test/live_camera/test_camera_fps_tests.py b/realsense2_camera/test/live_camera/test_camera_fps_tests.py index 31b2d31187..c4b91354d4 100644 --- a/realsense2_camera/test/live_camera/test_camera_fps_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_fps_tests.py @@ -29,6 +29,7 @@ sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils")) import pytest_rs_utils +import pytest_live_camera_utils from pytest_rs_utils import launch_descr_with_parameters from pytest_rs_utils import get_rosbag_file_path @@ -71,6 +72,9 @@ class TestCamera_TestFPS(pytest_rs_utils.RsTestBaseClass): def test_camera_test_fps(self,launch_descr_with_parameters): params = launch_descr_with_parameters[1] + if pytest_live_camera_utils.check_if_camera_connected(params['device_type']) == False: + print("Device not found? : " + params['device_type']) + return try: ''' initialize, run and check the data diff --git a/realsense2_camera/test/live_camera/test_camera_imu_tests.py b/realsense2_camera/test/live_camera/test_camera_imu_tests.py index 317833e331..384d249f47 100644 --- a/realsense2_camera/test/live_camera/test_camera_imu_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_imu_tests.py @@ -55,6 +55,9 @@ class TestLiveCamera_TestMotionSensor(pytest_rs_utils.RsTestBaseClass): def test_LiveCamera_check_motion_sensor(self,launch_descr_with_parameters): params = launch_descr_with_parameters[1] + if pytest_live_camera_utils.check_if_camera_connected(params['device_type']) == False: + print("Device not found? : " + params['device_type']) + return themes = [{'topic':get_node_heirarchy(params)+'/imu', 'msg_type':msg_Imu,'expected_data_chunks':1}, {'topic':get_node_heirarchy(params)+'/gyro/sample', 'msg_type':msg_Imu,'expected_data_chunks':1}, {'topic':get_node_heirarchy(params)+'/accel/sample', 'msg_type':msg_Imu,'expected_data_chunks':1}] diff --git a/realsense2_camera/test/live_camera/test_d415_basic_tests.py b/realsense2_camera/test/live_camera/test_d415_basic_tests.py index d93bbe5b57..649f84e7bd 100644 --- a/realsense2_camera/test/live_camera/test_d415_basic_tests.py +++ b/realsense2_camera/test/live_camera/test_d415_basic_tests.py @@ -55,8 +55,12 @@ class TestD415_Change_Resolution(pytest_rs_utils.RsTestBaseClass): def test_D415_Change_Resolution(self,launch_descr_with_parameters): params = launch_descr_with_parameters[1] + if pytest_live_camera_utils.check_if_camera_connected(params['device_type']) == False: + print("Device not found? : " + params['device_type']) + return failed_tests = [] num_passed = 0 + num_failed = 0 themes = [{'topic':get_node_heirarchy(params)+'/color/image_raw', 'msg_type':msg_Image,'expected_data_chunks':1}] config = pytest_live_camera_utils.get_profile_config(get_node_heirarchy(params)) diff --git a/realsense2_camera/test/live_camera/test_d455_basic_tests.py b/realsense2_camera/test/live_camera/test_d455_basic_tests.py index 0d2163a3e3..7848ff4986 100644 --- a/realsense2_camera/test/live_camera/test_d455_basic_tests.py +++ b/realsense2_camera/test/live_camera/test_d455_basic_tests.py @@ -29,6 +29,7 @@ sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils")) import pytest_rs_utils +import pytest_live_camera_utils from pytest_rs_utils import launch_descr_with_parameters from pytest_rs_utils import get_rosbag_file_path @@ -55,6 +56,10 @@ class TestD455_Change_Resolution(pytest_rs_utils.RsTestBaseClass): def test_D455_Change_Resolution(self,launch_descr_with_parameters): params = launch_descr_with_parameters[1] + if pytest_live_camera_utils.check_if_camera_connected(params['device_type']) == False: + print("Device not found? : " + params['device_type']) + return + themes = [ {'topic':get_node_heirarchy(params)+'/color/image_raw', 'msg_type':msg_Image, From 218e327cf050883a438ae6634a1aba9252bd04ae Mon Sep 17 00:00:00 2001 From: Samer Khshiboun <99127997+SamerKhshiboun@users.noreply.github.com> Date: Thu, 5 Oct 2023 22:10:44 +0300 Subject: [PATCH 66/76] Update README.md --- README.md | 1 - 1 file changed, 1 deletion(-) diff --git a/README.md b/README.md index 75822f5048..0311e9ed2b 100644 --- a/README.md +++ b/README.md @@ -188,7 +188,6 @@ Camera Name And Camera Namespace

-### Usage User can set the camera name and camera namespace, to distinguish between cameras and platforms, which helps identifying the right nodes and topics to work with. ### Example From b1e3963aa4f45a9d2f49403e376567be98b11c50 Mon Sep 17 00:00:00 2001 From: Samer Khshiboun <99127997+SamerKhshiboun@users.noreply.github.com> Date: Tue, 10 Oct 2023 03:58:57 +0300 Subject: [PATCH 67/76] Update README.md --- README.md | 71 +++++++++++++++---------------------------------------- 1 file changed, 19 insertions(+), 52 deletions(-) diff --git a/README.md b/README.md index 0311e9ed2b..dfe404158b 100644 --- a/README.md +++ b/README.md @@ -23,11 +23,11 @@
## Table of contents - * [ROS1 and ROS2 legacy](#legacy) + * [ROS1 and ROS2 legacy](#ros1-and-ros2-legacy) * [Installation](#installation) * [Usage](#usage) - * [Starting the camera node](#start-camera-node) - * [Camera name and namespace](#camera-name-and-namespace) + * [Starting the camera node](#start-the-camera-node) + * [Camera name and namespace](#camera-name-and-camera-namespace) * [Parameters](#parameters) * [ROS2-vs-Optical Coordination Systems](#coordination) * [TF from coordinate A to coordinate B](#tfs) @@ -43,9 +43,7 @@
-

- Legacy -

+# ROS1 and ROS2 Legacy
@@ -80,9 +78,7 @@
-

- Installation -

+# Installation
@@ -165,13 +161,9 @@
-

- Usage -

+# Usage -

- Start the camera node -

+## Start the camera node #### with ros2 run: ros2 run realsense2_camera realsense2_camera_node @@ -184,9 +176,7 @@
-

- Camera Name And Camera Namespace -

+## Camera Name And Camera Namespace User can set the camera name and camera namespace, to distinguish between cameras and platforms, which helps identifying the right nodes and topics to work with. @@ -247,13 +237,9 @@ User can set the camera name and camera namespace, to distinguish between camera /camera/camera/device_info ``` -
- -

- Parameters -

+## Parameters ### Available Parameters: - For the entire list of parameters type `ros2 param list`. @@ -371,9 +357,7 @@ The `/diagnostics` topic includes information regarding the device temperatures
-

- ROS2(Robot) vs Optical(Camera) Coordination Systems: -

+## ROS2(Robot) vs Optical(Camera) Coordination Systems: - Point Of View: - Imagine we are standing behind of the camera, and looking forward. @@ -389,9 +373,7 @@ The `/diagnostics` topic includes information regarding the device temperatures
-

- TF from coordinate A to coordinate B: -

+## TF from coordinate A to coordinate B: - TF msg expresses a transform from coordinate frame "header.frame_id" (source) to the coordinate frame child_frame_id (destination) [Reference](http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/Transform.html) - In RealSense cameras, the origin point (0,0,0) is taken from the left IR (infra1) position and named as "camera_link" frame @@ -403,10 +385,7 @@ The `/diagnostics` topic includes information regarding the device temperatures
-

- Extrinsics from sensor A to sensor B: -

- +## Extrinsics from sensor A to sensor B: - Extrinsic from sensor A to sensor B means the position and orientation of sensor A relative to sensor B. - Imagine that B is the origin (0,0,0), then the Extrensics(A->B) describes where is sensor A relative to sensor B. @@ -441,9 +420,7 @@ translation:
-

- Published Topics -

+## Published Topics The published topics differ according to the device and parameters. After running the above command with D435i attached, the following list of topics will be available (This is a partial list. For full one type `ros2 topic list`): @@ -485,9 +462,7 @@ Enabling stream adds matching topics. For instance, enabling the gyro and accel
-

- RGBD Topic -

+## RGBD Topic RGBD new topic, publishing [RGB + Depth] in the same message (see RGBD.msg for reference). For now, works only with depth aligned to color images, as color and depth images are synchronized by frame time tag. @@ -507,9 +482,7 @@ ros2 launch realsense2_camera rs_launch.py enable_rgbd:=true enable_sync:=true a
-

- Metadata topic -

+## Metadata topic The metadata messages store the camera's available metadata in a *json* format. To learn more, a dedicated script for echoing a metadata topic in runtime is attached. For instance, use the following command to echo the camera/depth/metadata topic: ``` @@ -518,10 +491,8 @@ python3 src/realsense-ros/realsense2_camera/scripts/echo_metadada.py /camera/cam
-

- Post-Processing Filters -

- +## Post-Processing Filters + The following post processing filters are available: - ```align_depth```: If enabled, will publish the depth image aligned to the color image on the topic `/camera/camera/aligned_depth_to_color/image_raw`. - The pointcloud, if created, will be based on the aligned depth image. @@ -551,17 +522,13 @@ Each of the above filters have it's own parameters, following the naming convent
-

- Available services -

+## Available services - device_info : retrieve information about the device - serial_number, firmware_version etc. Type `ros2 interface show realsense2_camera_msgs/srv/DeviceInfo` for the full list. Call example: `ros2 service call /camera/camera/device_info realsense2_camera_msgs/srv/DeviceInfo`
-

- Efficient intra-process communication: -

+## Efficient intra-process communication: Our ROS2 Wrapper node supports zero-copy communications if loaded in the same process as a subscriber node. This can reduce copy times on image/pointcloud topics, especially with big frame resolutions and high FPS. From 88818df2e6f03982f96a78884b0fcea08d6e573b Mon Sep 17 00:00:00 2001 From: Samer Khshiboun <99127997+SamerKhshiboun@users.noreply.github.com> Date: Tue, 10 Oct 2023 04:01:26 +0300 Subject: [PATCH 68/76] Update README.md --- README.md | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/README.md b/README.md index dfe404158b..522ec969a6 100644 --- a/README.md +++ b/README.md @@ -29,15 +29,15 @@ * [Starting the camera node](#start-the-camera-node) * [Camera name and namespace](#camera-name-and-camera-namespace) * [Parameters](#parameters) - * [ROS2-vs-Optical Coordination Systems](#coordination) - * [TF from coordinate A to coordinate B](#tfs) - * [Extrinsics from sensor A to sensor B](#extrinsics) - * [Topics](#topics) - * [RGBD Topic](#rgbd) - * [Metadata Topic](#metadata) - * [Post-Processing Filters](#filters) - * [Available Services](#services) - * [Efficient intra-process communication](#intra-process) + * [ROS2-vs-Optical Coordination Systems](#ros2robot-vs-opticalcamera-coordination-systems) + * [TF from coordinate A to coordinate B](#tf-from-coordinate-a-to-coordinate-b) + * [Extrinsics from sensor A to sensor B](#extrinsics-from-sensor-a-to-sensor-b) + * [Topics](#published-topics) + * [RGBD Topic](#rgbd-topic) + * [Metadata Topic](#metadata-topic) + * [Post-Processing Filters](#post-processing-filters) + * [Available Services](#available-services) + * [Efficient intra-process communication](#efficient-intra-process-communication) * [Contributing](CONTRIBUTING.md) * [License](LICENSE) From 9ef9638020b1debe3f48660304270f39b2927153 Mon Sep 17 00:00:00 2001 From: Arun Prasad <127019120+Arun-Prasad-V@users.noreply.github.com> Date: Mon, 4 Dec 2023 16:05:14 +0530 Subject: [PATCH 69/76] Update main.yml Using setup-ros v0.7 --- .github/workflows/main.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 55ed6e9468..ecd144d630 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -49,7 +49,7 @@ jobs: ./pr_check.sh - name: build ROS2 - uses: ros-tooling/setup-ros@v0.6 + uses: ros-tooling/setup-ros@v0.7 with: required-ros-distributions: ${{ matrix.ros_distro }} From e09414dc09fca43341c3c317e8df3e22084dd9d0 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Wed, 22 Nov 2023 06:04:50 +0530 Subject: [PATCH 70/76] Disabling hdr while updating exposure & gain values --- README.md | 3 +++ realsense2_camera/src/ros_sensor.cpp | 19 ++++++++++++++++++- 2 files changed, 21 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 0311e9ed2b..6c0bbf9615 100644 --- a/README.md +++ b/README.md @@ -531,7 +531,10 @@ The following post processing filters are available: * The depth FOV and the texture FOV are not similar. By default, pointcloud is limited to the section of depth containing the texture. You can have a full depth to pointcloud, coloring the regions beyond the texture with zeros, by setting `pointcloud.allow_no_texture_points` to true. * pointcloud is of an unordered format by default. This can be changed by setting `pointcloud.ordered_pc` to true. - ```hdr_merge```: Allows depth image to be created by merging the information from 2 consecutive frames, taken with different exposure and gain values. + - `depth_module.hdr_enabled`: to enable/disable HDR - The way to set exposure and gain values for each sequence in runtime is by first selecting the sequence id, using the `depth_module.sequence_id` parameter and then modifying the `depth_module.gain`, and `depth_module.exposure`. + - From FW versions 5.14.x.x and above, if HDR is enabled, the preset configs (like exposure, gain, etc.,) cannot be updated. + - The user should disable the HDR first using `depth_module.hdr_enabled` parameter and then, update the required presets. - To view the effect on the infrared image for each sequence id use the `filter_by_sequence_id.sequence_id` parameter. - To initialize these parameters in start time use the following parameters: - `depth_module.exposure.1` diff --git a/realsense2_camera/src/ros_sensor.cpp b/realsense2_camera/src/ros_sensor.cpp index 635424d8c5..73b9cac11b 100644 --- a/realsense2_camera/src/ros_sensor.cpp +++ b/realsense2_camera/src/ros_sensor.cpp @@ -102,6 +102,24 @@ void RosSensor::UpdateSequenceIdCallback() if (!supports(RS2_OPTION_SEQUENCE_ID)) return; + bool is_hdr_enabled = static_cast(get_option(RS2_OPTION_HDR_ENABLED)); + + // Deleter to revert back the RS2_OPTION_HDR_ENABLED value at the end. + auto deleter_to_revert_hdr = std::unique_ptr>(&is_hdr_enabled, + [&](bool* enable_back_hdr) { + if (enable_back_hdr && *enable_back_hdr) + { + set_option(RS2_OPTION_HDR_ENABLED, true); + } + }); + + // From FW version 5.14.x.x, if HDR is enabled, updating UVC controls like exposure, gain , etc are restricted. + // So, disable it before updating. + if (is_hdr_enabled) + { + set_option(RS2_OPTION_HDR_ENABLED, false); + } + int original_seq_id = static_cast(get_option(RS2_OPTION_SEQUENCE_ID)); // To Set back to default. std::string module_name = create_graph_resource_name(rs2_to_ros(get_info(RS2_CAMERA_INFO_NAME))); @@ -146,7 +164,6 @@ void RosSensor::UpdateSequenceIdCallback() ROS_WARN_STREAM("Setting alternative callback: Failed to set parameter:" << option_name << " : " << e.what()); return; } - } void RosSensor::set_sensor_parameter_to_ros(rs2_option option) From 573d6a76fd4c36d4539bec4d67225a8a6de525f1 Mon Sep 17 00:00:00 2001 From: gwen2018 Date: Wed, 13 Dec 2023 18:05:01 -0800 Subject: [PATCH 71/76] add error handling to avoid crash when asic temperature monitor occasionally fails --- realsense2_camera/src/base_realsense_node.cpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 9870d3ac59..a3ebd0e675 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -1140,10 +1140,18 @@ void BaseRealSenseNode::startDiagnosticsUpdater() { for (rs2_option option : _monitor_options) { - if (sensor->supports(option)) + try { - status.add(rs2_option_to_string(option), sensor->get_option(option)); - got_temperature = true; + if (sensor->supports(option)) + { + status.add(rs2_option_to_string(option), sensor->get_option(option)); + got_temperature = true; + } + } + catch(const std::exception& ex) + { + got_temperature = false; + ROS_WARN_STREAM("An error has occurred during monitoring: " << ex.what()); } } if (got_temperature) break; From 8384c891b22a74ccd8f0e1ea32a2aca11c3d3d21 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Thu, 14 Dec 2023 21:44:48 +0530 Subject: [PATCH 72/76] Return failure in SetParam callback in case of error --- README.md | 2 +- realsense2_camera/src/dynamic_params.cpp | 12 +++++++----- realsense2_camera/src/sensor_params.cpp | 9 +-------- 3 files changed, 9 insertions(+), 14 deletions(-) diff --git a/README.md b/README.md index ee99a27473..a77621972e 100644 --- a/README.md +++ b/README.md @@ -505,7 +505,7 @@ The following post processing filters are available: - `depth_module.hdr_enabled`: to enable/disable HDR - The way to set exposure and gain values for each sequence in runtime is by first selecting the sequence id, using the `depth_module.sequence_id` parameter and then modifying the `depth_module.gain`, and `depth_module.exposure`. - From FW versions 5.14.x.x and above, if HDR is enabled, the preset configs (like exposure, gain, etc.,) cannot be updated. - - The user should disable the HDR first using `depth_module.hdr_enabled` parameter and then, update the required presets. + - Disable the HDR first using `depth_module.hdr_enabled` parameter and then, update the required presets. - To view the effect on the infrared image for each sequence id use the `filter_by_sequence_id.sequence_id` parameter. - To initialize these parameters in start time use the following parameters: - `depth_module.exposure.1` diff --git a/realsense2_camera/src/dynamic_params.cpp b/realsense2_camera/src/dynamic_params.cpp index acce9038ca..e51dfb1818 100644 --- a/realsense2_camera/src/dynamic_params.cpp +++ b/realsense2_camera/src/dynamic_params.cpp @@ -25,6 +25,8 @@ namespace realsense2_camera _params_backend.add_on_set_parameters_callback( [this](const std::vector & parameters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; for (const auto & parameter : parameters) { try @@ -43,15 +45,15 @@ namespace realsense2_camera } } } - catch(const std::out_of_range& e) - {} catch(const std::exception& e) { - std::cerr << e.what() << ":" << parameter.get_name() << '\n'; + result.successful = false; + result.reason = e.what(); + ROS_WARN_STREAM("Set parameter {" << parameter.get_name() + << "} failed: " << e.what()); } } - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; + return result; }); monitor_update_functions(); // Start parameters update thread diff --git a/realsense2_camera/src/sensor_params.cpp b/realsense2_camera/src/sensor_params.cpp index ff0cf151e9..c347a1d979 100644 --- a/realsense2_camera/src/sensor_params.cpp +++ b/realsense2_camera/src/sensor_params.cpp @@ -70,14 +70,7 @@ std::map get_enum_method(rs2::options sensor, rs2_option optio template void param_set_option(rs2::options sensor, rs2_option option, const rclcpp::Parameter& parameter) { - try - { - sensor.set_option(option, parameter.get_value()); - } - catch(const std::exception& e) - { - std::cout << "Failed to set value: " << e.what() << std::endl; - } + sensor.set_option(option, parameter.get_value()); } void SensorParams::clearParameters() From c5e5d3b47687099976506d4658cdab6daf843f21 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Thu, 30 Nov 2023 02:43:18 +0530 Subject: [PATCH 73/76] Fixing the data_type of ROS Params exposure & gain --- realsense2_camera/include/ros_sensor.h | 1 + realsense2_camera/src/ros_sensor.cpp | 7 ++++--- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/realsense2_camera/include/ros_sensor.h b/realsense2_camera/include/ros_sensor.h index c4430bfd4f..57d224300e 100644 --- a/realsense2_camera/include/ros_sensor.h +++ b/realsense2_camera/include/ros_sensor.h @@ -105,6 +105,7 @@ namespace realsense2_camera void set_sensor_auto_exposure_roi(); void registerAutoExposureROIOptions(); void UpdateSequenceIdCallback(); + template void set_sensor_parameter_to_ros(rs2_option option); private: diff --git a/realsense2_camera/src/ros_sensor.cpp b/realsense2_camera/src/ros_sensor.cpp index 73b9cac11b..2eae865995 100644 --- a/realsense2_camera/src/ros_sensor.cpp +++ b/realsense2_camera/src/ros_sensor.cpp @@ -154,8 +154,8 @@ void RosSensor::UpdateSequenceIdCallback() { set_option(RS2_OPTION_SEQUENCE_ID, parameter.get_value()); std::vector > funcs; - funcs.push_back([this](){set_sensor_parameter_to_ros(RS2_OPTION_GAIN);}); - funcs.push_back([this](){set_sensor_parameter_to_ros(RS2_OPTION_EXPOSURE);}); + funcs.push_back([this](){set_sensor_parameter_to_ros(RS2_OPTION_GAIN);}); + funcs.push_back([this](){set_sensor_parameter_to_ros(RS2_OPTION_EXPOSURE);}); _params.getParameters()->pushUpdateFunctions(funcs); }); } @@ -166,11 +166,12 @@ void RosSensor::UpdateSequenceIdCallback() } } +template void RosSensor::set_sensor_parameter_to_ros(rs2_option option) { std::string module_name = create_graph_resource_name(rs2_to_ros(get_info(RS2_CAMERA_INFO_NAME))); const std::string option_name(module_name + "." + create_graph_resource_name(rs2_option_to_string(option))); - float value = get_option(option); + auto value = static_cast(get_option(option)); _params.getParameters()->setRosParamValue(option_name, &value); } From 25ceeaec89f9528ba7c163845c061ba16a456348 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Sat, 16 Dec 2023 01:21:05 +0530 Subject: [PATCH 74/76] Added live camera test for dynamic seq_id update --- .../test/live_camera/test_d455_basic_tests.py | 53 +++++++++++++++++++ .../test/utils/pytest_rs_utils.py | 23 ++++++++ 2 files changed, 76 insertions(+) diff --git a/realsense2_camera/test/live_camera/test_d455_basic_tests.py b/realsense2_camera/test/live_camera/test_d455_basic_tests.py index 7848ff4986..fb2f58cab6 100644 --- a/realsense2_camera/test/live_camera/test_d455_basic_tests.py +++ b/realsense2_camera/test/live_camera/test_d455_basic_tests.py @@ -97,3 +97,56 @@ def test_D455_Change_Resolution(self,launch_descr_with_parameters): self.shutdown() +test_params_seq_id_update = { + 'camera_name': 'D455', + 'device_type': 'D455', + 'exposure_1' : 2000, + 'gain_1' : 20, + 'exposure_2' : 3000, + 'gain_2' : 30, + } +''' +This test sets the sequence ID param and validates the corresponding Exposure & Gain values. +''' +@pytest.mark.d455 +@pytest.mark.parametrize("launch_descr_with_parameters", [test_params_seq_id_update],indirect=True) +@pytest.mark.launch(fixture=launch_descr_with_parameters) +class Test_D455_Seq_ID_Update(pytest_rs_utils.RsTestBaseClass): + def test_D455_Seq_ID_update(self,launch_descr_with_parameters): + params = launch_descr_with_parameters[1] + if pytest_live_camera_utils.check_if_camera_connected(params['device_type']) == False: + print("Device not found? : " + params['device_type']) + return + + try: + ''' + initialize, run and check the data + ''' + print("Starting camera test...") + self.init_test("RsTest"+params['camera_name']) + self.wait_for_node(params['camera_name']) + self.create_param_ifs(get_node_heirarchy(params)) + + assert self.set_bool_param('depth_module.hdr_enabled', False) + + assert self.set_integer_param('depth_module.sequence_id', 1) + assert self.set_integer_param('depth_module.exposure', params['exposure_1']) + assert self.set_integer_param('depth_module.gain', params['gain_1']) + + assert self.set_integer_param('depth_module.sequence_id', 2) + assert self.set_integer_param('depth_module.exposure', params['exposure_2']) + assert self.set_integer_param('depth_module.gain', params['gain_2']) + + assert self.set_integer_param('depth_module.sequence_id', 1) + assert self.get_integer_param('depth_module.exposure') == params['exposure_1'] + assert self.get_integer_param('depth_module.gain') == params['gain_1'] + + assert self.set_integer_param('depth_module.sequence_id', 2) + assert self.get_integer_param('depth_module.exposure') == params['exposure_2'] + assert self.get_integer_param('depth_module.gain') == params['gain_2'] + + finally: + #this step is important because the test will fail next time + pytest_rs_utils.kill_realsense2_camera_node() + self.shutdown() + diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index db9c09c3c0..0a0069115f 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/realsense2_camera/test/utils/pytest_rs_utils.py @@ -808,6 +808,20 @@ def send_param(self, req): pass return False + def get_param(self, req): + future = self.get_param_if.call_async(req) + while rclpy.ok(): + rclpy.spin_once(self.node) + if future.done(): + try: + response = future.result() + return response.values[0] + except Exception as e: + print("exception raised:") + print(e) + pass + return None + def set_string_param(self, param_name, param_value): req = SetParameters.Request() new_param_value = ParameterValue(type=ParameterType.PARAMETER_STRING, string_value=param_value) @@ -826,6 +840,15 @@ def set_integer_param(self, param_name, param_value): req.parameters = [Parameter(name=param_name, value=new_param_value)] return self.send_param(req) + def get_integer_param(self, param_name): + req = GetParameters.Request() + req.names = [param_name] + value = self.get_param(req) + if (value == None) or (value.type == ParameterType.PARAMETER_NOT_SET): + return None + else: + return value.integer_value + def spin_for_data(self,themes, timeout=5.0): ''' timeout value varies depending upon the system, it needs to be more if From 46108438e348fd91824282978fae209c4448bff5 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Mon, 18 Dec 2023 21:30:34 +0530 Subject: [PATCH 75/76] Disabling HDR during INIT --- realsense2_camera/src/dynamic_params.cpp | 9 ++++++++- realsense2_camera/src/ros_sensor.cpp | 8 ++++++++ 2 files changed, 16 insertions(+), 1 deletion(-) diff --git a/realsense2_camera/src/dynamic_params.cpp b/realsense2_camera/src/dynamic_params.cpp index e51dfb1818..45db52d90a 100644 --- a/realsense2_camera/src/dynamic_params.cpp +++ b/realsense2_camera/src/dynamic_params.cpp @@ -151,7 +151,14 @@ namespace realsense2_camera }; if (result_value != initial_value && func) { - func(rclcpp::Parameter(param_name, result_value)); + try + { + func(rclcpp::Parameter(param_name, result_value)); + } + catch(const std::exception& e) + { + ROS_WARN_STREAM("Set parameter {" << param_name << "} failed: " << e.what()); + } } return result_value; } diff --git a/realsense2_camera/src/ros_sensor.cpp b/realsense2_camera/src/ros_sensor.cpp index 73b9cac11b..b32fa7ca2b 100644 --- a/realsense2_camera/src/ros_sensor.cpp +++ b/realsense2_camera/src/ros_sensor.cpp @@ -85,6 +85,14 @@ RosSensor::~RosSensor() void RosSensor::setParameters(bool is_rosbag_file) { std::string module_name = create_graph_resource_name(rs2_to_ros(get_info(RS2_CAMERA_INFO_NAME))); + + // From FW version 5.14.x.x, if HDR is enabled, updating UVC controls like exposure, gain , etc are restricted. + // So, during init of the node, forcefully disabling the HDR upfront and update it with new values. + if((!is_rosbag_file) && supports(RS2_OPTION_HDR_ENABLED)) + { + set_option(RS2_OPTION_HDR_ENABLED, false); + } + _params.registerDynamicOptions(*this, module_name); // for rosbag files, don't set hdr(sequence_id) / gain / exposure options From 0ab7e4fb64b18073835b4e13e9ec5bbac9ee0ca7 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Fri, 22 Dec 2023 19:55:27 +0530 Subject: [PATCH 76/76] Added urdf & mesh files for D405 model --- .../pointcloud/rs_d405_pointcloud_launch.py | 92 ++++++++++ realsense2_description/meshes/d405.stl | Bin 0 -> 194684 bytes realsense2_description/urdf/_d405.urdf.xacro | 165 ++++++++++++++++++ .../urdf/test_d405_camera.urdf.xacro | 10 ++ 4 files changed, 267 insertions(+) create mode 100644 realsense2_camera/examples/pointcloud/rs_d405_pointcloud_launch.py create mode 100644 realsense2_description/meshes/d405.stl create mode 100644 realsense2_description/urdf/_d405.urdf.xacro create mode 100644 realsense2_description/urdf/test_d405_camera.urdf.xacro diff --git a/realsense2_camera/examples/pointcloud/rs_d405_pointcloud_launch.py b/realsense2_camera/examples/pointcloud/rs_d405_pointcloud_launch.py new file mode 100644 index 0000000000..e4db15f0bc --- /dev/null +++ b/realsense2_camera/examples/pointcloud/rs_d405_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 D405. +# 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_d405_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': "d405", '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_d405_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(), '/rviz/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_description/meshes/d405.stl b/realsense2_description/meshes/d405.stl new file mode 100644 index 0000000000000000000000000000000000000000..413d260f6d2294e19401298bf698be7e4cfa502f GIT binary patch literal 194684 zcmb@v2fQ6s^*uf!O+W~}O0P=qki0t?q$3cTK+353uBxkBhI^d1PLfgp&03Gclb z25bm|6%hC;3K66Tf&wb#zt%ow*17LK@b~%r-{(U-&OK|dea_52Wz9bK?!3(&+pn|p z|8M`yxA8(*w)VKaSIn>te^DJD? zvC*EeJF{-egCn$7C&%%RtyLX1DK`$ab&9odDHDOGJr7U2 ze$td@))zgrfkc#Wv>T3@wIZ79gL9}|Efaw_&qJIm`xeJ-6vvXL+<|B}h?#X$tCItx zPaejrrWS2&jQ`HCv8yTbx**yOf}>i4Xs&yNjENf;VN67=q8Ssz^}!LBvKtBb?g8vs8pJu_=>7$HWF< z@evWtbs}_3R9lFmB8-Vmd2ocqc|@qCbc9n|7$u4@CN^bq=$P0bZ2X9b<~k8NCaNur zY7xNC6ZMm(Z1Z`Hf`p^paM+9-IhyN(bEsV{6QN@w)+Lc0 zYo9nWH02INyFu8BEFzlgM92uo^_8#1PZlVeF)@@o5bXwGE4_$lt`o7|ejDc_Hn^|n zlaU)^eJ}&Hh7`};c@jREQLMGp0!8aulUyOrvgE*z6r!EAk-6<713B) z42W}&dAhakd5arIV{1ln!8&slmy~0TW3hARDds!mba1Q-L@yBknXtHV1iR!=L}O_s zAP)Itp8VJEzgx29jjb8Qlq=^bPPqSc&3(m&tDio3DPvLYHw zs{!%IA6LxJdTEAnG`40G*Ysz_@U_n}j#cNKv-rhvXM^KwK+FQ-P9W4S%Zg|$eI1Bv zk6b^W{l+@R(b$?%EPmr>!xtalHjX8C`f_o^Lv3&naUBqU148Yxtcb=E`)ZBvZ({32 z_ElrcA?_V}PUJXn_&L5Ew}9i2H`i$eg!?2#kmLWVbAceoYMw(q31#JQ?clRJ5gZr3 zu?}`Eja`=6S9cyXA@;80SP+O8(YuGfG{d}V*xenx$(e!gHK7%Hs^z5As|iy;t%h?YaGEYIr!`b zL}O`tAhyTTKIeIh=i%9Pwp>I_eDFf!;M0D~Y8QZmQCkA>@r1>37aO}QGgbnk@#^0r zH_q37q22oFpVl&t#+DlyRypb-<6zX@^8<2h1&-Ozw_6Vbp>|p3=o1i)B}UYf7=4z& z|FRv8tuS^u4vu5n|Ct~6LPi7Q~ z*kH(_b@wr``2kF5uyN`*%E;$s@Sn3=T8(Vl1w_2eNW8zC6E?ajW z6N6oHD59~%QFJm!hJW}mv9UFyIAx<1>o6w%?ZM@0HZq@wyxNh5#`G9N+O{V)IFgAcq^TltVoUW#wqb#OuMap&t|JJ8Ol$3S(l! z(K#kIpSU|1MmY9v_c4)uMZ4tSy$OiMQs80V^+WM>uuBe%i4oCQI?|1~6~^7_$eCZletbrK(~axZ@2qx6#qu1xZcvY3_2t#FYznS# zhC90_5b&&83jgURikNT0RzOdw$1l1C5a(dUNBcj=s!u;z?B*AY?X`;ivqC*)`@ECm=BkIlTevpH8^eo zg6&uZ2(_ySwUlKCVrE_zh}W=AR9k>10Hb>5NcNuYAMS&9b{?t}L{CeEz$nmf9 zMu9^Sp%y&}Wkooze&D^DfDtZm&{ptp!{Os&UwwGYINuI>g?6;mt|HV@mT?8X3=j`v z)fZYwTcM}#`|Ek}o_~BhSFeu(!KeLxAk?lR)KcOnicm{g_VZ@E%2Qi>(l~Zq1-ty? zcy&3-`vb88I2OR1t9D&2hZtz-;Fmt=`8wXUa4pK!B-evsUFueP6XSJfDIl_}`J@xk z>}!c>;~mTPc-KNi_=JdNXF5^Jicq_qh+xTiH3{!nR>Zp&as<0XG&|FYQYI!K)NUst zSaKYr@s8y{ylWvxuuDX6Q!&OwcCjZmRO&E?i~v`g57{{YeRS4l@$?cadj8U zig2qwBCbR`&KPnEc94xNBIt=*O}h5EvLe)OCxYHKULA@%c+Wd;8%MB9M6)w>ch;>F z!O{K}o{|gj<|N#6e$t6(_MG%8aHuDph``PISr$077R!xY_k_BYB@zBz|Hzu7C5vP%Z*((Vz`wh5q|VBuR<*% zn*FjHmE1Z}5o)&+5iGHOJ;44;sr*_lq1 zvLe)zPDHT8`thDu^eWgTqS={Fl(Hh!ZYLsGGKx4}kt5h8qS={Flrk|oCWcy_2=1fY zPM8QC69?l6*IB>eP`fNQb|*@i2ptncEh3sd?nEgoLhW`Uf+g1HUo$2Ky8#hKxMqi} zh)^qxiA}9euXIcdwStFXOr%%c$Hc)9;kxrFj90wp!4^51ovFL$-N(dGs}sRH*o;?! zLw)Z=1a9Y*j)}_A*mX~6!=Ynhs6~!uXX@gn``t_isNv8tG1MX-M4x>2e7m^c_BTt|kA&%y{U!5|)>ZuL)s9hOxiE=kY+t?MMmeLVUZQW2W zWgHK_v0v@VqiUABA)F_QP)k`h)sKm5VoX$9l$h}xA$)hu^Iw2fU6YAM5C zGs3B@fcVkwCq@pIBcjbxsMUDVt`#xBt5)D(t5S_O_ky};{OwZwTWW?H0a3D zX3LR>ScgnWX!*WEjW(wfmu;Oyu)q2!C&&X-Z!iNBT*F@OLmDh)6$a5dOL9f=EAU5dL}Vf=EAU z5dP`uf=EAU5dK-{f=EAU5dO|~L126;IG(UiFwzwqr&z`qR&Xp!`pTX}j(ZsY>>hAz z1j5Q38!-Y|xw(IgD7A}P1;;7cg{6WcX~JQ5P`y%wT8bRzm9lfpMVrhMj*S@QQm+ib zQ5r3@PZXhTN;&isMJRVfpwO-`&K4X=*+O_yaJ*$%+m|Br6Gdn*L`0h$+Cq-&;Pb6* z?th9<-xZ;jA_9dtCVIOg&<1KT@q|j3=PG^T8iXG)x**a|8iXHLQiQc!efP7>;4Sy< z;Ml=_;h4cTkY^_6!+4+4mZzUI2)#EEVP5Gt*yJ}4G7ko?=#6>Ofq;j>ThC$~$hwX* zry`Ww>Z5iR5M8pa^?a0!qK0VBd1==5APCY_>Q$3Noo0NaV*G1Y>O1a#%%OK>=MgZBvR+yOIJ|$MG1HpUVWzC?TV5W}eCUlf&801Mb4u1+_0AGsm@zD*z=gO|92=xRjju^p% zd!-ycx(4v29DdBOS{(?EFl*lg6vmTjfx@T#a!JAy1+&4~F1sm8%OPQnVtp7T3C5H~#MkuG6`qBtp@~(8j;` zuU&DK&ow%0ah1>2H_M7Pg#V{qsVzltoy{Eu(J4Z0DMBqd#2bg5U9G#z?0)8rO@C^I z`Yy|Rn*n<(*1vq};(GU)P1z9bZV0W#a$^aY_Ca@yu2x;QDK%D?S6f?aZi>yD!vqSM2Gb{uR2J!7pKPFuD<{HV*V9lS(dl78JB}rGpWXiwo-NkOvZe4WIgVhL9O1g- z=!WR@FrXcR(d0_{jJonh8|CrYWqELf^JK+Z5q$C;;@+KS_rH1A*?I8f(j%Gw?R?*8 zqs=_WQb6-M|GFVUYXgGqaF&Shy=xro#W_E9c0(xa83&*0oi^PvKD&W0Tpt`kj^N2auYA-R2cz@mM}`_l zuuDX^?l`(3$f1Z(b_m7>?g{2=HGK-w z62URJd3L)YIz0?%$Kn2M=g!trMR@ucckX#@p5qkZyh;#;_SZd{7{*3u2iObOC+0g4 zF$S&dIJzNxYa^nuL=5gn9_|Nvvol-rr=1Xc3BSB2S;>zn0m!$icbn_;d#ZA5y!;H7p^;wZir3~Q?D?8 zz&`&5KCiO&k!QCXBA^w~%paK5dcr(NcFwh-O<_)CJ6LwUbVIO5L#@!e&Jr>HepIx} zUJPr7z!&ZfuRD%z2%;4c$`0Y5E92nP5Y|4*7p@PE==3n49RjPRJgl$yREITOu*-IY z>w_c6p@>j+2p{Lx4n}8Kiw3*o2-gQkkV6rn>=0PJ=3%|dQHE=3waaq2J~)CL!4pM0 z1XkgBShr)I!1LqQ>cK7%;rieRawsB{9RmNh-5RTT`iQ`j7}s;591#Rnjy*ip4ccEf zG!bF_mbLi_o`$Y#AKXD(uOx_!i0}!^@ZDx)mT^C9^)V0;KIufDr)@2fAj~Vofvweu zU~9uj6u1+_0I$Lvu6zR#YF811bFfyp=kz^1$Ls~Shm+nlue28Jh6rYlFw(ktrK@7S z=ZPzmm3kF=WMDhO zC!Jn}_C-V+CG(0SS+}{Cqi9&^n^zGLJ|PF}#?=5kwApSTht}eV6V@n$BN)$N^@?7w znT&jBUtBGvUWHFw%Ns=C8|YQ|q!Tf?SK$+faHE7TTXaPZ7W^G-;*RL>l%PCtEC~qf z#o$$lm4J>MZItW|YX7jT2)A19jHqCX9K8RX^KQanUWM_s6A}0#f(74>#uMcT+!4_Z z2;RXk8t~qPSu}7*1PcSa;xiUThQJpQ?SN3bick*L&0TkO-M=YfmqRZpww`g_0CYsO$)UEEKmEqy&Cf0{gmOm&3hi8L zT{p*##i8H-B7}Of=QZq3GyUo9$4q{=Ug= z#Xr7%f+5tyh%gSVwdnUI6(i^RB7}PK;3tO_pFDos;9ePrB37GmSh2xo4x#AiRh#Wl zud?j+g4#HrWA!j1j6>}@&H8pjbmi!Zm@>+FrRbDHJwfgN!7JJlyS0>ob{=+eP?Zm# zj8H_~TyHSkOEvxDy(-vO>S0r#z3UNL%gSmgMc^J6>`B_i-7DDJEUTpyp`R#1J?um@ zt>sgQdtEyY<~4*KX)tU@^NAw()Z+>AXhk6A3Pum@BAN<5nJjDjn)>W3{X`MkrWB!` za8y9-nJ0sLrJpE*qYxsxOFJ+I797p!J4UyHBL&OazSJxIL=oDi6rt=Kr7;$oCmiE3 zig)o!eOH89N;xnm6`Y&sJ7$}LBe#{MeJMg)t_W>Yico(!A7XAY-#G_j{_4^W^<5Ea zDdo_108w!M>ul5D2zXL(CM81ADMCHrT#kPGf_8BJMvq!8MX03#9MN~Qw%}|{uh1g} z=R%fKg!-yIGcRZm5=fZ*QP~R1y zmQoIF2coOSxH6zkDT4DiMjWf92(^?V)L)De1?NTjjxnZK6tnffcBt=)P)jKXXJd@# z<`w61jPq7jZ&QSRq6lqMir~D*(c1ZYdn_w=iby|c5XzS#(mt_S)c23g!M@yJ+PU_R zpY^R)YDp2k9p09CW#zPeT@bajT+#lzBGeN{$@;mK0-a=*vQ$8h^}5)I~2hYl=C7HijF>6 zJJc)Ade!C~RIfOfbEY9e zJ&XwBP`irg%3%nt#WezFx&NI*5o(EEMUAZ8GoOu#1jIX|Q_>cOCxav2P>ZOi9Y(TAma2#jPb9VK}tvmOReCf5Gd`GOh zr+vq4g<7<$ma^=MGsac#-MMg?pMFpOr=xDId#@gA9Ewg6-?{Dd>fD9*FDJioaL<44 zyQSW01AZ&kkXjLHDa)Sy%jjy$rSB}i{;iw)ADeztee7lD8i%4&#HX(vQ(f`o&&s97 zzTWfMtQ+dz9>~*QLuy5+r7S!4@dK*k-~4a+jh%kbKmEY#>JOK?#5fe4B3}LMTUGDe zOH?l$@k!5{qpqsY`4=X_cS!-}vQ;^#X@qWgLo55yMYh zty+1TqS|e%S9_k>_u_ibzg%Y=icm{g_Qd^5RA+v9=jyqC{ki|1(=VtWzxxK`P;`pe z_Rp`EPi$~#_3h1G?OEylc0Fv?&Bmb!wUlLlzwwFk;3bZ&CLj7}|A}Xgs~?(ji*YDA zMa=*4f#vk)POnZJ@{M7C`^IMMj1Ob1 z6NeZt7;jxTgc~v5!&qmWBgPBHp7+FRv7A~$jJFu;jB~_z!FVHwqEiH7yv0~&oFm2y z#vbh|LM`FjjhDW&#aL&YBgPBH8#xr6A`s&(#yaC1F>|I^!HM zUNGLsq39HW7|$8&jB~_z!Pp~*BGeMTVf^Sda>hF295G%n-pHZo6oDAe8S9L5#CXBj zBZnf?QkEgcbH+O395G%n-pHZo6oDAe8S9L5#CX9-B8MW>625C`iDPrdI^!HMUNGLs zq39HW7|$8&jB~_z!Pp~*q7mZ-ql%WY3^ATF)*0uDP!2_B8DczVtTWCL;{{`n9Ewgk z5aT&xopFx%D;RHNS9FR%jOUDXMpQtMLlNP+YX>40DwO?kJ|{=PPePgKfMA(5BO-7p z!rBxNjsr2CGuDYkj2DczK{)JsmLbM-#=4)+3&x&!Vr8{t^Lfr#_w#wdcq0c!h=@)R zi1D1U?&tG@u}2O?V6=#qY(CE!>wZ2j7;ogj2ocdK0x_O5*8O~5F!soy2#gldlFjEi zW8Kf^1>=nz7$G7$MIgp=#=4)+3&tKf6oJtqTC({(XRQ1AykNYM10zI4rwGJ&&RF;J zdBNBthaxaqL`ycG=ZtkfpBIcba$tmr=oEn%Z!y;Wd|oh;$e{>~7SWQ;=Pky%pU(@% z8#ypSM0AQkjJFu;em*Z4d*r|f5z&b8f>A|FHlMc`>wZ3u2<1?;&F4Lgbw8gMj6HH- zOo`}}12NviSoiaJ!RR47Mu><`5s2|&jCDp7Bi6;6&F2vjuDf<{JuqN2dt2lp@A$e0#n3!)~3ZdrtK> zMd&ArQ0^3={_4)_gTJ3q-?{75F6~g?6`__=4sD0-&(*^e(YM`{dg59a;UA&J?BOnP4B{? zb`_zPQVy-<=O}M?1hRv=acE!pNFC5`DTlK&!fGi(Epzm+)UlQ|vgglYhaZWwq! zwB#zbjrc`GQ9^5NM;K_&@kfKu#^+Z?tgL|b2 z&eDilYlos!4y~m-mad34qZZLwaAsp`5y1s#OqR7xDTjWdh{0Q~UU4OWQPkR@9!3uH z3S(bXqcA=eoMY*gdK(car0r0IdYB^A6V5)E1qS!ZI64uS0Suw&$kC=(&Ze84U2Rv^ z_C zpILGY?v;L`2-Sd64)u!T2xm-ksD~*+6+uO)r4*r_a4zSFK#sw^(oYnDu{dgSDTjK+ zah)?JIn=`xp{k=I)KZF2PdI~e79hvqUg;-_P&G2;P_H=8a>gWwdYB^At|A8K&`%Vh z+>T?|;b-S}ZpQCU7*YT6Khq234AhJ+oRKPyJkph$>%BaI(Eo2;x-4R7|{c{kBhYgyDpFh4&{_+O=?gZpT{dRis^CKKbV|PT+T>l(I z;$ed(;_@$VlJ7e=zdK<>{pthLi#w+|j>hhYqPhM#h{VGNO~lJM`+B~8cfyEzp-ZP1 zqn>jdjolGNbDfBuW$$XOYu|*BBAQwwik}`lz3BZHE%9#8fiSa#3C5Zt}VfB4hTq4hoqLTmB95aI6Z^^4!t+G6_& z!?b1J9o%@G**%B+ZH}J%w>mpPXsvXPdib`hS}VVJ(6C<*KP5qEt#mdx zVu9iL&)%+vZn^bZ)uF$bUYs#|WHIuPh3vTvE!Q5&vagKE^C!N3S@zfT4M)@0uBdiuBB?!!pNWqt)+JpPvL{(^Fh z%bvV@_x$oT&+Iw+#vuuZ_FIa+ZS_6!F}M6;=)HX@LOoG5PB<-cM{BdAt{k?=lP6VY z>@>YNaie4HNz~p=@8GXbKO{eBhaG#qzREvJMW`o=&a%_)yrwmE*K>O&9Y3Zz{rqXg z#~+O`4z-)kwXcjGm%sVQ(yfa|U)isSsf$i4#vgg6-9hEcvOmr7+tw-@jqbViv7-|X z^-%e;>~B9hEx&Q~1zSC*Y+Wjc+EsLxJ^$L|)~Khh?fLgACsm&vFts@QC->=jJfBOYQ+h67Poye!8p`zI-lPUg4B8IMgM{(927g}E_UzWX4Ki#@w==`le-*8mIp&lw9erM~I6Y$&Qx3}K< z&4#6Ns9i;8+1ULiwPrlGQ0uLqj;fy8XL2#}#}`>U6rHx?k-bOc^W^=l1FxE0Dni?# z=q!8tr2|@jIboSr>&>xM&*(`-tGwJe)UK{kvTU_I_Q~HoWA6N-A3W8gh!+>UrMT?0 ztE{h-4{v22p4obG^4_ggZa6yOP!E+a%icKr+xhpl`%mlnHO?rNL+vU$%jW*-X8G1{ z&7Ci@#+&6HAKg^+jJev{p#ozr zR%f4ZeX;(2*Bgh{($!#=z4rDN`EDPrm(P3ealMMrnM^&*vd#Z>OKa91m$w%C>M;q2 zdZ>I^_Wc=~<#S%VdOmag@uhO8T}2}o`lUtk-%Z~&KmWes~%#^ zKmXj;10T(5ZM@A9l_IpYiq5hf|GPx~`pIkLAB}sjRD{}9Z?mk}fA7|FFPzYNY5tQF z4y~npS@!s`Rw(r zk5@RknzqIGV(ea1tsRO^IUas|V(X)^d*z$(jITW2`^X&6j&)A)R)mr!ZqpOn_ zJEhof!ZhPhbjoqk2Zy(gKl+sX^5Zrx6`>r8&aw%8oAmr;mFrqZojE4qP`kQs&$3TG zJg9ZWS5MEcx%sAk<(TmC3B`M@>DE`umu2TZyG73&^WD+fVB$&D?46D+hVD4s+M(!_ zO*DIhw~xAU=;Qb%o%ZC>0}##XUeSNqWWQnm+-7#q zN(bV%w#2K{6Wv+Ny7_{kc{wCMboD)|?S4D5*ks!DVuiaGsyoq*S91>Cx#xnThU7o^ zboT_IJ({A&-Sy*PKbiDG>!72Ls^ny0yPF->bH}yg@>~0sD;1&l zL(%v}=$Du8*>a<^^V8;^-LD9?yxGg@&ify?+&yf=YewgbFMe0TK}54NRrEt!FEMOP zy-$A3`tv63Afnl4D*9`ePaL`|_Bpe$$LaK{=}|?WzT|d2H%u9yU;pxqzD`8g_iHcU zn;kDXuV?9*Yv!Y$$rBFkQN6SH{m;Xv51pQ`mH%SM@C1?8(%m*y3GO1DmZ9r&E7#OA z{63*}y9ufiL{OQ~9fa2M`-IxD>~(% zDgkw(qB21p18aqvqYqUH%(3?DQkB5U#>#lgp?p9ADXD%aLf=}YZ)K=sK+mYCWl+b! z-p@Gv$l#woizxq;P{)9tQBlk2M4*R#EgReYJ*SQVJ)@+S(TPBR`C2x%`#VS-1A0b3 zwTw;#`pehSH}2Fipl6iSGCC3HRl93c>amOTcaS;;^o)vH26YVVtZQl++Va#Abqwek z6}1dSXf5g(*n|4}LB#;dK}FSoItH|cnp#F$%f@z(pz;9)q@wzv2zrI_#MiR1-H#-` z?B0!osswh!*1L+vni|Rn6p)hYha%JyzB$2;+do&-F`#FZ)G`#I9x58Y!3!M&dPYSp zLlNqUqOqfgDgkw(qB21p1H1PD?NCejJuE06P(VtmABxagX*;M&K%JMWO7C#Hq;fUEEN?NMQAPM0|!(Is1qfX3F;V73#=WANY@Nh zKA?bk>udd!wOaK+hW3oKZkjDZRRZcnNo9gM1{4BohjOT;ETi%P1*D(qha$9A z%0V3idPYCB4C)xzy&H$3Qv@{zXbmNG21QU~z>dpWt{!6afsO$^qn}!aBGhi0UqY<} znn_8$L=kFNJ%mmTRRZcnNo9gM26nyH4)sbcWf_$ZC?Ne*KNO+0Qm?2=K%MBNGC>^! zyZ4$}hOX|@-2_z$s1v}=l zN^+OR_D{VP7251fa)ETH_V2^J1L-)0Q$7)YZ zmQiDX*3d_tK@qyoOwrUBpf&VSXHbOh8Wo*o)EJ;O^ipR~g!-;%{LUye251fa)EN|^ zzUwoVWz=J!%k)!|QH0jg=Pk>q`9S;Wqwb>!t)=%S%cx^O&*-O?p$M&|_X%??bPVVj zz0@)kp|!LZxZAWWi^}I^P#ub^d)XUcZ58$j0JNb_bVEoj%kc1)h=A5m`MeD3Nnt-g zoh0xv8^T|wSA!tbLqU@RT0`aYGN?d>eWuv8d<}n{9D^Vd4->E2&>AYAm%;pturuY` z>d4nzC&wU&#KQ&+KpR>^-~UIx<~k8j4oaVwksz8{mP_IHxb$iej-*Wu zA32aS(eLvznBx)l%gmXG<*+LzL+X{(N<3_?cXBkgbQiDr`&1>MPM9)59RuqF8-4Iz zC7$!aTL(G8L*)Ysh^Zfnpz?uv+{P#>A6U)!@rpVI^o(Agmr-NyXB@m^*gxAnXFstV z^D-*v8NEI)BO(dY9rB0_7ipNNQg8U4^ROv|9U61>t{uK!#nXqHjO zfS%Fk^D-hrYf;C*E)qOcqo`v*&*=4e88vp_L6+ zQk8%@(dY9rYV7Jw%iw6k)_Nt?$}umaf;!RX^D-iW;|LL~#fW7&=4Dh+CwhHeMnq^W zZIk6?R8U#^d|pOGq`o&ET3$v4b)w(rWkdw^pU`h9n#uqVfR+r1W_i z5kchxJ8tVM<+Hqu5_*Pd8PrMw2ep~dCgrodj0$>2>GLuoLhUNr@-j-O6Q)d1$G}eB z+M#@DJE(j>0V#c6Mnq^k6m5AKCDaL1Ca7az_ih|&7kL2j)TZTSR8T-n{ZIsz5A2_< zuawX7GD@furc5Y8+oXJ!mr+3hDSci>^h)h2+VV0gs1v44P{+XT-P)mEDYxZilu$rS z{ZNF~N?T4<0_ub*6Vx%Vdp8cPr8Au6WmHf=O#M)V&SdJLWm!~ECrp`8gw|4TEt8{! z0%GciBD9uzXn7eGlmk-@sAE7muy*LYs~%#^hbjSe!juU`XloU1Sr#QU6Vpo+p&lyQ zGC3+}CZ?AtLOoHm|6cAHCsC)#3)=JkU)G?rER6Z}G z#!h|!hg!0{j1qc=X&F>L0zzx098@KsPLw_`BOGLuYM6Yfa}6Sb+DvFu+77BKP+2OU$q^B(rF-XQe@=A;Dof=vITA#(msK>?6{swg&*VrD zT#x$m4YH4>x&oD@@|he7qPYjU+ork#m8J5T90?-$uKi}29Q{y0Dxb*_5qb~OexoYE zji=Acuswh~S7K}RLUt2H%*!x@&&x0bKk>T>tL66zMa;{H9J=S|vSaZa)EMZM&&x0l z+ETlI)nCNCjOdl_Qj|~8PGca4&&x0l+ETlI|5?PmjOdl_Ih0S)PB9>d&&x0l+ETlI z$6Ca^4CA0zzLxEgl~2)@0fCb{F)zb7XiM$-J#G>6GK|CLWkiJXDcW)-Z~`gjWf+3C z)UMwZ7cnm*dZjyTZ`XG5icUFnhpp(8gE|J!nfNS=8f%4uc^P;s6VF%U9YQ?4o@Gm1 zw_Ek%#6|L#7TAQD91$_*ncM3hZgYt}?cZMSruu8uHCE2Df1EN$|7e`(coP{A>%9Y2 z+h?9}EP-dfNA;X%->2vH-HKBqt0T*z6M@$I`>g0K_IkN@At>luU_9wW;GX!WPfrg` z-Q$|x#i7{#5o&EG0#AyM2|We%`-cwd+x~*9T4!v1$^Zm<)$ST~4D2HP9UQ%I-8&NJ zOtwZ|#-Gsg?QqV-=VdTPj3ebfZ{N%M$DW+$3nMQ>5nAh`zZ_9N{3DlhnPqDoHM941 zoB`Pb84$<4JF@;g&Y8F~Cdo;($cL58CCl`^gmWerBg>)_fmrwBhobk~eo7zDvOJ4S zj!pzdAb+3rblrMu{kHF{N5<##ZogcDNPVYD6wtT*?9Bf8aL(jA$g)rl?S&M*((EUD zufjQ#A0d-N5qz#f?QPOA29dI!I`)K2|VzI#Z% z)|dZLDndO`be3(s^tt`7A8}3V__1TE!t zH1_9zysm%JC3m#`G}lSh!$Zc`YwzcBofVz7W9?D9lp~HkFn{IJ*GffbI~1K|htK?T z|6V;4TAw~}N_EjZ=hf@C@3hfJ?NZ0U{@M4{-N)WjPTPG{zTenudlYf&Ll@SctbB*{ zmGWiT%?Hn4?t9A9txIPdm2jws%7-l01t*t3yy}Gf8$aK?R1USPXzXhr=r1=vc0@k$ zomb29mlxF^?lIZgp}6joYd-_I;v8H)=FFc;_zk4r*Ar-_2|1}tFQj- z@_O`flZ-=aArByauLWjTQl#O-dbXD#Z^!idhwW}Ut1+eXn<(KALr}c*; zMpc*ZGPyqKd3Od&Inq_e{d?_Dy}RQptp{Fuu~dYvG8CO<>wj=a_1%4+Zf$zosm#l$ zv3s}YN)c)aE15Oxa+kx`Z*B9QR!>7$-M-^?k9~IA;_}q_(P~Wu|EH9(tITN3k5fR#>RCB_ewY-dq z=S+NFMnq^W^$@e#B99LH^Qn1wAMh55AIkqzvAZM)(*85eldiIP<9;M5L!$59O6IQ?vm%X zENL9U7HjG6qlD}K8=@19yzqY>oBw9VCr!^#yWzKFhzMoJ(G8)s267zPcX~cmAlQa*<`W%~I1ThHxjN=|BvdEh+Pz&}?W96`IG7VD>$kiwGlD*oD` zma{d0qjuK?0g%=5pA1CMEY5SjJw9iwyZ`49c3pnYE4B1Rh=FLtc+SYxPguq;D8=7C zOMji~ix8b?yn|fuq@0nfpRlaI`lftejOavX8DczV#xZv-xnh~(OCQ3JUnOQ zc776mbCB0V+1>MQ2(8s=2`hkGmdqLJT8p;uD^2$MjJzJoj-wkwYjs+}+wdKqY%y}R z7H#1dt?ZX6c|DXJM>mAl>a>()i18L9S8LH0e*MdS#go@V*>QA3Xsu35cw_S1o;{59 z&RTx%v*=1Wz6e1}igy3MiCm5yh(pY^ZX5}}UuttO#+K$Uj1KyVL%Y_x>#mF#?_=aT zbi;x9)qaPU^KQ!54MAIq2xZ5C7%v&QTP!4ZB15D{w0=JS%VP7bw& zdDnj5nH-_)I4~*q}*>QA3Xf5S)2*h~B$R&r`!u)E# zu}wrM5021U%I6S>@rsd44z-1Ow_tn|5z2!jw3hNY1Y*2meVcxah!X_e=2S;cv z<#Pzcc*V#ihuXqyX}?oVL?{oA&|1pp5Qy=LkxP!$78OTE68~Iza0Km!TCAU1LX20q z*mnP!ts=Yq(y1O{zZ0iu)N=?H@HfDd1mV2mnw^o0>g`~c9Q@TqM$%W0@9p3dMBp&H ziU?)L!8JQ$ogCp4+T|}IGA3CoTpxhQ!Y7KrXqTXEcCHw?R?SP=&P>c1$b%)@U zCUVtQ!htcx{f?@uF9`W$AOhSNW87a{0l@DW_}z@B?7J(MlI`Whw%`2^iOwjm^ zX;5olAD{2?^h?8L@3zktilE=@Ve%;&y694)^Y_=dr?tc4*FSRV%TtTTva_s~+C>Is z{HC!i>zg$$U$a=Hwb?D--BJ_Mx4Z2t`Af47-%=6Er|2v@Wv)f@?_9N8zVS*6_a1)g)x|#FyxBO^E;8ui zcS0h=bm>j=kN>_={?SW|4poHmDH<83`1YhNa8B{{&5wU%&qppUwpwXtK1JiL%*xyKoc+M~{G@F+d1U%;zEj+@`*h<-?J^$2o9cDO z?%8_&J7?q%{B+GN6`_17diXE4=((g%SK%%?7462Az=2UA55XQ{!sisl`0&?j7n_0r zu!+>PmR+x%CB`^n-Nr=E?rm9Xt(KnK&oah1V!dLVGsY1~hR|AidL6xs7)PvEjB`aO zpQ4fXiWo<%SB!JUI3me7)UKWr$H;&fN32(jb44hhqJcn+Bi1X%Ib$4=WE^T&PfKSR zV;r$wG0qjCe2RwRg&0SySB!JUI3me7)NXounK6!7uNdcwP(DRx8Dku=UNO!Y-~&##yDcnI8wVBAB=ItddWCf zgz}|m#yDc#)Ip6qMThGy2L0@u#Gw1c#h{CAo1M*DKPJTeK@+Kns2a9?EmwAy7~_a_ zzc+B_hn(H!ED`2fIUmIsN355AKCcl;W>;(J$qt*(E5v%ApU)$L9?>)MDca`q3b9`L z`MgFXSuM4TnKPbHvH840te1X1j|h%J!9zvcd|n~eEzTL^h$Q1syO=rS$qt*(E5v%~ z=kti*C=@(Yw9V%gV%_4LF^)(w4z;UU{?M-w&F3Xzz4G&U zjYu*MwaXYsRCx}@IAXo>^La!lpQ3F(??NF9%*B#^=Y{{jyVcj0e6Dn<36!U9QHVu+)5S$}8iKzp?)Qk=I!Hy4NSxANBC}Ak)8g z4&A4Q0%f0E@YUPv#XjK~&J?Yf6rmi-og$#g#J4@_$zHgA;`Niec%=xntLT&?yba^k zt9G4IS3`Ji7vBvcZQ}nAf9+fmT1$H*)aL)TbLc+tjgopecsmrKwH6q6M*a5w{FTb| zubo3rv^SN!zT0in>UEbq-fC%^6q6#fmbNcNKtqaet<;m&rAO4e+wVbYeYRW?+9pLS zrkz83t)T*&@7fM+UzXkS@&41cUaWU@a`-%=y1nU|(-onXvaI#mmeViW`cmjp+^sfm zkrkaH?#QoM{pqX!I?W+C3ljxPNo)Pdu=Aclz2=V`4MKY%MelL^B`fDU{w;76#^K(p z@t2M2%Sq3q260RK5}L8DnSGf$=#smK701G#a>)NTMiUfgS1Z_@sg*fX)n0G za&3uyr3mY3?L%lX;F#~StF0Y;63|LT7>7sL^(?#WOQ%iy%aE6#QMU8&{w%!VGjGsZ zJi`U=sPkpk85gbE@_p5bkbdJgDA$?w)tCqSSH0}rlUF4N)Y+OEDBpjmhpAT&P5bI< z^X+=qot+5A9G+S8Jw@;H-#70(WAXy45fR?@kwbmQm{KspTs!W5__jL_`ed#F2yK(1 zabo?0Iaiy}`;K`PvPHB0 z3U3t2p>0ar@wWpHn{xYUb6Ptv`WGAv*+NC9h?NfgVAW;T-OId!>R(fHCx?2NdbQjU zAFTS}->x8}WveW|f-9P1(=bkW*@Sc+hu>a^Z>R^jNQi11dIo+ujM3Hb1B zs~r5vT;>(m2pC_j9qK#hU%mr%L?%&lI7Svh12`_nR{EySJ^*mizgx$hN(CTI##a!F($R%P+6K z?&>G}c7lypiqKw2(U{d%$E;=?m{putm@U=A)T@!dTYZg(7hT6%?h!?M)*912Lf@rZ z`)uZ1{jllF+dc1suv#hi`#(PNj#b`&HMHFQV!(p*N?VJ0-TINem}O_IB7K!0%r^Qh zMemDt+;PrVtgrYaaBkA~@_b*PdRuOE^|Y0}SDgLXFOJ*)rh)TNygK%uo36Icc9+|8 z)rkncbH?UV9H`r(u-NB?X9LTl-pUwl)@d%u}F;p6+wD~=zpOYpz849R?sYi$KwM$MVMmrvC0p z=M`svj^pIeHl<#@bl34ypZ@H2<4}aw;*8BX%e7<6SAH<{_K9a0g7YlrJaVX4+P*B? z{@AOh-o2#bP=xZO9B&?S{nVddJYfKb)>1z5XYF;}Fa6mF$MeS@@49L)B2^=}Xs#17 z2%?ra7c@B*UGA>dSMK1iFSs+u-y6O%^mq50AAyf2nca1A41%x|(gBfp)&Av%^X8k_ zuP?Y$%Ehb)jH9vZem%lnC&wTNo^$t}xZg2}UbW|%bCdj)x%fK=?o92!Hd}_d2K>Ih zBJlC7t-DT+K@dD!5)n-qfcAK_VK20W95_``-08_b(8gp=ftZLwdm(Z<{Mvp0F)*}H5t%kF;s z5GTpEd3*U`C*g@#gw|qvh;Y5IEKZVFr%f65({Ie~QAAqHy}_oUhgJ9s==?0)EM$%v(gE`o1f;;z=Gi=8v9=cdsK zLTfP=iEuIaPuzzc=X!PMi}*HL<C5v{9&G3hmJ(lDne^%o3iY;-?^bR z<<{+ot#Qzp1fjE~wkgY2Jt5E6XkXs5`B&CT5Zcpe?>;i$@cfnio*4SCZBm4KqG){6 z)cc3$&)mFi&#Mo;(z^EB(~Ga}aZK^#TkrH~n^KM|e|%qSld(4pTjq#SxgxY3ipKXR z{%BnOTE0qaw;Mm}z2=H(#W$}TV;pK%GkI{@>79#P7p#0t&z&Efk}E=IWaYy*c`P(0 zAO85lvFXLT7#D%d(Z{JuV-=_2kw^hmCD1LOoPI ze81?qzi55`pYydoy7=gXL+vU$%f9*J+gh7GwrFcNJSFR_G`ZM+y^D+8-g~D{y;ANh z`@tK#=HK7p(boFcz1mWQ)=FEx=Dpv}U;V#%^VN2Gw*OD-O)7rA;uXfBwfO!BZ)tpA zeRkRYt&jIyw)O9Sj?ERJ>mc|>x8~h^-`0Zf zADwWhT}9(NGrxC9>wwF5Yn?cKRQ}=jZz`&%t}gnvez#A(Qf_<`(3P9!hrBg!zS$hF zw-lkZ(w2`JvPpjN-!{yzdG`Cx<84jz|*_*bWy=~jpWABd56`?bk zdWiGRU)dy|aMPOkUiX~aQiOV_d|CGCQn$3uKlsYlIkS#VIMlA9v0j*SOzV$B?r+^b z0lmB9RmCfRy}7vI?RWZg-c=9r?H7A}H6Qi&oPSg6=uJoEJDzw^@vRAyi-WMr z&^1Hq)w7?y-1_q_JLC)Q^2e4Uv{t$TT2{Tw~ku+n0$pRCloJ!|IPs%YAMU! zz5d6o<90bdfAq^U`xK#e`3{e{Id08x;*k&cJpGk(T0gpMY{H>?HEk1AgO#6XP5Ahb z{KogrZ7GL(qI_9)!h)Bz=3RI1{NUgIy|w>3XBX$6F}2tV_gvAbSGSIRy65H7Z*8r& z`ANAV)GI|}mGQfgtuJ46az142VXcq18B=`g`f0|Y=#=Br)n@hlYJsO)hi!69t_bB& zbe28aw@2%ct547OcxOhBBGfM5cQTL3_3k>wMm=wSbZzT5=Z;A@bVsjzI3fMozk7DS z{LuXEA%ATx^uuF{H+Gv|e1iF0`BIMSKijP5rKhL0_J3@2t_bB&G=3TAqAhy%JZyY^ z(Bpgcy!gz>;yQfO6gg76RB^)U{Gy_F*x4&Q*ZR@WqjNSG3Q_hrGE{!lA8A`)&5r^Lk!*c+I@{Rh}<}-p%pr z4sTcA>(jnUy?r~IKJ=j0TKU@x4^I$jE!|BHIb*+Jf5Wd-EPvqH`G}`S7N=b`z4-A0 z#|_v!+j`X0uOo(CFm%k!A^8Dc-y=b&hwO#seoWf!#-TUiH}Cetp0JZccf!qXyM5+P zLnmX0`Yv`yoruP+?wwcMW9Oc`4;hkAnj`f}`zrPD_8T4^di~~yw7&7k=>s@IYZdKO z3GO2C?fALwApBar<~vA#y>|N#>KLDcFjZC2PL&{nT88d9)ULl?yB!O4jL$*XPDs&C zl^}vzhVD|-uD@QpJq~q@&q3JUNzqP~Ac9(k?m5)1zh1itrjGGB2-|Hb+NlymP|MKW zuiEw3Yqxi%j`2AN8%q@JR0$%eW$1oc?fUDr+ig?F_#A}o;uY;w2_l@9(d@8OyLP>H zvjKIC&q3I}KSfiOfI3l9nV^n=wL(oTL*K-tZWXVDA-m4ComZwTw;#Mrx~tOt+X@k9RCkC7@1JR3@lnV4qx5%g}bDmZ*F{0ja2d zC_-zc9Nhmy2PmllP{+VN+3rXB&RlaQsC+;H>8JXk2zrGv#Age*-_UTQ50wunASKn0 zP6S2-KSsG<+jL_Il@BN&71fVU1mfF|I5xKXk%7tw6p)JQM<)W&>T9{*gLH9DRRZcn zMP;HBf!MRBMEfwysC+;Hsi=M^LTjbHOC1AxMnx@yItF&{HMI;yrwHm8&@)PE8H&*R zp=jh*L6v|yQBs+pj)C2~aqv9|cEdH}Q_(nU3FQL{NJaHS5$Xx@A>#>*ETfJAJ)@$Q zp$PR*(a2kXjsZQRq?Vxw^+eHGMpXjpL`h|WItF&{HMIuKq{&qiqKkV zJE%%PohYeHP{+Vd-Z=QK1-oJEE9JxYctQDq0#Z@^P=wm0@_`+eGZ6?F{g85Ok*MW~00&NAv4&@)PE z8H!L(6paiBs1i^oDk>AyF|d2DsbwfS^@_>|6p)hYha$A)ipD+)ssz-DlF9^i4D76p zLuWEY<3tsd4=5lN)el9eCpyF7JK~{ZK+mYCWhg>DR5ZRh89D~^jFMW0BGeN_g991^ zw1$d019c3j6E(FAMW6(Gc2NaN!>W3n9U7~1wiy(9i=ouxo4C)xzy&H!j z)KZpFm4G_YPh~<8n!TauETi%P1*D|DQ8ZQ=P$i&F z^i!FjjsbKM>7N@^L@F|d0#4n?O3Y7EdC`l&N0 zLNi3vL&Q0B4CompwG2h5UFsOv+xotuRszkWpL&TR)UJA%WmF}gPV`fmppJn(b4{H= zy;4i~6(uMiP(b>rekej~rCw2EfY#7Uoq;+A_RNmMy@|7RNxJ`_#sICMk2-@QbakKZ zCa5t$Yv`lSpa@;7D;nQ<28{t)LqBx}Mff*y)}xBXHyc4?fY#7Yok0=W3u()#;6Tag zr;4M9w3hBasNg`!>7$CH2)!T5hhI2?jsZQRpIQcW4D7{gY8i@9OIc<*MmD{dT81LD zR(e0EV?fX7qn1G(1Ij^7Ekk#|>28~<1k{OMDiey(y{w|~?XB2rLY?TPGNB0dMA2DB zjR9IiFLee*s3+PB$e@9`0+pqg3X3AN7nCo{sC+;H>8JXk2(_zdtVJ)mcx9*){G~ta zB$FKkvn)c+wtZW^+f8si3$3B_Sr*LL2s;R=<$nJo<})ypBXS5rcPxS?2egLDXIa=U z2s&G8*ZuxSlVzb^=`Kb1?3V=^ub?$lKFfkSRoJC4XCjuvUW7iSUP-OQ!{#~>&>AYA zWkH=X?0%Uu5zEaUmk9MrY9$^v*NK4EQ28v2qOoOJ9bxZmzj5LA&P1qJf=E1Ut`h;R zq4HT4h3v4!u4P~Nv7HF@N)U;M&Gk->uqWqJ+0;@F>KM>7Ov|8-f%Spyo$+=do~Xh5 z7CG}n9RqrXX&H*3j)C=$`A!`JGqi69bqweky*@9a#!lX}4E8@fvpdLsVmanzRM0c} zd|pOGXf3vfh?tjw-;SQr=kqcmBCX|496H^HItKKNKA)En5$rSCWi7nhj;C|8j5-GN zj9#B*QDe_+T86fRI!$PwWpY%|Gx~idM?`3?l!H13^o%~A&roBZZ1)52&*KRm8&lvR z&$n16M?dt8KA*`E5n7AU%QnSKjuLuCpU>oo2(87~C&I-!bqweky*`s8BD5AGmWY_i zQ9;k>^_d(Ip|!N%vWz+g^o(AgWf2iti|SG6b<5>&fS%Fk^D-hr?}wr-%c6vy(eJY?YV72VgE|Iw!|0>*JeFm0RM0a@ zpUDvs>IpIj;u&ts%c!7dls+#bBB;%THYwWjGD_$f{XQ=vBD5Wf&NAv4&@=jdmPL)- zyXg$t4(c?aeU`~lLC+|CCPzeQt+XA~F`#Gk`z(tZJ9*=vj)C2<^_B8jCPxK5qx6{^ z5utXeV_?VaM<41K&@)P(mk|-vWd-Mi@wicYW3nP!3GX&~~UL%d)7TfSCHB2(6X2gE|KE4AU|cp|y0L zw``FTdWLBkiqKl>A+#mv7|=5+pJh>FCvQ4~BGNSjbqwekre#q12nb!5C>pvCbPVVj zre#paz)s#c6rq-~j5-GNjM8UWLWQM!yU;P9XOuq6qQ>ssbO!ZG+h>^^CG?ESXL3Y@)=IsijsZQR@_89GcJjtS z-6Zg(2<6J^K+hKFj3jhn`XSOpb`qTB%pmF`#EuK4+rFp4m9K*5TU3 z=S;|chdKuIjLPRsB#34|plIqC&@(EZGm#*4-QJ+7V?fWSe9lCIkX66zEU05Z&!~LP zL_|>k3H_F~oH_>djLK)EL_}IkcN5expl6gmBPAmAekdQlxdl1~^o+{qWz^VNo0g#n zsz*()BQK*DdPe2*G9n_arTbdy7|=8NeO^Y5UA<`;yo0*0ZT9NaF`#FZKFcCO$gWt> z)G?rER6ffhK{R_=-FZ{TfSytMoQVVxJkegjdmQK(&@(EZEfNvbe?q^pO${G)4Con^ z&*X>*wVR@;F>rTR>HF^ zKKCO*;LiDHNzWTm$KW{=pO=v!sBW_zc(*Ozctjn8r$&5UM$PjJJ}-l_nsHFaXxc{| zgC|pbUPeS{t(1d02G3{syo{RX7kEGL{yd&C=6Hqi#7^qsggq1ko&oV05E0==H6N*Z znv1Fg&zblviv)pa_2Y-0F{ARqvn)Q7BSByc@ne*pe51O;^D;hLBtbO3yI=Hl3JY}% zo*MCa83`ivE_IBi*QsOhWQxzrhzPBf_AYe{o-^@z88uH;_`HmWP7%~Gc+SM zP{-h@5ucY)WA|4&%9mwSC3w!nXIa$P$rsGa&~{L#3GK5?j*90@d?rUkXsxsz)G>H! z#OGzy*vT7*&Sc7Gc^MT?rue*!h)}ya!&zQN#d9V;FC!wT&4e~7+VV0=o-^@z84;nL zC_2liWAH?W&$6hY9M~BXZ3lIl&_2uLsCa6`XL3Y@)=Jw!9fK!Rd|pOGXf3|sfO23n zk>zESJpJPHG9p52sfU)AQSsD>&&#N>lP`Q;Mzou*8K`6MWQxzrhzR6K#C3`CSzboT zb0$77qsC6&I21ueByd|^M#)nnJ})C8kOvUIr%2J3mr)@vqvi85B0_zqj)DEUk0|OG zJZIwbG9p4fQMBb{lsq-!^D=6l-0*oB(JSgSp?#K@QSoGo&&!Ait(AI39fRjgd|pP) zQx)W(juH4$1a%Ca0r7bm5rH?n@q4PY-z+bq;yDwamk|+ampTS^#=ftpWAH4C&&!Ai zwW}UlUPj4NBR(&q=2-@xml3_9juF~tc^MT?rue*!h|pT8SJW|h2E^xO)I7^z9PUk= zt@_ga2Xzdd0r7bm5uvO5bT>gAgJ(c|UPeUdT3ykWmr?Nyh|kN22>&L|dQ{Ptmr?Ny zh|kN2259K70;RYyo`v@TIv0uj=^&#J};x@i4LEa!9A6&Ueo`F`wJHDHH_@(g&-2)MkE#zCH=(|vUg}Ynm;TFX^_*kow(^&L*+&1lKZls|{Pl*| zdHW0MIS*gX%A3t|QLUCzg!_M^>XmN0h=`mVL?}8%Xf5y=qLVL0j9+)ux(>sE%v*e9HK4v z!&=Mx0lUN5Cfe0rFph`-ht=YpQUvZtw3H&y4ny!hD?%+90{;wrN&qi?Gr_)w<$tD z=|GrQY*UI*c10+6mc9Ps2d2HQy|O!zgeOMElD3T{&aN9{x*>G*aR~ITdBu?k>%_|+ zDrsZ015V8K6W6=#nxjwXX_mVooLxnzC4AG=@e5bvSmnOmbG7AK-nKC~^aA6*8^U>_ z2(^SgWu-hI2Jw%pxp+x!vk z?9v!xx*?n=icm}VrjRR6GmbSEI;EwyT%5OUObjva#@&Gk=ZPZJ67NL(Hug+y1q5S0 zTxYo(LOp5Q>=O_Jyeh*d%~;Y7W6AI^u0Z#3z1v>@-M7o&uOD$x%OOHvF`ARU@*K{S z(7Qw{cjU+#4z=5^X*cDNr=-$P!js6cBs?Wqb!-89bt0`_Vb){pcL$lhLPATW-{D+YF#1 zcsGQzs|dBkCy{^d`CYD>`%!wb+ol)#naoAk2V1UT*FV1fTzhu2c0iy{5M43OtrkZE zwUqfk$Fbo5^%>$9_g})^wKB)S!4dk2B9uGjc>%38} z9e_Enw1s%ut=7^Qanuqjqk2H#iF=eSXKNLqmQdgR9LIbUwzAevf9WzmBfD{4dn848 zbbZr}mnR6dlp@p??z8c6UebO`5$Zc)`%$)uBhKJ`rCx=yUy(5){ltw#Zf{<_;<)NC!cBkx(Q+fP@|{JxG8M>4Y92^j>q%%pJOb zfPnO-RFx)(pikkgwcD(H&b`5WpO1NW_WJ*Sm)Wyt?|-eM2(^^@&`2#vZOYb9U7uMx}QKLU0~SeS|r~_lKQ**n=8jiU?09M`DBpWo!7;;v)*x$OsV-!3{mmgm|Ofxe_nhE>m5aeI&oDykPjETBGmUl zK3pWu?iBl0OMQedmVNQs;N)c#wI-%**~QA8MToP(S1^TvmZL=o!S zm?{2Ccs+axt1WjurKa8RY`XPTcT_HRMX05Le5lLE2R)mEqnXP;0HUMKbN2xnIjYAO8^R$HMzI5UKvb?YlXN_x?QmEKWA z=t0gB1NoRzGhRig?_`vCA5&_2cXp@Lw43^nC#upr;rZa47@jm zsjVh^OTyU)RHmxHhX=Q+H!9)&Rsw5c&l|IoLxnzB{$pMyxF_0 z5BC~ZU3B3kI7hp+|Kz{zU5^}jO}yscUw-KR{!Nb_iF)u~Kxipmhf>{$wHLlF+xg1T z)<-Bs#7Dy)>fdLXeNmT7;2_yLHPd2^bQeh(LYW-%|;cj zosWP}yPb&O&msJ{%IPE6B7!61h{sO1KEid1P`jOoV96mC!t?QaD9h1Dutfw%_z`EG zVSR+_6rpxI5y6r}tcK_OOFSd=5o{5`Cw1<3&a^(lb&62Corqw`A^hxPeFR%XaK>5e z-m|QaaGfI5ZYLsGatJ?*S|7m{5uAhZntnlC4OI}yQ> zL*O~D@cyuE73>m$C$&fB5__E@)NUstSaJx=J{8^{cD@RBiNK80!&!|z7_L)<+U-OH zOAf&`NtkzoEg~?V_sC3buTzBD?L-7i4iVPwS}I&0bIS>~9>HIVU=72afe=JH#H{c9 zteUjX(iKZBzv3TzMyz(UwL5q9QqNy^J=U(Z!gY$EkHHY(?+!8H;XSJPcKUlsAHf#U zd+hsaPxjhz)Aoo|KEpI zGhKd;jVgGjKShM=6rr(qB7!A{2vKP%UbAo52VGJ1evb7U|HnI_6^WQVV6+!Y-Ff?^JzP`M$M?3H(zD|lBW+aSIz_P5V2JQ{hgcge zI~!V`sMIXTy5zpLybI;>D>}!35 z>lC4OI}yQ>L)?UC#lMh z>@5)y?hvuYoU_%FmvcUvYelHtPDHTeqPi42QcGetiavrZBIY}Bmb&$`Q*1q&YhnUI z?RFx9CFkSk$jvwvdrR~YY!Q(k{+;^5#ZI<9nrmVLLhW`Uf+go;Ph=0=_{m35Yn~YG z60z)%nd{dpo&!&mYhupE{?bwNjIoGNsuQu`rZd&M_Jp}T_|Ql?5otYw4=u&(#_mM9 zb~OpRMa`Xn*!wQ#B)Y%5Yej@oj5k~>qN&G)sQH_7k3rr~V~Ys3!sEXl7u(8R)5n%* z-A_LG$Ra|iP6TVd&8_&w?=N%$A zGG^Q3NL!EQno$LW+U-O%{#X$|+O3aZiwKVJpI?5s_0e1t(TiQ-jdwZ7BEp?c1fRiG z#*g)F6?|wUorvI@9vUB7ir0-@H)BK}#?Mzas!)mu&Rzdpo{_lu%3ae%K&ahLM6hIo z3O>S|ScExo^?BwkWKOhyPZ8exfQSZ7jLwPS4x@VNv-yiKCpOoLV5z|n;qNZ0FvBsb zV2kJvj~QBoIkCAWCLlDDPDHTee1sW}K7uVG>hTK~VNPtWi3td`+ldI4oR2VH(MPaF z#A{zJQiM6Nxh5veS44z6oru5OxoFYMCBcVA(uoMZT~xt`mg03|ccNT7Ofx5jQUMWW zIR3qvOE7#f(v1nF!kpNY>huxjE7m-e3K51mku5UtoEYwOB3Sb{UnO(mU_Qd%UA=Wq zR3D99*R#<_GAD*ojEW;8%!$o4yd-ntV2JQ{7gd8z30f3P7k`MTGMfcQU%`e`l!{0pk77Kh45v4{g0Xv9LaF zdGF+SZsg7&1L7SZ)UG1bk}>zav1GL{eBAxo)J#i-Q!<_?hO;T>qaTR*fw;8;ap+%n zvQe$kbDTekbLWP}+%a-Bi#YkOEBjMRcW3ou_OpmT95dRV54ux8V{Z3|6M#^=iqHs+ zS@Y(#@%_J+o#Dc{BTo(k;y+vL6@9ofNJWHFj3itu!kt0h=A%04uPY}fiXp;_Cu|pM z?#>`t4@QN0lxkNIAzBxe^hX(LNL!(A4*F)}*dOi;lKsJPh5jhjt|HWuF?(TLu?Ii> z{L?b@Ep3Gnv+iD59J}t+@|Qqxlzaw++Es*Ff`)}3?LRnq{^Ue4jQpp!TPHq)?hNu$ zAlO#R0ikvk5yq~I>Lfh1^8)esn=AM;NOz(bp5Pg`SlypNx>G=7#`&4ycR;9JMTBS2 zMYSzvA4WA4|I7sfOhz)^IyEZ~WG?tEpj~}mBXBXECSs#t9sl^QUJ=4GF z=5D`)Ck;H0#M=Of<$$;w2(`;=MKqR92V&ShCSPG6V|_HXrWOZ0|5E=Sb~?be6;Dif z9%;V@)Q>(zGWuCX<>_`i?d=>PRPqpc5~{O~*y8W2GI z7Kouhs9jzwqOrst+<4?l`NQuWWPLQYrWU*3^XLAHFB@xpV7JzuM?$3lh@Sv42?({z zYeh7cIIb{4@*&q=Y4_DhpUMr%p^dio>=E0el z!M|Y3og6lZfOdZ$2*K0PP>R>7CAJmo@k5OGP>Qx#iji=?eP9Hi5k>HtZS}>~4iV6< zbu+Z?y%_Oomk9RY89V;Q`e5tw48e`75kO1^LhW`U8Xu>j4rgGLtcnp&AB`eE6u^p&qzn z5x!16q2~NM(Z;akJb1?WU$$;@$F@~OpmqJJrGI~vcz;+#bI0~zPv}9ku0Ml33$}mo z{z2ukzCX|h)~>DjOpiuHW9bMWzJnR`CO@tkTXrP&gpr6*;?E%W1>(zf>+&H#t}vFY zUHZV-jflq51RyrUn_vO|d^EP~6V(%*D2#T02FclH1Q6@{=Ofsq4?Ker(OB~Dj|%S( z`|D|#bBRBL^zV-f?+^Q&2fOrv*(V|zOaA>);r$Wj5^u}Sa6K}^+4IQw{;2T& z2y=w6|r+{wFz*P)aGisODcz;AhW68e_ zDm)|jmzL^0!AjmA38MR)tFdIv>*uVM&wuV()eD=PY2SmvZtxe-%}Lz*K%6*btWyz{{-W#ls#@5tg zo~{4X(|_eS>!Ue?d<}?G5Y_UxZ(XWgUMr%p^dS&8!p9HbLrd{``|rKc^Z5BkclcmE zj{fN!)%okcXQOJoPc3e~_2r&fK03l8n)AVrf!Ga*TYylzyjDbG=~c{y--ApX|EHyR zecpNZ^!(qO?(DAd(TUJfdP>$HSdTtLv_B$a4~9~mKAKaqr+_#Z?KBPTMMQIFYB6_l zd(XAExD|49cJ~J$Cd0=W@S%2jt%%0bb3m+w-a8w;NFR-@sl~D@-Pki??qAu~ZO$Nx zI0jMOj;PcwuNBc)dKQRBHa@ex3`Qe;G`6M|cTT*z=iRy7*F@`k%`Sn8IjSoKOe8T5O{s-^y?G^GYn^N{|=f8bA5n77Z;R}hD z-1YadL-fKLx3u|RM#6pk7q8-4ZECT?&^hX5_BqY&$%V5?V`fCj*YSn7JpSwVtZMGi zcOtl_?{@1qMaj?b6?r6fiil|L;BBz~kb2^sKK>Pw<^*{K> zry1+f*qT~QIeW(X^ux*NSjatq8=q zi0WPJ6a^n{@7~qC=SRoJwsQOJ0r3G4YM0k+tAJ>t!hU7`09yAH>=e;QIJG2#J$T@) zZqK?o?OP6rJAjxBgxc*yG?v(d`(u>+89POx&)rVE8;PfvV{JW}v%4jL*cyn>fl#}= z=GYC0CaN`o*n|5^*eRlq#+G}c_CDb->m!^rLMsf2KLRl)5Nem#d{zRYv9u}>KgLsg zKX!`fqp{`Yl5DX)1XKJAc?&%LaCpLF563<<* z2yA{tAbb7Es_YH{SV3m0KdY)<<+=fq%_J`~Yd>YNiBTT_dj4_vGW zb0SZE+<9c@oEYrVhawtFopWMi3nOuZC5kX7hLgtbIWgF!4@ESV=D{eLin(NzpA#Ef zQ;RWcE?I;*k>_e|_Tkt)ABZ*loEYrVhawtF*gwv>gC6F@#+E~b8IFH<^Oz#kZn##& zz;j|_%hf#0aN!it7@QO3*PtGg{hY|yX*aZ0Ks1&*=fuXA>%lN5Hm7}^b0XV{cIktC z6A+E1&N;EMfMGeelKn$y0{IkCBuA~0Y16UELsF+{?sI7Lc3-?I&WX*bC5J8k z_2w7OG_juJSNv_4`Rp%q@fSUK?Ug+3wJYoXZ*P9>uIJTX4n5u8@k)+e=)nS(n!3rs{Q-m2(_!|6!E~d7uEZpaiX>5H5%=;)@g8rmeLkU5eFT8 zOnujs$=0scY4Ca|LQ6g1^pFRr`PTtlEUiOYgrO~EQVT5DW;jggGXMZ7tzd6BcwUi=Svt1Su ziXI$6`$W;M)@ks1BnYRcZqfWZebQ1I748(lM_Ovj;(tR7jBdR2=Dy~I^WL8;M3Z zcvOn;zxvgMPJQ6{D>zOWiN;P{T+S`s^-zS?Nzthf&LepCY$O_$Mw=r1uW)rC)KZGT zd{}VaV7waP;Pv3I9&j!sLeYaGIIm+av{7k*0SPtILP_P7(Yi z2fmx=L;VfzgKtf|X{-rQsQ6ay@}a&{1hvii&Z7_YH@FYJ%h9^l z2jBj|-{3w_!-8+KP9N$!jS8*E-}>zGp}tcD-^>_^7NO|D5mdnE8l_L1&;rq4|og(l?D>#p{ltwssREp42 zd}H&y*j*34_xP6N9gRw(O`}qT`upE}XesrF->-dqz4qyS$Ki?PzwkRzc4cX2ZDTIy zDl^XD10%E)uho(8be09`O)`!{(b2|~?+O^d{2rZ@l9OCLJ50$s>a)9-rw!-_I zh;ThPLQAPXhuC_vxvG_)+Sd9|TVW+bM7SOtp{3NHL;PT&b*hCvS=0JZTNshtC&A2p zIe$~eUJs7YQtHnkPIztmYQ$x;S|4f)Pp=*EM1*UHnCIhzs_Rd`9cMVk=$&v)gnlO& zGmsCp%WL(G-J=0~s9n8wQ9XFyG1bb&GBzspj<?H z=BX|FU2FRTZv&2a|C|qmpxsbP{kfNXwV+f8+oSNkx- zaC$zp6nkySJH8E(mKuuIoelzBLhsGUXr4R!_ri@B=?{xQ>lQLz3pc`vSP}@1m`(&* zw~%KdA{tAb2()hDo=)375z$!cM4)wx@N8>68cUrBv~CfeZACPeIuU5yB0P7BXe@Oi z(7L`A(|RD-HgAx(02S;cr_2&>+hgMvV(udl@>fWwFi3rz& zBeazIbBL9n+P30)ls?oJz7y;kl!$OWI6_OQKZn3NwBmY{KGYW0p?2*?M7SOtp{3NH zLtq_Raji%nYKyyTTwM|ot_MeGDfQjYNd&!4X!&U9 zE5Ssw*W^J4AI=gG_!m~NO(fuaA=0Te8K7#(6%Pot3r1Z(5o9&~Z$7{hvp&dVRez}u zwF_pT^+ERG|K=kg$WaW4?zRf0Jgd?AP(*iI1q4fl2+3;f@`1{@*40v>-N;cK9HFH` zACPf4I6_N>7A1dhaD>R>dJ>vqN6WdYwO1T=Sa6LFeOQ}DHz}hP38jC*E7S>kw1e1txJvc&3sXvFn z+A8N7i$2sAPc^s}A|hN5j?hx-&mpk3%DKj(54DB0l|A_*B3uuS&{FEpA+WZ}xyGW8 z)RsGyBLYuW!KZC-gqBi&4uN?jr+xR&W*AzI0qr95&~gES4`-Q)q14D(S2CsSG2@}!OX?loJp7l#agYHCV?139TU{t+~FTK;S$@%xd zf?+9Jk6vD-cTz;)w6N54YA&4NrcRv*jh$Tmz{2nH(MxZPs=33l)F1*2hNWUuy+mj! zUI*U(fC%fOx#Q4^aL-Do47pOL2~(X3g{wTB_l2POs)#N*SO7wiQ~T;m-3K{cKr6 zyvA6teD|cSEJ91+nYWyu=_AnRkw=daW7$rrKWSa-L)ra`Xe@OiG!o2RMOZBjUXSLE zo#!J0b7j;Ek8^uhR2UhS8OC}v5q3s}QDPMqSSmqyRfO)=ZM<9NWKXjqd&g>3Cx~9! zCBi9*r-;C*PZ0Qj(%Rx(lK61%#Oc)o&tv4rHFXk%jjHkPq6+V;z`{?WvIvcY*8$O3 z>h#gra&-zEqrlrA*hjF-Q4$c1rA{AOiet>p=Z!W%w~xlVUGGFhVET6>)GnVc_m*k2 z0V1N8SFX>SKESh$G2p%|$m}6sk)_i0RddJA9}$7KoqrxX5zUl*dtgW{7XyWO&w>ru9~xWFA>^n zoDJN{Vt^iuh-Ngn`6D2LZi6k@vG!^D8S8{NUo{a*J#17O37=HALTVHwq7RgI^=Q1i z`6?iSRzpPa0{_ys!dk>KC;9g#!p^8P64wWAJ@>!+2v2b1uhR$bgi;)dVV%gRy4F{{ zA>J?>D1xoqwT82Z=8kKH;De*2Yn^Bj&7Bm%C#`EOY7xzy6v3ytYrSg`&7Bm{v~*`% zHFr|Pfc;^8=(DYD)wFu2kH%K&Bj|G&{EP2e>!Z1oB7)jRH=?=Ifrv^Nojy8NNO6>) zPTea@IJ9;#@5TM3IE!-KMV+b`73#o$POZb;aZv?yGgo47H|LIRK!g^>-mcAJCL&zB zF(!y!AC-+HlnOqAC7jVBkCS`6^btzYR>;65B3wHkg6IthE!7dzsV`N=(A`cPZg+qIeNM1<=u zL~lT7DfQ zJhS2v=M#&;*8QJ_fc)I>Nv zL{n-a`X>19L^P@js=(f8uCql$U%1w7^dy{0clafNM$2M)2)0kq({Fr#8^Wo$IAwmP znl6Z5_PIvYs0O&-3ve3ffDSz;qr27(ztb={f_)HF%-QEbTYYdu zQ_B7pjMs9XUQ`^fLGhja-_=%KHE&9}F&32T2agI`I9@~FJyAh{e{cj8dc01*{e6@8 zy8weD_|%495@?>_>D9a`rO(Rr5UfWSiOrKXJ%m2JSNh_+PlO z^NRnioX)5auh#^#4+0pKQiKr8S_NhaQHi}8Mt>Ul5r2T>MT8H@xb&5V3-<>`% zUSm{Rrv^PeL{rK>QQmhag0Y9+%3z(GT1)f0Io(e!MtF=}Bc!sC`-PHDABv#zQ|Oz< z_qQR!d__xL&tNZSANA3ca=-aB@O-6+MwKc3U75586~UUzd{qxTUnzq9ALc9e`M~p) zBAQZeBz}86Fz?1+?P33SwUwQ()JIdw{?f)EV^rLcaCnV`nZ zy6y_C<429Ie)GlD^6JUIsJGhxn);^`X>ZH=jLWXBAKUNx4uAXqyl!v$#P-!8=g(4g zB7XbemGyo9ytxCt>d?PWTmDyLsxO{7yURzYM_Ruf&i;7#KPQi^)}A=NoN=8U>+|ow zrk-;7wYB<75ht9mVEdzq`e_8FJ;X_v#QZ0PP4qb@H_PYpua#siXwHf+{?epHU z)lpCXq&n-hk@agmSJY2$eQVw6uko?K#@n`EpJS(L$7g3s5bX0%N?RI|t#>}s{^_o} zRhKXSMX5ftl(w5O=f3#a@WW@hu3YNW(bd6o|FVATHy6}r|N3ql`%2rNi}8Dpy>9BX z%cS1E+y3)XIr)Wsx)7mMTBiy39y|P#)m|Mh)W7UfulU)8^=rSst5$z0;>G&S z_7}&^Sxq=@n*^byw0_3?b-}9d<&FBv_J2lI@Ad6cKXLl`^&$1$i4XN{%=`bY*S{1` z?5lWk*EwsK`sD@Atsg(g)fR6c)cW3zR#;)#8>T(K{&VdMu1FD&ZM=Vd<#zo2V25bX z+r076aI@TL<;iD{O?+t0HCkid?w>ks)#WFZOYJZwL9ow5e<&K-(W{+O7Ki860!uDh zo%r7q>UqDJTp#h(t@ZVjj;ZIo`cixS)&?ik|2pZ4`lh}c5asuNKcaoh$oZ;$*V(fA z@f~C8i9fi=j&^OIv~?dg$F^TMZGq~M!!}P4TI$nd4y%v5=Cb-=_%`OvzinKuw*B1I z+uu(S8kI(C%sn$KP@eJd0#$qLril-=t7v@pKX7+Bbi%Oeo1N#YZkcm@ec728)rZ5o z)^@K^6Y7sAPOfjXHUEC^o#n;b4y*RtX8r`Bkto`jtH+;O9(dgx)kANNs75VyVm)G= z^Xru`GbloRLq+$r5#^UR&sF{8z|9kcwzQ&+Ir)b#mEXU7rE0=ji&htGH=#c6wexK~ z)NWdj9X6P;`m}H9YO%@lRogy0vHtGQ&antZr-FI~NNON!7^`EKXdJMMR` zZ>wV_UtW%z*6jb9Aebm?l34`!@dr_5LN?K-*Mrf-666ZMz+SbOiKtHO;x@q)9_2$KRi%@imn18GFt5?3dznt-+`4WVd zI&Ih$^`Wz$=zDOv1x)p){EBku5t~;>Y&NO>>#V2S9#kLd8^7oE^gqgdXMLc&`uPzF zLhGSuV;=d-AIl4GeXbm_$QB7gTUXJ>JpcT9)zI;GmXlUqup08!)yJ>{RY;yPo02#xu{%5zu$ z>D#n?Wv0y&gqG6!LD6{Fan;xZ)@xsTMsNGb-L9=?-eecMvc$f4WVpHYBKHC9%5u+f z)!GlgGHutNzndVm6xVX@eZqC=h?##@?fL13ydd+SjkqH}`92B?zsPqOmTyb8Iz! z^EHM)d%^ewvHES-)K6r)*j1*YjT!#psOr*x4QbDCSzm&XwXyDq4v2%sY)gQ0j3UJD;J5>LA;@RO7emEpSXl)e@ zA6WBzf9JFNmRk1ZQhjK`$sC|GMa~Z?;@6@u86{|KH2%lP(!q&wRmch;BKo z#J269Y52nnr#`gi>d%;Gj(ev)YS$auryX}}HN(Z{*AK1q%lbg9mbk)TZQZJmYaos* zu4+aOTc&*Bw50AKW*_owkLK<5XbCye!c%& zd)8y%FRjPHOTW|p=A9c_qdKBeyU)CEPQB5|d)B9o&_lt-*9MxP`haxK6%58)ttSnw6}O;rv#zxqyBI{wED;8zi)rAz04+i zBnY*uXv{u$JltM(l`++EeZMKE{NTX);#aS(hp#xb)^Doxt7yhMmM(98{phNGbZYyn zc@M9beEaHp&-1R1yhP48I7x||NMk0vG;2BG<)f>Qulp=PsNMAIZux6WIc=H4tAEeF zf2lq+LPZ<%%@#|PPmepKy7BO{%GHJ(R$ssA)%BYPPOTN8mW)~Z+Ih2AwEOhCyy#KGe4{n?5p2yPmv9_1=zuPZ0VIsQr)ql7&BRpSMm?-M{g2mHNgqFf-X5=Lsv;NY%mcM#_!>ZhC!K!-s(0ZE# zuB!Lj^v2|iCH=-Z;K2pUyGLwT4c&LK1fl(*{bbB{em-+~`;l8#JH9keg3wawD4F}6 znaTrq-@TgszrRu5e8Nx8w!bj#7V^6X(0G zocPGD)rqT3OAuO0ePeDnUzN-3wP|H0Oe>e%;`I8^FD|n+S0C!zn8n_FvwZHXy{eU; zpFKgS-LxJ{zx7J_z|p%_o4q|QL8x6t8?zo*VI#r3KX#az_QlL+)`vZFseL}wF7g8+ z)8ClMqdzPkdVcrnM_Yc~R)p3`(Z<|)#X{BF8;+E)Q6(6npwzH|Gs9EYQ6v5lOVKp6>ZFIE6-HD|J;Vvx|61rx2`s+ zKIFoSy82vw8}sB(=B)Odyjr!^2cIMe{c2Qy*e}_9@#>nzHmpYc-|Xe{&z@6%v)qNY z=4v;s`P+{#Ta8W2ajNRwD z`&&OczwYTh-}bpig1nQcAYshx+b&Zr@tX~*r62vetq84?qA_1>e_P+kufA;WHrKf7 zmfnNvA%8fhUVFoPYTZ>!_viMw=fL(1hfXVh^uVYDp*v~1mu1XKM;_gt`c}J~uwIJL zQo285%-w7Jq3?{1$CszwG&b>}k*Gg>U(HZdhb^~UwcXxdx8MKZg1U|UM84Iu71Fj^ zWw+(4caL15T71TPOGRjZXuBD+|N5&|SMI)AHDuPq5`>mgf5!ZI$ZhR=m$|1LG3$ZV zuxG~9kI#8dy)If;TRN@zkG{WXd+4}(%Ib*&s}uioRDIIc=hpk(e0Qzr6tTiq^Oi@Q z{dD>Bh4)DiT1t0xjA^ZXUf)^wPHsQ8)^XLmSC6iLH(^qJ%q{oSTYYB?P6MdUb8piB zSTo%5m-aSqz0!Mx|)H4R+bEz2C6S$_I}-DnV$?6>ZG4ueNNRdCB7CWs_1L zIuaF)FRw?ptuEVmu4?X!PwabVt4r&n9y`-MAKK5z{fa6s$bs8&wQ6iWS2b$8kIOT6 zzo_2w@JV(q(On$ve`Bt@WW{QR-F{HrJjb*Ip`~>67&Fs~8&((pYUXP5FYhlmI)8F~ z>X{SmNK_x{8@rs@M%6M~FIN5WZ!;wbt%ueRXJoJcv>N*U64jY&&z>MO5=9&H%5FW? zX2qh_wMB1&&`1=GGp{W-C~trJf^z8SgR1Z2J7ewVCfTv8qcR;=uV>4)XZXRV?Weyx zErAk%tL>Ba z`SO=;R2}=;jMe1%?@JKcAKFjYq3^-Y{K-SAH?AF(AheYFGiKc{W^OTIsmfxH6plZs-C)ZzGaCW^v<|~ab^|8{{qsmL(?<=zdMkWX?rTdp) z!f$^-b?#*kVI=O`_lq5`s@GZdWc!?JKO>JcDneoBZpl5WQ-1%K^0PN)E&J!ZvL3$j zsdkj;h*325!bj~{ZFj-D0 zGiK3OcB+=!=AH7R#g9)AT1uleX2VN%sp>;+Ef;+6z67DAbXV1w<)6a(?W8Bl8Rz{d zL1-x*`NsU>Wg>pwc|>6fwea=N{-sOn)~fL%7ea{mLRm0wwp2k7_ocx`Np@E8{U0Gg3wah3dX!R z@z>RQN1s*x;j7aVgqG4e8T0*L{Iq@3Zw_l;ksnvhH|~`Bg*Q&ObK*kJo>ITM<0*Dd z)Va%;4^Eu5ecee5luvyB*aV@abc7nS+Ol($D=czS`@Kj~cgt;e|6zIfmPaKBwX0}jF8ba^~uRPYJ{ne{t1_se|~Rr**j`vg3wZW7Glh>4GyhFJhDT1;P>_~^QEq-Z+z)c zJ9f1Xbi5g}#Q3ADoiCWV{PhDv5`>mY$L{QZ-?w^Th7HOk{?eBqbR_Bs#TWgXzpge~ zee3cM|2H*3Xeo^l+5DT_)js9O+uNfs`>ejlg!-nh#&`9gwj1*1r(RT6*a3NM?t>D9 zwshK7FTXsZ?7efN^6d}CBnY*uXk%8Ge025emA-B-G^&skNok zwt8W`vDK2V9MN9*it!0TPf61Ak2jt?rh4Y-JKAp!`EG(xyNbqocdk?0V|SXhe0Ygt zs{=NeP(Ss}$z4yBwxKcGUv)(LH#6t$yYUPvV)QRhub&*#Yo9@VR*bo3fpf|`XBb)T zj^|_e!e`YBAAYj!54D@N?z(5LP+ocCOy&7o9F-u{pQ5oAJ#uKd)Ri-|*O>X(1ffwW z+L*nrJ)xZEUq_dJ+wGugx!2CF-+T2W`$TDkX;e!c&@Rt8XTNgwGe;%}Eu|-o#;kJ9 z%k5`JpV{v)aw&lfyr5 zZ}X=E6NHvZqdMl}xvI(4#%0g0n^%)Rxv1X#^+W3mC*N6XDLpI1{?#%IRWDqzaJkPB zSf2<&OX*3YG5_45YMWO-Xdk}LvDKMlaH9Y9v36W(&9z1F1$pUN?Y~X^ci(#$SBltg z%QNfywxu4!y|to^dEw3L%Z+bcxLjhb1FN~``fdI2vq#(3Rl8azoIzgwlX9!YZfl>u z`B4c%{VCd*?@m~}{OyA$4qNdr$0P`iO3}uAcE+vcf1jDVeCqsB)e^tBpk8a_q4lhv z-&JdbX;h>4|8u$Xhx3=4ow{d&&{Ep!#{BG&7t7WxE3~)!>ps=P|GBtc@BRJjM@HXS z>&ckD4& zyupYBp>`E*%=I%2sa|;G|604A_lpFfcC}8%{Ai=^mY?_E-FMA>$5tQy`uZNaH}#=*6^$d753enM zdiS2~YY!Nm_)xoPJ@T`kYuBR}Z_jz~xCEi+oLVQmKaM=5Jngshv_E)hY=Tg`ipKl= zvwf@e{%^_h$KxI<-}&^a`mb9bSYN&JjkUgO_3eXq?T-6Zd*8K4IqtFt6NHx1cNdihkE)xS4=uzg$K`N~)LJTi*X}ZQ-|EZdziE#spG*+yPtnLp-Dl&f_2_=nZrWstN_}X}6>ZF2 z-x*ciH1)RjDkG2wDN$)8niGJX?cbq4{xMto$CDOMd}yXZ;?GRFZ;z^eceeIN?|+*3 zP`ipY<`(2w;{2!gBjj276Y!!lzd%pMBf<=VC^)xk-hnaW7QT*3Z@pVwIlA&^@kQrJ zetJgl5oQpCjWCUB=$b<+`k48_9V&koUoa~~?WSk+9$^MS*a#JE%;z)PU+wMXe|;WF z(V2yk=9kbmqoOZGXsI;6#QU(m6rpuW>!Cipt%7+yicWp7RT&jq+2{2X%!1caX}*s~ z*q$5!Vf`uE*wH~BZhYBHc#Mq8k-MVVO)f*8e~$=1BD)Ya!W0erwlqf~vqLx{LPm6x z<+~;PGFOJ9aykepwIzH7OGL1pTnjK`DrS#MsV&h0;qTKyXoP|$f+NKBBr}_0PP^FM z5@Rj=eL9FF!Za${ccZ?^P)EDrIf;A&jQYSi9HVNN<0|}pItV-BJ;y|% zA|nONl=aCm0%poEvQiYGrP650NC7jYkE|3jQjmR8 zkd>n76hTG`m?Ox*AR`6LlonYjiqKM8KV!&90W)PdSt(?s;KaBfD@A>%C1c1)0W;-(vQo%M!FhAx zSSftNa7MT{X$9m{gOLJe$}qB06hTG`PA)CNua^8Z87W|<^pTaK2#rdkHHM57FjIz+ zm7)mk4@G159E=n&Q!=tr$VdU>p&%=Tj1*)E*=sUVkOfmXRtgy@V5YRlN+BZ!r^j}* zYx|_FOGXNqDJ`;66rrWaNI@o2;aDkTq=1={k(HtdjY^}%moOM9V5Vebr6@w}DjH8M z7%5<;WMrj~kplig!EAM{Eg31u|0o&9;f6Qy*lcfSHn$l|n`em>V`KMW`h(U%*HKGbJZ0g^Uz%87xB4DT0g? zFjI1}QWT-3$VkCSknceY_Z5|=xUwW8<;Xq9N+BZ!j0d|Gr4OtXy<85P2hwmz``p8HDGbJZ0r4xa5wXdzB$u9xVBqztD6A|o^UxK}#!f{N< zF9FXaC&xq)S`V!s-XCDSfc=t_`J#xll=cT1DPX3w$Vwq21zd)LtQ7VjRvLEI=SIxL z!hLO%kuq}FG8tJZod~RXY$-BQus7-vWTb$Zl9QF92r^QzH(HRDlGdDz6fjdPD@74% zmy8tbrWTHsLPiRhDH&NQiqKN(&loaNz)Z==N+BZ!%#?zx6xJ3iSii>ARv;q<%#@t0 zluktG!4yqK3YaMwSt*@}(7GC}F=V8GnUa&0qKLG8>~4Y|iDaaJnUax}q6oE{qRB`B zGo?jV3K=P2KNMu8=oeD@RYXP#m?;@qDP*J|i@zW%MZeGZD_zLKN1t={AtMFMl#Hwt zMW|gxBNGse6fjdVvQiYG5h~gkGE%@y$;e6}BLzHzf~*ups3n|7fnNfiNk)zd87atw zFUU$!bh_#zhXq`gj64=as1HTs8wdOn@Jw3dn2?czjQ)bG6h){dWaEOv0xnC7JQhW0 ze<&I?2O|Z{ls>Xj$VdUtpdc$n5o*a8@=L%oX^~^12whX_8Xj*oa9F@)X_3dG2>s4f ze>gV)BL&QqHd!f(P`ipYhKv+2Q!=tr$VdTGpkV&4_F6iw$VdS*B_k_E5!!2Me~^&^ zW=cj@iXya>`oozp7%5<;WMrj~kpgByK~{>Q)2PTu0W&2dD@74n3ctD%f4LdIB?CqZ zm?;@qDP*L815%KcqV-6>amX(L&m<$qL=oE3X`hqB0xnBN9*ZKh&lQbt957PAOv%Vf zQH0t}$1WKuV5Vebr6@vUSAY1W9xzhCOv%VfQH0vnR=^Gm7%5<;WMrj~kphl{9VOZy z+UmxTkpgB)MplX<)UKlO#R5hOm?;@qDT+|LX%CW-0%l4^Rtgy@U~v>=rRaB*`op^s zj1(|aGO|(>p{3NfF=V8GnUax}LPiQW61L{*LoFFYMhciI8CfZcP`hb8$VdS*B_k_E z5o%Y_;H83*0%l51Rtgy@;Ck5SL+wI4)9Plrw*eU`V5Vebr6@xEDH@s_V5ESVl9QD} zMhaLJw&sdXeUOm?W=c+0iXzm9qM-)>MhciIIaw)+(AHHn^h>}<0W&2hD}{^{FgLpT zTrFWu4Mqx>DLGjwiqNk{^#_FwutmTc$;lWYBLzGITXRLIC1c1)0W&2hD@74nDvgS) z7qDM)GG7#-rP9_VBL&QqoU9ZwQowkyeXgbO`&d?w)%7_UDPX4LWThxV{V5vb3XBvm zQ-+h3LPiSm{tL2FbXQGx!i*s!1_&l+ z0%po^vQiYGk*Ggo$VdS*B_}I|j1*+p+aAhh4rsQO$ zC_+oAKdjxsNC7jYMOF$KDaf`j$V$;Vr8OtN1U!?B91}89kdt4Km7=9|4+vlM;IM$p z(jt#V5$aFT#*mQ$X3B7~QpiXFGo>Iag^Uzr#oI3k?SEubfsq1cN{g%%MW_!&8$(74 zm?yCM!h|8i}HjNe)H|m?=3~DP*L8DPW%u?PvT(oz+Bk&j%SP zV5a0`rI3*VPKKRJbQeeaAEOB0z%yx)V?ss> z*a&v)YCoqvNDd3QEE#z$iqKJ+j$JZRz)We8m7)l>t7v1$76EG{Cu4++6lC<<*3~+t ztxHAgxQnWE-y@36allh_uwVRGBGE%@yX_1veMhaLQ1z9O%q#&o-K2h3k z@Bu~&m?_i9N>PNC(iSm>j1(|aT4be=k%EkV+aKyfEg3^b3YaMwSt*K8yJ>%rkpgB) zi>wqys9i-HLw*T(CK)*_|Pc6B$>7;;#^WoePeq6qb;Xk=4?kpgB)PF4yT zDc~8{=Un>$zjtZ%)!lPWehGLcIXNa|q#)bgjuJ(vC1c280hc8wk3|vM|B6QIg8Kmu zNKXC-87W|<*ttY^IdzPICkze?xGXt&EQ-)l>JQ(8V5ESVl9QF92#rwvA#)k57qDM) zGG7#-b`=d34=_@|OlgsoLPiQQ?7PO5_6>Mfi=_ny11Rn6&WdDretKL zC_+o=SqMZtz(@fzB_}I|j1=Ue+OeyBpyLhSIN+CnXOfd+q6i&V+Ox)xUjm*Q?NuLigQou}Uk(EM53iu1Q2em)6-Jom)MhciI8CfZc&~{7P zii{L6Q!=tr6rpw%Z44PHV5a0`rI3+=jMajy6s=QQ4>D4~Ov%YgQG}k7q~{-Gq=1={ zla-lonYjiXbBetO5HBYD+_z0*n+e zQ!=tr$VdUh!uE&SOA0W+mVRtgy@U_98GYgFnFD;Y3Sz)Z==N>PNC zQs2q5c$Y3>hh4retKLC_` zA|nONl#HwtMQAB)b?kV7`vDF}PW}fODPX1)WTj}Sv_HsU0hc8sk3|uBmZrXqA%_KA zmW(_WMQADYhx|lvSioh;$zxH3Mv|Ujk&yysN={aaBD9o7h*bs{DPX2#WTlXi0%l4< zR*Ie|YTtm*2}TN-DH&NQiqQVhI-v)_NC7h?BP&G_YFE+7>jNWY;SaCP$VyR!+D+?0 zMoND6b1kw`6rtywS|?-3NC7h?BP&G_YFE)%UxASVW=c+03K=QjWE5nj=-XM}KE{xd z0%l51R*E9Dl)hJtAtMFMl$@*-MQER^Kja64kpgB)PF9K{w9gfda~v>Iz)Z=>N+BZ! z%#DJq6h){dJRe}BfSHn$m7)kOmDYod6fjeAvQo%M0SBZYD@9AC?^-faz)Z=>N>POR zQ#5#YV5ESVl9QF92(5>rvEBtE1~WEe}d$d1mb+wZfZ3u^QE?C6M25y+0tsoU?fqYLUW`t0avH?28xopY-2 z`&{RO8iVwqc5!|lE#Xx=er!&qf1ghs5jdlb5h~i|I_K2w_qommwc(i^&Flx(oVhXD zZrD#;VMtEhexFYr5zMe4!sk;*w9R$Ssj2UCoeOFjGCLZ%&atgDLYwQHQ|aI5I!6R@ zgrmPSDr8{iRQmTB*b$+n)Su0c&Zyh(v!e^@!TaoJ8@tbrj&-ux(HV97eRgz2Xw5Z3 zn;o4|x8G++7gWmf+0hZ5B9I-OQ@7t|M@NK~!s&agpUsZWsN3(eqYLV3`RwTE17{)8 zlFfB)QH9^rIcx2W{*^Qj}kWe2%Z%zh!lF?*2f zoKc0}=Q>A(mO`#`j1cR@NLje=$yL!K07)haDE=6 zLXL2Z)@DcN)b01#(Gj6`6^&wId-)PBZN9WY-_u0`Af%BnQC-rBuqjT!^`|Rj~8uzS+ z+D+?$T<4rB{65#YpyoUgS_-Ep1$Dh#&5=)?Q|aI5Q%3~ykfT3E<0S51UuV?R_u0_} z)jL>o>>I~AX@oXAI-}CR&yJ1=Ev5c!c63Iif1e#45n2kF%Q0G;9i3CR-)Bb`)PrZu zwNzSjWJhPz?f2Qy5rKW5=ugo$J36Oszt4^?s4&Dh}BW=z>b`L?HJ#qEiI2qciIE`|Rk5&{EiqEU1&?`W(5=8CCdwu5&@P z2=<^xrT&aTK6OT=f1ghs5n2k_ztOkNj?Spt@3W&LLR(k;;i(;eXGYzApB-ILv7R+o zyJnXGcebmclB$AV1u-6|$o<>h}BW=zE}pX*#uLB;;tS*spN~RvNqlZ(s-)Bch1oqxygm`NT8oACbs_^?<=ZL^6 zydXc^=Q_J*SSR% zexK_c5oxbAy@~Jh+aGLEh2Q5oM})RQibe)@AAcLbXJAJJV-G#3Xk(Baom02pXGa&* zhWFXg@e3*ajzV^HPThW=9bHi4+-FC}FH>ZK7St$r^9r(~bL#f{?C6M4yXn^*vZHhA z_WSJUh(KO&j8M@wJ36Oszt4^?sKV~Eqay-&uhEjtj?SsV@3W%|>V*63=!i~NeaMc^ zsr2u&qa#A2LY{3z+wAC^D*Qe>x}e^<&yJ2!ArmxOvf0r&mHvHpbVMNcIQED7v)R!( zb^CpGbU_t%pB)_$$Y_n0Z02%KO?{ua91*&1Pro^l9i3C@-)BchgqG3>Z3cEurGKA+ z9TB>2SARI|+iS0!D*QgzxuA|Zv!k^S($+;jbxx&!pHCeT+Ry1oM0Rve-F}}P9T93* z(Kb6er*6N`jxMO%?z5vKI`x6<=$yL!K07)hv=p*G3+j@)=L6Z%Id%Jec6337e4ia1 zzgW_59Aro5RN?p8(Gh|C=Gf9{pCdaur_#UAj*bZJ4@KMT=$yL!K07)h)NVR`ao83w1hSF9-HP=`uF+N z5usmOiniI&Id%Jec631ndDdJJ$YPF`Y<6@`-F}}P9T6HyS`TDL=hW@@+0hZ9b`@<5 za-DOk@cUfnf=c*&K9ETq(WwvQQ|DCr_xaQjp{0;1TTsK@Js-%9&Z*n)v!e@Y&a>vo zZ;t2`f$Zp)wx{{B0D;#ZokitjtJ!MMt_R7xy~6? z_j=}a_aW` z?C6M4yNbq2e3^SP>h}BW=z?10K07+rDQzp{I%m}F_qomm70a0&jeOCFP7%nb&Zyh( z^Qj|3OX;qW&5mwSx8G++7u4l_hk7rD-{6>L6rMx}qBPaP3jO8wdF=$yL!K0CUgjyj(Y?KNaH7S!E#&j)gybE@$B zT<3y{^__DGa)e{gA|oZAE2q-G&!>(E-S^QkX0xMns_^^l=z>b`9Elng@@!+Y*pqpG ziJU6@K07)hG!jMIT<4rB{65z?B9QAGBUH4_r_T9n0zRKQBD5Zgw%O4ws_^^l=z?19 z9J|`jX@4L)I-}CR&yJ1=`E*GnX^!_WR7`f_mp{U2V~{ z&yi1^Q-$B>Q%3|cqGKy4+8E?h=T!Rl`P31iZKY^r%Kq}q7Ipi5c633Fd!HR$P;r`1 zl(w7Aj?Spt@3W&LLQ5eJIkt$+bYcMcv{c%<$fs^m>EGv5M+CABV^qj{ zj=qtR^5-I>ZokitjtDKK{%m%1MiqXa9bHg)-DgL~eQMo}wAs-amHvHpbVMLiHu_8J zf$Zpvy8S*ox}XX>pL6YJWSADzK6lSKvZFJq@cZoOf?DJpC5k}SY_w#vqcbY~`|Rk5 zKvr?=e?{Bu=!^>aK0CUgZa?P|-R0B~W3!_(D*gNH=!no#>d$6JXVmTY+0hY!EY=vI z`m@>58GjqVXGceb+Euj8j&4!6-)Bb`)F0=#(ov#)W3!_(>h}BW=!igmbL`Eu&ygLS zQMcb`M@NL(RkY2H&Zyh(v!e@Y!?Oo9_Ou6)9i36P-)BchgqBK239_Rz>h}BW=z?nE z^nr}uh)xm6j?Spt@3W%|>YZ~WYN>Ru7unGnb^CpGbVMMlH2PDt&5q8f+wZfZ3+kP7 zT1apJcE(>5@HyKNp^+#WqMvtelu=XPXD%01cV`c3TOo5fwg_hGBX4g}>EGv5 zM}+pEqHR8PMx}qBPaP3zH*H;HF1M(u?=zPR>iIG|T5Fr0W*|GdMcsa%9UT!`O3y-U z26l_T4d64dBLdlNvE9_4&2?^3h2Q5o7u2=q6NNm~f(q$;qO=ukK6OT=f1ghs5n2kF z%dtglc63JFexDs(P=B8Np{3HcLauX*D*QgzIU+PFwy&5oW%-F}}P9T8d&MceG?jJo|k zJG!85yU&h}=rk&1M`zUS_u0`Ap{0h}BW=vXT44`fHTsPyl% zqa#Ak(vZs=ecN2;7Ipi5u5(1F-Sh+t`P41`Hh|BkjtI4z)&tqmZR+;>?C6M4yBeX* zjvh|kexDs(P`BS_N5_*&?HijN-J)*4&yJ1=8JJ31otoKsIpZFY2vy8S*oIwCX@McZ8Gj4J#-*SVl# zJ+q_rZGimU_?EEw)ESljeLi(W==)sJHaj|_ZokitjtCtkiZ;xSZc(@2XGceb_PL^M zc65un{XRRopsu~oj*e%|$a{{KY<6^uy8S*oIwG`GT61Jax2W6iv!e?th}BW=!igmbM%+`Kz8&n>h}BW=!npoE81p9x2W6iv!f$IBhj1yn;qSvZokit zjtJy8$1DQ%hmrX1Y%S{c`|Rk5P`iqz*0j|L|{ziUiTa$BPc|wU5>Hv_vs*#2%BhOyX8^*y$SdT znZ249Y_D;Tc{1OEu%)~u{}gcC(g(*bM}ljkyX^CTYg@|y-H+*SLDYP@QXd?J^ue*q zex(opdlS62cKzS|nEn<-&8I7kildM|I1)AA++W*K!k(l*HxfD0iQs5gghrxh_iRiD zVe6!5_iPZssJwT?ZGUgVwt}!Ad{i8_L~!gX^Fd?xfA?ehTM*WtqTRTq4>xujW=Lw+ z{vAz*{~^^=)GRtoj{$#@94gUtwMj#qd-sMk+s3Rx+gh){}p z{mkHTQBkj-JQH$EsMk+!LC6GT&xL$A*536w_4>(WA&;dK(Uh{8fWCF9*H2~&St*@} zP>(cP>h+VELRJbGDFt&^m?Nm@6hXayGE>M(pt;UQfbYp*H4}aIVRNWXHE=# zs6R#HH>9E0Pc932EQ-)l>f0FV^^<2pjtTYp$$ns-zI%e=8%}0z>h(jnKPQi+6T#=g zX}s#JO}&1yM#vbUUOzK+$V#D)(1Y4;*vo)kKXm(Z@>mq1rL+}{p@@rNr1DL}w~V#|_4=XP-y)Ai5!CDFS!X~rXzKNoXF`sNA~Y(E7OHj7>nE3mJQhW0 zB#JhMdi~Jt&&f)mUOyQRWTH^7pE*Cgrd~fYUmPoidi~Jt&&f)mUO!KdIoh>-($=M3 zKba|Hr6@v6QLmqQMUIt1y?*HS=VYZQLZi}XjiFvYbo+C%QWT+f6%7pt==GDCLRJd( z`pJG^KDE}Cdi~4=aoNVy>xXWCPF9K{G!jMQH`bxoPi6{PDb(xdX%(~86`_`lpp{JK@=VAvpPORQ#2SO(Cde8zt!xgUO&(5*d~fj zeNeBT%oMUxsMk*x2cuGiTEdqw^!lOOpOKY9y?$~Th){Hjpk6`C z2dURjo(VZ7)a&PdD0@(Ss3oumpw~|>3wbPx(0VA^80z&yx8G{^D?(dW(O?5Yub<2m zvQntmPfiAFu92kmp!z;}Cghkh(jne;9czig2q+zS-<=t>M3Xv++BTdi`Xkkd;Ece(v$G zbv2TX)`ea_nJHwYP_Lhi2d+hRO`Wd7slHFv2pJ=ap!z=dOt_v)qXi=+Co_et6h%;d zpF1FAr6}4M{w4s~FJ!(bLhWjV;2S`FpUf1pQmEI@-4u==jXnJ;qQXC!DP*N6LQADz zMby$KGli@aMQDWT&loEFlbJ$RiXzl575=$f z|Daw!`3q#Fq~Ee!198M~9Yno;GE>M(QG{FTxKh;X=iaD8P_LiN6tYqjLA`$Njgpm; z)`NQeWTudnq6oE1y?*YdI#vqx`k~uDoU9Z@Xesq)4Au9^Od%@;jFbX;_g1rCTQ~jM zroumz{@Y}wC_+o6?MC%|GE>M(QH0i3{o%Klp}tRM3Rx+N&^jp^nH=~{05Vg^N>PMH zqG-%M(Ca5Ng{&0n^^@^HR*HThrC&wV>nAgXtQ6|?GmD?B6#YJPN`RWy_kpw~}k3Rx+N&n1A==DRlKPM}Ndi~7kCo4q}Y6&YD==DRlKPM|i5!xS$ z#!L;pe(3h+WTjBApWFhnQWT+PMeNOd#sPO0Hm?%O^r6Z9F|Kzfe$D#-wCF&2WCFu2& zXF`sNA~beIqjjOzPc932EQ-)bv=xk@UO#mEbFxyX*H4ZFM~U`_wmSBGpw|!G{+z57 zMW|gxnDqY8C&`trT*|cCeZ6AmxVkQMQADY zjkf{x`k~vOla)ffesUyObM>K?jG>IqoQ6v*)L?iC_+o6txLUr zGE>M(pnAgXtQ6|?GuxiI zI9jK)=G5ya&x9Nk>h&`xpR5!urF%feP_Lg{7V=mWq5c#NJu>L^L$|+0Rtoj{p^$Gi z`>EH@tayGwX#ZnuL9ZXW{Z_MI5$Z$HcpE^kAG-b7gV5|(gw|Zq$o_<0KXm&u@>mq1 zktiA&*wE{TZok#+r(Qp~1$;iVpTS5em`(1U59;-kXF`q%_4>)f;9R1+INJZnl!abD zxh&+dC_+o=SiqiTwowkn9t5V^{k*?Lq4GlbJ$RiXwDW zrel|S{bZ()m7)l>t7v1W*H4}aIVRNWXSO|CSL>9vF7^7!Ga<)B5!zOY#^{G$KXm&u zGG7#-cGGc1y?!!N$V#DJKUo~i>!V&jbE^47X}h7%q1O-H{*0^?MQAB)5o4&=Po4=m zCe-U^MnC&QeW)d4sMinOeyiE92(_E`2le{NOd%^p5o%Y_nA@S(58ZyN*$+laL2d!_ zMb)nEMxy4>>xXWCMplX<)Sse_p(7AuEM? z{mi!KC{cu3!d@Tr`k~vOk(Htd?SDn%+ZlTO(CyF2N&zFKfI_~VOLS*b$CxqH>xXWC zMplX`E*4E6e<+ix}dsn^eJ`_6Ht zeFGl-smEo|?YEl!iqPIn`<#0HWTudnq6oFCXsEqGuOGVo8CfaR>nHnxJ*cs#JxINN zGE>M(QG}LCM+x=%$xI~)rwHoxlbJ$R3ibM#Vb77MrPAGD>h+VELRN|* z)Sse_p(7AuB}@T1wl^80z&yw?88*MG;y`TfrFW z^+UHmBP&G_T1x8#wKwSXL$}{*_EWE)To2BP)az$fJm*B6yNsb;KXm&uvQiYGrF8V* z><)VU(CxRH{nYCx8-Zh2OQj=`di`Xkkd>kcjY@qRL%n|J_FK(h+VELRN|*w3MEO7(=~&azM!cpk6=oPdRqA4|Kd4L%n`7Q^-nD zgpMoiS$v;EuOGVo8JRDN&{FCTI}_0Bhi-pH9*ZJ0lJxnYUO#yzs# zPi6{PDT>fi>4^z{69B&r&>|~E5!!BPTT!ncy8RhBCe-UEi=*?2()t-gy?*HSTg`q& zP_Lg10zQM<($HssUO#mEt!6*<`pK|hf2iHGb*a}+o(VZ7ico)w##$75{p7Nc$D#<0 zO3}tpuOGVoRxXW?)$CVu z1A6_??YEl!)az&7KU-JrYMqc91-*VUQ^-nDg!)soG1TjaZvQm0QWT+4DcTt7^+UJc zYW7pFpIipAQWTv=MZJD9Q^-nDgqG4)$GC!CKXm(BWTjBAAA0vzvtLW4{XxBcGE>M( zQG}kQsc)!wK(C)X6LL%xp{3NHG1TjaZhwnB7DZ?z>6sVx`pHZoD@74nN+ZP11oZl$ z+drJF6zcUuz20i}>xrWFjWN{ghi-q1tQ19Pe`uYM^#Z+q@=VAvQH0u6G!A8;*ALzP z7I`d+P`hb8z({G4nL<{IBJ`Y7>tqb|`k~w3A}d7^YFE*CpF^)7y8RhBCe-UECxfgM zeLL%$0nZ22_o2d{kz=9=?LkH3eGY|xDE((-z9>TN>YE6nC%AtQ1A4C9L|O*ALzP7Fj8Z&{Ans)axfRg{&0n^+UbhYW8cX^j%B6 zelkPORQ?xPE>nAgXtQ19PJrr#W_4=XP4+dIa;zJ|RoB-@>L$4pY{VlRm)Q4sf zB>oKb`pHZoD@74%SJBL*?zQ>Uyt?SZON!^;ulvVbJhtZb5zjr>f9{S4+Uq~A@N)mF z7ame~BW~TZzyIx*#&jWqrRgJ%p7vz_c~9-1pc6zdBe`sj2m22>>2O;r*yY~`kLvD; zW&g1ox~SAgh&J(IYrf|L5B9G#{)nWu^fA{_ul2vX-Qj7?ZB&ZT2nUbqr>FJxPyX9s z*2hP~AL`#{nSJebh_E{83M_RiY;ehq9HCIF^MIRSk_3d!8dk@}o-x25IyBg}_N(p*F zgI8fPK`#)zB25DAePrDw-V*IoYd=!YA6a*q6H4PMJa+iyc|X$CB~jOacC%Hu2ba9- za@Sy7^aG>hiJS3>i1llw1T?5ex=CR4EqyX*FfKHZ!RI`LzmHzmP(t-cHwon3Ge>;X z`=RuB4dmKxYXhNlE8$lcBmA7Ju5=BhTM55|N+2hW-x%~rXZ>&ujHT_Ky~;I|V5E$2 z4J9!8*6ngdMo%U%^7q|05TrB0TZNo>&Fs}4R|#rogtrQLXTRUoAQ-7h+Y4KSjmur*tzUVreD9_I;P&kw`cb*pq03vw)uJmOF2A(#V)s&d$8-O)eCneY zPa!t$_OtTn*+Hh)Y?W$gtEkr`PPqDx@@MY|l|kuGzNai7`-`@$RYF@ux~Z|`owp?o zB^aqmeDk{hnH#=4%(Z8}ZbQl02Q(`9Wa7MyFO-}UNlzxeaK*1n&h4Zp6Z@_HUCFtf zbd$K@wvWxdcw3ljm0@oZ2e(Z!)17OgbaH zmRz&_rR7I<53S-oz-%ynTq!{>63E9)!>f-H+H+o!$%N(y=_c`wneWYTm7zW7HJnW7 zx`cF-*!*QT_o%V`_ILKWyM(pqV|U!z`{uEywe(yGdf{5VVRlXL^_#vgC;WaWK`#(5 zf5%O||Nx`(ya-Z~ZA3wOn1**s%D4-oKw4=BTef_CRlo4Q=_M#N$iv?_IfFAV^Pc6cj5)!bJZK)s->qr)$9J(MNZHQ8n()`p5xOI>+3`%RF8C%cwy_Od+QGg>jb68 zYuL)Wx9qmC%20YTAuD}vJNtpva@Hyz?a&~hdgC>09o3T+S&wTtt1CUMyD|+QiMBrP zVb`E7fo<#S9(Dj3;jL0aTZmRQ`r#V39_(@TYU{StDz-M$Xb`sk>~ZxQJz zjQ0E5N4pZb7t#Fa3~qeig7Wu2e4DLjSM{~lvY!9oyk+S)`qAl-&@AVRz-LV0UwG*(!+Qe_9uKV|p=UrVK|5%iZhiRi z+(UElVts|pIRlY+JZO;6Gj)P=AYNhjViUV3Ab17@=T{u39f*f)gzRsBZ$i)137u;@ z18QhX>IAiO4fTS}xX`8@2;>Gvyq*EUdw}D#ofweNxOF1eusOX13op(rv@z>LgU3Uw zNa&e1f@=o*{Lh2e&%NxF3k!H@W#F8FNIV`iNaz{RO3)6(uCHD*+}l2t1O(53U~R{7 z+BujWbO;`8?TD7W2ztZOh92-aO_&<;e*4<~r0PH-;CHMAvlg4(%;dckH~w2F4r zxc}JlEB0wOdIkh%862mb6B@TpUencBAUKEPIPIW8LgUtnyj4H3&wp5c z)cP5X1%l%h$7u%*5*oKoK42oJOje6s6QP>B-%Nlaq9#l1>*CT8yr`R z1;R##KOKe!Z6^knANU+Ej>Pl~nO-0)Z~RGduAwcdYfw87)KD+jjN77NxxqZA7YNHc ze})W#c5GE7G;W<>q(E4`>at!j76_|2{tOu!v~xn^)(J)mgjLZQ)Lo4Qf};}0Y3GE- ztrK~xkQ-p(Ww~}A-!8$%IEDs~2MrQ>hO`pg<8(T=+GoA|)N-9I;WHVu1aHjP4;&8~ zB=ihuC1?lYLaSBFtY(4W84$Rlcvpzyv~xn^)(J)mgv}oqSFXWWATToUBo!L80};or z6FgHVkSlm{n`>xG>IAiO4fTS}xM&saKs;-C{&t%S^$ZB)9iBSlIPIL!xOF1eIL7ML z(XU$LD>B9cfr^7?;n1KRG)QRNI>AVRurXEGNSV`EAUIxeoOVuV+&YoBDsTh8R(#S3 zf;KdegVoqIVqpLHXr~tldNqgvas&NIC1C*(YaPAATM4L*EH9 zu0{&|Xd~1MHX{XMspZE4t5;|V-+-zYY#tB&Xd^%~uF^r{CL8T<-7M_S`92d~YU{hy z&H$ff`6?TYh^t;~iPE7}ZA86Kq?@gxdR>pl1970W=;xLn@ZvjDw1i%;c|5eLjR36# z?Vz!<-3we{c=1iH#)Zw}L8FbRw=iqfVBeRN$8kN*=ILqOm~m-9L`g z&IyfMC-PP;|IKd2Y47N{24jKXJ%9%7pg}_8)(J*(VqkyTzhTwK=eQtv51_&0PQ*V# zW#E}QK^o50?{Xp*cP6m8AArqWMw8%57+$&-Wu(ajn8pce>Tw~r_21wvQh z;|aDM1Y6kb2jVV$@^asEpoQF}LZk8Su7gG=`#0?pPoTg80_yi{*Ip=(^F=SOh21wGPPtNaeuSH)OIPwt1V zSd|{%DkbOz4UV3yRX$3%=7g8hhcgK;OfitkL}#dn*q^&Moc!8eEM1)H;7vmZ*R z-gphGOa2=oh|BkNu=V|5-YO;N1w!AejVI^@Lf_rxgpG#uu7JG-!TZ$gxe~lnlM&u3CA1%;Ga}T)_?sutu(gQ)9uaKP8Q~f>>-+Djs26N% zXM}6m%EEus1zt8I_qYzCUPicv67&L1?Tl~@>*0Nvx8cR!!sh-n5d&CyrcO*8iAjTT zLF-Pqk%Nh#7YJ5x`zEs&?^*P-=P&a2=YMwd$^E_e|EJOUJM1~cXRqF5-*~L{(X%I- z-!uYVj1-8Khwaea=g13OL*w#z(3nO*Luvce?6v>fJ^VymgHGqDlRn;GeDg}z(6~GvG^P>IP}=^s>`A*7 z`+cY98fV^lP5=7EpKuM0%i}>~8UYQZ?b}G+epIpSx*c6(i_=1Wv-!dc|2%LBcP$QeGk@uoLgLR^WVD0D{ood zKW?)#T|?vYc+i+eKtpN!cKJ0I6|Y-z^c?zO`CxTviEC(F9uFGR2xusc$oL7*kePQO zR+fXKDjz2h;5D7VMnR_otJw_L~l7$Cl(;$%O2Mt4KE&V5FMUD zToP&r0y%D(>l(;$%O2Mt4KE&V5FMUDToP&r0y%D(>l(;$%O2Mt4KE&V5FMUDToP&r z0y%D(>l(-h<{IhXr5PoJ-NYsVjhv1-J}`IG669SUUYd8baU6cE3Y|d<(MPLDgcc&l zE!PXM$Z^Xg_lhlaJ=)`mpzeKgB9DX|w_JA(IGXP`FuXKTz3uRxMh-S&})8#dcoF6KA)GC>#l(uw@h*kdd-heFW4H% z=kwBX-8GQomPxKbulW(`1zRKed|q0vcQLMzIGXP`FvhluDb?u+_J|t z=run=yJ@2czy{$q%~#rCU^XDxCZ zD{-*MamzQ>60A`&JLKRr0$z+12;?}{?$AJvTkd&W_2TjQ5x7=L+c&>kj$m_V~c+{@lO=uoeu~Ud0Rw z)xc;m!sDu5<&UeYUKmpr*9nX%({?*2R6_}kloQ%sqx%>GXuI9}KCW=HAJ*PJMjg&5 zp;sZ-u&da|_^lHfDJL|RYG`Y70wd2x$|$ydWNmj_uSibl8H{uidfjqDuZijPF*2c- zkFkpJd!DYS5*n%U$34$u`Dk|y)Ga&beaZ=q8@;Bq%DhI5h5zFJ(GR_b4UKelmC);! z6M1`GkNrs6t%Q2f9@F#K5BG|#A~9Ygol!#j)Myn5kBjkzs~zt-u3OZr7|c9XZ!xBD zuR$nX`Qx6e7pf3O{QL=21>^+?rE?8EgX)Id1A*#>{Dhqo8W*(+IXQm~B~;Hf20>|1 zpo*cABCZmu-RXS${}z_FbT7I0B`+>4Y?Ydxk?CEt+1bZ&n9t97bfEsLL=GU`5Q%f z%=ycQw_ozdg{@N4Gcu)fV%a0xl#f1h)^On~jx4n5E1{7(oqe9zr~Jew>xKvHx>I56 z+VqS}>72OswZ9r}amyXUW#2g}n{kxTNS)5nJHMuUde1G(8$Y;1VQbvloG3s1 z=a7U#I-S1$&kYuGylnRcd-hrgTnh*lN}A+AFpyZ$9hL zLg#HIG?G1sS-WPq>$=6|hqu_KuvKb$My7O5TyWTt!vzoRR9?9L;A}ouLL+rLUp{7! z;emr*dHU|V6t=ES&&YK4$%%IzaQEC*`@Xh(?hl6)T9K8|NcKC(e?7SO%`Yx2kF)$h z^+7JgRdK3z8p+<0e0s0BbN}Vt7t&Xmgvqz35{fb`+k#kUDOlQ670>9)6h^lC#;TiQBP1yte&S`krEoo z?%%D5j`K&-(@6HsCl~Blpq`+XU~i6`hKABP zVRfWHJwYweotF|C$-YN%-QoiE1hoWvbL2EMl+FpOBL(USY6(`|$ZzyR35{gmwKeD! zs3)i;*qb9mp`mn6SRE-)Pf$y+T19?CLkW%4=~x{pP)|@xEbscrb!gZ~cN?@u>U6A* z6sRYtB}%AQu7SDSayDMx{ue zgG0-~BIck!3x|fToN@wlx#eIt=3rX4DPb9vB6$v$mV-siL4Otw4PBAt1m<$f!EVgK zw3<}HGAc##94svdi=4t8S>rj@r6mQg8^=U{0$*o`^p&%&Xh>+77rTy8ly6LT=_Ae6BDNRd1TOUuDU zF$dE;s54oa-X4# zxMuKYeYjS6Zfnj@BBU~4-i@t79l@L)_Xg>CyS80h*y*5-U`~&FgY=X`30oDUNWM2H zFsH}8L3-YfogHRq)T^989l@L)_Xg>SkP@~kNRfPRP+(4vdxP}69XmVB(9p;U)Dg_- zac_{GMk!&df)vU31_kEyxHm}8+pz<~3=NH(KpnxH9`^?6DVGwqDoBxhZ%|-Pk9z}u z-i~{&Pjqqubp&&I+#954OG??j}o>jNRfPR(8Zh{_Xg>CJNEyWp`noz zs3Vxup;82It7WsJ${tUsm$neZ_O~5f+;zswOK51U8#aG=|E2rB(={p^#2NR$u{e2k z;e?*4?Ag)s8+YiRzV>3H$7{@vlu+$)8X8w^XdLwO4c(cwCwZ$>{fwXN)L(r4KYBkZ z8^kW#eYpF@tIu&l&s6s8==&>Q(?9-+)v+J#8nYuMR2$b6KjHT#G*)HLj&^bWlUa{s|{jc}Ds+>4ewNPh?ydO%aS6+83W>6}jRjsOalUk)-RkfQ$sF&W^ zs!g~b+N$!ZHe0BH8if;vFB+GXE~A7t?s6NRQ7nnSFefCNcoCPV_DwCd*iF{bf3l% z>ZN*Um9F8cxGS(XNaL!qLFgJz&s4VWcVe!L*YN#E(opS8qthO@uAwV3n^Tjz&9$kP zR5n`GPWb*T5gI99k>#?(7mVPLqUa`VWA=E2Y z>XtP_Rcmogsut$8z3R!JR6@P-x;u{0s#ev<`u$K1^{T4fBtpIPK2>eP9aIgy&*Mj; z61kU-_DUt!u$4s|?f$d{*)`>UL=E?fEo`~x+A8&mBXJ6$Ua|U2A=E35{3(Qb#rrXZ kP%q8-bZrOqJE&Dc$8jRe$nS^7&FPBC2^~v1Vmh7w2MAp!MgRZ+ literal 0 HcmV?d00001 diff --git a/realsense2_description/urdf/_d405.urdf.xacro b/realsense2_description/urdf/_d405.urdf.xacro new file mode 100644 index 0000000000..5ca17f2116 --- /dev/null +++ b/realsense2_description/urdf/_d405.urdf.xacro @@ -0,0 +1,165 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/realsense2_description/urdf/test_d405_camera.urdf.xacro b/realsense2_description/urdf/test_d405_camera.urdf.xacro new file mode 100644 index 0000000000..5a2b313f5d --- /dev/null +++ b/realsense2_description/urdf/test_d405_camera.urdf.xacro @@ -0,0 +1,10 @@ + + + + + + + + + +