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/base_realsense_node.h

@@ -39,7 +39,6 @@
 #include <sensor_msgs/msg/camera_info.hpp>
 #include <sensor_msgs/msg/image.hpp>
 #include <sensor_msgs/msg/imu.hpp>
-#include <geometry_msgs/msg/pose_stamped.hpp>
 
 #include <tf2/LinearMath/Quaternion.h>
 #include <tf2_ros/transform_broadcaster.h>

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/include/profile_manager.h

@@ -103,12 +103,4 @@ namespace realsense2_camera
         protected:
             std::map<stream_index_pair, std::shared_ptr<int> > _fps;
     };
-
-    class PoseProfilesManager : public MotionProfilesManager
-    {
-        public:
-            using MotionProfilesManager::MotionProfilesManager;
-            void registerProfileParameters(std::vector<stream_profile> all_profiles, std::function<void()> update_sensor_func) override;
-    };
-
 }

realsense2_camera/launch/rs_launch.py

@@ -44,9 +44,6 @@
                            {'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': 'enable_confidence',            'default': 'true', 'description': 'enable confidence'},
                            {'name': 'gyro_fps',                     'default': '0', 'description': "''"},
                            {'name': 'accel_fps',                    'default': '0', 'description': "''"},
                            {'name': 'enable_gyro',                  'default': 'false', 'description': "'enable gyro stream'"},

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/scripts/rs2_test.py

@@ -333,7 +333,7 @@ def run_tests(tests):
             listener_res = msg_retriever.wait_for_messages(themes)
             if 'static_tf' in [test['type'] for test in rec_tests]:
                 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']
+                frame_ids = ['camera_link', 'camera_depth_frame', 'camera_infra1_frame', 'camera_infra2_frame', 'camera_color_frame']
                 coupled_frame_ids = [xx for xx in itertools.combinations(frame_ids, 2)]
                 listener_res['static_tf'] = get_tfs(coupled_frame_ids)
 

realsense2_camera/scripts/show_center_depth.py

@@ -31,8 +31,6 @@ def __init__(self, depth_image_topic, depth_info_topic):
         self.bridge = CvBridge()
         self.sub = self.create_subscription(msg_Image, depth_image_topic, self.imageDepthCallback, 1)
         self.sub_info = self.create_subscription(CameraInfo, depth_info_topic, self.imageDepthInfoCallback, 1)
-        confidence_topic = depth_image_topic.replace('depth', 'confidence')
-        self.sub_conf = self.create_subscription(msg_Image, confidence_topic, self.confidenceCallback, 1)
         self.intrinsics = None
         self.pix = None
         self.pix_grade = None
@@ -62,17 +60,6 @@ def imageDepthCallback(self, data):
         except ValueError as e:
             return
 
-    def confidenceCallback(self, data):
-        try:
-            cv_image = self.bridge.imgmsg_to_cv2(data, data.encoding)
-            grades = np.bitwise_and(cv_image >> 4, 0x0f)
-            if (self.pix):
-                self.pix_grade = grades[self.pix[1], self.pix[0]]
-        except CvBridgeError as e:
-            print(e)
-            return
-
-
 
     def imageDepthInfoCallback(self, cameraInfo):
         try:
@@ -106,7 +93,6 @@ def main():
     print ('Application subscribes to %s and %s topics.' % (depth_image_topic, depth_info_topic))
     print ('Application then calculates and print the range to the closest object.')
     print ('If intrinsics data is available, it also prints the 3D location of the object')
-    print ('If a confedence map is also available in the topic %s, it also prints the confidence grade.' % depth_image_topic.replace('depth', 'confidence'))
     print ()
 
     listener = ImageListener(depth_image_topic, depth_info_topic)

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/profile_manager.cpp

@@ -591,20 +591,3 @@ void MotionProfilesManager::registerFPSParams()
     }
 }
 
-///////////////////////////////////////////////////////////////////////////////////////
-
-void PoseProfilesManager::registerProfileParameters(std::vector<stream_profile> all_profiles, std::function<void()> update_sensor_func)
-{
-    std::set<stream_index_pair> checked_sips;
-    for (auto& profile : all_profiles)
-    {
-        if (!profile.is<pose_stream_profile>()) continue;
-        _all_profiles.push_back(profile);
-        stream_index_pair sip(profile.stream_type(), profile.stream_index());
-        checked_sips.insert(sip);
-    }
-    registerSensorUpdateParam("enable_%s", checked_sips, _enabled_profiles, true, update_sensor_func);
-    registerSensorUpdateParam("%s_fps", checked_sips, _fps, 0, update_sensor_func);
-    registerSensorQOSParam("%s_qos", checked_sips, _profiles_image_qos_str, HID_QOS);
-    registerSensorQOSParam("%s_info_qos", checked_sips, _profiles_info_qos_str, DEFAULT_QOS);
-}

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/ros_sensor.cpp

@@ -176,12 +176,6 @@ void RosSensor::registerSensorParameters()
     {
         _profile_managers.push_back(profile_manager);
     }
-    profile_manager = std::make_shared<PoseProfilesManager>(_params.getParameters(), _logger);
-    profile_manager->registerProfileParameters(all_profiles, _update_sensor_func);
-    if (profile_manager->isTypeExist())
-    {
-        _profile_managers.push_back(profile_manager);
-    }
 }
 
 void RosSensor::runFirstFrameInitialization()