Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Exposing stream_format params to user #2811

Merged
merged 10 commits into from
Jul 31, 2023
Merged

Exposing stream_format params to user #2811

Show file tree
Hide file tree
Changes from 5 commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
82d7730
Exposing stream_format params to user
Arun-Prasad-V Jul 8, 2023
068f0c6
Validate the user's input stream_format
Arun-Prasad-V Jul 10, 2023
da410fb
Warn user if the selected profile cannot be opened
Arun-Prasad-V Jul 12, 2023
5aa03f4
Merge branch 'IntelRealSense:ros2-development' into expose_stream_for…
Arun-Prasad-V Jul 19, 2023
0325781
Added stream_format param descriptors
Arun-Prasad-V Jul 19, 2023
1c6199d
Query default stream_format from LRS
Arun-Prasad-V Jul 27, 2023
ae6ac8a
Merge branch 'IntelRealSense:ros2-development' into expose_stream_for…
Arun-Prasad-V Jul 27, 2023
4af70f8
Updated new stream format params names
Arun-Prasad-V Jul 31, 2023
189918a
Updated function name
Arun-Prasad-V Jul 31, 2023
1e64da2
Hardcode stream format if there is no default profile
Arun-Prasad-V Jul 31, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 5 additions & 3 deletions realsense2_camera/include/profile_manager.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -68,18 +68,20 @@ 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;
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();
bool isSameProfileValues(const rs2::stream_profile& profile, const int width, const int height, const int fps, const rs2_format format);
void registerVideoSensorParams(std::set<stream_index_pair> sips);
std::string get_profiles_descriptions();
std::string get_profile_formats_descriptions(stream_index_pair sip);

private:
std::string _module_name;
std::map<stream_index_pair, rs2_format> _allowed_formats;
std::map<stream_index_pair, rs2_format> _formats;
int _fps;
int _width, _height;
bool _is_profile_exist;
Expand Down
1 change: 1 addition & 0 deletions realsense2_camera/include/ros_utils.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);

}

5 changes: 5 additions & 0 deletions realsense2_camera/launch/rs_launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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'},
Arun-Prasad-V marked this conversation as resolved.
Show resolved Hide resolved
{'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'},
Expand Down
16 changes: 8 additions & 8 deletions realsense2_camera/launch/rs_multi_camera_launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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'},
Arun-Prasad-V marked this conversation as resolved.
Show resolved Hide resolved
{'name': 'camera_name_2', 'default': 'camera2', 'description': 'camera unique name'},
]

def set_configurable_parameters(local_params):
Expand All @@ -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) +
Expand Down
121 changes: 109 additions & 12 deletions realsense2_camera/src/profile_manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -160,6 +160,21 @@ void ProfilesManager::addWantedProfiles(std::vector<rs2::stream_profile>& 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)
Arun-Prasad-V marked this conversation as resolved.
Show resolved Hide resolved
{
auto sip = x.first;
auto stream_enabled = x.second;

if (*stream_enabled && !found_sips[sip])
{
ROS_WARN_STREAM("Couldn't open " << ros_stream_to_string(sip.first) << ":" << sip.second << " stream "
Arun-Prasad-V marked this conversation as resolved.
Show resolved Hide resolved
<< "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.");
}
}
}

std::string ProfilesManager::profile_string(const rs2::stream_profile& profile)
Expand Down Expand Up @@ -204,29 +219,27 @@ VideoProfilesManager::VideoProfilesManager(std::shared_ptr<Parameters> 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<rs2::video_stream_profile>())
return false;
auto video_profile = profile.as<rs2::video_stream_profile>();
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<stream_profile> all_profiles, std::function<void()> update_sensor_func)
Expand All @@ -251,7 +264,7 @@ void VideoProfilesManager::registerProfileParameters(std::vector<stream_profile>
ROS_DEBUG_STREAM(__LINE__ << ": _enabled_profiles[" << ros_stream_to_string(sip.first) << ":" << sip.second << "]: " << *(_enabled_profiles[sip]));
}

registerVideoSensorParams();
registerVideoSensorParams(checked_sips);
}
}

Expand All @@ -275,7 +288,48 @@ std::string VideoProfilesManager::get_profiles_descriptions()
return descriptors;
}

void VideoProfilesManager::registerVideoSensorParams()
std::string VideoProfilesManager::get_profile_formats_descriptions(stream_index_pair sip)
Arun-Prasad-V marked this conversation as resolved.
Show resolved Hide resolved
{
std::set<std::string> profile_formats_str;
for (auto& profile : _all_profiles)
{
auto video_profile = profile.as<rs2::video_stream_profile>();
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)
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

should these formats be hard coded? what will happen if different camera models support different default profiles?
can't we use liberalsense api to fetch/query the default profiles for each sip ?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Sure. Will do so.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

LibRealSense APIs gives the default profiles of color and depth streams only. Couldn't get those for Infra streams.

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@Arun-Prasad-V,
can we query the device about default profiles for the given streams, and if default is not given, then to hard coded it ?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes, we can query about default profiles and configure the format, if found. If not, maybe can we choose the first found format whose profile's resolution matches with depth stream? So that there won't be anything hard coded.

_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<stream_index_pair> sips)
{
// Set default values:
rs2::stream_profile default_profile = getDefaultProfile();
Expand Down Expand Up @@ -311,7 +365,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;
Expand All @@ -333,12 +387,55 @@ void VideoProfilesManager::registerVideoSensorParams()
}
else
{
ROS_ERROR_STREAM("Given value, " << parameter.get_value<std::string>() << " is invalid. Set ROS param back to: " << crnt_profile_str.str());
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());
}
}, crnt_descriptor);
_parameters_names.push_back(param_name);

for (auto sip : sips)
{
std::string param_name(_module_name + ".profile." + STREAM_NAME(sip) + "_stream_format");
Arun-Prasad-V marked this conversation as resolved.
Show resolved Hide resolved
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<std::string>());
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 " << 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 <your_node_name> " << 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);
}
}

///////////////////////////////////////////////////////////////////////////////////////
Expand Down
16 changes: 16 additions & 0 deletions realsense2_camera/src/ros_utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -80,6 +80,22 @@ 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)
Arun-Prasad-V marked this conversation as resolved.
Show resolved Hide resolved
{
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;
converted = true;
break;
}
}
return converted;
}

static const rmw_qos_profile_t rmw_qos_profile_latched =
{
RMW_QOS_POLICY_HISTORY_KEEP_LAST,
Expand Down