fix: adapts IKSolver params names to ROS2 convention (. instead of …

…`/`) so that they can be correctly set from yaml params file
fzi-forschungszentrum-informatik · Oct 30, 2024 · 7b19e3b · 7b19e3b
1 parent 2a0268a
commit 7b19e3b
Show file tree

Hide file tree

Showing 6 changed files with 26 additions and 8 deletions.
diff --git a/cartesian_controller_base/include/cartesian_controller_base/DampedLeastSquaresSolver.h b/cartesian_controller_base/include/cartesian_controller_base/DampedLeastSquaresSolver.h
@@ -103,8 +103,7 @@ class DampedLeastSquaresSolver : public IKSolver
   KDL::Jacobian m_jnt_jacobian;
 
   // Dynamic parameters
-  std::shared_ptr<rclcpp::Node> m_handle;  ///< handle for dynamic parameter interaction
-  const std::string m_params = "solver/damped_least_squares";  ///< namespace for parameter access
+  const std::string m_params = "solver.damped_least_squares";  ///< namespace for parameter access
   double m_alpha;                                              ///< damping coefficient
 };
 

diff --git a/cartesian_controller_base/include/cartesian_controller_base/ForwardDynamicsSolver.h b/cartesian_controller_base/include/cartesian_controller_base/ForwardDynamicsSolver.h
@@ -119,8 +119,7 @@ class ForwardDynamicsSolver : public IKSolver
   KDL::JntSpaceInertiaMatrix m_jnt_space_inertia;
 
   // Dynamic parameters
-  std::shared_ptr<rclcpp::Node> m_handle;  ///< handle for dynamic parameter interaction
-  const std::string m_params = "solver/forward_dynamics";  ///< namespace for parameter access
+  const std::string m_params = "solver.forward_dynamics";  ///< namespace for parameter access
 
   /**
      * Virtual link mass

diff --git a/cartesian_controller_base/include/cartesian_controller_base/IKSolver.h b/cartesian_controller_base/include/cartesian_controller_base/IKSolver.h
@@ -170,6 +170,25 @@ class IKSolver
      */
   void applyJointLimits();
 
+  template <typename ParameterT>
+  auto auto_declare(const std::string & name, const ParameterT & default_value)
+  {
+    if (!m_handle->has_parameter(name))
+    {
+      return m_handle->declare_parameter<ParameterT>(name, default_value);
+    }
+    else
+    {
+      return m_handle->get_parameter(name).get_value<ParameterT>();
+    }
+  }
+
+#if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON
+  std::shared_ptr<rclcpp_lifecycle::LifecycleNode> m_handle;
+#else
+  std::shared_ptr<rclcpp::Node> m_handle;
+#endif
+
   //! The underlying physical system
   KDL::Chain m_chain;
 

diff --git a/cartesian_controller_base/src/DampedLeastSquaresSolver.cpp b/cartesian_controller_base/src/DampedLeastSquaresSolver.cpp
@@ -79,7 +79,7 @@ trajectory_msgs::msg::JointTrajectoryPoint DampedLeastSquaresSolver::getJointCon
   // \f$ \dot{q} = ( J^T J + \alpha^2 I )^{-1} J^T f \f$
   ctrl::MatrixND identity;
   identity.setIdentity(m_number_joints, m_number_joints);
-  m_handle->get_parameter(m_params + "/alpha", m_alpha);
+  m_handle->get_parameter(m_params + ".alpha", m_alpha);
 
   m_current_velocities.data =
     (m_jnt_jacobian.data.transpose() * m_jnt_jacobian.data + m_alpha * m_alpha * identity)
@@ -122,7 +122,7 @@ bool DampedLeastSquaresSolver::init(std::shared_ptr<rclcpp_lifecycle::LifecycleN
   m_jnt_jacobian_solver.reset(new KDL::ChainJntToJacSolver(m_chain));
   m_jnt_jacobian.resize(m_number_joints);
 
-  nh->declare_parameter<double>(m_params + "/alpha", 1.0);
+  auto_declare(m_params + ".alpha", 1.0);
 
   return true;
 }

diff --git a/cartesian_controller_base/src/ForwardDynamicsSolver.cpp b/cartesian_controller_base/src/ForwardDynamicsSolver.cpp
@@ -136,7 +136,7 @@ bool ForwardDynamicsSolver::init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode
   m_jnt_space_inertia.resize(m_number_joints);
 
   // Set the initial value if provided at runtime, else use default value.
-  m_min = nh->declare_parameter<double>(m_params + "/link_mass", 0.1);
+  m_min = auto_declare(m_params + ".link_mass", 0.1);
 
   RCLCPP_INFO(nh->get_logger(), "Forward dynamics solver initialized");
   RCLCPP_INFO(nh->get_logger(), "Forward dynamics solver has control over %i joints",

diff --git a/cartesian_controller_base/src/IKSolver.cpp b/cartesian_controller_base/src/IKSolver.cpp
@@ -102,11 +102,12 @@ void IKSolver::synchronizeJointPositions(
   }
 }
 
-bool IKSolver::init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> /*nh*/,
+bool IKSolver::init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> nh,
                     const KDL::Chain & chain, const KDL::JntArray & upper_pos_limits,
                     const KDL::JntArray & lower_pos_limits)
 {
   // Initialize
+  m_handle = nh;
   m_chain = chain;
   m_number_joints = m_chain.getNrOfJoints();
   m_current_positions.data = ctrl::VectorND::Zero(m_number_joints);