From 76c9843cb8e962c07d662adb3f5a4b9ba4a30a19 Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Thu, 28 Mar 2024 17:05:58 +0100 Subject: [PATCH 1/2] Fix #259 - `ParameterAlreadyDeclaredException` for parameter `position_proportional_gain` (#261) (cherry picked from commit 2526653e29d6e35b87a2a0409fd9a881ba944cf0) --- gz_ros2_control/src/gz_system.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/gz_ros2_control/src/gz_system.cpp b/gz_ros2_control/src/gz_system.cpp index eb07c6db..a886cc7d 100644 --- a/gz_ros2_control/src/gz_system.cpp +++ b/gz_ros2_control/src/gz_system.cpp @@ -205,8 +205,13 @@ bool GazeboSimSystem::initSim( constexpr double default_gain = 0.1; - this->dataPtr->position_proportional_gain_ = this->nh_->declare_parameter( - "position_proportional_gain", default_gain); + try { + this->dataPtr->position_proportional_gain_ = this->nh_->declare_parameter( + "position_proportional_gain", default_gain); + } catch (rclcpp::exceptions::ParameterAlreadyDeclaredException & ex) { + this->nh_->get_parameter( + "position_proportional_gain", this->dataPtr->position_proportional_gain_); + } RCLCPP_INFO_STREAM( this->nh_->get_logger(), From 036d33b9bd9380b74f260efbbfb0f2b355d6f52b Mon Sep 17 00:00:00 2001 From: Alejandro Hernandez Cordero Date: Thu, 28 Mar 2024 23:10:40 +0100 Subject: [PATCH 2/2] make linters happy Signed-off-by: Alejandro Hernandez Cordero --- gz_ros2_control/src/gz_system.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gz_ros2_control/src/gz_system.cpp b/gz_ros2_control/src/gz_system.cpp index a886cc7d..4ef5b28d 100644 --- a/gz_ros2_control/src/gz_system.cpp +++ b/gz_ros2_control/src/gz_system.cpp @@ -208,7 +208,7 @@ bool GazeboSimSystem::initSim( try { this->dataPtr->position_proportional_gain_ = this->nh_->declare_parameter( "position_proportional_gain", default_gain); - } catch (rclcpp::exceptions::ParameterAlreadyDeclaredException & ex) { + } catch (rclcpp::exceptions::ParameterAlreadyDeclaredException & ex) { this->nh_->get_parameter( "position_proportional_gain", this->dataPtr->position_proportional_gain_); }