Skip to content

Commit

Permalink
PR #3052 from Arun-Prasad-V: Support for selecting profile for each s…
Browse files Browse the repository at this point in the history 
…tream_type

Support for selecting profile for each stream_type
  • Loading branch information
@SamerKhshiboun
SamerKhshiboun authored Mar 26, 2024
2 parents 2ac1f8b + 0ca2510 commit 2a054eb
Show file tree
Hide file tree
Showing 5 changed files with 175 additions and 86 deletions.
8 changes: 4 additions & 4 deletions README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -244,7 +244,7 @@
#### with ros2 launch:
ros2 launch realsense2_camera rs_launch.py
ros2 launch realsense2_camera rs_launch.py depth_module.profile:=1280x720x30 pointcloud.enable:=true
ros2 launch realsense2_camera rs_launch.py depth_module.depth_profile:=1280x720x30 pointcloud.enable:=true
<hr>
Expand Down Expand Up @@ -323,10 +323,10 @@ User can set the camera name and camera namespace, to distinguish between camera
#### Parameters that can be modified during runtime:
- All of the filters and sensors inner parameters.
- Video Sensor Parameters: (```depth_module``` and ```rgb_camera```)
- They have, at least, the **profile** parameter.
- They have, at least, the **<stream_type>_profile** parameter.
- The profile parameter is a string of the following format: \<width>X\<height>X\<fps> (The dividing character can be X, x or ",". Spaces are ignored.)
- For example: ```depth_module.profile:=640x480x30 rgb_camera.profile:=1280x720x30```
- Since infra, infra1, infra2 and depth are all streams of the depth_module, their width, height and fps are defined by the same param **depth_module.profile**
- For example: ```depth_module.depth_profile:=640x480x30 depth_module.infra_profile:=640x480x30 rgb_camera.color_profile:=1280x720x30```
- Note: The param **depth_module.infra_profile** is common for all infra streams. i.e., infra 0, 1 & 2.
- If the specified combination of parameters is not available by the device, the default or previously set configuration will be used.
- Run ```ros2 param describe <your_node_name> <param_name>``` to get the list of supported profiles.
- Note: Should re-enable the stream for the change to take effect.
Expand Down
12 changes: 6 additions & 6 deletions realsense2_camera/include/profile_manager.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -68,22 +68,22 @@ namespace realsense2_camera
VideoProfilesManager(std::shared_ptr<Parameters> parameters, const std::string& module_name, rclcpp::Logger logger, bool force_image_default_qos = false);
bool isWantedProfile(const rs2::stream_profile& profile) override;
void registerProfileParameters(std::vector<stream_profile> all_profiles, std::function<void()> update_sensor_func) override;
int getHeight() {return _height;};
int getWidth() {return _width;};
int getFPS() {return _fps;};
int getHeight(rs2_stream stream_type) {return _height[stream_type];};
int getWidth(rs2_stream stream_type) {return _width[stream_type];};
int getFPS(rs2_stream stream_type) {return _fps[stream_type];};

private:
bool isSameProfileValues(const rs2::stream_profile& profile, const int width, const int height, const int fps, const rs2_format format);
rs2::stream_profile validateAndGetSuitableProfile(rs2_stream stream_type, rs2::stream_profile given_profile);
void registerVideoSensorProfileFormat(stream_index_pair sip);
void registerVideoSensorParams(std::set<stream_index_pair> sips);
std::string get_profiles_descriptions();
std::string get_profiles_descriptions(rs2_stream stream_type);
std::string getProfileFormatsDescriptions(stream_index_pair sip);

