Added feature to remove position, velocity and effort refeence if needed

KNR-PW · Dec 1, 2024 · 23d2c5d · 23d2c5d
1 parent 6338a1b
commit 23d2c5d
Show file tree

Hide file tree

Showing 3 changed files with 49 additions and 8 deletions.
diff --git a/joint_controller_ros2_control/include/joint_controller_ros2_control/joint_controller.hpp b/joint_controller_ros2_control/include/joint_controller_ros2_control/joint_controller.hpp
@@ -110,7 +110,12 @@ namespace joint_controller
     std::vector<JointCommands> joint_commands_;
     std::vector<JointStates> joint_states_;
 
-    /* For checking if kp and kd interfaces are used */
+    /* For checking if velocity and feedforward effort reference interfaces are used */
+    bool has_position_interface_ = false;
+    bool has_velocity_interface_ = false;
+    bool has_feedforward_effort_interface_ = false;
+
+    /* For checking if kp and kd reference interfaces are used */
     bool has_kp_scale_interface_ = false;
     bool has_kd_scale_interface_ = false;
 

diff --git a/joint_controller_ros2_control/src/joint_controller.cpp b/joint_controller_ros2_control/src/joint_controller.cpp
@@ -153,9 +153,9 @@ controller_interface::return_type JointController::update_reference_from_subscri
                 && (!std::isnan(joint_reference_msg.feedforward_effort[index])))
         {
             JointCommands& joint_command = joint_commands_[index];
-            joint_command.desired_position_ = joint_reference_msg.desired_position[i];
-            joint_command.desired_velocity_ = joint_reference_msg.desired_velocity[i];
-            joint_command.feedforward_effort_ = joint_reference_msg.feedforward_effort[i];
+            if(has_position_interface_) joint_command.desired_position_ = joint_reference_msg.desired_position[i];
+            if(has_velocity_interface_) joint_command.desired_velocity_ = joint_reference_msg.desired_velocity[i];
+            if(has_feedforward_effort_interface_) joint_command.feedforward_effort_ = joint_reference_msg.feedforward_effort[i];
             if(has_kp_scale_interface_) joint_command.kp_scale_ = joint_reference_msg.kp_scale[i];
             if(has_kd_scale_interface_) joint_command.kd_scale_ = joint_reference_msg.kd_scale[i];
         }
@@ -191,9 +191,9 @@ controller_interface::return_type JointController::update_reference_from_subscri
                 && (!std::isnan(joint_reference_msg.feedforward_effort[index])))
         {
             JointCommands& joint_command = joint_commands_[index];
-            joint_command.desired_position_ = joint_reference_msg.desired_position[i];
-            joint_command.desired_velocity_ = joint_reference_msg.desired_velocity[i];
-            joint_command.feedforward_effort_ = joint_reference_msg.feedforward_effort[i];
+            if(has_position_interface_) joint_command.desired_position_ = joint_reference_msg.desired_position[i];
+            if(has_velocity_interface_) joint_command.desired_velocity_ = joint_reference_msg.desired_velocity[i];
+            if(has_feedforward_effort_interface_) joint_command.feedforward_effort_ = joint_reference_msg.feedforward_effort[i];
             if(has_kp_scale_interface_) joint_command.kp_scale_ = joint_reference_msg.kp_scale[i];
             if(has_kd_scale_interface_) joint_command.kd_scale_ = joint_reference_msg.kd_scale[i];
         }
@@ -243,6 +243,42 @@ controller_interface::CallbackReturn JointController::configure_joints()
         return CallbackReturn::FAILURE;
     }
 
+    if(std::find(params_.reference_interfaces.begin(), params_.reference_interfaces.end(), hardware_interface::HW_IF_POSITION) != params_.reference_interfaces.end())
+    {
+        has_position_interface_ = true;
+    }
+    else
+    {
+        for(auto& joint_command : joint_commands_)
+        {
+            joint_command.desired_position_= 0.0;
+        }
+    }
+
+    if(std::find(params_.reference_interfaces.begin(), params_.reference_interfaces.end(), hardware_interface::HW_IF_VELOCITY) != params_.reference_interfaces.end())
+    {
+        has_velocity_interface_ = true;
+    }
+    else
+    {
+        for(auto& joint_command : joint_commands_)
+        {
+            joint_command.desired_velocity_ = 0.0;
+        }
+    }
+
+    if(std::find(params_.reference_interfaces.begin(), params_.reference_interfaces.end(), hardware_interface::HW_IF_EFFORT) != params_.reference_interfaces.end())
+    {
+        has_feedforward_effort_interface_ = true;
+    }
+    else
+    {
+        for(auto& joint_command : joint_commands_)
+        {
+            joint_command.feedforward_effort_ = 0.0;
+        }
+    }
+
     if(std::find(params_.reference_interfaces.begin(), params_.reference_interfaces.end(), "kp_scale") != params_.reference_interfaces.end())
     {
         has_kp_scale_interface_ = true;

diff --git a/joint_controller_ros2_control/src/joint_controller_parameters.yaml b/joint_controller_ros2_control/src/joint_controller_parameters.yaml
@@ -67,7 +67,7 @@ joint_controller:
     read_only: true,
     validation: {
       not_empty<>: null,
-      size_gt<>: 2,
+      size_gt<>: 0,
       size_lt<>: 6,
       subset_of<>: [["position", "velocity", "effort", "kp_scale", "kd_scale"]],
     }