Added the changes from https://github.com/IntelRealSense/realsense-ro…

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/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)

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,24 +284,24 @@ 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))
         {
 
             // intra-process do not support latched QoS, so we need to disable intra-process for this topic
             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
@@ -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: