From b0634ea082bf25afe6b58ef69603016e19b3151b Mon Sep 17 00:00:00 2001 From: "keshav29kapur@gmail.com" Date: Fri, 9 Jun 2023 11:20:09 +0530 Subject: [PATCH] Added Registration for Command Interface #1 --- .../config/cartpole_controller_effort.yaml | 2 +- .../chained_controller.hpp | 4 ++-- .../chaining_controller/effort_controller.hpp | 1 + chaining_controller/src/effort_controller.cpp | 20 +++++++++++++++++++ 4 files changed, 24 insertions(+), 3 deletions(-) diff --git a/chaining_controller/config/cartpole_controller_effort.yaml b/chaining_controller/config/cartpole_controller_effort.yaml index 45abb72..973f222 100644 --- 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 index e71b5a1..9b89ada 100644 --- 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 & effort); diff --git a/chaining_controller/include/chaining_controller/effort_controller.hpp b/chaining_controller/include/chaining_controller/effort_controller.hpp index e5a1f6e..1336106 100644 --- 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 &); + void registerJointCommand(const std::vector &, const std::vector &); int n_joints_; std::vector joint_names_; diff --git a/chaining_controller/src/effort_controller.cpp b/chaining_controller/src/effort_controller.cpp index 95a1b56..1f7f93c 100644 --- 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 & joint_names, const std::vector & 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)