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

Fix ros2 parameter descriptions and range values #2600

Merged
merged 6 commits into from
Jan 31, 2023
Merged

Fix ros2 parameter descriptions and range values #2600

Changes from 4 commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
60f7583
Fix ros2 sensor controls steps and add control default value to param…
SamerKhshiboun Jan 18, 2023
d2dacdf
fix tabs
SamerKhshiboun Jan 18, 2023
c9fc09b
changed to static_cast and added descriptor name and type
SamerKhshiboun Jan 25, 2023
1a86b0d
realsense2_camera/src/sensor_params.cpp
SamerKhshiboun Jan 30, 2023
675eb51
fix code review and split big function into smaller functions
SamerKhshiboun Jan 31, 2023
3e711ad
fix float_to_double method
SamerKhshiboun Jan 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
54 changes: 46 additions & 8 deletions realsense2_camera/src/sensor_params.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -89,40 +89,78 @@ void SensorParams::set_parameter(rs2::options sensor, rs2_option option, const s
{
const std::string option_name(module_name + "." + create_graph_resource_name(rs2_option_to_string(option)));
T option_value;
rs2::option_range op_range;
try
{
option_value = static_cast<T>(sensor.get_option(option));
op_range = sensor.get_option_range(option);
float current_val = sensor.get_option(option);
if(std::is_same<T, double>::value && current_val > 0)
Copy link
Collaborator

Choose a reason for hiding this comment

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

!= 0?

{
// casting from float to double, while trying to increase precision
// in order to be comply with the FloatingPointRange configurations
// and to succeed the rclcpp NodeParameters::declare_parameter call
float range = op_range.max - op_range.min;
int temp_val = range / current_val;
option_value = static_cast<double>(range) / temp_val;
}
else
{
option_value = static_cast<T>(current_val);
}
}
catch(const std::exception& ex)
{
ROS_ERROR_STREAM("An error has occurred while calling sensor for: " << option_name << ":" << ex.what());
return;
}
rs2::option_range op_range = sensor.get_option_range(option);

rcl_interfaces::msg::ParameterDescriptor crnt_descriptor;

// set descriptor name to be the option name in order to get more detailed warnings/errors
crnt_descriptor.name = option_name;

std::stringstream desc;
desc << sensor.get_option_description(option) << std::endl << description_addition;
crnt_descriptor.description = desc.str();
if (std::is_same<T, int>::value || std::is_same<T, bool>::value)
{
rcl_interfaces::msg::IntegerRange range;
range.from_value = int(op_range.min);
range.to_value = int(op_range.max);
range.from_value = static_cast<int64_t>(op_range.min);
range.to_value = static_cast<int64_t>(op_range.max);
range.step = static_cast<uint64_t>(op_range.step);
desc << " default value: " << static_cast<int64_t>(op_range.def);
crnt_descriptor.integer_range.push_back(range);
if (std::is_same<T, bool>::value)
if (std::is_same<T, bool>::value)
{
crnt_descriptor.type = rclcpp::PARAMETER_BOOL;
ROS_DEBUG_STREAM("Declare: BOOL::" << option_name << " = " << option_value << "[" << op_range.min << ", " << op_range.max << "]");
}
else
{
crnt_descriptor.type = rclcpp::PARAMETER_INTEGER;
ROS_DEBUG_STREAM("Declare: INT::" << option_name << " = " << option_value << "[" << op_range.min << ", " << op_range.max << "]");
}
}
else
{
rcl_interfaces::msg::FloatingPointRange range;
range.from_value = double(op_range.min);
range.to_value = double(op_range.max);
range.from_value = static_cast<double>(op_range.min);
Copy link
Collaborator

Choose a reason for hiding this comment

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

Maybe needs conversion too?

range.to_value = static_cast<double>(op_range.max);

// casting from float to double, while trying to increase precision
// in order to be comply with the FloatingPointRange configurations
// and to succeed the rclcpp NodeParameters::declare_parameter call
float r = op_range.max - op_range.min;
int steps = r / op_range.step;
range.step = static_cast<double>(r) / steps;

desc << " default value: " << static_cast<double>(op_range.def);
crnt_descriptor.floating_point_range.push_back(range);
crnt_descriptor.type = rclcpp::PARAMETER_DOUBLE;
ROS_DEBUG_STREAM("Declare: DOUBLE::" << option_name << " = " << option_value);
}

crnt_descriptor.description = desc.str();

T new_val;
try
{
Expand Down