Skip to content

Commit

Permalink
Fix crashing due to an invalid parameter in the initial value. (#271)
Browse files Browse the repository at this point in the history
Co-authored-by: Alejandro Hernández Cordero <[email protected]>
(cherry picked from commit cdae6b8)

# Conflicts:
#	gazebo_ros2_control/src/gazebo_system.cpp
  • Loading branch information
Wiktor-99 authored and mergify[bot] committed Feb 12, 2024
1 parent bc91ec2 commit dd1f542
Showing 1 changed file with 20 additions and 1 deletion.
21 changes: 20 additions & 1 deletion gazebo_ros2_control/src/gazebo_system.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -205,14 +205,33 @@ void GazeboSystem::registerJoints(

RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tState:");

auto get_initial_value = [this](const hardware_interface::InterfaceInfo & interface_info) {
auto get_initial_value =
[this, joint_name](const hardware_interface::InterfaceInfo & interface_info) {
double initial_value{0.0};
if (!interface_info.initial_value.empty()) {
<<<<<<< HEAD
double value = std::stod(interface_info.initial_value);
RCLCPP_INFO(this->nh_->get_logger(), "\t\t\t found initial value: %f", value);
return value;
} else {
return 0.0;
=======
try {
initial_value = hardware_interface::stod(interface_info.initial_value);
RCLCPP_INFO(this->nh_->get_logger(), "\t\t\t found initial value: %f", initial_value);
} catch (std::invalid_argument &) {
RCLCPP_ERROR_STREAM(
this->nh_->get_logger(),
"Failed converting initial_value string to real number for the joint "
<< joint_name
<< " and state interface " << interface_info.name
<< ". Actual value of parameter: " << interface_info.initial_value
<< ". Initial value will be set to 0.0");
throw std::invalid_argument("Failed converting initial_value string");
}
>>>>>>> cdae6b8 (Fix crashing due to an invalid parameter in the initial value. (#271))
}
return initial_value;
};

double initial_position = std::numeric_limits<double>::quiet_NaN();
Expand Down

0 comments on commit dd1f542

Please sign in to comment.