private:
std::string _module_name;
std::map<stream_index_pair, rs2_format> _formats;
int _fps;
int _width, _height;
std::map<rs2_stream, int> _fps, _width, _height;
bool _is_profile_exist;
bool _force_image_default_qos;
};
Expand Down
5 changes: 3 additions & 2 deletions realsense2_camera/launch/rs_launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,15 +34,16 @@
{'name': 'log_level', 'default': 'info', 'description': 'debug log level [DEBUG|INFO|WARN|ERROR|FATAL]'},
{'name': 'output', 'default': 'screen', 'description': 'pipe node output [screen|log]'},
{'name': 'enable_color', 'default': 'true', 'description': 'enable color stream'},
{'name': 'rgb_camera.profile', 'default': '0,0,0', 'description': 'color image width'},
{'name': 'rgb_camera.color_profile', 'default': '0,0,0', 'description': 'color stream profile'},
{'name': 'rgb_camera.color_format', 'default': 'RGB8', 'description': 'color stream format'},
{'name': 'rgb_camera.enable_auto_exposure', 'default': 'true', 'description': 'enable/disable auto exposure for color image'},
{'name': 'enable_depth', 'default': 'true', 'description': 'enable depth stream'},
{'name': 'enable_infra', 'default': 'false', 'description': 'enable infra0 stream'},
{'name': 'enable_infra1', 'default': 'false', 'description': 'enable infra1 stream'},
{'name': 'enable_infra2', 'default': 'false', 'description': 'enable infra2 stream'},
{'name': 'depth_module.profile', 'default': '0,0,0', 'description': 'depth module profile'},
{'name': 'depth_module.depth_profile', 'default': '0,0,0', 'description': 'depth stream profile'},
{'name': 'depth_module.depth_format', 'default': 'Z16', 'description': 'depth stream format'},
{'name': 'depth_module.infra_profile', 'default': '0,0,0', 'description': 'infra streams (0/1/2) profile'},
{'name': 'depth_module.infra_format', 'default': 'RGB8', 'description': 'infra0 stream format'},
{'name': 'depth_module.infra1_format', 'default': 'Y8', 'description': 'infra1 stream format'},
{'name': 'depth_module.infra2_format', 'default': 'Y8', 'description': 'infra2 stream format'},
Expand Down
208 changes: 138 additions & 70 deletions realsense2_camera/src/profile_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -141,6 +141,40 @@ std::map<stream_index_pair, rs2::stream_profile> ProfilesManager::getDefaultProf
return sip_default_profiles;
}

rs2::stream_profile VideoProfilesManager::validateAndGetSuitableProfile(rs2_stream stream_type, rs2::stream_profile given_profile)
{
rs2::stream_profile suitable_profile = given_profile;
bool is_given_profile_suitable = false;

for (auto temp_profile : _all_profiles)
{
if (temp_profile.stream_type() == stream_type)
{
auto video_profile = given_profile.as<rs2::video_stream_profile>();

if (isSameProfileValues(temp_profile, video_profile.width(), video_profile.height(), video_profile.fps(), RS2_FORMAT_ANY))
{
is_given_profile_suitable = true;
break;
}
}
}

// If given profile is not suitable, choose the first available profile for the given stream.
if (!is_given_profile_suitable)
{
for (auto temp_profile : _all_profiles)
{
if (temp_profile.stream_type() == stream_type)
{
suitable_profile = temp_profile;
}
}
}

return suitable_profile;
}

