Merge branch 'IntelRealSense:ros2-development' into remove_l500_sr300

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 @@
 
 <hr>
 
+<h3 id="camera-name-and-namespace">
+  Camera Name And Camera Namespace
+</h3>
+
+### 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/humble/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
+```
+
+
+<hr>
+
+
 <h3 id="parameters">
   Parameters
 <h3>
@@ -268,7 +337,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`
-- ***<stream_name>*_frame_id**, ***<stream_name>*_optical_frame_id**, **aligned_depth_to_*<stream_name>*_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.
 
 - **unite_imu_method**:

realsense2_camera/CMakeLists.txt

@@ -147,15 +147,15 @@ endif()
 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)
+  set(SOURCES "${SOURCES}" src/ros_param_backend.cpp)
 elseif("$ENV{ROS_DISTRO}" STREQUAL "iron")
   message(STATUS "Build for ROS2 Iron")
   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DIRON")
-  set(SOURCES "${SOURCES}" src/ros_param_backend_foxy.cpp)
+  set(SOURCES "${SOURCES}" src/ros_param_backend.cpp)
 elseif("$ENV{ROS_DISTRO}" STREQUAL "rolling")
   message(STATUS "Build for ROS2 Rolling")
   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DROLLING")
-  set(SOURCES "${SOURCES}" src/ros_param_backend_rolling.cpp)
+  set(SOURCES "${SOURCES}" src/ros_param_backend.cpp)
 else()
   message(FATAL_ERROR "Unsupported ROS Distribution: " "$ENV{ROS_DISTRO}")
 endif()

realsense2_camera/include/constants.h

@@ -31,7 +31,7 @@
 #define ROS_WARN(...) RCLCPP_WARN(_logger, __VA_ARGS__)
 #define ROS_ERROR(...) RCLCPP_ERROR(_logger, __VA_ARGS__)
 
-// Based on: https://docs.ros2.org/foxy/api/rclcpp/logging_8hpp.html
+// Based on: https://docs.ros2.org/latest/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)
 #define ROS_WARN_STREAM(msg) RCLCPP_WARN_STREAM(_logger, msg)

realsense2_camera/scripts/rs2_listener.py

@@ -19,11 +19,10 @@
 from rclpy import qos
 from sensor_msgs.msg import Image as msg_Image
 import numpy as np
-import inspect
 import ctypes
 import struct
 import quaternion
-import os
+import tf2_ros
 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

realsense2_camera/src/base_realsense_node.cpp

@@ -22,7 +22,6 @@
 
 // Header files for disabling intra-process comms for static broadcaster.
 #include <rclcpp/publisher_options.hpp>
-
 #include <tf2_ros/qos.hpp>
 
 using namespace realsense2_camera;
@@ -171,7 +170,6 @@ void BaseRealSenseNode::initializeFormatsMaps()
     // 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;
@@ -856,7 +854,6 @@ bool BaseRealSenseNode::fillROSImageMsgAndReturnStatus(
     const rclcpp::Time& t,
     sensor_msgs::msg::Image* img_msg_ptr)
 {
-
     if (cv_matrix_image.empty())
     {
         ROS_ERROR_STREAM("cv::Mat is empty. Ignoring this frame.");
@@ -870,7 +867,7 @@ bool BaseRealSenseNode::fillROSImageMsgAndReturnStatus(
     }
     // Convert the CV::Mat into a ROS image message (1 copy is done here)
     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);
     img_msg_ptr->header.stamp = t;
@@ -891,7 +888,6 @@ bool BaseRealSenseNode::fillCVMatImageAndReturnStatus(
     auto& image = images[stream];
     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."

realsense2_camera/src/dynamic_params.cpp

@@ -113,7 +113,7 @@ namespace realsense2_camera
         try
         {
             ROS_DEBUG_STREAM("setParam::Setting parameter: " << param_name);
-            descriptor.dynamic_typing=true; // Without this, undeclare_parameter() throws in Galactic onward.
+            descriptor.dynamic_typing=true; // Without this, undeclare_parameter() throws error.
             if (!_node.get_parameter(param_name, result_value))
             {
                 result_value = _node.declare_parameter(param_name, initial_value, descriptor);

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();

realsense2_camera/src/rs_node_setup.cpp

@@ -225,6 +225,8 @@ void BaseRealSenseNode::startPublishers(const std::vector<stream_profile>& profi
             if (sensor.rs2::sensor::is<rs2::depth_sensor>())
                 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";
 
@@ -309,6 +311,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<realsense2_camera_msgs::msg::RGBD>("~/rgbd",
                 rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos), qos));
         }
@@ -396,6 +400,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<realsense2_camera_msgs::srv::DeviceInfo>(
             "~/device_info",
             [&](const realsense2_camera_msgs::srv::DeviceInfo::Request::SharedPtr req,