diff --git a/README.md b/README.md index d7b247bf87..b68e7a948e 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,35 @@ #### Parameters that can be modified during runtime: - All of the filters and sensors inner parameters. +- Video Sensor Parameters: (```depth_module``` and ```rgb_camera```) + - They have, at least, the **profile** parameter. + - The profile parameter is a string of the following format: \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. + - Note: Should re-enable the stream for the change to take effect. + - ****_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.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. + - 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 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. - - 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 d171b8e4c7..b753ff67a9 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; @@ -73,13 +73,15 @@ namespace realsense2_camera int getFPS() {return _fps;}; private: - bool isSameProfileValues(const rs2::stream_profile& profile, const int width, const int height, const int fps); - void registerVideoSensorParams(); + bool isSameProfileValues(const rs2::stream_profile& profile, const int width, const int height, const int fps, const rs2_format format); + void registerVideoSensorProfileFormat(stream_index_pair sip); + void registerVideoSensorParams(std::set sips); std::string get_profiles_descriptions(); + std::string getProfileFormatsDescriptions(stream_index_pair sip); 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/include/ros_utils.h b/realsense2_camera/include/ros_utils.h index 4fa45899e7..4df1def396 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(); + 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 953bcb73d1..2f93d4de32 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.depth_format', 'default': 'Z16', 'description': 'depth stream format'}, + {'name': 'depth_module.infra_format', 'default': 'RGB8', 'description': 'infra0 stream format'}, + {'name': 'depth_module.infra1_format', 'default': 'Y8', 'description': 'infra1 stream format'}, + {'name': 'depth_module.infra2_format', 'default': 'Y8', 'description': 'infra2 stream format'}, {'name': 'enable_depth', 'default': 'true', 'description': 'enable depth stream'}, {'name': 'rgb_camera.profile', 'default': '0,0,0', 'description': 'color image width'}, + {'name': 'rgb_camera.color_format', 'default': 'RGB8', 'description': 'color stream format'}, {'name': 'rgb_camera.enable_auto_exposure', 'default': 'true', 'description': 'enable/disable auto exposure for color image'}, {'name': 'enable_color', 'default': 'true', 'description': 'enable color stream'}, {'name': 'enable_infra', 'default': 'false', 'description': 'enable infra0 stream'}, diff --git a/realsense2_camera/src/profile_manager.cpp b/realsense2_camera/src/profile_manager.cpp index befe814ae4..beffd2d574 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) @@ -160,6 +161,22 @@ 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 & profile : _enabled_profiles) + { + auto sip = profile.first; + auto stream_enabled = profile.second; + + if (*stream_enabled && !found_sips[sip]) + { + ROS_WARN_STREAM("Couldn't open " << ros_stream_to_string(sip.first) << ":" << sip.second << " stream " + << "due to wrong profile selection. " + << "Please verify and update the profile settings (such as width, height, fps, format) " + << "and re-enable the stream for the changes to take effect. " + << "Run 'rs-enumerate-devices' command to know the list of profiles supported by the sensors."); + } + } } std::string ProfilesManager::profile_string(const rs2::stream_profile& profile) @@ -204,29 +221,27 @@ VideoProfilesManager::VideoProfilesManager(std::shared_ptr parameter _module_name(module_name), _force_image_default_qos(force_image_default_qos) { - _allowed_formats[{RS2_STREAM_DEPTH, 0}] = RS2_FORMAT_Z16; - _allowed_formats[{RS2_STREAM_INFRARED, 0}] = RS2_FORMAT_RGB8; - _allowed_formats[{RS2_STREAM_INFRARED, 1}] = RS2_FORMAT_Y8; - _allowed_formats[{RS2_STREAM_INFRARED, 2}] = RS2_FORMAT_Y8; + } -bool VideoProfilesManager::isSameProfileValues(const rs2::stream_profile& profile, const int width, const int height, const int fps) +bool VideoProfilesManager::isSameProfileValues(const rs2::stream_profile& profile, const int width, const int height, const int fps, const rs2_format format) { if (!profile.is()) 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.find(sip) == _allowed_formats.end() || video_profile.format() == _allowed_formats[sip] )); + (RS2_FORMAT_ANY == format || + video_profile.format() == format)); } bool VideoProfilesManager::isWantedProfile(const rs2::stream_profile& profile) { - return isSameProfileValues(profile, _width, _height, _fps); + stream_index_pair sip = {profile.stream_type(), profile.stream_index()}; + return isSameProfileValues(profile, _width, _height, _fps, _formats[sip]); } void VideoProfilesManager::registerProfileParameters(std::vector all_profiles, std::function update_sensor_func) @@ -251,7 +266,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); } } @@ -275,16 +290,89 @@ std::string VideoProfilesManager::get_profiles_descriptions() return descriptors; } -void VideoProfilesManager::registerVideoSensorParams() +std::string VideoProfilesManager::getProfileFormatsDescriptions(stream_index_pair sip) +{ + std::set 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) + _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(); + // Set the stream formats + for (auto sip : sips) + { + registerVideoSensorProfileFormat(sip); + } + + // Overwrite the _formats with default values queried from LibRealsense + for (auto sip_default_profile : sip_default_profiles) + { + stream_index_pair sip = sip_default_profile.first; + + default_profile = sip_default_profile.second; + video_profile = default_profile.as(); + + if (isSameProfileValues(default_profile, _width, _height, _fps, video_profile.format())) + { + _formats[sip] = video_profile.format(); + } + } + // Register ROS parameter: std::string param_name(_module_name + ".profile"); rcl_interfaces::msg::ParameterDescriptor crnt_descriptor; @@ -311,7 +399,7 @@ void VideoProfilesManager::registerVideoSensorParams() for (const auto& profile : _all_profiles) { found = false; - if (isSameProfileValues(profile, temp_width, temp_height, temp_fps)) + if (isSameProfileValues(profile, temp_width, temp_height, temp_fps, RS2_FORMAT_ANY)) { _width = temp_width; _height = temp_height; @@ -333,12 +421,55 @@ void VideoProfilesManager::registerVideoSensorParams() } else { - ROS_ERROR_STREAM("Given value, " << parameter.get_value() << " 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()); } }, crnt_descriptor); _parameters_names.push_back(param_name); + + for (auto sip : sips) + { + std::string param_name(_module_name + "." + STREAM_NAME(sip) + "_format"); + std::string param_value = rs2_format_to_string(_formats[sip]); + rcl_interfaces::msg::ParameterDescriptor crnt_descriptor; + crnt_descriptor.description = "Available options are:\n" + getProfileFormatsDescriptions(sip); + _params.getParameters()->setParam(param_name, param_value, [this, sip](const rclcpp::Parameter& parameter) + { + std::string format_str(parameter.get_value()); + rs2_format temp_format = string_to_rs2_format(format_str); + bool found = false; + + if (temp_format != RS2_FORMAT_ANY) + { + for (const auto& profile : _all_profiles) + { + stream_index_pair profile_sip = {profile.stream_type(), profile.stream_index()}; + + if (sip == profile_sip && temp_format == profile.format()) + { + ROS_WARN_STREAM("re-enable the " << STREAM_NAME(sip) << " stream for the change to take effect."); + found = true; + _formats[sip] = temp_format; + break; + } + } + } + if (!found) + { + ROS_ERROR_STREAM(STREAM_NAME(sip) <<" stream doesn't support " << format_str <<" format. " + << "Run 'ros2 param describe " << parameter.get_name() + << "' to get the list of supported formats. " + << "Setting the ROS param '" << parameter.get_name() <<"' back to: " << _formats[sip]); + _params.getParameters()->queueSetRosValue(parameter.get_name(), + (std::string)rs2_format_to_string(_formats[sip])); + } + }, crnt_descriptor); + _parameters_names.push_back(param_name); + } } /////////////////////////////////////////////////////////////////////////////////////// @@ -396,9 +527,12 @@ void MotionProfilesManager::registerFPSParams() } // Overwrite with default values: - rs2::stream_profile default_profile = getDefaultProfile(); - stream_index_pair sip(default_profile.stream_type(), default_profile.stream_index()); - *(_fps[sip]) = default_profile.as().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) diff --git a/realsense2_camera/src/ros_utils.cpp b/realsense2_camera/src/ros_utils.cpp index b543be2af0..b4661126cf 100644 --- a/realsense2_camera/src/ros_utils.cpp +++ b/realsense2_camera/src/ros_utils.cpp @@ -80,6 +80,22 @@ std::string create_graph_resource_name(const std::string &original_name) return fixed_name; } +rs2_format string_to_rs2_format(std::string str) +{ + 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; + break; + } + } + return format; +} + static const rmw_qos_profile_t rmw_qos_profile_latched = { RMW_QOS_POLICY_HISTORY_KEEP_LAST,