Added Registration for Command Interface #1

ChiratheRobotics · Jun 9, 2023 · b0634ea · b0634ea
1 parent bdfaf08
commit b0634ea
Show file tree

Hide file tree

Showing 4 changed files with 24 additions and 3 deletions.
diff --git a/chaining_controller/config/cartpole_controller_effort.yaml b/chaining_controller/config/cartpole_controller_effort.yaml
@@ -1,6 +1,6 @@
 controller_manager:
   ros__parameters:
-    update_rate: 100  # Hz
+    update_rate: 1  # Hz
 
     effort_controller_upper:
       type: chaining_controller/EffortController

diff --git a/chaining_controller/include/chaining_controller/chained_controller.hpp b/chaining_controller/include/chaining_controller/chained_controller.hpp
@@ -34,8 +34,8 @@ class ChainedController : public controller_interface::ChainableControllerInterf
   bool update();
 
 private:
-  double EFFORT_MAX = 15;
-  double EFFORT_MIN = 5;
+  double EFFORT_MAX = 1000;
+  double EFFORT_MIN = -1000;
 
   void registerJointCommand(const std::vector<double> & effort);
 

diff --git a/chaining_controller/include/chaining_controller/effort_controller.hpp b/chaining_controller/include/chaining_controller/effort_controller.hpp
@@ -30,6 +30,7 @@ class EffortController : public controller_interface::ControllerInterface
 
 private:
   void registerJointFeedback(const std::vector<std::string> &);
+  void registerJointCommand(const std::vector<std::string> &, const std::vector<double> &);
 
   int n_joints_;
   std::vector<std::string> joint_names_;

diff --git a/chaining_controller/src/effort_controller.cpp b/chaining_controller/src/effort_controller.cpp
@@ -64,6 +64,7 @@ controller_interface::return_type chaining_controller::EffortController::update(
   const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
 {
   registerJointFeedback(joint_names_);
+  registerJointCommand(joint_names_, joint_effort_);
   return controller_interface::return_type::OK;
 }
 
@@ -102,6 +103,25 @@ void chaining_controller::EffortController::registerJointFeedback(
   }
 }
 
+void chaining_controller::EffortController::registerJointCommand(
+  const std::vector<std::string> & joint_names, const std::vector<double> & joint_efforts)
+{
+  for (int i = 0; i < n_joints_; i++)
+  {
+    std::string joint_name(joint_names[i]);
+
+    auto effort_command = std::find_if(
+      command_interfaces_.begin(), command_interfaces_.end(),
+      [&joint_name](const hardware_interface::LoanedCommandInterface & interface)
+      {
+        return interface.get_prefix_name() == "chained_controller" &&
+               interface.get_interface_name() == hardware_interface::HW_IF_EFFORT;
+      });
+
+    effort_command->set_value(joint_efforts[i]);
+  }
+}
+
 #include "pluginlib/class_list_macros.hpp"
 PLUGINLIB_EXPORT_CLASS(
   chaining_controller::EffortController, controller_interface::ControllerInterface)