Support for selecting profile for each stream_type

README.md

@@ -172,7 +172,7 @@
 
   #### with ros2 launch:
     ros2 launch realsense2_camera rs_launch.py
-    ros2 launch realsense2_camera rs_launch.py depth_module.profile:=1280x720x30 pointcloud.enable:=true
+    ros2 launch realsense2_camera rs_launch.py depth_module.depth_profile:=1280x720x30 pointcloud.enable:=true
 
 <hr>
 
@@ -251,10 +251,10 @@ User can set the camera name and camera namespace, to distinguish between camera
 #### 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.
+  - They have, at least, the **<stream_type>_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 and depth are all streams of the depth_module, their width, height and fps are defined by the same param **depth_module.profile**
+    - For example: ```depth_module.depth_profile:=640x480x30 depth_module.infra_profile:=640x480x30 rgb_camera.color_profile:=1280x720x30```
+    - Note: The param **depth_module.infra_profile** is common for all infra streams. i.e., infra 0, 1 & 2.
     - 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.

realsense2_camera/include/profile_manager.h

@@ -68,22 +68,22 @@ namespace realsense2_camera
             VideoProfilesManager(std::shared_ptr<Parameters> parameters, const std::string& module_name, rclcpp::Logger logger, bool force_image_default_qos = false);
             bool isWantedProfile(const rs2::stream_profile& profile) override;
             void registerProfileParameters(std::vector<stream_profile> all_profiles, std::function<void()> update_sensor_func) override;
-            int getHeight() {return _height;};
-            int getWidth() {return _width;};
-            int getFPS() {return _fps;};
+            int getHeight(rs2_stream stream_type) {return _height[stream_type];};
+            int getWidth(rs2_stream stream_type) {return _width[stream_type];};
+            int getFPS(rs2_stream stream_type) {return _fps[stream_type];};
 
         private:
             bool isSameProfileValues(const rs2::stream_profile& profile, const int width, const int height, const int fps, const rs2_format format);
+            rs2::stream_profile validateAndGetSuitableProfile(rs2_stream stream_type, rs2::stream_profile given_profile);
             void registerVideoSensorProfileFormat(stream_index_pair sip);
             void registerVideoSensorParams(std::set<stream_index_pair> sips);
-            std::string get_profiles_descriptions();
+            std::string get_profiles_descriptions(rs2_stream stream_type);
             std::string getProfileFormatsDescriptions(stream_index_pair sip);
 
         private:
             std::string _module_name;
             std::map<stream_index_pair, rs2_format>  _formats;
-            int      _fps;
-            int _width, _height;
+            std::map<rs2_stream, int> _fps, _width, _height;
             bool _is_profile_exist;
             bool _force_image_default_qos;
     };

realsense2_camera/launch/rs_launch.py

@@ -34,15 +34,16 @@
                            {'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': 'enable_color',                 'default': 'true', 'description': 'enable color stream'},
-                           {'name': 'rgb_camera.profile',           'default': '0,0,0', 'description': 'color image width'},
+                           {'name': 'rgb_camera.color_profile',     'default': '0,0,0', 'description': 'color stream profile'},
                            {'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_depth',                 'default': 'true', 'description': 'enable depth stream'},
                            {'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': 'depth_module.profile',         'default': '0,0,0', 'description': 'depth module profile'},
+                           {'name': 'depth_module.depth_profile',   'default': '0,0,0', 'description': 'depth stream profile'},
                            {'name': 'depth_module.depth_format',    'default': 'Z16', 'description': 'depth stream format'},
+                           {'name': 'depth_module.infra_profile',   'default': '0,0,0', 'description': 'infra streams (0/1/2) profile'},
                            {'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'},

realsense2_camera/src/profile_manager.cpp

@@ -139,6 +139,40 @@ std::map<stream_index_pair, rs2::stream_profile> ProfilesManager::getDefaultProf
     return sip_default_profiles;
 }
 
