Skip to content

Commit

Permalink
Fix call to state interface even in open_loop (#9)
Browse files Browse the repository at this point in the history 
* Fix call to state interface even in open_loop

* Update without WheelHandle struct

---------

Co-authored-by: Your Name <[email protected]>
  • Loading branch information
@robotcopper
robotcopper and Your Name authored Apr 11, 2024
1 parent dd54b9c commit a32d693
Show file tree
Hide file tree
Showing 2 changed files with 39 additions and 30 deletions.
9 changes: 4 additions & 5 deletions include/omnidirectional_controllers/omnidirectional_controller.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,8 @@

namespace omnidirectional_controllers {
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
using StateInterfaceReferenceWrapper = std::reference_wrapper<const hardware_interface::LoanedStateInterface>;
using CommandInterfaceReferenceWrapper = std::reference_wrapper<hardware_interface::LoanedCommandInterface>;

class OmnidirectionalController : public controller_interface::ControllerInterface {
public:
Expand All @@ -58,13 +60,10 @@ class OmnidirectionalController : public controller_interface::ControllerInterfa
~OmnidirectionalController();

protected:
struct WheelHandle {
std::reference_wrapper<const hardware_interface::LoanedStateInterface> velocity_state;
std::reference_wrapper<hardware_interface::LoanedCommandInterface> velocity_command;
};

std::vector<std::string> wheel_names_;
std::vector<WheelHandle> registered_wheel_handles_;
std::vector<StateInterfaceReferenceWrapper> registered_wheel_state_ifs_;
std::vector<CommandInterfaceReferenceWrapper> registered_wheel_cmd_ifs_;

// Default parameters for axebot
RobotParams robot_params_{0.1, 0.0505, 0.0};
Expand Down
60 changes: 35 additions & 25 deletions src/omnidirectional_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::string> conf_names;
for (const auto & joint_name : wheel_names_) {
conf_names.push_back(joint_name + "/" + HW_IF_VELOCITY);
Expand Down Expand Up @@ -213,50 +216,56 @@ 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");
return CallbackReturn::ERROR;
}

// 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;
Expand Down Expand Up @@ -349,9 +358,9 @@ controller_interface::return_type OmnidirectionalController::update(
period.seconds());
} else {
std::vector<double> 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) {
Expand Down Expand Up @@ -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;
}
Expand Down

0 comments on commit a32d693

Please sign in to comment.