Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix call to state interface even in open_loop #9

Merged
merged 4 commits into from
Apr 11, 2024
Merged

Fix call to state interface even in open_loop #9

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
b88fd25
Fix call to state interface even in open_loop
Apr 7, 2024
f1a5526
Update unecessary 'else' condition in state_interface_configuration
Apr 7, 2024
289a477
Update without WheelHandle struct
Apr 8, 2024
863d461
style: clean code fix: remove unnecessary else
Apr 10, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 11 additions & 5 deletions include/omnidirectional_controllers/omnidirectional_controller.hpp
View file
Open in desktop
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,17 @@ 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_;
// 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_;
mateusmenezes95 marked this conversation as resolved.
Show resolved Hide resolved

// Default parameters for axebot
RobotParams robot_params_{0.1, 0.0505, 0.0};
Expand Down
70 changes: 41 additions & 29 deletions src/omnidirectional_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -97,11 +97,16 @@ InterfaceConfiguration OmnidirectionalController::command_interface_configuratio
}

InterfaceConfiguration OmnidirectionalController::state_interface_configuration() const {
std::vector<std::string> 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 {
mateusmenezes95 marked this conversation as resolved.
Show resolved Hide resolved
std::vector<std::string> 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(
Expand Down Expand Up @@ -213,50 +218,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 +360,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 +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;
}
Expand Down
Loading