+rs2::stream_profile VideoProfilesManager::validateAndGetSuitableProfile(rs2_stream stream_type, rs2::stream_profile given_profile)
+{
+    rs2::stream_profile suitable_profile = given_profile;
+    bool is_given_profile_suitable = false;
+
+    for (auto temp_profile : _all_profiles)
+    {
+        if (temp_profile.stream_type() == stream_type)
+        {
+            auto video_profile = given_profile.as<rs2::video_stream_profile>();
+
+            if (isSameProfileValues(temp_profile, video_profile.width(), video_profile.height(), video_profile.fps(), RS2_FORMAT_ANY))
+            {
+                is_given_profile_suitable = true;
+                break;
+            }
+        }
+    }
+
+    // If given profile is not suitable, choose the first available profile for the given stream.
+    if (!is_given_profile_suitable)
+    {
+        for (auto temp_profile : _all_profiles)
+        {
+            if (temp_profile.stream_type() == stream_type)
+            {
+                suitable_profile = temp_profile;
+            }
+        }
+    }
+
+    return suitable_profile;
+}
+
 void ProfilesManager::addWantedProfiles(std::vector<rs2::stream_profile>& wanted_profiles)
 {    
     std::map<stream_index_pair, bool> found_sips;
@@ -241,7 +275,7 @@ bool VideoProfilesManager::isSameProfileValues(const rs2::stream_profile& profil
 bool VideoProfilesManager::isWantedProfile(const rs2::stream_profile& profile)
 {
     stream_index_pair sip = {profile.stream_type(), profile.stream_index()};
-    return isSameProfileValues(profile, _width, _height, _fps, _formats[sip]);
+    return isSameProfileValues(profile, _width[sip.first], _height[sip.first], _fps[sip.first], _formats[sip]);
 }
 
 void VideoProfilesManager::registerProfileParameters(std::vector<stream_profile> all_profiles, std::function<void()> update_sensor_func)
@@ -270,15 +304,18 @@ void VideoProfilesManager::registerProfileParameters(std::vector<stream_profile>
     }
 }
 
-std::string VideoProfilesManager::get_profiles_descriptions()
+std::string VideoProfilesManager::get_profiles_descriptions(rs2_stream stream_type)
 {
     std::set<std::string> profiles_str;
     for (auto& profile : _all_profiles)
     {
-        auto video_profile = profile.as<rs2::video_stream_profile>();
-        std::stringstream crnt_profile_str;
-        crnt_profile_str << video_profile.width() << "x" << video_profile.height() << "x" << video_profile.fps();
-        profiles_str.insert(crnt_profile_str.str());
+        if (stream_type == profile.stream_type())
+        {
+            auto video_profile = profile.as<rs2::video_stream_profile>();
+            std::stringstream crnt_profile_str;
+            crnt_profile_str << video_profile.width() << "x" << video_profile.height() << "x" << video_profile.fps();
+            profiles_str.insert(crnt_profile_str.str());
+        }
     }
     std::stringstream descriptors_strm;
     for (auto& profile_str : profiles_str)
@@ -333,25 +370,52 @@ void VideoProfilesManager::registerVideoSensorProfileFormat(stream_index_pair si
 
 void VideoProfilesManager::registerVideoSensorParams(std::set<stream_index_pair> sips)
 {
-    // Set default values:
-    std::map<stream_index_pair, rs2::stream_profile> sip_default_profiles = getDefaultProfiles();
-
-    rs2::stream_profile default_profile = sip_default_profiles.begin()->second;
+    std::set<rs2_stream> available_streams;
 
-    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())
+    for (auto sip : sips)
     {
-        default_profile = sip_default_profiles[COLOR];
+        available_streams.insert(sip.first);
     }
 
-    auto video_profile = default_profile.as<rs2::video_stream_profile>();
+    // Set default values:
+    std::map<stream_index_pair, rs2::stream_profile> sip_default_profiles = getDefaultProfiles();
 
-    _width = video_profile.width();
-    _height = video_profile.height();
-    _fps = video_profile.fps();
+    for (auto stream_type : available_streams)
+    {
+        rs2::stream_profile default_profile = sip_default_profiles.begin()->second;
+        switch (stream_type)
+        {
+            case RS2_STREAM_COLOR:
+                if (sip_default_profiles.find(COLOR) != sip_default_profiles.end())
+                {
+                    default_profile = sip_default_profiles[COLOR];
+                }
+                break;
+            case RS2_STREAM_DEPTH:
+                if (sip_default_profiles.find(DEPTH) != sip_default_profiles.end())
+                {
+                    default_profile = sip_default_profiles[DEPTH];
+                }
+                break;
+            case RS2_STREAM_INFRARED:
+                if (sip_default_profiles.find(INFRA1) != sip_default_profiles.end())
+                {
+                    default_profile = sip_default_profiles[INFRA1];
+                }
+                else if (sip_default_profiles.find(DEPTH) != sip_default_profiles.end())
+                {
+                    default_profile = validateAndGetSuitableProfile(stream_type, sip_default_profiles[DEPTH]);
+                }
+                break;
+            default:
+                default_profile = validateAndGetSuitableProfile(stream_type, sip_default_profiles.begin()->second);
+        }
+
+        auto video_profile = default_profile.as<rs2::video_stream_profile>();
+        _width[stream_type] = video_profile.width();
+        _height[stream_type] = video_profile.height();
+        _fps[stream_type] = video_profile.fps();
+    }
 
     // Set the stream formats
     for (auto sip : sips)
@@ -364,72 +428,76 @@ void VideoProfilesManager::registerVideoSensorParams(std::set<stream_index_pair>
     {
         stream_index_pair sip = sip_default_profile.first;
 
-        default_profile = sip_default_profile.second;
-        video_profile = default_profile.as<rs2::video_stream_profile>();
+        auto default_profile = sip_default_profile.second;
+        auto video_profile = default_profile.as<rs2::video_stream_profile>();
 
-        if (isSameProfileValues(default_profile, _width, _height, _fps, video_profile.format()))
+        if (isSameProfileValues(default_profile, _width[sip.first], _height[sip.first], _fps[sip.first], video_profile.format()))
         {
             _formats[sip] = video_profile.format();
         }
     }
 
-    // Register ROS parameter:
-    std::string param_name(_module_name + ".profile");
-    rcl_interfaces::msg::ParameterDescriptor crnt_descriptor;
-    crnt_descriptor.description = "Available options are:\n" + get_profiles_descriptions();
-    std::stringstream crnt_profile_str;
-    crnt_profile_str << _width << "x" << _height << "x" << _fps;
-    _params.getParameters()->setParam<std::string>(param_name, crnt_profile_str.str(), [this](const rclcpp::Parameter& parameter)
-            {
-                std::regex self_regex("\\s*([0-9]+)\\s*[xX,]\\s*([0-9]+)\\s*[xX,]\\s*([0-9]+)\\s*", std::regex_constants::ECMAScript);
-                std::smatch match;
-                std::string profile_str(parameter.get_value<std::string>());
-                bool found = std::regex_match(profile_str, match, self_regex);
-                bool request_default(false);
-                if (found)
+    for (auto stream_type : available_streams)
+    {
+        // Register ROS parameter:
+        std::stringstream param_name_str;
+        param_name_str << _module_name << "." << create_graph_resource_name(ros_stream_to_string(stream_type)) << "_profile";
+        rcl_interfaces::msg::ParameterDescriptor crnt_descriptor;
+        crnt_descriptor.description = "Available options are:\n" + get_profiles_descriptions(stream_type);
+        std::stringstream crnt_profile_str;
+        crnt_profile_str << _width[stream_type] << "x" << _height[stream_type] << "x" << _fps[stream_type];
+        _params.getParameters()->setParam<std::string>(param_name_str.str(), crnt_profile_str.str(), [this, stream_type](const rclcpp::Parameter& parameter)
                 {
-                    int temp_width(std::stoi(match[1])), temp_height(std::stoi(match[2])), temp_fps(std::stoi(match[3]));
-                    if (temp_width <= 0 || temp_height <= 0 || temp_fps <= 0)
-                    {
-                        found = false;
-                        request_default = true;
-                    }
-                    else
+                    std::regex self_regex("\\s*([0-9]+)\\s*[xX,]\\s*([0-9]+)\\s*[xX,]\\s*([0-9]+)\\s*", std::regex_constants::ECMAScript);
+                    std::smatch match;
+                    std::string profile_str(parameter.get_value<std::string>());
+                    bool found = std::regex_match(profile_str, match, self_regex);
+                    bool request_default(false);
+                    if (found)
                     {
-                        for (const auto& profile : _all_profiles)
+                        int temp_width(std::stoi(match[1])), temp_height(std::stoi(match[2])), temp_fps(std::stoi(match[3]));
+                        if (temp_width <= 0 || temp_height <= 0 || temp_fps <= 0)
                         {
                             found = false;
-                            if (isSameProfileValues(profile, temp_width, temp_height, temp_fps, RS2_FORMAT_ANY))
+                            request_default = true;
+                        }
+                        else
+                        {
+                            for (const auto& profile : _all_profiles)
                             {
-                                _width = temp_width;
-                                _height = temp_height;
-                                _fps = temp_fps;
-                                found = true;
-                                ROS_WARN_STREAM("re-enable the stream for the change to take effect.");
-                                break;
+                                found = false;
+                                if (isSameProfileValues(profile, temp_width, temp_height, temp_fps, RS2_FORMAT_ANY))
+                                {
+                                    _width[stream_type] = temp_width;
+                                    _height[stream_type] = temp_height;
+                                    _fps[stream_type] = temp_fps;
+                                    found = true;
+                                    ROS_WARN_STREAM("re-enable the stream for the change to take effect.");
+                                    break;
+                                }
                             }
                         }
                     }
-                }
-                if (!found)
-                {
-                    std::stringstream crnt_profile_str;
-                    crnt_profile_str << _width << "x" << _height << "x" << _fps;
-                    if (request_default)
-                    {
-                        ROS_INFO_STREAM("Set ROS param " << parameter.get_name() << " to default: " << crnt_profile_str.str());
-                    }
-                    else
+                    if (!found)
                     {
-                        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());
+                        std::stringstream crnt_profile_str;
+                        crnt_profile_str << _width[stream_type] << "x" << _height[stream_type] << "x" << _fps[stream_type];
+                        if (request_default)
+                        {
+                            ROS_INFO_STREAM("Set ROS param " << parameter.get_name() << " to default: " << crnt_profile_str.str());
+                        }
+                        else
+                        {
+                            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());
                     }
-                    _params.getParameters()->queueSetRosValue(parameter.get_name(), crnt_profile_str.str());
-                }
-            }, crnt_descriptor);
-    _parameters_names.push_back(param_name);
+                }, crnt_descriptor);
+        _parameters_names.push_back(param_name_str.str());
+    }
 
     for (auto sip : sips)
     {