From c48ab44a02d7a4a275b9569e0b3b0c4403403486 Mon Sep 17 00:00:00 2001
From: Wiktor Bajor <wiktorbajor1@gmail.com>
Date: Wed, 24 Jan 2024 18:49:51 +0100
Subject: [PATCH 1/2] Fix crashing due to an invalid parameter in the initial
 value.

---
 gazebo_ros2_control/src/gazebo_system.cpp | 22 ++++++++++++++++------
 1 file changed, 16 insertions(+), 6 deletions(-)

diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp
index 16cd23d0..7e9b4a8f 100644
--- a/gazebo_ros2_control/src/gazebo_system.cpp
+++ b/gazebo_ros2_control/src/gazebo_system.cpp
@@ -233,14 +233,24 @@ 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 = hardware_interface::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");
+          }
         }
+        return initial_value;
       };
 
     double initial_position = std::numeric_limits<double>::quiet_NaN();

From 48ecb02678b630d43a754ec8cd6e6a9bcb30d51f Mon Sep 17 00:00:00 2001
From: Wiktor Bajor <wiktorbajor1@gmail.com>
Date: Wed, 24 Jan 2024 19:23:41 +0100
Subject: [PATCH 2/2] Re-throw exception after logging issue

---
 gazebo_ros2_control/src/gazebo_system.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp
index 7e9b4a8f..c688541c 100644
--- a/gazebo_ros2_control/src/gazebo_system.cpp
+++ b/gazebo_ros2_control/src/gazebo_system.cpp
@@ -248,6 +248,7 @@ void GazeboSystem::registerJoints(
                 << " 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;