Merge branch 'ros2-development' into ros2-reduce-changes-with-hkr

README.md

@@ -186,16 +186,6 @@
 <h3 id="parameters">
   Parameters
 <h3>
-
-### Sensor Parameters:
-- Each sensor has a unique set of parameters.
-- Video sensors, such as depth_module or rgb_camera have, at least, the 'profile' parameter.</br>
-  - The profile parameter is a string of the following format: \<width>X\<height>X\<fps> (The deviding character can be X, x or ",". Spaces are ignored.)
-  - For example: ```depth_module.profile:=640x480x30```
-- Since infra1, infra2 and depth are all streams of the depth_module, their width, height and fps are defined by their common sensor.
-- If the specified combination of parameters is not available by the device, the default configuration will be used.
-
-</br>
 
 ### Available Parameters:
 - For the entire list of parameters type `ros2 param list`.
@@ -206,16 +196,35 @@
 
 #### Parameters that can be modified during runtime:
 - All of the filters and sensors inner parameters.
+- Video Sensor Parameters: (```depth_module``` and ```rgb_camera```)
+  - They have, at least, the **profile** parameter.
+    - The profile parameter is a string of the following format: \<width>X\<height>X\<fps> (The dividing character can be X, x or ",". Spaces are ignored.)
+    - For example: ```depth_module.profile:=640x480x30 rgb_camera.profile:=1280x720x30```
+    - Since infra, infra1, infra2, fisheye, fisheye1, fisheye2 and depth are all streams of the depth_module, their width, height and fps are defined by the same param **depth_module.profile**
+    - If the specified combination of parameters is not available by the device, the default or previously set configuration will be used.
+      - Run ```ros2 param describe <your_node_name> <param_name>``` to get the list of supported profiles.
+    - Note: Should re-enable the stream for the change to take effect.
+  - ***<stream_name>*_format**
+    - This parameter is a string used to select the stream format.
+    - <stream_name> can be any of *infra, infra1, infra2, color, depth, fisheye, fisheye1, fisheye2*.
+    - For example: ```depth_module.depth_format:=Z16 depth_module.infra1_format:=y8 rgb_camera.color_format:=RGB8```
+    - This parameter supports both lower case and upper case letters.
+    - If the specified parameter is not available by the stream, the default or previously set configuration will be used.
+      - Run ```ros2 param describe <your_node_name> <param_name>``` to get the list of supported formats.
+    - Note: Should re-enable the stream for the change to take effect.
+  - If the stream doesn't support the user selected profile \<width>X\<height>X\<fps> + \<format>, it will not be opened and a warning message will be shown.
+    - Should update the profile settings and re-enable the stream for the change to take effect.
+    - Run ```rs-enumerate-devices``` command to know the list of profiles supported by the connected sensors.
 - **enable_*<stream_name>***: 
   - Choose whether to enable a specified stream or not. Default is true for images and false for orientation streams.
-  - <stream_name> can be any of *infra1, infra2, color, depth, fisheye, fisheye1, fisheye2, gyro, accel, pose*.
+  - <stream_name> can be any of *infra, infra1, infra2, color, depth, fisheye, fisheye1, fisheye2, gyro, accel, pose*.
   - For example: ```enable_infra1:=true enable_color:=false```
 - **enable_sync**:
   - gathers closest frames of different sensors, infra red, color and depth, to be sent with the same timetag.
   - This happens automatically when such filters as pointcloud are enabled.
 - ***<stream_type>*_qos**: 
   - Sets the QoS by which the topic is published.
