Fix crashing due to an invalid parameter in the initial value. (backp…

…ort #271) (#283) * Fix crashing due to an invalid parameter in the initial value. (#271) Co-authored-by: Alejandro Hernández Cordero <[email protected]> (cherry picked from commit cdae6b8) # Conflicts: # gazebo_ros2_control/src/gazebo_system.cpp * Fixed crash Signed-off-by: Alejandro Hernandez Cordero <[email protected]> --------- Signed-off-by: Alejandro Hernandez Cordero <[email protected]> Co-authored-by: Wiktor Bajor <[email protected]> Co-authored-by: Alejandro Hernandez Cordero <[email protected]>
ros-controls · Feb 13, 2024 · f01edf6 · f01edf6
1 parent 0aa891f
commit f01edf6
Showing 1 changed file with 17 additions and 6 deletions.
diff --git 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<double>::quiet_NaN();