PR #2811 from Arun-Prasad-V: Exposing stream formats params to user

IntelRealSense · Jul 31, 2023 · 6a2a0fc · 6a2a0fc
2 parents 9b7daae + 1e64da2
commit 6a2a0fc
Show file tree

Hide file tree

Showing 6 changed files with 208 additions and 41 deletions.
diff --git a/README.md b/README.md
@@ -185,16 +185,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`.
@@ -205,16 +195,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)

diff --git a/realsense2_camera/include/profile_manager.h b/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;

diff --git a/realsense2_camera/include/ros_utils.h b/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);
 
 }
 
diff --git a/realsense2_camera/launch/rs_launch.py b/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'},

diff --git a/realsense2_camera/src/profile_manager.cpp b/realsense2_camera/src/profile_manager.cpp
@@ -114,28 +114,29 @@ bool ProfilesManager::isTypeExist()
     return (!_enabled_profiles.empty());
 }
 
-rs2::stream_profile ProfilesManager::getDefaultProfile()
+std::map<stream_index_pair, rs2::stream_profile> ProfilesManager::getDefaultProfiles()
 {
-    rs2::stream_profile default_profile;
+    std::map<stream_index_pair, rs2::stream_profile> sip_default_profiles;
     if (_all_profiles.empty()) 
         throw std::runtime_error("Wrong commands sequence. No profiles set.");
 
     for (auto profile : _all_profiles)
     {
         stream_index_pair sip(profile.stream_type(), profile.stream_index());
-        if (profile.is_default())
+        if (profile.is_default() && (sip_default_profiles.find(sip) == sip_default_profiles.end()))
         {
-            default_profile = profile;
-            break;
+            sip_default_profiles[sip] = profile;
         }
     }
-    if (!(default_profile.get()))
+
+    if (sip_default_profiles.empty())
     {
         ROS_INFO_STREAM("No default profile found. Setting the first available profile as the default one.");
-        default_profile = _all_profiles.front();
+        rs2::stream_profile first_profile = _all_profiles.front();
+        sip_default_profiles[{first_profile.stream_type(), first_profile.stream_index()}] = first_profile;
     }
 
-    return default_profile;
+    return sip_default_profiles;
 }
 
 void ProfilesManager::addWantedProfiles(std::vector<rs2::stream_profile>& wanted_profiles)
@@ -160,6 +161,22 @@ void ProfilesManager::addWantedProfiles(std::vector<rs2::stream_profile>& wanted
             ROS_DEBUG_STREAM("Found profile for " << ros_stream_to_string(sip.first) << ":" << sip.second);
         }
     }
+
+    // Warn the user if the enabled stream cannot be opened due to wrong profile selection
+    for (auto const & profile : _enabled_profiles)
+    {
+        auto sip = profile.first;
+        auto stream_enabled = profile.second;
+
+        if (*stream_enabled && !found_sips[sip])
+        {
+            ROS_WARN_STREAM("Couldn't open " << ros_stream_to_string(sip.first) << ":" << sip.second << " stream "
+                            << "due to wrong profile selection. "
+                            << "Please verify and update the profile settings (such as width, height, fps, format) "
+                            << "and re-enable the stream for the changes to take effect. "
+                            << "Run 'rs-enumerate-devices' command to know the list of profiles supported by the sensors.");
+        }
+    }
 }
 
 std::string ProfilesManager::profile_string(const rs2::stream_profile& profile)
@@ -204,29 +221,27 @@ VideoProfilesManager::VideoProfilesManager(std::shared_ptr<Parameters> parameter
     _module_name(module_name),
     _force_image_default_qos(force_image_default_qos)
 {
-    _allowed_formats[{RS2_STREAM_DEPTH, 0}] = RS2_FORMAT_Z16;
-    _allowed_formats[{RS2_STREAM_INFRARED, 0}] = RS2_FORMAT_RGB8;
-    _allowed_formats[{RS2_STREAM_INFRARED, 1}] = RS2_FORMAT_Y8;
-    _allowed_formats[{RS2_STREAM_INFRARED, 2}] = RS2_FORMAT_Y8;
+
 }
 
-bool VideoProfilesManager::isSameProfileValues(const rs2::stream_profile& profile, const int width, const int height, const int fps)
+bool VideoProfilesManager::isSameProfileValues(const rs2::stream_profile& profile, const int width, const int height, const int fps, const rs2_format format)
 {
     if (!profile.is<rs2::video_stream_profile>())
         return false;
     auto video_profile = profile.as<rs2::video_stream_profile>();
     ROS_DEBUG_STREAM("Sensor profile: " << profile_string(profile));
 
-    stream_index_pair sip = {video_profile.stream_type(), video_profile.stream_index()};
     return ((video_profile.width() == width) &&
             (video_profile.height() == height) &&
             (video_profile.fps() == fps) &&
-            (_allowed_formats.find(sip) == _allowed_formats.end() || video_profile.format() == _allowed_formats[sip] ));
+            (RS2_FORMAT_ANY == format || 
+             video_profile.format() == format));
 }
 
 bool VideoProfilesManager::isWantedProfile(const rs2::stream_profile& profile)
 {
-    return isSameProfileValues(profile, _width, _height, _fps);
+    stream_index_pair sip = {profile.stream_type(), profile.stream_index()};
+    return isSameProfileValues(profile, _width, _height, _fps, _formats[sip]);
 }
 
 void VideoProfilesManager::registerProfileParameters(std::vector<stream_profile> all_profiles, std::function<void()> update_sensor_func)
@@ -251,7 +266,7 @@ void VideoProfilesManager::registerProfileParameters(std::vector<stream_profile>
             ROS_DEBUG_STREAM(__LINE__ << ": _enabled_profiles[" << ros_stream_to_string(sip.first) << ":" << sip.second << "]: " << *(_enabled_profiles[sip]));
         }
 
-        registerVideoSensorParams();
+        registerVideoSensorParams(checked_sips);
     }
 }
 
@@ -275,16 +290,89 @@ std::string VideoProfilesManager::get_profiles_descriptions()
     return descriptors;
 }
 
-void VideoProfilesManager::registerVideoSensorParams()
+std::string VideoProfilesManager::getProfileFormatsDescriptions(stream_index_pair sip)
+{
+    std::set<std::string> profile_formats_str;
+    for (auto& profile : _all_profiles)
+    {
+        auto video_profile = profile.as<rs2::video_stream_profile>();
+        stream_index_pair profile_sip = {video_profile.stream_type(), video_profile.stream_index()};
+
+        if (sip == profile_sip)
+        {
+            std::stringstream crnt_profile_str;
+            crnt_profile_str << video_profile.format();
+            profile_formats_str.insert(crnt_profile_str.str());
+        }
+    }
+    std::stringstream descriptors_strm;
+    for (auto& profile_format_str : profile_formats_str)
+    {
+        descriptors_strm << profile_format_str << "\n";
+    }
+    std::string descriptors(descriptors_strm.str());
+    descriptors.pop_back();
+    return descriptors;
+}
+
+void VideoProfilesManager::registerVideoSensorProfileFormat(stream_index_pair sip)
+{
+    if (sip == DEPTH)
+        _formats[DEPTH] = RS2_FORMAT_Z16;
+    else if (sip == INFRA0)
+        _formats[INFRA0] = RS2_FORMAT_RGB8;
+    else if (sip == INFRA1)
+        _formats[INFRA1] = RS2_FORMAT_Y8;
+    else if (sip == INFRA2)
+        _formats[INFRA2] = RS2_FORMAT_Y8;
+    else if (sip == COLOR)
+        _formats[COLOR] = RS2_FORMAT_RGB8;
+    else
+        _formats[sip] = RS2_FORMAT_ANY;
+}
+
+void VideoProfilesManager::registerVideoSensorParams(std::set<stream_index_pair> sips)
 {
     // Set default values:
-    rs2::stream_profile default_profile = getDefaultProfile();
-    auto video_profile = default_profile.as<rs2::video_stream_profile>();
+    std::map<stream_index_pair, rs2::stream_profile> sip_default_profiles = getDefaultProfiles();
+
+    rs2::stream_profile default_profile = sip_default_profiles.begin()->second;
+
+    if (sip_default_profiles.find(DEPTH) != sip_default_profiles.end())
+    {
+        default_profile = sip_default_profiles[DEPTH];
+    } 
+    else if (sip_default_profiles.find(COLOR) != sip_default_profiles.end())
+    {
+        default_profile = sip_default_profiles[COLOR];
+    }
 
+    auto video_profile = default_profile.as<rs2::video_stream_profile>();
+
     _width = video_profile.width();
     _height = video_profile.height();
     _fps = video_profile.fps();
 
+    // Set the stream formats
+    for (auto sip : sips)
+    {
+        registerVideoSensorProfileFormat(sip);
+    }
+
+    // Overwrite the _formats with default values queried from LibRealsense
+    for (auto sip_default_profile : sip_default_profiles)
+    {
+        stream_index_pair sip = sip_default_profile.first;
+
+        default_profile = sip_default_profile.second;
+        video_profile = default_profile.as<rs2::video_stream_profile>();
+
+        if (isSameProfileValues(default_profile, _width, _height, _fps, video_profile.format()))
+        {
+            _formats[sip] = video_profile.format();
+        }
+    }
+
     // Register ROS parameter:
     std::string param_name(_module_name + ".profile");
     rcl_interfaces::msg::ParameterDescriptor crnt_descriptor;
@@ -311,7 +399,7 @@ void VideoProfilesManager::registerVideoSensorParams()
                         for (const auto& profile : _all_profiles)
                         {
                             found = false;
-                            if (isSameProfileValues(profile, temp_width, temp_height, temp_fps))
+                            if (isSameProfileValues(profile, temp_width, temp_height, temp_fps, RS2_FORMAT_ANY))
                             {
                                 _width = temp_width;
                                 _height = temp_height;
@@ -333,12 +421,55 @@ void VideoProfilesManager::registerVideoSensorParams()
                     }
                     else
                     {
-                        ROS_ERROR_STREAM("Given value, " << parameter.get_value<std::string>() << " is invalid. Set ROS param back to: " << crnt_profile_str.str());
+                        ROS_ERROR_STREAM("Given value, " << parameter.get_value<std::string>() << " is invalid. "
+                                                    << "Run 'ros2 param describe <your_node_name> " << parameter.get_name() 
+                                                    << "' to get the list of supported profiles. "
+                                                    << "Setting ROS param back to: " << crnt_profile_str.str());
                     }
                     _params.getParameters()->queueSetRosValue(parameter.get_name(), crnt_profile_str.str());
                 }
             }, crnt_descriptor);
     _parameters_names.push_back(param_name);
+
+    for (auto sip : sips)
+    {
+        std::string param_name(_module_name + "." + STREAM_NAME(sip) + "_format");
+        std::string param_value = rs2_format_to_string(_formats[sip]);
+        rcl_interfaces::msg::ParameterDescriptor crnt_descriptor;
+        crnt_descriptor.description = "Available options are:\n" + getProfileFormatsDescriptions(sip);
+        _params.getParameters()->setParam(param_name, param_value, [this, sip](const rclcpp::Parameter& parameter)
+                {
+                    std::string format_str(parameter.get_value<std::string>());
+                    rs2_format temp_format = string_to_rs2_format(format_str);
+                    bool found = false;
+
+                    if (temp_format != RS2_FORMAT_ANY)
+                    {
+                        for (const auto& profile : _all_profiles)
+                        {
+                            stream_index_pair profile_sip = {profile.stream_type(), profile.stream_index()};
+
+                            if (sip == profile_sip && temp_format == profile.format())
+                            {
+                                ROS_WARN_STREAM("re-enable the " << STREAM_NAME(sip) << " stream for the change to take effect.");
+                                found = true;
+                                _formats[sip] = temp_format;
+                                break;
+                            }
+                        }
+                    }
+                    if (!found)
+                    {
+                        ROS_ERROR_STREAM(STREAM_NAME(sip) <<" stream doesn't support " << format_str <<" format. "
+                                << "Run 'ros2 param describe <your_node_name> " << parameter.get_name() 
+                                << "' to get the list of supported formats. "
+                                << "Setting the ROS param '" << parameter.get_name() <<"' back to: " << _formats[sip]);
+                        _params.getParameters()->queueSetRosValue(parameter.get_name(), 
+                                                    (std::string)rs2_format_to_string(_formats[sip]));
+                    }
+                }, crnt_descriptor);
+        _parameters_names.push_back(param_name);
+    }
 }
 
 ///////////////////////////////////////////////////////////////////////////////////////
@@ -396,9 +527,12 @@ void MotionProfilesManager::registerFPSParams()
     }
 
     // Overwrite with default values:
-    rs2::stream_profile default_profile = getDefaultProfile();
-    stream_index_pair sip(default_profile.stream_type(), default_profile.stream_index());
-    *(_fps[sip]) = default_profile.as<rs2::motion_stream_profile>().fps();
+    std::map<stream_index_pair, rs2::stream_profile> sip_default_profiles = getDefaultProfiles();
+    for (auto sip_default_profile : sip_default_profiles)
+    {
+        stream_index_pair sip = sip_default_profile.first;
+        *(_fps[sip]) = sip_default_profile.second.as<rs2::motion_stream_profile>().fps();
+    }
 
     // Register ROS parameters:
     for (auto& fps : _fps)