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

…ort ros-controls#271) (ros-controls#282)

* Fix crashing due to an invalid parameter in the initial value. (ros-controls#271)

Co-authored-by: Alejandro Hernández Cordero <[email protected]>
(cherry picked from commit cdae6b8)

# Conflicts:
#	gazebo_ros2_control/src/gazebo_system.cpp

* Fixed merge

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]>

gazebo_ros2_control/src/gazebo_system.cpp

@@ -249,14 +249,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();