From 3b6b0d1568fbbf233995d7ff3b3511a8b959015d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Ga=C3=ABl=20=C3=89corchard?= <gael.ecorchard@cvut.cz>
Date: Fri, 18 Mar 2022 15:52:29 +0100
Subject: [PATCH] [moveit_cpp] Fix double param declaration (#1097)
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Signed-off-by: Gaël Écorchard <gael.ecorchard@cvut.cz>
---
 .../joint_limits/joint_limits_rosparam.hpp    | 50 ++++++++++++-------
 1 file changed, 32 insertions(+), 18 deletions(-)

diff --git a/moveit_planners/pilz_industrial_motion_planner/include/joint_limits/joint_limits_rosparam.hpp b/moveit_planners/pilz_industrial_motion_planner/include/joint_limits/joint_limits_rosparam.hpp
index 672ba60cba..b860fa49cc 100644
--- a/moveit_planners/pilz_industrial_motion_planner/include/joint_limits/joint_limits_rosparam.hpp
+++ b/moveit_planners/pilz_industrial_motion_planner/include/joint_limits/joint_limits_rosparam.hpp
@@ -27,32 +27,46 @@
 #include "joint_limits/joint_limits.hpp"
 #include "rclcpp/rclcpp.hpp"
 
+namespace
+{
+/** declare a parameter if not already declared. */
+template <typename T>
+void declare_parameter(const rclcpp::Node::SharedPtr& node, const std::string& name, T default_value)
+{
+  if (not node->has_parameter(name))
+  {
+    node->declare_parameter<T>(name, default_value);
+  }
+}
+}  // namespace
+
 namespace joint_limits
 {
 inline bool declare_parameters(const std::string& joint_name, const rclcpp::Node::SharedPtr& node,
                                const std::string& param_ns)
 {
   const std::string param_base_name = (param_ns.empty() ? "" : param_ns + ".") + "joint_limits." + joint_name;
+
   try
   {
-    node->declare_parameter<bool>(param_base_name + ".has_position_limits", false);
-    node->declare_parameter<double>(param_base_name + ".min_position", std::numeric_limits<double>::quiet_NaN());
-    node->declare_parameter<double>(param_base_name + ".max_position", std::numeric_limits<double>::quiet_NaN());
-    node->declare_parameter<bool>(param_base_name + ".has_velocity_limits", false);
-    node->declare_parameter<double>(param_base_name + ".min_velocity", std::numeric_limits<double>::quiet_NaN());
-    node->declare_parameter<double>(param_base_name + ".max_velocity", std::numeric_limits<double>::quiet_NaN());
-    node->declare_parameter<bool>(param_base_name + ".has_acceleration_limits", false);
-    node->declare_parameter<double>(param_base_name + ".max_acceleration", std::numeric_limits<double>::quiet_NaN());
-    node->declare_parameter<bool>(param_base_name + ".has_jerk_limits", false);
-    node->declare_parameter<double>(param_base_name + ".max_jerk", std::numeric_limits<double>::quiet_NaN());
-    node->declare_parameter<bool>(param_base_name + ".has_effort_limits", false);
-    node->declare_parameter<double>(param_base_name + ".max_effort", std::numeric_limits<double>::quiet_NaN());
-    node->declare_parameter<bool>(param_base_name + ".angle_wraparound", false);
-    node->declare_parameter<bool>(param_base_name + ".has_soft_limits", false);
-    node->declare_parameter<double>(param_base_name + ".k_position", std::numeric_limits<double>::quiet_NaN());
-    node->declare_parameter<double>(param_base_name + ".k_velocity", std::numeric_limits<double>::quiet_NaN());
-    node->declare_parameter<double>(param_base_name + ".soft_lower_limit", std::numeric_limits<double>::quiet_NaN());
-    node->declare_parameter<double>(param_base_name + ".soft_upper_limit", std::numeric_limits<double>::quiet_NaN());
+    declare_parameter(node, param_base_name + ".has_position_limits", false);
+    declare_parameter(node, param_base_name + ".min_position", std::numeric_limits<double>::quiet_NaN());
+    declare_parameter(node, param_base_name + ".max_position", std::numeric_limits<double>::quiet_NaN());
+    declare_parameter(node, param_base_name + ".has_velocity_limits", false);
+    declare_parameter(node, param_base_name + ".min_velocity", std::numeric_limits<double>::quiet_NaN());
+    declare_parameter(node, param_base_name + ".max_velocity", std::numeric_limits<double>::quiet_NaN());
+    declare_parameter(node, param_base_name + ".has_acceleration_limits", false);
+    declare_parameter(node, param_base_name + ".max_acceleration", std::numeric_limits<double>::quiet_NaN());
+    declare_parameter(node, param_base_name + ".has_jerk_limits", false);
+    declare_parameter(node, param_base_name + ".max_jerk", std::numeric_limits<double>::quiet_NaN());
+    declare_parameter(node, param_base_name + ".has_effort_limits", false);
+    declare_parameter(node, param_base_name + ".max_effort", std::numeric_limits<double>::quiet_NaN());
+    declare_parameter(node, param_base_name + ".angle_wraparound", false);
+    declare_parameter(node, param_base_name + ".has_soft_limits", false);
+    declare_parameter(node, param_base_name + ".k_position", std::numeric_limits<double>::quiet_NaN());
+    declare_parameter(node, param_base_name + ".k_velocity", std::numeric_limits<double>::quiet_NaN());
+    declare_parameter(node, param_base_name + ".soft_lower_limit", std::numeric_limits<double>::quiet_NaN());
+    declare_parameter(node, param_base_name + ".soft_upper_limit", std::numeric_limits<double>::quiet_NaN());
   }
   catch (const std::exception& ex)
   {