From af3cd227a49b0a71892e5a659640c5adcf966997 Mon Sep 17 00:00:00 2001
From: PrasRsRos <pras.rs.ros@gmail.com>
Date: Mon, 28 Aug 2023 20:48:49 +0530
Subject: [PATCH 1/4] 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<std::mutex> lock_guard(_mutex_publisher);
     if ((_is_enabled) && (!_pointcloud_publisher))
     {
-        _pointcloud_publisher = _node.create_publisher<sensor_msgs::msg::PointCloud2>("depth/color/points", 
+        _pointcloud_publisher = _node.create_publisher<sensor_msgs::msg::PointCloud2>("~/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<rs2_format> 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::msg::PointCloud2>();
 
     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<rs2::filter> filter, 
+AlignDepthFilter::AlignDepthFilter(std::shared_ptr<rs2::filter> filter,
     std::function<void(const rclcpp::Parameter&)> update_align_depth_func,
     std::shared_ptr<Parameters> 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<SyncedImuPublisher>(_node.create_publisher<sensor_msgs::msg::Imu>("imu", 5));
+    _synced_imu_publisher = std::make_shared<SyncedImuPublisher>(_node.create_publisher<sensor_msgs::msg::Imu>("~/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<void(rs2::frame)> frame_callback_function = [this](rs2::frame frame){
         bool is_filter(_filters.end() != find_if(_filters.begin(), _filters.end(), [](std::shared_ptr<NamedFilter> 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> rosSensor;
-        if (sensor.is<rs2::depth_sensor>() || 
+        if (sensor.is<rs2::depth_sensor>() ||
             sensor.is<rs2::color_sensor>() ||
             sensor.is<rs2::fisheye_sensor>())
         {
@@ -225,8 +225,8 @@ void BaseRealSenseNode::startPublishers(const std::vector<stream_profile>& profi
             if (sensor.rs2::sensor::is<rs2::depth_sensor>())
                 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<stream_profile>& 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<stream_profile>& profi
         else if (profile.is<rs2::motion_stream_profile>())
         {
             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<sensor_msgs::msg::Imu>(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<IMUInfo>(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<stream_profile>& profi
         else if (profile.is<rs2::pose_stream_profile>())
         {
             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<nav_msgs::msg::Odometry>(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<realsense2_camera_msgs::msg::Metadata>(topic_metadata, 
+        std::string topic_metadata("~/" + stream_name + "/metadata");
+        _metadata_publishers[sip] = _node.create_publisher<realsense2_camera_msgs::msg::Metadata>(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<stream_profile>& profi
             rclcpp::PublisherOptionsWithAllocator<std::allocator<void>> 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<realsense2_camera_msgs::msg::Extrinsics>(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<realsense2_camera_msgs::msg::Extrinsics>(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<realsense2_camera_msgs::msg::RGBD>("rgbd",
+            _rgbd_publisher = _node.create_publisher<realsense2_camera_msgs::msg::RGBD>("~/rgbd",
                 rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos), qos));
         }
         else {
@@ -327,7 +327,7 @@ void BaseRealSenseNode::startRGBDPublisherIfNeeded()
 }
 
 void BaseRealSenseNode::updateSensors()
-{    
+{
     std::lock_guard<std::mutex> lock_guard(_update_sensor_mutex);
     try{
         for(auto&& sensor : _available_ros_sensors)
@@ -337,7 +337,7 @@ void BaseRealSenseNode::updateSensors()
             std::vector<stream_profile> wanted_profiles;
 
             bool is_profile_changed(sensor->getUpdatedProfiles(wanted_profiles));
-            bool is_video_sensor = (sensor->is<rs2::depth_sensor>() || sensor->is<rs2::color_sensor>() || sensor->is<rs2::fisheye_sensor>());          
+            bool is_video_sensor = (sensor->is<rs2::depth_sensor>() || sensor->is<rs2::color_sensor>() || sensor->is<rs2::fisheye_sensor>());
 
             // 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 <pras.rs.ros@gmail.com>
Date: Mon, 28 Aug 2023 21:43:41 +0530
Subject: [PATCH 2/4] 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 <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 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
 
 <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.

From ec52d3b7c5f17136fe4e27c08168a716ea75bb03 Mon Sep 17 00:00:00 2001
From: PrasRsRos <pras.rs.ros@gmail.com>
Date: Mon, 28 Aug 2023 21:58:12 +0530
Subject: [PATCH 3/4] 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 <node> <parameter_name>` 
   - 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_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
 </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>
 
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<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);});

From bd385e999c18f2d5a91c82daa858758b02455cb9 Mon Sep 17 00:00:00 2001
From: PrasRsRos <pras.rs.ros@gmail.com>
Date: Mon, 28 Aug 2023 22:09:03 +0530
Subject: [PATCH 4/4] 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