void ProfilesManager::addWantedProfiles(std::vector<rs2::stream_profile>& wanted_profiles)
{
std::map<stream_index_pair, bool> found_sips;
Expand Down Expand Up @@ -243,7 +277,7 @@ bool VideoProfilesManager::isSameProfileValues(const rs2::stream_profile& profil
bool VideoProfilesManager::isWantedProfile(const rs2::stream_profile& profile)
{
stream_index_pair sip = {profile.stream_type(), profile.stream_index()};
return isSameProfileValues(profile, _width, _height, _fps, _formats[sip]);
return isSameProfileValues(profile, _width[sip.first], _height[sip.first], _fps[sip.first], _formats[sip]);
}

void VideoProfilesManager::registerProfileParameters(std::vector<stream_profile> all_profiles, std::function<void()> update_sensor_func)
Expand Down Expand Up @@ -272,15 +306,18 @@ void VideoProfilesManager::registerProfileParameters(std::vector<stream_profile>
}
}

std::string VideoProfilesManager::get_profiles_descriptions()
std::string VideoProfilesManager::get_profiles_descriptions(rs2_stream stream_type)
{
std::set<std::string> profiles_str;
for (auto& profile : _all_profiles)
{
auto video_profile = profile.as<rs2::video_stream_profile>();
std::stringstream crnt_profile_str;
crnt_profile_str << video_profile.width() << "x" << video_profile.height() << "x" << video_profile.fps();
profiles_str.insert(crnt_profile_str.str());
if (stream_type == profile.stream_type())
{
auto video_profile = profile.as<rs2::video_stream_profile>();
std::stringstream crnt_profile_str;
crnt_profile_str << video_profile.width() << "x" << video_profile.height() << "x" << video_profile.fps();
profiles_str.insert(crnt_profile_str.str());
}
}
std::stringstream descriptors_strm;
for (auto& profile_str : profiles_str)
Expand Down Expand Up @@ -335,25 +372,52 @@ void VideoProfilesManager::registerVideoSensorProfileFormat(stream_index_pair si

void VideoProfilesManager::registerVideoSensorParams(std::set<stream_index_pair> sips)
{
// Set default values:
std::map<stream_index_pair, rs2::stream_profile> sip_default_profiles = getDefaultProfiles();

rs2::stream_profile default_profile = sip_default_profiles.begin()->second;
std::set<rs2_stream> available_streams;

if (sip_default_profiles.find(DEPTH) != sip_default_profiles.end())
{
default_profile = sip_default_profiles[DEPTH];
}
else if (sip_default_profiles.find(COLOR) != sip_default_profiles.end())
for (auto sip : sips)
{
default_profile = sip_default_profiles[COLOR];
available_streams.insert(sip.first);
}

auto video_profile = default_profile.as<rs2::video_stream_profile>();
// Set default values:
std::map<stream_index_pair, rs2::stream_profile> sip_default_profiles = getDefaultProfiles();

_width = video_profile.width();
_height = video_profile.height();
_fps = video_profile.fps();
for (auto stream_type : available_streams)
{
rs2::stream_profile default_profile = sip_default_profiles.begin()->second;
switch (stream_type)
{
case RS2_STREAM_COLOR:
if (sip_default_profiles.find(COLOR) != sip_default_profiles.end())
{
default_profile = sip_default_profiles[COLOR];
}
break;
case RS2_STREAM_DEPTH:
if (sip_default_profiles.find(DEPTH) != sip_default_profiles.end())
{
default_profile = sip_default_profiles[DEPTH];
}
break;
case RS2_STREAM_INFRARED:
if (sip_default_profiles.find(INFRA1) != sip_default_profiles.end())
{
default_profile = sip_default_profiles[INFRA1];
}
else if (sip_default_profiles.find(DEPTH) != sip_default_profiles.end())
{
default_profile = validateAndGetSuitableProfile(stream_type, sip_default_profiles[DEPTH]);
}
break;
default:
default_profile = validateAndGetSuitableProfile(stream_type, sip_default_profiles.begin()->second);
}

auto video_profile = default_profile.as<rs2::video_stream_profile>();
_width[stream_type] = video_profile.width();
_height[stream_type] = video_profile.height();
_fps[stream_type] = video_profile.fps();
}

// Set the stream formats
for (auto sip : sips)
Expand All @@ -366,72 +430,76 @@ void VideoProfilesManager::registerVideoSensorParams(std::set<stream_index_pair>
{
stream_index_pair sip = sip_default_profile.first;

default_profile = sip_default_profile.second;
video_profile = default_profile.as<rs2::video_stream_profile>();
auto default_profile = sip_default_profile.second;
auto video_profile = default_profile.as<rs2::video_stream_profile>();

if (isSameProfileValues(default_profile, _width, _height, _fps, video_profile.format()))
if (isSameProfileValues(default_profile, _width[sip.first], _height[sip.first], _fps[sip.first], video_profile.format()))
{
_formats[sip] = video_profile.format();
}
}

// Register ROS parameter:
std::string param_name(_module_name + ".profile");
rcl_interfaces::msg::ParameterDescriptor crnt_descriptor;
crnt_descriptor.description = "Available options are:\n" + get_profiles_descriptions();
std::stringstream crnt_profile_str;
crnt_profile_str << _width << "x" << _height << "x" << _fps;
_params.getParameters()->setParam<std::string>(param_name, crnt_profile_str.str(), [this](const rclcpp::Parameter& parameter)
{
std::regex self_regex("\\s*([0-9]+)\\s*[xX,]\\s*([0-9]+)\\s*[xX,]\\s*([0-9]+)\\s*", std::regex_constants::ECMAScript);
std::smatch match;
std::string profile_str(parameter.get_value<std::string>());
bool found = std::regex_match(profile_str, match, self_regex);
bool request_default(false);
if (found)
for (auto stream_type : available_streams)
{
// Register ROS parameter:
std::stringstream param_name_str;
param_name_str << _module_name << "." << create_graph_resource_name(ros_stream_to_string(stream_type)) << "_profile";
rcl_interfaces::msg::ParameterDescriptor crnt_descriptor;
crnt_descriptor.description = "Available options are:\n" + get_profiles_descriptions(stream_type);
std::stringstream crnt_profile_str;
crnt_profile_str << _width[stream_type] << "x" << _height[stream_type] << "x" << _fps[stream_type];
_params.getParameters()->setParam<std::string>(param_name_str.str(), crnt_profile_str.str(), [this, stream_type](const rclcpp::Parameter& parameter)
{
int temp_width(std::stoi(match[1])), temp_height(std::stoi(match[2])), temp_fps(std::stoi(match[3]));
if (temp_width <= 0 || temp_height <= 0 || temp_fps <= 0)
{
found = false;
request_default = true;
}
else
std::regex self_regex("\\s*([0-9]+)\\s*[xX,]\\s*([0-9]+)\\s*[xX,]\\s*([0-9]+)\\s*", std::regex_constants::ECMAScript);
std::smatch match;
std::string profile_str(parameter.get_value<std::string>());
bool found = std::regex_match(profile_str, match, self_regex);
bool request_default(false);
if (found)
{
for (const auto& profile : _all_profiles)
int temp_width(std::stoi(match[1])), temp_height(std::stoi(match[2])), temp_fps(std::stoi(match[3]));
if (temp_width <= 0 || temp_height <= 0 || temp_fps <= 0)
{
found = false;
if (isSameProfileValues(profile, temp_width, temp_height, temp_fps, RS2_FORMAT_ANY))
request_default = true;
}
else
{
for (const auto& profile : _all_profiles)
{
_width = temp_width;
_height = temp_height;
_fps = temp_fps;
found = true;
ROS_WARN_STREAM("re-enable the stream for the change to take effect.");
break;
found = false;
if (isSameProfileValues(profile, temp_width, temp_height, temp_fps, RS2_FORMAT_ANY))
{
_width[stream_type] = temp_width;
_height[stream_type] = temp_height;
_fps[stream_type] = temp_fps;
found = true;
ROS_WARN_STREAM("re-enable the stream for the change to take effect.");
break;
}
}
}
}
}
if (!found)
{
std::stringstream crnt_profile_str;
crnt_profile_str << _width << "x" << _height << "x" << _fps;
if (request_default)
{
ROS_INFO_STREAM("Set ROS param " << parameter.get_name() << " to default: " << crnt_profile_str.str());
}
else
if (!found)
{
ROS_ERROR_STREAM("Given value, " << parameter.get_value<std::string>() << " is invalid. "
<< "Run 'ros2 param describe <your_node_name> " << parameter.get_name()
<< "' to get the list of supported profiles. "
<< "Setting ROS param back to: " << crnt_profile_str.str());
std::stringstream crnt_profile_str;
crnt_profile_str << _width[stream_type] << "x" << _height[stream_type] << "x" << _fps[stream_type];
if (request_default)
{
ROS_INFO_STREAM("Set ROS param " << parameter.get_name() << " to default: " << crnt_profile_str.str());
}
else
{
ROS_ERROR_STREAM("Given value, " << parameter.get_value<std::string>() << " is invalid. "
<< "Run 'ros2 param describe <your_node_name> " << parameter.get_name()
<< "' to get the list of supported profiles. "
<< "Setting ROS param back to: " << crnt_profile_str.str());
}
_params.getParameters()->queueSetRosValue(parameter.get_name(), crnt_profile_str.str());
}
_params.getParameters()->queueSetRosValue(parameter.get_name(), crnt_profile_str.str());
}
}, crnt_descriptor);
_parameters_names.push_back(param_name);
}, crnt_descriptor);
_parameters_names.push_back(param_name_str.str());
}

for (auto sip : sips)
{
Expand Down
Loading

0 comments on commit 2a054eb

Please sign in to comment.