Merge PR #2859 from PrasRsRos: Fix tests (topic now has camera name)

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 <node> <parameter_name>` 
-  - 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 <node> <parameter_name> <value>`
-  - For example: `ros2 param set /camera/camera depth_module.emitter_on_off 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.
@@ -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
@@ -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
 
 <hr>
 
@@ -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
 ```
 
 <hr>
@@ -455,10 +455,10 @@ python3 src/realsense-ros/realsense2_camera/scripts/echo_metadada.py /camera/dep
 </h3>
 
 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.</br>
     * 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.
@@ -487,7 +487,7 @@ Each of the above filters have it's own parameters, following the naming convent
   Available services
 </h3>
   
-- 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`
 
 <hr>
 

realsense2_camera/src/rs_node_setup.cpp

@@ -404,7 +404,7 @@ void BaseRealSenseNode::updateSensors()
 void BaseRealSenseNode::publishServices()
 {
     _device_info_srv = _node.create_service<realsense2_camera_msgs::srv::DeviceInfo>(
-            "device_info",
+            "~/device_info",
             [&](const realsense2_camera_msgs::srv::DeviceInfo::Request::SharedPtr req,
                         realsense2_camera_msgs::srv::DeviceInfo::Response::SharedPtr res)
                         {getDeviceInfo(req, res);});

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:

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

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

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

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

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