Skip to content

Commit

Permalink
support for choosing IR profile
Browse files Browse the repository at this point in the history
  • Loading branch information
Arun-Prasad-V committed Feb 23, 2024
1 parent ec7c093 commit 0df86e5
Show file tree
Hide file tree
Showing 3 changed files with 122 additions and 74 deletions.
9 changes: 4 additions & 5 deletions realsense2_camera/include/profile_manager.h
Original file line number Diff line number Diff line change
Expand Up @@ -68,9 +68,9 @@ 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) {return _height[stream];};
int getWidth(rs2_stream stream) {return _width[stream];};
int getFPS(rs2_stream stream) {return _fps[stream];};

private:
bool isSameProfileValues(const rs2::stream_profile& profile, const int width, const int height, const int fps, const rs2_format format);
Expand All @@ -82,8 +82,7 @@ namespace realsense2_camera
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
159 changes: 94 additions & 65 deletions realsense2_camera/src/profile_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -241,7 +241,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 @@ -333,25 +333,48 @@ 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> 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];
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 : streams)
{
rs2::stream_profile default_profile;
switch (stream)
{
case RS2_STREAM_INFRARED:
if (sip_default_profiles.find(INFRA1) != sip_default_profiles.end())
{
default_profile = sip_default_profiles[INFRA1];
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_COLOR:
if (sip_default_profiles.find(COLOR) != sip_default_profiles.end())
{
default_profile = sip_default_profiles[COLOR];
}
break;
default:
default_profile = sip_default_profiles.begin()->second;
}

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

// Set the stream formats
for (auto sip : sips)
Expand All @@ -364,72 +387,78 @@ 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 : streams)
{
// Register ROS parameter:
std::stringstream param_name_str;
param_name_str << _module_name << "." << stream << "_stream_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[stream] << "x" << _height[stream] << "x" << _fps[stream];
_params.getParameters()->setParam<std::string>(param_name_str.str(), crnt_profile_str.str(), [this, stream](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)
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)
{
found = false;
request_default = true;
}
else
{
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] = temp_width;
_height[stream] = temp_height;
_fps[stream] = 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] << "x" << _height[stream] << "x" << _fps[stream];
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
28 changes: 24 additions & 4 deletions realsense2_camera/src/ros_sensor.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -379,8 +379,18 @@ void RosSensor::set_sensor_auto_exposure_roi()
{
try
{
int width = std::dynamic_pointer_cast<VideoProfilesManager>(_profile_managers[0])->getWidth();
int height = std::dynamic_pointer_cast<VideoProfilesManager>(_profile_managers[0])->getHeight();
rs2_stream stream_type;
if (this->rs2::sensor::is<rs2::depth_sensor>())
{
stream_type = RS2_STREAM_DEPTH;
}
else if (this->rs2::sensor::is<rs2::color_sensor>())
{
stream_type = RS2_STREAM_COLOR;
}

int width = std::dynamic_pointer_cast<VideoProfilesManager>(_profile_managers[0])->getWidth(stream_type);
int height = std::dynamic_pointer_cast<VideoProfilesManager>(_profile_managers[0])->getHeight(stream_type);

bool update_roi_range(false);
if (_auto_exposure_roi.max_x > width)
Expand Down Expand Up @@ -411,8 +421,18 @@ void RosSensor::registerAutoExposureROIOptions()

if (this->rs2::sensor::is<rs2::roi_sensor>())
{
int width = std::dynamic_pointer_cast<VideoProfilesManager>(_profile_managers[0])->getWidth();
int height = std::dynamic_pointer_cast<VideoProfilesManager>(_profile_managers[0])->getHeight();
rs2_stream stream_type;
if (this->rs2::sensor::is<rs2::depth_sensor>())
{
stream_type = RS2_STREAM_DEPTH;
}
else if (this->rs2::sensor::is<rs2::color_sensor>())
{
stream_type = RS2_STREAM_COLOR;
}

int width = std::dynamic_pointer_cast<VideoProfilesManager>(_profile_managers[0])->getWidth(stream_type);
int height = std::dynamic_pointer_cast<VideoProfilesManager>(_profile_managers[0])->getHeight(stream_type);

int max_x(width-1);
int max_y(height-1);
Expand Down

0 comments on commit 0df86e5

Please sign in to comment.