From 198cc6ba73a55f266469fe6483d7105691e5c882 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 13 Feb 2024 09:45:03 +0100 Subject: [PATCH] Fix crashing due to an invalid parameter in the initial value. (backport #271) (#282) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Fix crashing due to an invalid parameter in the initial value. (#271) Co-authored-by: Alejandro Hernández Cordero (cherry picked from commit cdae6b8a8f638d87146482e99bf76cf36530e5a6) # Conflicts: # gazebo_ros2_control/src/gazebo_system.cpp * Fixed merge Signed-off-by: Alejandro Hernandez Cordero --------- Signed-off-by: Alejandro Hernandez Cordero Co-authored-by: Wiktor Bajor <69388767+Wiktor-99@users.noreply.github.com> Co-authored-by: Alejandro Hernandez Cordero --- gazebo_ros2_control/src/gazebo_system.cpp | 23 +++++++++++++++++------ 1 file changed, 17 insertions(+), 6 deletions(-) diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp index a9292bd9..2b76adc9 100644 --- a/gazebo_ros2_control/src/gazebo_system.cpp +++ b/gazebo_ros2_control/src/gazebo_system.cpp @@ -205,14 +205,25 @@ 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()) { - 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 = std::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"); + } } + return initial_value; }; double initial_position = std::numeric_limits::quiet_NaN();