-  - <stream_type> can be any of *infra, color, fisheye, depth, gyro, accel, pose*.
+  - <stream_type> can be any of *infra, infra1, infra2, color, depth, fisheye, fisheye1, fisheye2, gyro, accel, pose*.
   -  Available values are the following strings: `SYSTEM_DEFAULT`, `DEFAULT`, `PARAMETER_EVENTS`, `SERVICES_DEFAULT`, `PARAMETERS`, `SENSOR_DATA`.
   - For example: ```depth_qos:=SENSOR_DATA```
   - Reference: [ROS2 QoS profiles formal documentation](https://docs.ros.org/en/rolling/Concepts/About-Quality-of-Service-Settings.html#qos-profiles)

realsense2_camera/include/profile_manager.h

@@ -51,7 +51,7 @@ namespace realsense2_camera
             rmw_qos_profile_t getInfoQOS(const stream_index_pair& sip) const;
 
         protected:
-            rs2::stream_profile getDefaultProfile();
+            std::map<stream_index_pair, rs2::stream_profile> getDefaultProfiles();
 
         protected:
             rclcpp::Logger _logger;
@@ -73,13 +73,15 @@ namespace realsense2_camera
             int getFPS() {return _fps;};
 
         private:
-            bool isSameProfileValues(const rs2::stream_profile& profile, const int width, const int height, const int fps);
-            void registerVideoSensorParams();
+            bool isSameProfileValues(const rs2::stream_profile& profile, const int width, const int height, const int fps, const rs2_format format);
+            void registerVideoSensorProfileFormat(stream_index_pair sip);
+            void registerVideoSensorParams(std::set<stream_index_pair> sips);
             std::string get_profiles_descriptions();
+            std::string getProfileFormatsDescriptions(stream_index_pair sip);
 
         private:
             std::string _module_name;
-            std::map<stream_index_pair, rs2_format>  _allowed_formats;
+            std::map<stream_index_pair, rs2_format>  _formats;
             int      _fps;
             int _width, _height;
             bool _is_profile_exist;

realsense2_camera/include/ros_utils.h

@@ -45,6 +45,7 @@ namespace realsense2_camera
     std::string create_graph_resource_name(const std::string &original_name);
     const rmw_qos_profile_t qos_string_to_qos(std::string str);
     const std::string list_available_qos_strings();
+    rs2_format string_to_rs2_format(std::string str);
 
 }
 

realsense2_camera/launch/rs_launch.py

@@ -31,8 +31,13 @@
                            {'name': 'log_level',                    'default': 'info', 'description': 'debug log level [DEBUG|INFO|WARN|ERROR|FATAL]'},
                            {'name': 'output',                       'default': 'screen', 'description': 'pipe node output [screen|log]'},
                            {'name': 'depth_module.profile',         'default': '0,0,0', 'description': 'depth module profile'},                           
+                           {'name': 'depth_module.depth_format',    'default': 'Z16', 'description': 'depth stream format'},
+                           {'name': 'depth_module.infra_format',    'default': 'RGB8', 'description': 'infra0 stream format'},
+                           {'name': 'depth_module.infra1_format',   'default': 'Y8', 'description': 'infra1 stream format'},
+                           {'name': 'depth_module.infra2_format',   'default': 'Y8', 'description': 'infra2 stream format'},
                            {'name': 'enable_depth',                 'default': 'true', 'description': 'enable depth stream'},
                            {'name': 'rgb_camera.profile',           'default': '0,0,0', 'description': 'color image width'},
+                           {'name': 'rgb_camera.color_format',      'default': 'RGB8', 'description': 'color stream format'},
                            {'name': 'rgb_camera.enable_auto_exposure', 'default': 'true', 'description': 'enable/disable auto exposure for color image'},
                            {'name': 'enable_color',                 'default': 'true', 'description': 'enable color stream'},
                            {'name': 'enable_infra',                 'default': 'false', 'description': 'enable infra0 stream'},

realsense2_camera/src/base_realsense_node.cpp

@@ -1003,7 +1003,9 @@ void BaseRealSenseNode::publishRGBD(const cv::Mat& rgb_cv_matrix, const cv::Mat&
         auto depth_camera_info = _camera_info.at(DEPTH);
         msg->depth_camera_info = depth_camera_info;
 
+        realsense2_camera_msgs::msg::RGBD *msg_address = msg.get();
         _rgbd_publisher->publish(std::move(msg));
+        ROS_DEBUG_STREAM("rgbd stream published, message address: " << std::hex << msg_address);
     }
 }