Magnezijum-Robotics/magrob
2
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

Refactored mg_control

Browse Source
This commit is contained in:
Pimpest
2025-02-25 10:58:28 +01:00
parent b5d474ff01
commit 581eca8720
2 changed files with 47 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
mg_control/include/mg_control/mg_control.hpp
View File

@ -3,7 +3,6 @@
#include <vector>
#include <string>
#include "libserial/SerialPort.h"
#include "hardware_interface/handle.hpp"
#include "hardware_interface/system_interface.hpp"
@ -11,24 +10,28 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "pluginlib/class_list_macros.hpp"
namespace mg {
class MgStepperInterface : public hardware_interface::SystemInterface {
public:
CallbackReturn on_init(const hardware_interface::HardwareInfo &info) override;
CallbackReturn on_configure(const rclcpp_lifecycle::State &) override;
CallbackReturn on_shutdown(const rclcpp_lifecycle::State &) override;
std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
public:
CallbackReturn on_init(const hardware_interface::HardwareInfo& info) override;
CallbackReturn on_configure(const rclcpp_lifecycle::State&) override;
CallbackReturn on_shutdown(const rclcpp_lifecycle::State&) override;
std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
hardware_interface::return_type read(const rclcpp::Time &time, const rclcpp::Duration &period) override;
hardware_interface::return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override;
private:
std::string serial_port_name;
hardware_interface::return_type read(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) override;
hardware_interface::return_type write(const rclcpp::Time& /*time*/,
const rclcpp::Duration& /*period*/) override;
private:
std::string serial_port_name;
LibSerial::SerialPort odrive_serial_interface;
double left_wheel_vel_cmd;
double left_wheel_pos_state;
double right_wheel_vel_cmd;
double right_wheel_pos_state;
double left_wheel_vel_cmd = 0;
double left_wheel_pos_state = 0;
double right_wheel_vel_cmd = 0;
double right_wheel_pos_state = 0;
};
}

78
mg_control/src/mg_control.cpp
View File

@ -3,97 +3,77 @@
#include <cmath>
CallbackReturn mg::MgStepperInterface::on_init(const hardware_interface::HardwareInfo &info)
{
if(hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS){
CallbackReturn mg::MgStepperInterface::on_init(const hardware_interface::HardwareInfo& info) {
if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) {
return CallbackReturn::ERROR;
}
if(info_.hardware_parameters.find("device_path") != info.hardware_parameters.end()) {
if (info_.hardware_parameters.find("device_path") != info.hardware_parameters.end()) {
serial_port_name = info_.hardware_parameters["device_path"];
} else {
serial_port_name = "/dev/ttyACM0";
}
left_wheel_pos_state = 0;
left_wheel_pos_state = 0;
right_wheel_pos_state = 0;
left_wheel_vel_cmd = 0;
right_wheel_vel_cmd = 0;
left_wheel_vel_cmd = 0;
right_wheel_vel_cmd = 0;
return CallbackReturn::SUCCESS;
}
CallbackReturn mg::MgStepperInterface::on_configure(const rclcpp_lifecycle::State &)
{
try{
if(!odrive_serial_interface.IsOpen()){
CallbackReturn mg::MgStepperInterface::on_configure(const rclcpp_lifecycle::State&) {
try {
if (!odrive_serial_interface.IsOpen()) {
odrive_serial_interface.Open(serial_port_name);
}
}
catch (const LibSerial::OpenFailed &er) {
} catch (const LibSerial::OpenFailed& er) {
RCLCPP_FATAL(rclcpp::get_logger("MgStepperInterface"),
"Failed to open serial port (Is the stepper driver plugged in)");
"Failed to open serial port (Is the stepper driver plugged in)");
return CallbackReturn::ERROR;
}
return CallbackReturn::SUCCESS;
}
CallbackReturn mg::MgStepperInterface::on_shutdown(const rclcpp_lifecycle::State &)
{
if(odrive_serial_interface.IsOpen()){
CallbackReturn mg::MgStepperInterface::on_shutdown(const rclcpp_lifecycle::State&) {
if (odrive_serial_interface.IsOpen()) {
odrive_serial_interface.Close();
}
return CallbackReturn::SUCCESS;
}
std::vector<hardware_interface::StateInterface> mg::MgStepperInterface::export_state_interfaces() {
std::vector<hardware_interface::StateInterface> mg::MgStepperInterface::export_state_interfaces()
{
std::vector<hardware_interface::StateInterface> state_interfaces;
state_interfaces.reserve(2);
state_interfaces.push_back(hardware_interface::StateInterface(
"left_motor", hardware_interface::HW_IF_POSITION, &left_wheel_pos_state
));
state_interfaces.push_back(hardware_interface::StateInterface(
"right_motor", hardware_interface::HW_IF_POSITION, &right_wheel_pos_state
));
return state_interfaces;
state_interfaces.emplace_back("left_motor", hardware_interface::HW_IF_POSITION, &left_wheel_pos_state);
state_interfaces.emplace_back("right_motor", hardware_interface::HW_IF_POSITION, &right_wheel_pos_state);
return state_interfaces;
}
std::vector<hardware_interface::CommandInterface> mg::MgStepperInterface::export_command_interfaces() {
std::vector<hardware_interface::CommandInterface> mg::MgStepperInterface::export_command_interfaces()
{
std::vector<hardware_interface::CommandInterface> command_interfaces;
command_interfaces.reserve(2);
command_interfaces.push_back(hardware_interface::CommandInterface(
"left_motor", hardware_interface::HW_IF_VELOCITY, &left_wheel_vel_cmd
));
command_interfaces.push_back(hardware_interface::CommandInterface(
"right_motor", hardware_interface::HW_IF_VELOCITY, &right_wheel_vel_cmd
));
return command_interfaces;
command_interfaces.emplace_back("left_motor", hardware_interface::HW_IF_VELOCITY, &left_wheel_vel_cmd);
command_interfaces.emplace_back("right_motor", hardware_interface::HW_IF_VELOCITY, &right_wheel_vel_cmd);
return command_interfaces;
}
hardware_interface::return_type mg::MgStepperInterface::read(const rclcpp::Time &, const rclcpp::Duration &)
{
return hardware_interface::return_type::OK ;
hardware_interface::return_type mg::MgStepperInterface::read(const rclcpp::Time&, const rclcpp::Duration&) {
return hardware_interface::return_type::OK;
}
hardware_interface::return_type mg::MgStepperInterface::write(const rclcpp::Time &, const rclcpp::Duration &)
{
hardware_interface::return_type mg::MgStepperInterface::write(const rclcpp::Time&, const rclcpp::Duration&) {
std::string cmd_left;
std::string cmd_right;
cmd_left = std::to_string(left_wheel_vel_cmd/2*M_PI) + " ";
cmd_right = std::to_string(-right_wheel_vel_cmd/2*M_PI);
cmd_left = std::to_string(left_wheel_vel_cmd / 2 * M_PI) + " ";
cmd_right = std::to_string(-right_wheel_vel_cmd / 2 * M_PI);
try {
odrive_serial_interface.Write(cmd_left + cmd_right);
} catch(const std::runtime_error &e){
return hardware_interface::return_type::ERROR;
}
} catch (const std::runtime_error& e) { return hardware_interface::return_type::ERROR; }
return hardware_interface::return_type::OK;
}