proof of concept

fzi-forschungszentrum-informatik · May 26, 2023 · bd548c1 · bd548c1
1 parent 1239413
commit bd548c1
Show file tree

Hide file tree

Showing 3 changed files with 6 additions and 3 deletions.
diff --git a/cartesian_compliance_controller/src/cartesian_compliance_controller.cpp b/cartesian_compliance_controller/src/cartesian_compliance_controller.cpp
@@ -119,6 +119,8 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cartes
 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianComplianceController::on_activate(
     const rclcpp_lifecycle::State & previous_state)
 {
+
+  on_configure(previous_state);
   // Base::on_activation(..) will get called twice,
   // but that's fine.
   using TYPE = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;

diff --git a/cartesian_controller_base/src/ForwardDynamicsSolver.cpp b/cartesian_controller_base/src/ForwardDynamicsSolver.cpp
@@ -147,7 +147,8 @@ namespace cartesian_controller_base{
     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);
+    if(!nh->has_parameter(m_params + "/link_mass"))
+      m_min = nh->declare_parameter<double>(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", m_number_joints);

diff --git a/cartesian_controller_base/src/cartesian_controller_base.cpp b/cartesian_controller_base/src/cartesian_controller_base.cpp
@@ -135,10 +135,10 @@ controller_interface::return_type CartesianControllerBase::init(const std::strin
 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianControllerBase::on_configure(
     const rclcpp_lifecycle::State & previous_state)
 {
-  if (m_configured)
+  /*if (m_configured)
   {
     return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
-  }
+  }*/
 
   // Load user specified inverse kinematics solver
   std::string ik_solver = get_node()->get_parameter("ik_solver").as_string();