diff --git a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp index d4a776a0..2008cacd 100644 --- 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 index 6a3b61a6..af3e73be 100644 --- 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_); }