From 0ca251087a5a5879e3bda86de6871eb8b5f5404c Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Wed, 20 Mar 2024 11:23:38 +0530 Subject: [PATCH] Support for selecting profile for each stream_type --- README.md | 8 +- realsense2_camera/include/profile_manager.h | 12 +- realsense2_camera/launch/rs_launch.py | 5 +- realsense2_camera/src/profile_manager.cpp | 208 +++++++++++++------- realsense2_camera/src/ros_sensor.cpp | 28 ++- 5 files changed, 175 insertions(+), 86 deletions(-) diff --git a/README.md b/README.md index bf23475d2b..86d4f0fa67 100644 --- a/README.md +++ b/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
@@ -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 **_profile** parameter. - The profile parameter is a string of the following format: \X\X\ (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 ``` to get the list of supported profiles. - Note: Should re-enable the stream for the change to take effect. diff --git a/realsense2_camera/include/profile_manager.h b/realsense2_camera/include/profile_manager.h index 3021e95f6c..a63cc7f3b0 100644 --- a/realsense2_camera/include/profile_manager.h +++ b/realsense2_camera/include/profile_manager.h @@ -68,22 +68,22 @@ namespace realsense2_camera VideoProfilesManager(std::shared_ptr 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 all_profiles, std::function 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 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 _formats; - int _fps; - int _width, _height; + std::map _fps, _width, _height; bool _is_profile_exist; bool _force_image_default_qos; }; diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py index 4e7aee856c..0fa7938ae1 100644 --- a/realsense2_camera/launch/rs_launch.py +++ b/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'}, diff --git a/realsense2_camera/src/profile_manager.cpp b/realsense2_camera/src/profile_manager.cpp index 95dadbbad6..be1598a5f8 100644 --- a/realsense2_camera/src/profile_manager.cpp +++ b/realsense2_camera/src/profile_manager.cpp @@ -139,6 +139,40 @@ std::map 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(); + + 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& wanted_profiles) { std::map 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 all_profiles, std::function update_sensor_func) @@ -270,15 +304,18 @@ void VideoProfilesManager::registerProfileParameters(std::vector } } -std::string VideoProfilesManager::get_profiles_descriptions() +std::string VideoProfilesManager::get_profiles_descriptions(rs2_stream stream_type) { std::set profiles_str; for (auto& profile : _all_profiles) { - auto video_profile = profile.as(); - 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(); + 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 sips) { - // Set default values: - std::map sip_default_profiles = getDefaultProfiles(); - - rs2::stream_profile default_profile = sip_default_profiles.begin()->second; + std::set 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(); + // Set default values: + std::map 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(); + _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 sip = sip_default_profile.first; - default_profile = sip_default_profile.second; - video_profile = default_profile.as(); + auto default_profile = sip_default_profile.second; + auto video_profile = default_profile.as(); - 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(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()); - 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(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()); + 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() << " is invalid. " - << "Run 'ros2 param describe " << 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() << " is invalid. " + << "Run 'ros2 param describe " << 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) { diff --git a/realsense2_camera/src/ros_sensor.cpp b/realsense2_camera/src/ros_sensor.cpp index 35a4ef8ca3..b6598079f0 100644 --- a/realsense2_camera/src/ros_sensor.cpp +++ b/realsense2_camera/src/ros_sensor.cpp @@ -405,8 +405,18 @@ void RosSensor::set_sensor_auto_exposure_roi() { try { - int width = std::dynamic_pointer_cast(_profile_managers[0])->getWidth(); - int height = std::dynamic_pointer_cast(_profile_managers[0])->getHeight(); + rs2_stream stream_type; + if (this->rs2::sensor::is()) + { + stream_type = RS2_STREAM_DEPTH; + } + else if (this->rs2::sensor::is()) + { + stream_type = RS2_STREAM_COLOR; + } + + int width = std::dynamic_pointer_cast(_profile_managers[0])->getWidth(stream_type); + int height = std::dynamic_pointer_cast(_profile_managers[0])->getHeight(stream_type); bool update_roi_range(false); if (_auto_exposure_roi.max_x > width) @@ -437,8 +447,18 @@ void RosSensor::registerAutoExposureROIOptions() if (this->rs2::sensor::is()) { - int width = std::dynamic_pointer_cast(_profile_managers[0])->getWidth(); - int height = std::dynamic_pointer_cast(_profile_managers[0])->getHeight(); + rs2_stream stream_type; + if (this->rs2::sensor::is()) + { + stream_type = RS2_STREAM_DEPTH; + } + else if (this->rs2::sensor::is()) + { + stream_type = RS2_STREAM_COLOR; + } + + int width = std::dynamic_pointer_cast(_profile_managers[0])->getWidth(stream_type); + int height = std::dynamic_pointer_cast(_profile_managers[0])->getHeight(stream_type); int max_x(width-1); int max_y(height-1);