From b88fd25a1cacf2eb243e2d87e64d30a125a79bdd Mon Sep 17 00:00:00 2001 From: Your Name Date: Sun, 7 Apr 2024 11:20:12 +0200 Subject: [PATCH 1/4] Fix call to state interface even in open_loop --- src/omnidirectional_controller.cpp | 61 +++++++++++++++++++----------- 1 file changed, 38 insertions(+), 23 deletions(-) diff --git a/src/omnidirectional_controller.cpp b/src/omnidirectional_controller.cpp index 3059c88..c3d7b6f 100644 --- a/src/omnidirectional_controller.cpp +++ b/src/omnidirectional_controller.cpp @@ -97,11 +97,16 @@ InterfaceConfiguration OmnidirectionalController::command_interface_configuratio } InterfaceConfiguration OmnidirectionalController::state_interface_configuration() const { - std::vector conf_names; - for (const auto & joint_name : wheel_names_) { - conf_names.push_back(joint_name + "/" + HW_IF_VELOCITY); + if (odom_params_.open_loop) { //return an empty configuration if "open loop" mode is activated. + return {interface_configuration_type::NONE, {}}; + } + else { + std::vector conf_names; + for (const auto & joint_name : wheel_names_) { + conf_names.push_back(joint_name + "/" + HW_IF_VELOCITY); + } + return {interface_configuration_type::INDIVIDUAL, conf_names}; } - return {interface_configuration_type::INDIVIDUAL, conf_names}; } CallbackReturn OmnidirectionalController::on_configure( @@ -213,13 +218,10 @@ CallbackReturn OmnidirectionalController::on_configure( return CallbackReturn::SUCCESS; } -CallbackReturn OmnidirectionalController::on_activate( - const rclcpp_lifecycle::State & previous_state) { +CallbackReturn OmnidirectionalController::on_activate(const rclcpp_lifecycle::State & previous_state) { auto logger = node_->get_logger(); - RCLCPP_DEBUG(logger, - "Called on_activate. Previous state was %s", - previous_state.label().c_str()); + RCLCPP_DEBUG(logger, "Called on_activate. Previous state was %s", previous_state.label().c_str()); if (wheel_names_.empty()) { RCLCPP_ERROR(logger, "No wheel names specified"); @@ -229,34 +231,47 @@ CallbackReturn OmnidirectionalController::on_activate( // register handles registered_wheel_handles_.reserve(wheel_names_.size()); for (const auto & wheel_name : wheel_names_) { - std::string interface_name = wheel_name + "/" + HW_IF_VELOCITY; - const auto state_handle = std::find_if(state_interfaces_.cbegin(), state_interfaces_.cend(), - [&interface_name](const auto & interface) { - return interface.get_name() == interface_name; - }); + const hardware_interface::LoanedStateInterface* state_handle = nullptr; - if (state_handle == state_interfaces_.cend()) { - RCLCPP_ERROR(logger, "Unable to obtain joint state handle for %s", wheel_name.c_str()); - return CallbackReturn::ERROR; + if (!odom_params_.open_loop) { + std::string interface_name = wheel_name + "/" + HW_IF_VELOCITY; + + const auto state_handle_iterator = std::find_if(state_interfaces_.cbegin(), state_interfaces_.cend(), + [&interface_name](const auto & interface) { + return interface.get_name() == interface_name; + }); + + if (state_handle_iterator == state_interfaces_.cend()) { + RCLCPP_ERROR(logger, "Unable to obtain joint state handle for %s", wheel_name.c_str()); + return CallbackReturn::ERROR; + } + + // store a pointer to the state interface found + state_handle = &(*state_handle_iterator); } - const auto command_handle = std::find_if( + const auto& state_handle_ref = *state_handle; //store a constant reference to the interface found + + const auto command_handle_iterator = std::find_if( command_interfaces_.begin(), command_interfaces_.end(), - [&interface_name](const auto & interface) { + [&wheel_name](const auto & interface) { + std::string interface_name = wheel_name + "/" + HW_IF_VELOCITY; return interface.get_name() == interface_name; }); - if (command_handle == command_interfaces_.end()) { + if (command_handle_iterator == command_interfaces_.end()) { RCLCPP_ERROR(logger, "Unable to obtain joint command handle for %s", wheel_name.c_str()); return CallbackReturn::ERROR; } registered_wheel_handles_.emplace_back( - WheelHandle{std::ref(*state_handle), std::ref(*command_handle)}); + WheelHandle{state_handle_ref, std::ref(*command_handle_iterator)}); - RCLCPP_INFO(logger, "Got command interface: %s", command_handle->get_name().c_str()); - RCLCPP_INFO(logger, "Got state interface: %s", state_handle->get_name().c_str()); + RCLCPP_INFO(logger, "Got command interface: %s", command_handle_iterator->get_name().c_str()); + if (!odom_params_.open_loop) { + RCLCPP_INFO(logger, "Got state interface: %s", state_handle->get_name().c_str()); + } } subscriber_is_active_ = true; From f1a5526b6254a8c4a271460aed7c7fa07ee8d3f1 Mon Sep 17 00:00:00 2001 From: Your Name Date: Sun, 7 Apr 2024 19:55:20 +0200 Subject: [PATCH 2/4] Update unecessary 'else' condition in state_interface_configuration --- src/omnidirectional_controller.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/omnidirectional_controller.cpp b/src/omnidirectional_controller.cpp index c3d7b6f..36f38e3 100644 --- a/src/omnidirectional_controller.cpp +++ b/src/omnidirectional_controller.cpp @@ -100,13 +100,11 @@ InterfaceConfiguration OmnidirectionalController::state_interface_configuration( if (odom_params_.open_loop) { //return an empty configuration if "open loop" mode is activated. return {interface_configuration_type::NONE, {}}; } - else { - std::vector conf_names; - for (const auto & joint_name : wheel_names_) { - conf_names.push_back(joint_name + "/" + HW_IF_VELOCITY); - } - return {interface_configuration_type::INDIVIDUAL, conf_names}; + std::vector conf_names; + for (const auto & joint_name : wheel_names_) { + conf_names.push_back(joint_name + "/" + HW_IF_VELOCITY); } + return {interface_configuration_type::INDIVIDUAL, conf_names}; } CallbackReturn OmnidirectionalController::on_configure( From 289a477236b7221260a16e455d3bdd1a95c90a86 Mon Sep 17 00:00:00 2001 From: Your Name Date: Mon, 8 Apr 2024 21:11:21 +0200 Subject: [PATCH 3/4] Update without WheelHandle struct --- .../omnidirectional_controller.hpp | 16 ++++-- src/omnidirectional_controller.cpp | 57 +++++++++---------- 2 files changed, 39 insertions(+), 34 deletions(-) diff --git a/include/omnidirectional_controllers/omnidirectional_controller.hpp b/include/omnidirectional_controllers/omnidirectional_controller.hpp index 5744182..dd5efe3 100644 --- a/include/omnidirectional_controllers/omnidirectional_controller.hpp +++ b/include/omnidirectional_controllers/omnidirectional_controller.hpp @@ -41,6 +41,8 @@ namespace omnidirectional_controllers { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +using StateInterfaceReferenceWrapper = std::reference_wrapper; +using CommandInterfaceReferenceWrapper = std::reference_wrapper; class OmnidirectionalController : public controller_interface::ControllerInterface { public: @@ -58,13 +60,17 @@ class OmnidirectionalController : public controller_interface::ControllerInterfa ~OmnidirectionalController(); protected: - struct WheelHandle { - std::reference_wrapper velocity_state; - std::reference_wrapper velocity_command; - }; std::vector wheel_names_; - std::vector registered_wheel_handles_; + std::vector registered_wheel_state_ifs_; + std::vector registered_wheel_cmd_ifs_; + // struct WheelHandle { + // std::reference_wrapper velocity_state; + // std::reference_wrapper velocity_command; + // }; + + // std::vector wheel_names_; + // std::vector registered_wheel_handles_; // Default parameters for axebot RobotParams robot_params_{0.1, 0.0505, 0.0}; diff --git a/src/omnidirectional_controller.cpp b/src/omnidirectional_controller.cpp index 36f38e3..ac72576 100644 --- a/src/omnidirectional_controller.cpp +++ b/src/omnidirectional_controller.cpp @@ -100,11 +100,13 @@ InterfaceConfiguration OmnidirectionalController::state_interface_configuration( if (odom_params_.open_loop) { //return an empty configuration if "open loop" mode is activated. return {interface_configuration_type::NONE, {}}; } - std::vector conf_names; - for (const auto & joint_name : wheel_names_) { - conf_names.push_back(joint_name + "/" + HW_IF_VELOCITY); + else { + std::vector conf_names; + for (const auto & joint_name : wheel_names_) { + conf_names.push_back(joint_name + "/" + HW_IF_VELOCITY); + } + return {interface_configuration_type::INDIVIDUAL, conf_names}; } - return {interface_configuration_type::INDIVIDUAL, conf_names}; } CallbackReturn OmnidirectionalController::on_configure( @@ -227,49 +229,45 @@ CallbackReturn OmnidirectionalController::on_activate(const rclcpp_lifecycle::St } // register handles - registered_wheel_handles_.reserve(wheel_names_.size()); + registered_wheel_state_ifs_.reserve(wheel_names_.size()); + registered_wheel_cmd_ifs_.reserve(wheel_names_.size()); for (const auto & wheel_name : wheel_names_) { - const hardware_interface::LoanedStateInterface* state_handle = nullptr; - if (!odom_params_.open_loop) { std::string interface_name = wheel_name + "/" + HW_IF_VELOCITY; - const auto state_handle_iterator = std::find_if(state_interfaces_.cbegin(), state_interfaces_.cend(), + const auto state_handle = std::find_if(state_interfaces_.cbegin(), state_interfaces_.cend(), [&interface_name](const auto & interface) { return interface.get_name() == interface_name; - }); + } + ); - if (state_handle_iterator == state_interfaces_.cend()) { + if (state_handle == state_interfaces_.cend()) { RCLCPP_ERROR(logger, "Unable to obtain joint state handle for %s", wheel_name.c_str()); return CallbackReturn::ERROR; } - // store a pointer to the state interface found - state_handle = &(*state_handle_iterator); - } + registered_wheel_state_ifs_.emplace_back(*state_handle); - const auto& state_handle_ref = *state_handle; //store a constant reference to the interface found + RCLCPP_INFO(logger, "Got state interface: %s", state_handle->get_name().c_str()); + } - const auto command_handle_iterator = std::find_if( + const auto command_handle = std::find_if( command_interfaces_.begin(), command_interfaces_.end(), [&wheel_name](const auto & interface) { std::string interface_name = wheel_name + "/" + HW_IF_VELOCITY; return interface.get_name() == interface_name; - }); + } + ); - if (command_handle_iterator == command_interfaces_.end()) { + if (command_handle == command_interfaces_.end()) { RCLCPP_ERROR(logger, "Unable to obtain joint command handle for %s", wheel_name.c_str()); return CallbackReturn::ERROR; } - registered_wheel_handles_.emplace_back( - WheelHandle{state_handle_ref, std::ref(*command_handle_iterator)}); + registered_wheel_cmd_ifs_.emplace_back(*command_handle); - RCLCPP_INFO(logger, "Got command interface: %s", command_handle_iterator->get_name().c_str()); - if (!odom_params_.open_loop) { - RCLCPP_INFO(logger, "Got state interface: %s", state_handle->get_name().c_str()); - } + RCLCPP_INFO(logger, "Got command interface: %s", command_handle->get_name().c_str()); } subscriber_is_active_ = true; @@ -362,9 +360,9 @@ controller_interface::return_type OmnidirectionalController::update( period.seconds()); } else { std::vector wheels_angular_velocity({0, 0, 0}); - wheels_angular_velocity[0] = registered_wheel_handles_[0].velocity_state.get().get_value(); - wheels_angular_velocity[1] = registered_wheel_handles_[1].velocity_state.get().get_value(); - wheels_angular_velocity[2] = registered_wheel_handles_[2].velocity_state.get().get_value(); + wheels_angular_velocity[0] = registered_wheel_state_ifs_[0].get().get_value(); + wheels_angular_velocity[1] = registered_wheel_state_ifs_[1].get().get_value(); + wheels_angular_velocity[2] = registered_wheel_state_ifs_[2].get().get_value(); try { odometry_.update(wheels_angular_velocity, period.seconds()); } catch(const std::runtime_error& e) { @@ -410,9 +408,10 @@ controller_interface::return_type OmnidirectionalController::update( wheels_angular_velocity = omni_robot_kinematics_.getWheelsAngularVelocities(body_vel_setpoint); // Set wheels velocities: - registered_wheel_handles_[0].velocity_command.get().set_value(wheels_angular_velocity.at(0)); - registered_wheel_handles_[1].velocity_command.get().set_value(wheels_angular_velocity.at(1)); - registered_wheel_handles_[2].velocity_command.get().set_value(wheels_angular_velocity.at(2)); + registered_wheel_cmd_ifs_[0].get().set_value(wheels_angular_velocity.at(0)); + registered_wheel_cmd_ifs_[1].get().set_value(wheels_angular_velocity.at(1)); + registered_wheel_cmd_ifs_[2].get().set_value(wheels_angular_velocity.at(2)); + return controller_interface::return_type::OK; } From 863d46106c1a0790fd8d0c375e66ee5410d78289 Mon Sep 17 00:00:00 2001 From: Your Name Date: Wed, 10 Apr 2024 17:13:31 +0200 Subject: [PATCH 4/4] style: clean code fix: remove unnecessary else --- .../omnidirectional_controller.hpp | 7 ------- src/omnidirectional_controller.cpp | 10 ++++------ 2 files changed, 4 insertions(+), 13 deletions(-) diff --git a/include/omnidirectional_controllers/omnidirectional_controller.hpp b/include/omnidirectional_controllers/omnidirectional_controller.hpp index dd5efe3..7aabd86 100644 --- a/include/omnidirectional_controllers/omnidirectional_controller.hpp +++ b/include/omnidirectional_controllers/omnidirectional_controller.hpp @@ -64,13 +64,6 @@ class OmnidirectionalController : public controller_interface::ControllerInterfa std::vector wheel_names_; std::vector registered_wheel_state_ifs_; std::vector registered_wheel_cmd_ifs_; - // struct WheelHandle { - // std::reference_wrapper velocity_state; - // std::reference_wrapper velocity_command; - // }; - - // std::vector wheel_names_; - // std::vector registered_wheel_handles_; // Default parameters for axebot RobotParams robot_params_{0.1, 0.0505, 0.0}; diff --git a/src/omnidirectional_controller.cpp b/src/omnidirectional_controller.cpp index ac72576..0bdf0a7 100644 --- a/src/omnidirectional_controller.cpp +++ b/src/omnidirectional_controller.cpp @@ -100,13 +100,11 @@ InterfaceConfiguration OmnidirectionalController::state_interface_configuration( if (odom_params_.open_loop) { //return an empty configuration if "open loop" mode is activated. return {interface_configuration_type::NONE, {}}; } - else { - std::vector conf_names; - for (const auto & joint_name : wheel_names_) { - conf_names.push_back(joint_name + "/" + HW_IF_VELOCITY); - } - return {interface_configuration_type::INDIVIDUAL, conf_names}; + std::vector conf_names; + for (const auto & joint_name : wheel_names_) { + conf_names.push_back(joint_name + "/" + HW_IF_VELOCITY); } + return {interface_configuration_type::INDIVIDUAL, conf_names}; } CallbackReturn OmnidirectionalController::on_configure(