Merge mg_wheel_interface with magrob

mg_control/src/mg_control.cpp

@@ -0,0 +1,99 @@
+#include "mg_control/mg_control.hpp"
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+
+#include <cmath>
+
+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()) {
+        serial_port_name = info_.hardware_parameters["device_path"];
+    } else {
+        serial_port_name = "/dev/ttyACM0";
+    }
+
+    left_wheel_pos_state = 0;
+    right_wheel_pos_state = 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()){
+            odrive_serial_interface.Open(serial_port_name);
+        }
+    }
+    catch (const LibSerial::OpenFailed &er) {
+        RCLCPP_FATAL(rclcpp::get_logger("MgStepperInterface"),
+            "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()){
+        odrive_serial_interface.Close();
+    }
+    return CallbackReturn::SUCCESS;
+}
+
+
+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;
+}
+
+
+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;
+}
+
+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 &)
+{
+    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);
+
+    try {
+        odrive_serial_interface.Write(cmd_left + cmd_right);
+    } catch(const std::runtime_error &e){
+        return hardware_interface::return_type::ERROR;
+    }
+    return hardware_interface::return_type::OK;
+}