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

…`/`) so that they can be correctly set from yaml params file
fzi-forschungszentrum-informatik · May 19, 2024 · ebb4708 · ebb4708
1 parent dc80337
commit ebb4708
Show file tree

Hide file tree

Showing 6 changed files with 27 additions and 9 deletions.
diff --git a/cartesian_controller_base/include/cartesian_controller_base/DampedLeastSquaresSolver.h b/cartesian_controller_base/include/cartesian_controller_base/DampedLeastSquaresSolver.h
@@ -106,8 +106,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
@@ -122,8 +122,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
@@ -174,6 +174,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)
@@ -126,7 +126,7 @@ bool DampedLeastSquaresSolver::init(std::shared_ptr<rclcpp::Node> nh,
   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
@@ -140,7 +140,7 @@ bool ForwardDynamicsSolver::init(std::shared_ptr<rclcpp::Node> nh,
   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
@@ -103,14 +103,15 @@ void IKSolver::synchronizeJointPositions(
 }
 
 #if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON
-bool IKSolver::init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> /*nh*/,
+bool IKSolver::init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> nh,
 #else
-bool IKSolver::init(std::shared_ptr<rclcpp::Node> /*nh*/,
+bool IKSolver::init(std::shared_ptr<rclcpp::Node> nh,
 #endif
                     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);