Add rclcpp exceptions

ros-controls · Jan 1, 2024 · 45a601a · 45a601a
1 parent 962ee62
commit 45a601a
Show file tree

Hide file tree

Showing 2 changed files with 29 additions and 5 deletions.
diff --git a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp
@@ -314,8 +314,20 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element
       robot_hw_sim_type_str_.c_str());
     try {
       node_ros2->declare_parameter("hold_joints", rclcpp::ParameterValue(hold_joints));
-    } catch (const std::exception & e) {
-      RCLCPP_ERROR(impl_->model_nh_->get_logger(), "%s", e.what());
+    } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException & e) {
+      RCLCPP_ERROR(
+        impl_->model_nh_->get_logger(), "Parameter 'hold_joints' has already been declared, %s",
+        e.what());
+    } catch (const rclcpp::exceptions::InvalidParametersException & e) {
+      RCLCPP_ERROR(
+        impl_->model_nh_->get_logger(), "Parameter 'hold_joints' has invalid name, %s", e.what());
+    } catch (const rclcpp::exceptions::InvalidParameterValueException & e) {
+      RCLCPP_ERROR(
+        impl_->model_nh_->get_logger(), "Parameter 'hold_joints' value is invalid, %s", e.what());
+    } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) {
+      RCLCPP_ERROR(
+        impl_->model_nh_->get_logger(), "Parameter 'hold_joints' value has wrong type, %s",
+        e.what());
     }
     if (!gazeboSystem->initSim(
         node_ros2,

diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp
@@ -133,10 +133,22 @@ bool GazeboSystem::initSim(
 
   try {
     this->dataPtr->hold_joints_ = this->nh_->get_parameter("hold_joints").as_bool();
-  } catch (const std::exception & e) {
-    RCLCPP_WARN(
+  } catch (rclcpp::exceptions::ParameterUninitializedException & ex) {
+    RCLCPP_ERROR(
       this->nh_->get_logger(),
-      "Parameter 'hold_joints' not found, with error %s", e.what());
+      "Parameter 'hold_joints' not initialized, with error %s", ex.what());
+    RCLCPP_WARN_STREAM(
+      this->nh_->get_logger(), "Using default value: " << this->dataPtr->hold_joints_);
+  } catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) {
+    RCLCPP_ERROR(
+      this->nh_->get_logger(),
+      "Parameter 'hold_joints' not declared, with error %s", ex.what());
+    RCLCPP_WARN_STREAM(
+      this->nh_->get_logger(), "Using default value: " << this->dataPtr->hold_joints_);
+  } catch (rclcpp::ParameterTypeException & ex) {
+    RCLCPP_ERROR(
+      this->nh_->get_logger(),
+      "Parameter 'hold_joints' has wrong type: %s", ex.what());
     RCLCPP_WARN_STREAM(
       this->nh_->get_logger(), "Using default value: " << this->dataPtr->hold_joints_);
   }