diff --git a/include/omnidirectional_controllers/omnidirectional_controller.hpp b/include/omnidirectional_controllers/omnidirectional_controller.hpp index 5744182..7aabd86 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,10 @@ 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_; // 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 3059c88..0bdf0a7 100644 --- a/src/omnidirectional_controller.cpp +++ b/src/omnidirectional_controller.cpp @@ -97,6 +97,9 @@ InterfaceConfiguration OmnidirectionalController::command_interface_configuratio } InterfaceConfiguration OmnidirectionalController::state_interface_configuration() const { + 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); @@ -213,13 +216,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"); @@ -227,36 +227,45 @@ CallbackReturn OmnidirectionalController::on_activate( } // 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_) { - 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; - }); + if (!odom_params_.open_loop) { + std::string interface_name = wheel_name + "/" + HW_IF_VELOCITY; - if (state_handle == state_interfaces_.cend()) { - RCLCPP_ERROR(logger, "Unable to obtain joint state handle for %s", wheel_name.c_str()); - return CallbackReturn::ERROR; + 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 == state_interfaces_.cend()) { + RCLCPP_ERROR(logger, "Unable to obtain joint state handle for %s", wheel_name.c_str()); + return CallbackReturn::ERROR; + } + + registered_wheel_state_ifs_.emplace_back(*state_handle); + + RCLCPP_INFO(logger, "Got state interface: %s", state_handle->get_name().c_str()); } const auto command_handle = 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()) { 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)}); + registered_wheel_cmd_ifs_.emplace_back(*command_handle); 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()); } subscriber_is_active_ = true; @@ -349,9 +358,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) { @@ -397,9 +406,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; }