From 82d77305d93b4a23d2b386e541573a01f32437b7 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Sat, 8 Jul 2023 21:07:56 +0530 Subject: [PATCH 1/8] Exposing stream_format params to user --- realsense2_camera/include/profile_manager.h | 5 ++- realsense2_camera/launch/rs_launch.py | 5 +++ realsense2_camera/src/profile_manager.cpp | 46 +++++++++++++++++---- 3 files changed, 47 insertions(+), 9 deletions(-) diff --git a/realsense2_camera/include/profile_manager.h b/realsense2_camera/include/profile_manager.h index d171b8e4c7..f7b1b6ba20 100644 --- a/realsense2_camera/include/profile_manager.h +++ b/realsense2_camera/include/profile_manager.h @@ -68,18 +68,19 @@ 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; + void registerVideoSensorProfileFormat(stream_index_pair sip); int getHeight() {return _height;}; int getWidth() {return _width;}; int getFPS() {return _fps;}; private: bool isSameProfileValues(const rs2::stream_profile& profile, const int width, const int height, const int fps); - void registerVideoSensorParams(); + void registerVideoSensorParams(std::set sips); std::string get_profiles_descriptions(); private: std::string _module_name; - std::map _allowed_formats; + std::map _allowed_formats; int _fps; int _width, _height; bool _is_profile_exist; diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py index 89f6be7a85..ab20013f60 100644 --- 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.profile.depth_stream_format', 'default': 'Z16', 'description': 'depth stream format'}, + {'name': 'depth_module.profile.infra_stream_format', 'default': 'RGB8', 'description': 'infra0 stream format'}, + {'name': 'depth_module.profile.infra1_stream_format', 'default': 'Y8', 'description': 'infra1 stream format'}, + {'name': 'depth_module.profile.infra2_stream_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.profile.color_stream_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 index befe814ae4..f391004c57 100644 --- a/realsense2_camera/src/profile_manager.cpp +++ b/realsense2_camera/src/profile_manager.cpp @@ -204,10 +204,7 @@ VideoProfilesManager::VideoProfilesManager(std::shared_ptr 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) @@ -221,7 +218,8 @@ bool VideoProfilesManager::isSameProfileValues(const rs2::stream_profile& profil 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] )); + (_allowed_formats[sip] == rs2_format_to_string(RS2_FORMAT_ANY) || + rs2_format_to_string(video_profile.format()) == _allowed_formats[sip])); } bool VideoProfilesManager::isWantedProfile(const rs2::stream_profile& profile) @@ -251,7 +249,7 @@ void VideoProfilesManager::registerProfileParameters(std::vector ROS_DEBUG_STREAM(__LINE__ << ": _enabled_profiles[" << ros_stream_to_string(sip.first) << ":" << sip.second << "]: " << *(_enabled_profiles[sip])); } - registerVideoSensorParams(); + registerVideoSensorParams(checked_sips); } } @@ -274,8 +272,28 @@ std::string VideoProfilesManager::get_profiles_descriptions() descriptors.pop_back(); return descriptors; } +void VideoProfilesManager::registerVideoSensorProfileFormat(stream_index_pair sip) +{ + if (sip == DEPTH) + _allowed_formats[DEPTH] = rs2_format_to_string(RS2_FORMAT_Z16); + + else if (sip == INFRA0) + _allowed_formats[INFRA0] = rs2_format_to_string(RS2_FORMAT_RGB8); + + else if (sip == INFRA1) + _allowed_formats[INFRA1] = rs2_format_to_string(RS2_FORMAT_Y8); -void VideoProfilesManager::registerVideoSensorParams() + else if (sip == INFRA2) + _allowed_formats[INFRA2] = rs2_format_to_string(RS2_FORMAT_Y8); + + else if (sip == COLOR) + _allowed_formats[COLOR] = rs2_format_to_string(RS2_FORMAT_RGB8); + + else + _allowed_formats[{sip.first, sip.second}] = rs2_format_to_string(RS2_FORMAT_ANY); +} + +void VideoProfilesManager::registerVideoSensorParams(std::set sips) { // Set default values: rs2::stream_profile default_profile = getDefaultProfile(); @@ -339,6 +357,20 @@ void VideoProfilesManager::registerVideoSensorParams() } }, crnt_descriptor); _parameters_names.push_back(param_name); + + for (auto sip : sips) + { + std::string param_name(_module_name + ".profile." + STREAM_NAME(sip) + "_stream_format"); + registerVideoSensorProfileFormat(sip); + _params.getParameters()->setParam(param_name, _allowed_formats[sip], + [this, sip](const rclcpp::Parameter& parameter) + { + std::string format_str(parameter.get_value()); + _allowed_formats[sip] = format_str; + ROS_WARN_STREAM("re-enable the stream for the change to take effect."); + }); + _parameters_names.push_back(param_name); + } } /////////////////////////////////////////////////////////////////////////////////////// From 068f0c610ce6371d582d5a65f3c0d41d010bcad9 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Tue, 11 Jul 2023 01:49:43 +0530 Subject: [PATCH 2/8] Validate the user's input stream_format --- realsense2_camera/include/profile_manager.h | 2 +- realsense2_camera/include/ros_utils.h | 1 + realsense2_camera/src/profile_manager.cpp | 41 ++++++++++++--------- realsense2_camera/src/ros_utils.cpp | 15 ++++++++ 4 files changed, 40 insertions(+), 19 deletions(-) diff --git a/realsense2_camera/include/profile_manager.h b/realsense2_camera/include/profile_manager.h index f7b1b6ba20..02aa6820f3 100644 --- a/realsense2_camera/include/profile_manager.h +++ b/realsense2_camera/include/profile_manager.h @@ -80,7 +80,7 @@ namespace realsense2_camera private: std::string _module_name; - std::map _allowed_formats; + std::map _allowed_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 index 4fa45899e7..9b17157676 100644 --- 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(); + bool string_to_rs2_format( std::string str , rs2_format* format); } diff --git a/realsense2_camera/src/profile_manager.cpp b/realsense2_camera/src/profile_manager.cpp index f391004c57..70633a7dbf 100644 --- a/realsense2_camera/src/profile_manager.cpp +++ b/realsense2_camera/src/profile_manager.cpp @@ -204,7 +204,7 @@ VideoProfilesManager::VideoProfilesManager(std::shared_ptr parameter _module_name(module_name), _force_image_default_qos(force_image_default_qos) { - + } bool VideoProfilesManager::isSameProfileValues(const rs2::stream_profile& profile, const int width, const int height, const int fps) @@ -218,8 +218,8 @@ bool VideoProfilesManager::isSameProfileValues(const rs2::stream_profile& profil return ((video_profile.width() == width) && (video_profile.height() == height) && (video_profile.fps() == fps) && - (_allowed_formats[sip] == rs2_format_to_string(RS2_FORMAT_ANY) || - rs2_format_to_string(video_profile.format()) == _allowed_formats[sip])); + (_allowed_formats[sip] == RS2_FORMAT_ANY || + _allowed_formats[sip] == video_profile.format())); } bool VideoProfilesManager::isWantedProfile(const rs2::stream_profile& profile) @@ -275,22 +275,17 @@ std::string VideoProfilesManager::get_profiles_descriptions() void VideoProfilesManager::registerVideoSensorProfileFormat(stream_index_pair sip) { if (sip == DEPTH) - _allowed_formats[DEPTH] = rs2_format_to_string(RS2_FORMAT_Z16); - + _allowed_formats[DEPTH] = RS2_FORMAT_Z16; else if (sip == INFRA0) - _allowed_formats[INFRA0] = rs2_format_to_string(RS2_FORMAT_RGB8); - + _allowed_formats[INFRA0] = RS2_FORMAT_RGB8; else if (sip == INFRA1) - _allowed_formats[INFRA1] = rs2_format_to_string(RS2_FORMAT_Y8); - + _allowed_formats[INFRA1] = RS2_FORMAT_Y8; else if (sip == INFRA2) - _allowed_formats[INFRA2] = rs2_format_to_string(RS2_FORMAT_Y8); - + _allowed_formats[INFRA2] = RS2_FORMAT_Y8; else if (sip == COLOR) - _allowed_formats[COLOR] = rs2_format_to_string(RS2_FORMAT_RGB8); - + _allowed_formats[COLOR] = RS2_FORMAT_RGB8; else - _allowed_formats[{sip.first, sip.second}] = rs2_format_to_string(RS2_FORMAT_ANY); + _allowed_formats[{sip.first, sip.second}] = RS2_FORMAT_ANY; } void VideoProfilesManager::registerVideoSensorParams(std::set sips) @@ -362,12 +357,22 @@ void VideoProfilesManager::registerVideoSensorParams(std::set { std::string param_name(_module_name + ".profile." + STREAM_NAME(sip) + "_stream_format"); registerVideoSensorProfileFormat(sip); - _params.getParameters()->setParam(param_name, _allowed_formats[sip], - [this, sip](const rclcpp::Parameter& parameter) + std::string param_value = rs2_format_to_string(_allowed_formats[sip]); + _params.getParameters()->setParam(param_name, param_value, + [this, sip, param_value](const rclcpp::Parameter& parameter) { std::string format_str(parameter.get_value()); - _allowed_formats[sip] = format_str; - ROS_WARN_STREAM("re-enable the stream for the change to take effect."); + rs2_format format = RS2_FORMAT_ANY; + if (string_to_rs2_format(format_str , &format)) + { + _allowed_formats[sip] = format; + ROS_WARN_STREAM("re-enable the stream for the change to take effect."); + } + else + { + ROS_WARN_STREAM("Given " << sip.first <<" stream format is invalid. Setting to default format: " << param_value); + _params.getParameters()->queueSetRosValue(parameter.get_name(), param_value); + } }); _parameters_names.push_back(param_name); } diff --git a/realsense2_camera/src/ros_utils.cpp b/realsense2_camera/src/ros_utils.cpp index b543be2af0..397fe148d1 100644 --- a/realsense2_camera/src/ros_utils.cpp +++ b/realsense2_camera/src/ros_utils.cpp @@ -80,6 +80,21 @@ std::string create_graph_resource_name(const std::string &original_name) return fixed_name; } +bool string_to_rs2_format( std::string str , rs2_format* format) +{ + bool is_found = false; + for (int i = 0; i < RS2_FORMAT_COUNT; i++) + { + if (str.compare(rs2_format_to_string((rs2_format)i)) == 0) + { + *format = (rs2_format)i; + is_found = true; + break; + } + } + return is_found; +} + static const rmw_qos_profile_t rmw_qos_profile_latched = { RMW_QOS_POLICY_HISTORY_KEEP_LAST, From da410fbf0b9fe22811f4e19c856bfed8d8047b95 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Wed, 12 Jul 2023 20:24:38 +0530 Subject: [PATCH 3/8] Warn user if the selected profile cannot be opened --- realsense2_camera/include/profile_manager.h | 4 +- realsense2_camera/src/profile_manager.cpp | 80 +++++++++++++-------- 2 files changed, 53 insertions(+), 31 deletions(-) diff --git a/realsense2_camera/include/profile_manager.h b/realsense2_camera/include/profile_manager.h index 02aa6820f3..388a6a4099 100644 --- a/realsense2_camera/include/profile_manager.h +++ b/realsense2_camera/include/profile_manager.h @@ -74,13 +74,13 @@ namespace realsense2_camera int getFPS() {return _fps;}; private: - bool isSameProfileValues(const rs2::stream_profile& profile, const int width, const int height, const int fps); + bool isSameProfileValues(const rs2::stream_profile& profile, const int width, const int height, const int fps, const rs2_format format); void registerVideoSensorParams(std::set sips); std::string get_profiles_descriptions(); private: std::string _module_name; - std::map _allowed_formats; + std::map _formats; int _fps; int _width, _height; bool _is_profile_exist; diff --git a/realsense2_camera/src/profile_manager.cpp b/realsense2_camera/src/profile_manager.cpp index 70633a7dbf..4747b6f3cf 100644 --- a/realsense2_camera/src/profile_manager.cpp +++ b/realsense2_camera/src/profile_manager.cpp @@ -160,6 +160,16 @@ void ProfilesManager::addWantedProfiles(std::vector& wanted ROS_DEBUG_STREAM("Found profile for " << ros_stream_to_string(sip.first) << ":" << sip.second); } } + for (auto const & x : _enabled_profiles) + { + auto sip = x.first; + auto stream_enabled = x.second; + if (*stream_enabled && !found_sips[sip]) + { + ROS_WARN_STREAM("Couldn't find profile for " << ros_stream_to_string(sip.first) << ":" << sip.second << " stream." + << " Update the profile settings and re-enable the stream for the change to take effect."); + } + } } std::string ProfilesManager::profile_string(const rs2::stream_profile& profile) @@ -207,24 +217,24 @@ VideoProfilesManager::VideoProfilesManager(std::shared_ptr parameter } -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()) return false; auto video_profile = profile.as(); 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[sip] == RS2_FORMAT_ANY || - _allowed_formats[sip] == video_profile.format())); + (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 all_profiles, std::function update_sensor_func) @@ -275,17 +285,17 @@ std::string VideoProfilesManager::get_profiles_descriptions() void VideoProfilesManager::registerVideoSensorProfileFormat(stream_index_pair sip) { if (sip == DEPTH) - _allowed_formats[DEPTH] = RS2_FORMAT_Z16; + _formats[DEPTH] = RS2_FORMAT_Z16; else if (sip == INFRA0) - _allowed_formats[INFRA0] = RS2_FORMAT_RGB8; + _formats[INFRA0] = RS2_FORMAT_RGB8; else if (sip == INFRA1) - _allowed_formats[INFRA1] = RS2_FORMAT_Y8; + _formats[INFRA1] = RS2_FORMAT_Y8; else if (sip == INFRA2) - _allowed_formats[INFRA2] = RS2_FORMAT_Y8; + _formats[INFRA2] = RS2_FORMAT_Y8; else if (sip == COLOR) - _allowed_formats[COLOR] = RS2_FORMAT_RGB8; + _formats[COLOR] = RS2_FORMAT_RGB8; else - _allowed_formats[{sip.first, sip.second}] = RS2_FORMAT_ANY; + _formats[{sip.first, sip.second}] = RS2_FORMAT_ANY; } void VideoProfilesManager::registerVideoSensorParams(std::set sips) @@ -324,7 +334,7 @@ void VideoProfilesManager::registerVideoSensorParams(std::set 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; @@ -357,23 +367,35 @@ void VideoProfilesManager::registerVideoSensorParams(std::set { std::string param_name(_module_name + ".profile." + STREAM_NAME(sip) + "_stream_format"); registerVideoSensorProfileFormat(sip); - std::string param_value = rs2_format_to_string(_allowed_formats[sip]); - _params.getParameters()->setParam(param_name, param_value, - [this, sip, param_value](const rclcpp::Parameter& parameter) - { - std::string format_str(parameter.get_value()); - rs2_format format = RS2_FORMAT_ANY; - if (string_to_rs2_format(format_str , &format)) - { - _allowed_formats[sip] = format; - ROS_WARN_STREAM("re-enable the stream for the change to take effect."); - } - else - { - ROS_WARN_STREAM("Given " << sip.first <<" stream format is invalid. Setting to default format: " << param_value); - _params.getParameters()->queueSetRosValue(parameter.get_name(), param_value); - } - }); + std::string param_value = rs2_format_to_string(_formats[sip]); + _params.getParameters()->setParam(param_name, param_value, [this, sip](const rclcpp::Parameter& parameter) + { + std::string format_str(parameter.get_value()); + rs2_format temp_format = RS2_FORMAT_ANY; + bool found = false; + if (string_to_rs2_format(format_str , &temp_format)) + { + 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 " << sip.first << " stream for the change to take effect."); + found = true; + _formats[sip] = temp_format; + break; + } + } + } + if (!found) + { + ROS_WARN_STREAM(STREAM_NAME(sip) <<" stream doesn't support " << format_str <<" format. " + << "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])); + } + }); _parameters_names.push_back(param_name); } } From 0325781929b16c6e0286361b83990b0e8f18af29 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Wed, 19 Jul 2023 20:38:32 +0530 Subject: [PATCH 4/8] Added stream_format param descriptors --- realsense2_camera/include/profile_manager.h | 1 + .../launch/rs_multi_camera_launch.py | 16 +++--- realsense2_camera/src/profile_manager.cpp | 52 ++++++++++++++++--- realsense2_camera/src/ros_utils.cpp | 9 ++-- 4 files changed, 59 insertions(+), 19 deletions(-) diff --git a/realsense2_camera/include/profile_manager.h b/realsense2_camera/include/profile_manager.h index 388a6a4099..046448c178 100644 --- a/realsense2_camera/include/profile_manager.h +++ b/realsense2_camera/include/profile_manager.h @@ -77,6 +77,7 @@ namespace realsense2_camera bool isSameProfileValues(const rs2::stream_profile& profile, const int width, const int height, const int fps, const rs2_format format); void registerVideoSensorParams(std::set sips); std::string get_profiles_descriptions(); + std::string get_profile_formats_descriptions(stream_index_pair sip); private: std::string _module_name; diff --git a/realsense2_camera/launch/rs_multi_camera_launch.py b/realsense2_camera/launch/rs_multi_camera_launch.py index 2856067ced..e2d0882ab7 100644 --- a/realsense2_camera/launch/rs_multi_camera_launch.py +++ b/realsense2_camera/launch/rs_multi_camera_launch.py @@ -17,9 +17,9 @@ # Use this launch file to launch 2 devices. # The Parameters available for definition in the command line for each camera are described in rs_launch.configurable_parameters # For each device, the parameter name was changed to include an index. -# For example: to set camera_name for device1 set parameter camera_name1. +# For example: to set camera_name for device1 set parameter camera_name_1. # command line example: -# ros2 launch realsense2_camera rs_multi_camera_launch.py camera_name1:=D400 device_type2:=l5. device_type1:=d4.. +# ros2 launch realsense2_camera rs_multi_camera_launch.py camera_name_1:=D400 device_type_2:=l5. device_type_1:=d4.. """Launch realsense2_camera node.""" import copy @@ -33,8 +33,8 @@ sys.path.append(str(pathlib.Path(__file__).parent.absolute())) import rs_launch -local_parameters = [{'name': 'camera_name1', 'default': 'camera1', 'description': 'camera unique name'}, - {'name': 'camera_name2', 'default': 'camera2', 'description': 'camera unique name'}, +local_parameters = [{'name': 'camera_name_1', 'default': 'camera1', 'description': 'camera unique name'}, + {'name': 'camera_name_2', 'default': 'camera2', 'description': 'camera unique name'}, ] def set_configurable_parameters(local_params): @@ -53,14 +53,14 @@ def add_node_action(context : LaunchContext): package = "tf2_ros", executable = "static_transform_publisher", arguments = ["0", "0", "0", "0", "0", "0", - context.launch_configurations['camera_name1'] + "_link", - context.launch_configurations['camera_name2'] + "_link"] + context.launch_configurations['camera_name_1'] + "_link", + context.launch_configurations['camera_name_2'] + "_link"] ) return [node] def generate_launch_description(): - params1 = duplicate_params(rs_launch.configurable_parameters, '1') - params2 = duplicate_params(rs_launch.configurable_parameters, '2') + params1 = duplicate_params(rs_launch.configurable_parameters, '_1') + params2 = duplicate_params(rs_launch.configurable_parameters, '_2') return LaunchDescription( rs_launch.declare_configurable_parameters(local_parameters) + rs_launch.declare_configurable_parameters(params1) + diff --git a/realsense2_camera/src/profile_manager.cpp b/realsense2_camera/src/profile_manager.cpp index 4747b6f3cf..27492a7ff2 100644 --- a/realsense2_camera/src/profile_manager.cpp +++ b/realsense2_camera/src/profile_manager.cpp @@ -160,14 +160,19 @@ void ProfilesManager::addWantedProfiles(std::vector& 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 & x : _enabled_profiles) { auto sip = x.first; auto stream_enabled = x.second; + if (*stream_enabled && !found_sips[sip]) { - ROS_WARN_STREAM("Couldn't find profile for " << ros_stream_to_string(sip.first) << ":" << sip.second << " stream." - << " Update the profile settings and re-enable the stream for the change to take effect."); + ROS_WARN_STREAM("Couldn't open " << ros_stream_to_string(sip.first) << ":" << sip.second << " stream " + << "due to wrong profile selection. " + << "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 sensors."); } } } @@ -282,6 +287,32 @@ std::string VideoProfilesManager::get_profiles_descriptions() descriptors.pop_back(); return descriptors; } + +std::string VideoProfilesManager::get_profile_formats_descriptions(stream_index_pair sip) +{ + std::set profile_formats_str; + for (auto& profile : _all_profiles) + { + auto video_profile = profile.as(); + 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) @@ -295,7 +326,7 @@ void VideoProfilesManager::registerVideoSensorProfileFormat(stream_index_pair si else if (sip == COLOR) _formats[COLOR] = RS2_FORMAT_RGB8; else - _formats[{sip.first, sip.second}] = RS2_FORMAT_ANY; + _formats[sip] = RS2_FORMAT_ANY; } void VideoProfilesManager::registerVideoSensorParams(std::set sips) @@ -356,7 +387,10 @@ void VideoProfilesManager::registerVideoSensorParams(std::set } else { - ROS_ERROR_STREAM("Given value, " << parameter.get_value() << " is invalid. Set ROS param back to: " << crnt_profile_str.str()); + 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()); } @@ -368,6 +402,8 @@ void VideoProfilesManager::registerVideoSensorParams(std::set std::string param_name(_module_name + ".profile." + STREAM_NAME(sip) + "_stream_format"); registerVideoSensorProfileFormat(sip); std::string param_value = rs2_format_to_string(_formats[sip]); + rcl_interfaces::msg::ParameterDescriptor crnt_descriptor; + crnt_descriptor.description = "Available options are:\n" + get_profile_formats_descriptions(sip); _params.getParameters()->setParam(param_name, param_value, [this, sip](const rclcpp::Parameter& parameter) { std::string format_str(parameter.get_value()); @@ -381,7 +417,7 @@ void VideoProfilesManager::registerVideoSensorParams(std::set if (sip == profile_sip && temp_format == profile.format()) { - ROS_WARN_STREAM("re-enable the " << sip.first << " stream for the change to take effect."); + ROS_WARN_STREAM("re-enable the " << STREAM_NAME(sip) << " stream for the change to take effect."); found = true; _formats[sip] = temp_format; break; @@ -390,12 +426,14 @@ void VideoProfilesManager::registerVideoSensorParams(std::set } if (!found) { - ROS_WARN_STREAM(STREAM_NAME(sip) <<" stream doesn't support " << format_str <<" format. " + ROS_ERROR_STREAM(STREAM_NAME(sip) <<" stream doesn't support " << format_str <<" format. " + << "Run 'ros2 param describe " << 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); } } diff --git a/realsense2_camera/src/ros_utils.cpp b/realsense2_camera/src/ros_utils.cpp index 397fe148d1..bb2525c143 100644 --- a/realsense2_camera/src/ros_utils.cpp +++ b/realsense2_camera/src/ros_utils.cpp @@ -80,19 +80,20 @@ std::string create_graph_resource_name(const std::string &original_name) return fixed_name; } -bool string_to_rs2_format( std::string str , rs2_format* format) +bool string_to_rs2_format(std::string str , rs2_format* format) { - bool is_found = false; + bool converted = false; for (int i = 0; i < RS2_FORMAT_COUNT; i++) { + transform(str.begin(), str.end(), str.begin(), ::toupper); if (str.compare(rs2_format_to_string((rs2_format)i)) == 0) { *format = (rs2_format)i; - is_found = true; + converted = true; break; } } - return is_found; + return converted; } static const rmw_qos_profile_t rmw_qos_profile_latched = From 1c6199dace5ef9d962fae93f678389b69bec1363 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Thu, 27 Jul 2023 21:57:58 +0530 Subject: [PATCH 5/8] Query default stream_format from LRS --- README.md | 35 ++++-- realsense2_camera/include/profile_manager.h | 5 +- .../launch/rs_multi_camera_launch.py | 16 +-- realsense2_camera/src/profile_manager.cpp | 108 ++++++++++++------ 4 files changed, 105 insertions(+), 59 deletions(-) diff --git a/README.md b/README.md index d7b247bf87..352927ae1d 100644 --- a/README.md +++ b/README.md @@ -185,16 +185,6 @@

Parameters

- -### 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.
- - The profile parameter is a string of the following format: \X\X\ (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. - -
### Available Parameters: - For the entire list of parameters type `ros2 param list`. @@ -205,16 +195,37 @@ #### 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: \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, 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 ``` to get the list of supported profiles. + - To automatically choose a default profile, set the param to '0x0x0'. + - Note: Should re-enable the stream for the change to take effect. + - ****_stream_format** + - This parameter is a string used to select the stream format. + - can be any of *infra, infra1, infra2, color, depth, fisheye, fisheye1, fisheye2*. + - For example: ```depth_module.profile.depth_stream_format:=Z16 depth_module.profile.infra1_stream_format:=y8 rgb_camera.profile.color_stream_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 ``` to get the list of supported formats. + - To automatically choose a suitable format, set the param to 'ANY'. + - Note: Should re-enable the stream for the change to take effect. + - If the stream doesn't support the user selected profile \X\X\ + \, it will not be opened and awarning message will be shown. + - Should update the profile setting 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_****: - Choose whether to enable a specified stream or not. Default is true for images and false for orientation streams. - - can be any of *infra1, infra2, color, depth, fisheye, fisheye1, fisheye2, gyro, accel, pose*. + - 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. - ****_qos**: - Sets the QoS by which the topic is published. - - can be any of *infra, color, fisheye, depth, gyro, accel, pose*. + - 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 index 046448c178..365f64df8f 100644 --- 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 getDefaultProfiles(); protected: rclcpp::Logger _logger; @@ -68,7 +68,6 @@ 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; - void registerVideoSensorProfileFormat(stream_index_pair sip); int getHeight() {return _height;}; int getWidth() {return _width;}; int getFPS() {return _fps;}; @@ -77,7 +76,7 @@ namespace realsense2_camera bool isSameProfileValues(const rs2::stream_profile& profile, const int width, const int height, const int fps, const rs2_format format); void registerVideoSensorParams(std::set sips); std::string get_profiles_descriptions(); - std::string get_profile_formats_descriptions(stream_index_pair sip); + std::string getProfileFormatsDescriptions(stream_index_pair sip); private: std::string _module_name; diff --git a/realsense2_camera/launch/rs_multi_camera_launch.py b/realsense2_camera/launch/rs_multi_camera_launch.py index e2d0882ab7..2856067ced 100644 --- a/realsense2_camera/launch/rs_multi_camera_launch.py +++ b/realsense2_camera/launch/rs_multi_camera_launch.py @@ -17,9 +17,9 @@ # Use this launch file to launch 2 devices. # The Parameters available for definition in the command line for each camera are described in rs_launch.configurable_parameters # For each device, the parameter name was changed to include an index. -# For example: to set camera_name for device1 set parameter camera_name_1. +# For example: to set camera_name for device1 set parameter camera_name1. # command line example: -# ros2 launch realsense2_camera rs_multi_camera_launch.py camera_name_1:=D400 device_type_2:=l5. device_type_1:=d4.. +# ros2 launch realsense2_camera rs_multi_camera_launch.py camera_name1:=D400 device_type2:=l5. device_type1:=d4.. """Launch realsense2_camera node.""" import copy @@ -33,8 +33,8 @@ sys.path.append(str(pathlib.Path(__file__).parent.absolute())) import rs_launch -local_parameters = [{'name': 'camera_name_1', 'default': 'camera1', 'description': 'camera unique name'}, - {'name': 'camera_name_2', 'default': 'camera2', 'description': 'camera unique name'}, +local_parameters = [{'name': 'camera_name1', 'default': 'camera1', 'description': 'camera unique name'}, + {'name': 'camera_name2', 'default': 'camera2', 'description': 'camera unique name'}, ] def set_configurable_parameters(local_params): @@ -53,14 +53,14 @@ def add_node_action(context : LaunchContext): package = "tf2_ros", executable = "static_transform_publisher", arguments = ["0", "0", "0", "0", "0", "0", - context.launch_configurations['camera_name_1'] + "_link", - context.launch_configurations['camera_name_2'] + "_link"] + context.launch_configurations['camera_name1'] + "_link", + context.launch_configurations['camera_name2'] + "_link"] ) return [node] def generate_launch_description(): - params1 = duplicate_params(rs_launch.configurable_parameters, '_1') - params2 = duplicate_params(rs_launch.configurable_parameters, '_2') + params1 = duplicate_params(rs_launch.configurable_parameters, '1') + params2 = duplicate_params(rs_launch.configurable_parameters, '2') return LaunchDescription( rs_launch.declare_configurable_parameters(local_parameters) + rs_launch.declare_configurable_parameters(params1) + diff --git a/realsense2_camera/src/profile_manager.cpp b/realsense2_camera/src/profile_manager.cpp index 27492a7ff2..449ead61fa 100644 --- 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 ProfilesManager::getDefaultProfiles() { - rs2::stream_profile default_profile; + std::map 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& wanted_profiles) @@ -162,10 +163,10 @@ void ProfilesManager::addWantedProfiles(std::vector& wanted } // Warn the user if the enabled stream cannot be opened due to wrong profile selection - for (auto const & x : _enabled_profiles) + for (auto const & profile : _enabled_profiles) { - auto sip = x.first; - auto stream_enabled = x.second; + auto sip = profile.first; + auto stream_enabled = profile.second; if (*stream_enabled && !found_sips[sip]) { @@ -288,7 +289,7 @@ std::string VideoProfilesManager::get_profiles_descriptions() return descriptors; } -std::string VideoProfilesManager::get_profile_formats_descriptions(stream_index_pair sip) +std::string VideoProfilesManager::getProfileFormatsDescriptions(stream_index_pair sip) { std::set profile_formats_str; for (auto& profile : _all_profiles) @@ -313,31 +314,49 @@ std::string VideoProfilesManager::get_profile_formats_descriptions(stream_index_ 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 sips) { // Set default values: - rs2::stream_profile default_profile = getDefaultProfile(); - auto video_profile = default_profile.as(); + std::map 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(); + _width = video_profile.width(); _height = video_profile.height(); _fps = video_profile.fps(); + _formats[{default_profile.stream_type(), default_profile.stream_index()}] = video_profile.format(); + + // Set default _formats for other streams + for (auto sip_default_profile : sip_default_profiles) + { + stream_index_pair sip = sip_default_profile.first; + + if (_formats.find(sip) == _formats.end()) + { + default_profile = sip_default_profile.second; + video_profile = default_profile.as(); + + if (isSameProfileValues(default_profile, _width, _height, _fps, video_profile.format())) + { + _formats[sip] = video_profile.format(); + } + else + { + _formats[sip] = RS2_FORMAT_ANY; + } + } + } // Register ROS parameter: std::string param_name(_module_name + ".profile"); @@ -400,16 +419,29 @@ void VideoProfilesManager::registerVideoSensorParams(std::set for (auto sip : sips) { std::string param_name(_module_name + ".profile." + STREAM_NAME(sip) + "_stream_format"); - registerVideoSensorProfileFormat(sip); + + if (_formats.find(sip) == _formats.end()) + { + _formats[sip] = RS2_FORMAT_ANY; + } + std::string param_value = rs2_format_to_string(_formats[sip]); rcl_interfaces::msg::ParameterDescriptor crnt_descriptor; - crnt_descriptor.description = "Available options are:\n" + get_profile_formats_descriptions(sip); + 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()); rs2_format temp_format = RS2_FORMAT_ANY; bool found = false; - if (string_to_rs2_format(format_str , &temp_format)) + + if (format_str == rs2_format_to_string(RS2_FORMAT_ANY)) + { + ROS_WARN_STREAM("A suitable format for " << STREAM_NAME(sip) << " stream will be chosen automatically. " + << "re-enable the stream for the change to take effect."); + found = true; + _formats[sip] = RS2_FORMAT_ANY; + } + else if (string_to_rs2_format(format_str , &temp_format)) { for (const auto& profile : _all_profiles) { @@ -429,6 +461,7 @@ void VideoProfilesManager::registerVideoSensorParams(std::set ROS_ERROR_STREAM(STREAM_NAME(sip) <<" stream doesn't support " << format_str <<" format. " << "Run 'ros2 param describe " << parameter.get_name() << "' to get the list of supported formats. " + << "To automatically choose a suitable format, set the param to 'ANY'. " << "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])); @@ -493,9 +526,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().fps(); + std::map 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().fps(); + } // Register ROS parameters: for (auto& fps : _fps) From 4af70f8e530a10d40ecba4399bb5fa9931b7f3d1 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Mon, 31 Jul 2023 19:45:12 +0530 Subject: [PATCH 6/8] Updated new stream format params names --- README.md | 6 +-- realsense2_camera/include/ros_utils.h | 2 +- realsense2_camera/launch/rs_launch.py | 10 ++--- realsense2_camera/src/profile_manager.cpp | 47 +++++++++++++---------- realsense2_camera/src/ros_utils.cpp | 10 ++--- 5 files changed, 40 insertions(+), 35 deletions(-) diff --git a/README.md b/README.md index 352927ae1d..e08669085d 100644 --- a/README.md +++ b/README.md @@ -202,16 +202,14 @@ - 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 ``` to get the list of supported profiles. - - To automatically choose a default profile, set the param to '0x0x0'. - Note: Should re-enable the stream for the change to take effect. - - ****_stream_format** + - ****_format** - This parameter is a string used to select the stream format. - can be any of *infra, infra1, infra2, color, depth, fisheye, fisheye1, fisheye2*. - - For example: ```depth_module.profile.depth_stream_format:=Z16 depth_module.profile.infra1_stream_format:=y8 rgb_camera.profile.color_stream_format:=RGB8``` + - 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 ``` to get the list of supported formats. - - To automatically choose a suitable format, set the param to 'ANY'. - Note: Should re-enable the stream for the change to take effect. - If the stream doesn't support the user selected profile \X\X\ + \, it will not be opened and awarning message will be shown. - Should update the profile setting and re-enable the stream for the change to take effect. diff --git a/realsense2_camera/include/ros_utils.h b/realsense2_camera/include/ros_utils.h index 9b17157676..79acf4ae52 100644 --- a/realsense2_camera/include/ros_utils.h +++ b/realsense2_camera/include/ros_utils.h @@ -45,7 +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(); - bool string_to_rs2_format( std::string str , rs2_format* format); + rs2_format 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 index 8ae3b4d715..2f93d4de32 100644 --- a/realsense2_camera/launch/rs_launch.py +++ b/realsense2_camera/launch/rs_launch.py @@ -31,13 +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.profile.depth_stream_format', 'default': 'Z16', 'description': 'depth stream format'}, - {'name': 'depth_module.profile.infra_stream_format', 'default': 'RGB8', 'description': 'infra0 stream format'}, - {'name': 'depth_module.profile.infra1_stream_format', 'default': 'Y8', 'description': 'infra1 stream format'}, - {'name': 'depth_module.profile.infra2_stream_format', 'default': 'Y8', 'description': 'infra2 stream format'}, + {'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.profile.color_stream_format', 'default': 'RGB8', 'description': 'color stream format'}, + {'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 index 449ead61fa..9d885cf2be 100644 --- a/realsense2_camera/src/profile_manager.cpp +++ b/realsense2_camera/src/profile_manager.cpp @@ -337,7 +337,7 @@ void VideoProfilesManager::registerVideoSensorParams(std::set _fps = video_profile.fps(); _formats[{default_profile.stream_type(), default_profile.stream_index()}] = video_profile.format(); - // Set default _formats for other streams + // Set the stream format from the default profiles provided by LibRealsense for (auto sip_default_profile : sip_default_profiles) { stream_index_pair sip = sip_default_profile.first; @@ -351,9 +351,30 @@ void VideoProfilesManager::registerVideoSensorParams(std::set { _formats[sip] = video_profile.format(); } - else + } + } + + // Set the format for the streams which doesn't have default profiles + for (auto sip : sips) + { + if (_formats.find(sip) == _formats.end()) + { + for (const auto& profile : _all_profiles) { - _formats[sip] = RS2_FORMAT_ANY; + bool found = false; + stream_index_pair profile_sip(profile.stream_type(), profile.stream_index()); + + if (sip == profile_sip && + isSameProfileValues(profile, _width, _height, _fps, profile.as().format())) + { + _formats[sip] = profile.format(); + found = true; + break; + } + if (!found) + { + _formats[sip] = RS2_FORMAT_ANY; + } } } } @@ -418,31 +439,18 @@ void VideoProfilesManager::registerVideoSensorParams(std::set for (auto sip : sips) { - std::string param_name(_module_name + ".profile." + STREAM_NAME(sip) + "_stream_format"); - - if (_formats.find(sip) == _formats.end()) - { - _formats[sip] = RS2_FORMAT_ANY; - } - + 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()); - rs2_format temp_format = RS2_FORMAT_ANY; + rs2_format temp_format = rs2_format_string_to_rs2_format(format_str); bool found = false; - if (format_str == rs2_format_to_string(RS2_FORMAT_ANY)) + if (temp_format != RS2_FORMAT_ANY) { - ROS_WARN_STREAM("A suitable format for " << STREAM_NAME(sip) << " stream will be chosen automatically. " - << "re-enable the stream for the change to take effect."); - found = true; - _formats[sip] = RS2_FORMAT_ANY; - } - else if (string_to_rs2_format(format_str , &temp_format)) - { for (const auto& profile : _all_profiles) { stream_index_pair profile_sip = {profile.stream_type(), profile.stream_index()}; @@ -461,7 +469,6 @@ void VideoProfilesManager::registerVideoSensorParams(std::set ROS_ERROR_STREAM(STREAM_NAME(sip) <<" stream doesn't support " << format_str <<" format. " << "Run 'ros2 param describe " << parameter.get_name() << "' to get the list of supported formats. " - << "To automatically choose a suitable format, set the param to 'ANY'. " << "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])); diff --git a/realsense2_camera/src/ros_utils.cpp b/realsense2_camera/src/ros_utils.cpp index bb2525c143..0623a645f4 100644 --- a/realsense2_camera/src/ros_utils.cpp +++ b/realsense2_camera/src/ros_utils.cpp @@ -80,20 +80,20 @@ std::string create_graph_resource_name(const std::string &original_name) return fixed_name; } -bool string_to_rs2_format(std::string str , rs2_format* format) +rs2_format rs2_format_string_to_rs2_format(std::string str) { - bool converted = false; + rs2_format format = RS2_FORMAT_ANY; + for (int i = 0; i < RS2_FORMAT_COUNT; i++) { transform(str.begin(), str.end(), str.begin(), ::toupper); if (str.compare(rs2_format_to_string((rs2_format)i)) == 0) { - *format = (rs2_format)i; - converted = true; + format = (rs2_format)i; break; } } - return converted; + return format; } static const rmw_qos_profile_t rmw_qos_profile_latched = From 189918a281edc6c3a99374586c5d699fa29dc89d Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Mon, 31 Jul 2023 19:57:46 +0530 Subject: [PATCH 7/8] Updated function name --- realsense2_camera/include/ros_utils.h | 2 +- realsense2_camera/src/profile_manager.cpp | 2 +- realsense2_camera/src/ros_utils.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/realsense2_camera/include/ros_utils.h b/realsense2_camera/include/ros_utils.h index 79acf4ae52..4df1def396 100644 --- a/realsense2_camera/include/ros_utils.h +++ b/realsense2_camera/include/ros_utils.h @@ -45,7 +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 rs2_format_string_to_rs2_format(std::string str); + rs2_format string_to_rs2_format(std::string str); } diff --git a/realsense2_camera/src/profile_manager.cpp b/realsense2_camera/src/profile_manager.cpp index 9d885cf2be..9fba0adbe7 100644 --- a/realsense2_camera/src/profile_manager.cpp +++ b/realsense2_camera/src/profile_manager.cpp @@ -446,7 +446,7 @@ void VideoProfilesManager::registerVideoSensorParams(std::set _params.getParameters()->setParam(param_name, param_value, [this, sip](const rclcpp::Parameter& parameter) { std::string format_str(parameter.get_value()); - rs2_format temp_format = rs2_format_string_to_rs2_format(format_str); + rs2_format temp_format = string_to_rs2_format(format_str); bool found = false; if (temp_format != RS2_FORMAT_ANY) diff --git a/realsense2_camera/src/ros_utils.cpp b/realsense2_camera/src/ros_utils.cpp index 0623a645f4..b4661126cf 100644 --- a/realsense2_camera/src/ros_utils.cpp +++ b/realsense2_camera/src/ros_utils.cpp @@ -80,7 +80,7 @@ std::string create_graph_resource_name(const std::string &original_name) return fixed_name; } -rs2_format rs2_format_string_to_rs2_format(std::string str) +rs2_format string_to_rs2_format(std::string str) { rs2_format format = RS2_FORMAT_ANY; From 1e64da2beeb1b0ea1dba9cc7ca910f6c9475bd83 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Mon, 31 Jul 2023 22:02:47 +0530 Subject: [PATCH 8/8] Hardcode stream format if there is no default profile --- README.md | 4 +- realsense2_camera/include/profile_manager.h | 1 + realsense2_camera/src/profile_manager.cpp | 64 ++++++++++----------- 3 files changed, 32 insertions(+), 37 deletions(-) diff --git a/README.md b/README.md index e08669085d..b68e7a948e 100644 --- a/README.md +++ b/README.md @@ -211,8 +211,8 @@ - If the specified parameter is not available by the stream, the default or previously set configuration will be used. - Run ```ros2 param describe ``` 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 \X\X\ + \, it will not be opened and awarning message will be shown. - - Should update the profile setting and re-enable the stream for the change to take effect. + - If the stream doesn't support the user selected profile \X\X\ + \, 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_****: - Choose whether to enable a specified stream or not. Default is true for images and false for orientation streams. diff --git a/realsense2_camera/include/profile_manager.h b/realsense2_camera/include/profile_manager.h index 365f64df8f..b753ff67a9 100644 --- a/realsense2_camera/include/profile_manager.h +++ b/realsense2_camera/include/profile_manager.h @@ -74,6 +74,7 @@ namespace realsense2_camera private: 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 sips); std::string get_profiles_descriptions(); std::string getProfileFormatsDescriptions(stream_index_pair sip); diff --git a/realsense2_camera/src/profile_manager.cpp b/realsense2_camera/src/profile_manager.cpp index 9fba0adbe7..beffd2d574 100644 --- a/realsense2_camera/src/profile_manager.cpp +++ b/realsense2_camera/src/profile_manager.cpp @@ -172,7 +172,8 @@ void ProfilesManager::addWantedProfiles(std::vector& wanted { ROS_WARN_STREAM("Couldn't open " << ros_stream_to_string(sip.first) << ":" << sip.second << " stream " << "due to wrong profile selection. " - << "Update the profile settings and re-enable the stream for the change to take effect. " + << "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."); } } @@ -314,6 +315,22 @@ std::string VideoProfilesManager::getProfileFormatsDescriptions(stream_index_pai 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 sips) { // Set default values: @@ -335,47 +352,24 @@ void VideoProfilesManager::registerVideoSensorParams(std::set _width = video_profile.width(); _height = video_profile.height(); _fps = video_profile.fps(); - _formats[{default_profile.stream_type(), default_profile.stream_index()}] = video_profile.format(); - // Set the stream format from the default profiles provided by LibRealsense + // 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; - if (_formats.find(sip) == _formats.end()) - { - default_profile = sip_default_profile.second; - video_profile = default_profile.as(); - - if (isSameProfileValues(default_profile, _width, _height, _fps, video_profile.format())) - { - _formats[sip] = video_profile.format(); - } - } - } + default_profile = sip_default_profile.second; + video_profile = default_profile.as(); - // Set the format for the streams which doesn't have default profiles - for (auto sip : sips) - { - if (_formats.find(sip) == _formats.end()) + if (isSameProfileValues(default_profile, _width, _height, _fps, video_profile.format())) { - for (const auto& profile : _all_profiles) - { - bool found = false; - stream_index_pair profile_sip(profile.stream_type(), profile.stream_index()); - - if (sip == profile_sip && - isSameProfileValues(profile, _width, _height, _fps, profile.as().format())) - { - _formats[sip] = profile.format(); - found = true; - break; - } - if (!found) - { - _formats[sip] = RS2_FORMAT_ANY; - } - } + _formats[sip] = video_profile.format(); } }