diff --git a/gz_ros2_control/src/gz_system.cpp b/gz_ros2_control/src/gz_system.cpp index 351bfc87..eb07c6db 100644 --- a/gz_ros2_control/src/gz_system.cpp +++ b/gz_ros2_control/src/gz_system.cpp @@ -204,15 +204,14 @@ bool GazeboSimSystem::initSim( this->dataPtr->joints_.resize(this->dataPtr->n_dof_); constexpr double default_gain = 0.1; - if (!this->nh_->get_parameter_or( - "position_proportional_gain", - this->dataPtr->position_proportional_gain_, default_gain)) - { - RCLCPP_WARN_STREAM( - this->nh_->get_logger(), - "The position_proportional_gain parameter was not defined, defaulting to: " << - default_gain); - } + + this->dataPtr->position_proportional_gain_ = this->nh_->declare_parameter( + "position_proportional_gain", default_gain); + + RCLCPP_INFO_STREAM( + this->nh_->get_logger(), + "The position_proportional_gain has been set to: " << + this->dataPtr->position_proportional_gain_); if (this->dataPtr->n_dof_ == 0) { RCLCPP_ERROR_STREAM(this->nh_->get_logger(), "There is no joint available");