Skip to content

Commit

Permalink
Added stream_format param descriptors
Browse files Browse the repository at this point in the history
  • Loading branch information
@Arun-Prasad-V
Arun-Prasad-V committed Jul 19, 2023
1 parent 5aa03f4 commit 0325781
Show file tree
Hide file tree
Showing 4 changed files with 59 additions and 19 deletions.
1 change: 1 addition & 0 deletions realsense2_camera/include/profile_manager.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -77,6 +77,7 @@ namespace realsense2_camera
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;
Expand Down
16 changes: 8 additions & 8 deletions realsense2_camera/launch/rs_multi_camera_launch.py
View file
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'},
{'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
52 changes: 45 additions & 7 deletions realsense2_camera/src/profile_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -160,14 +160,19 @@ 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)
{
auto sip = x.first;
auto stream_enabled = x.second;

if (*stream_enabled && !found_sips[sip])
{
ROS_WARN_STREAM("Couldn't find profile for " << ros_stream_to_string(sip.first) << ":" << sip.second << " stream."
<< " Update the profile settings and re-enable the stream for the change to take effect.");
ROS_WARN_STREAM("Couldn't open " << ros_stream_to_string(sip.first) << ":" << sip.second << " stream "
<< "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.");
}
}
}
Expand Down Expand Up @@ -282,6 +287,32 @@ std::string VideoProfilesManager::get_profiles_descriptions()
descriptors.pop_back();
return descriptors;
}

std::string VideoProfilesManager::get_profile_formats_descriptions(stream_index_pair sip)
{
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)
Expand All @@ -295,7 +326,7 @@ void VideoProfilesManager::registerVideoSensorProfileFormat(stream_index_pair si
else if (sip == COLOR)
_formats[COLOR] = RS2_FORMAT_RGB8;
else
_formats[{sip.first, sip.second}] = RS2_FORMAT_ANY;
_formats[sip] = RS2_FORMAT_ANY;
}

void VideoProfilesManager::registerVideoSensorParams(std::set<stream_index_pair> sips)
Expand Down Expand Up @@ -356,7 +387,10 @@ void VideoProfilesManager::registerVideoSensorParams(std::set<stream_index_pair>
}
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());
}
Expand All @@ -368,6 +402,8 @@ void VideoProfilesManager::registerVideoSensorParams(std::set<stream_index_pair>
std::string param_name(_module_name + ".profile." + STREAM_NAME(sip) + "_stream_format");
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>());
Expand All @@ -381,7 +417,7 @@ void VideoProfilesManager::registerVideoSensorParams(std::set<stream_index_pair>

if (sip == profile_sip && temp_format == profile.format())
{
ROS_WARN_STREAM("re-enable the " << sip.first << " stream for the change to take effect.");
ROS_WARN_STREAM("re-enable the " << STREAM_NAME(sip) << " stream for the change to take effect.");
found = true;
_formats[sip] = temp_format;
break;
Expand All @@ -390,12 +426,14 @@ void VideoProfilesManager::registerVideoSensorParams(std::set<stream_index_pair>
}
if (!found)
{
ROS_WARN_STREAM(STREAM_NAME(sip) <<" stream doesn't support " << format_str <<" format. "
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
9 changes: 5 additions & 4 deletions realsense2_camera/src/ros_utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -80,19 +80,20 @@ 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)
bool string_to_rs2_format(std::string str , rs2_format* format)
{
bool is_found = false;
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;
is_found = true;
converted = true;
break;
}
}
return is_found;
return converted;
}

static const rmw_qos_profile_t rmw_qos_profile_latched =
Expand Down

0 comments on commit 0325781

Please sign in to comment.