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 <vector>
#include <string> #include <string>
#include "libserial/SerialPort.h" #include "libserial/SerialPort.h"
#include "hardware_interface/handle.hpp" #include "hardware_interface/handle.hpp"
#include "hardware_interface/system_interface.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 "hardware_interface/types/hardware_interface_return_values.hpp"
#include "pluginlib/class_list_macros.hpp" #include "pluginlib/class_list_macros.hpp"
namespace mg { namespace mg {
class MgStepperInterface : public hardware_interface::SystemInterface { class MgStepperInterface : public hardware_interface::SystemInterface {
public: public:
CallbackReturn on_init(const hardware_interface::HardwareInfo &info) override; CallbackReturn on_init(const hardware_interface::HardwareInfo& info) override;
CallbackReturn on_configure(const rclcpp_lifecycle::State &) override; CallbackReturn on_configure(const rclcpp_lifecycle::State&) override;
CallbackReturn on_shutdown(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::StateInterface> export_state_interfaces() override;
std::vector<hardware_interface::CommandInterface> export_command_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; hardware_interface::return_type read(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) override;
private: hardware_interface::return_type write(const rclcpp::Time& /*time*/,
std::string serial_port_name; const rclcpp::Duration& /*period*/) override;
private:
std::string serial_port_name;
LibSerial::SerialPort odrive_serial_interface; LibSerial::SerialPort odrive_serial_interface;
double left_wheel_vel_cmd;
double left_wheel_pos_state; double left_wheel_vel_cmd = 0;
double right_wheel_vel_cmd; double left_wheel_pos_state = 0;
double right_wheel_pos_state; 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> #include <cmath>
CallbackReturn mg::MgStepperInterface::on_init(const hardware_interface::HardwareInfo &info) CallbackReturn mg::MgStepperInterface::on_init(const hardware_interface::HardwareInfo& info) {
{ if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) {
if(hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS){
return CallbackReturn::ERROR; 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"]; serial_port_name = info_.hardware_parameters["device_path"];
} else { } else {
serial_port_name = "/dev/ttyACM0"; serial_port_name = "/dev/ttyACM0";
} }
left_wheel_pos_state = 0; left_wheel_pos_state = 0;
right_wheel_pos_state = 0; right_wheel_pos_state = 0;
left_wheel_vel_cmd = 0; left_wheel_vel_cmd = 0;
right_wheel_vel_cmd = 0; right_wheel_vel_cmd = 0;
return CallbackReturn::SUCCESS; return CallbackReturn::SUCCESS;
} }
CallbackReturn mg::MgStepperInterface::on_configure(const rclcpp_lifecycle::State&) {
CallbackReturn mg::MgStepperInterface::on_configure(const rclcpp_lifecycle::State &) try {
{ if (!odrive_serial_interface.IsOpen()) {
try{
if(!odrive_serial_interface.IsOpen()){
odrive_serial_interface.Open(serial_port_name); odrive_serial_interface.Open(serial_port_name);
} }
} } catch (const LibSerial::OpenFailed& er) {
catch (const LibSerial::OpenFailed &er) {
RCLCPP_FATAL(rclcpp::get_logger("MgStepperInterface"), 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::ERROR;
} }
return CallbackReturn::SUCCESS; return CallbackReturn::SUCCESS;
} }
CallbackReturn mg::MgStepperInterface::on_shutdown(const rclcpp_lifecycle::State&) {
CallbackReturn mg::MgStepperInterface::on_shutdown(const rclcpp_lifecycle::State &) if (odrive_serial_interface.IsOpen()) {
{
if(odrive_serial_interface.IsOpen()){
odrive_serial_interface.Close(); odrive_serial_interface.Close();
} }
return CallbackReturn::SUCCESS; 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; std::vector<hardware_interface::StateInterface> state_interfaces;
state_interfaces.reserve(2); state_interfaces.reserve(2);
state_interfaces.push_back(hardware_interface::StateInterface( state_interfaces.emplace_back("left_motor", hardware_interface::HW_IF_POSITION, &left_wheel_pos_state);
"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);
));
state_interfaces.push_back(hardware_interface::StateInterface( return state_interfaces;
"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; std::vector<hardware_interface::CommandInterface> command_interfaces;
command_interfaces.reserve(2); command_interfaces.reserve(2);
command_interfaces.push_back(hardware_interface::CommandInterface( command_interfaces.emplace_back("left_motor", hardware_interface::HW_IF_VELOCITY, &left_wheel_vel_cmd);
"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);
));
command_interfaces.push_back(hardware_interface::CommandInterface( return command_interfaces;
"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 &) hardware_interface::return_type mg::MgStepperInterface::read(const rclcpp::Time&, const rclcpp::Duration&) {
{ return hardware_interface::return_type::OK;
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_left;
std::string cmd_right; std::string cmd_right;
cmd_left = std::to_string(left_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); cmd_right = std::to_string(-right_wheel_vel_cmd / 2 * M_PI);
try { try {
odrive_serial_interface.Write(cmd_left + cmd_right); odrive_serial_interface.Write(cmd_left + cmd_right);
} catch(const std::runtime_error &e){ } catch (const std::runtime_error& e) { return hardware_interface::return_type::ERROR; }
return hardware_interface::return_type::ERROR;
}
return hardware_interface::return_type::OK; return hardware_interface::return_type::OK;
} }