diff --git a/README.md b/README.md
index a77621972e..81727a7726 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 9b889d8561..ab2b68101e 100644
--- a/realsense2_camera/launch/rs_launch.py
+++ b/realsense2_camera/launch/rs_launch.py
@@ -33,15 +33,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'},
@@ -83,8 +84,9 @@
{'name': 'reconnect_timeout', 'default': '6.', 'description': 'Timeout(seconds) between consequtive reconnection attempts'},
{'name': 'enable_safety', 'default': 'false', 'description': "'enable safety stream'"},
{'name': 'enable_labeled_point_cloud', 'default': 'false', 'description': "'enable labeled point cloud stream'"},
+ {'name': 'depth_mapping_camera.labeled_point_cloud_profile', 'default': '0,0,0', 'description': "'Label PointCloud stream profile'"},
{'name': 'enable_occupancy', 'default': 'false', 'description': "'enable occupancy stream'"},
- {'name': 'depth_mapping_camera.profile', 'default': '0,0,0', 'description': "'depth mapping camera profile'"},
+ {'name': 'depth_mapping_camera.occupancy_profile', 'default': '0,0,0', 'description': "'Occupancy stream profile'"},
]
def declare_configurable_parameters(parameters):
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 dfd5daed99..8560981f02 100644
--- a/realsense2_camera/src/ros_sensor.cpp
+++ b/realsense2_camera/src/ros_sensor.cpp
@@ -379,8 +379,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)
@@ -411,8 +421,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);