Forgor to add new files :(

mg_wheel_interface/include/mg_wheel_interface/mg_wheel_interface.hpp

@@ -0,0 +1,37 @@
+#ifndef MG_WHEEL_INTERFACE_HPP_
+#define MG_WHEEL_INTERFACE_HPP_
+#include <vector>
+#include <string>
+
+
+#include "libserial/SerialPort.h"
+#include "hardware_interface/handle.hpp"
+#include "hardware_interface/system_interface.hpp"
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+#include "hardware_interface/types/hardware_interface_return_values.hpp"
+#include "pluginlib/class_list_macros.hpp"
+
+
+namespace mg_wheel_interface {
+    class MgOdriveInterface : 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;
+        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;
+        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;
+    };
+}
+
+PLUGINLIB_EXPORT_CLASS(mg_wheel_interface::MgOdriveInterface, hardware_interface::SystemInterface)
+
+#endif

mg_wheel_interface/mg_wheel_interface.xml

@@ -0,0 +1,7 @@
+    <library path="mg_wheel_interface">
+        <class 
+            name="mg_wheel_interface/MgOdriveInterface"
+            type="mg_wheel_interface::MgOdriveInterface" 
+            base_class_type="hardware_interface::SystemInterface"
+        />
+    </library>

mg_wheel_interface/src/mg_wheel_interface.cpp

@@ -0,0 +1,138 @@
+#include "mg_wheel_interface/mg_wheel_interface.hpp"
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+
+#include <cmath>
+
+CallbackReturn mg_wheel_interface::MgOdriveInterface::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_wheel_interface::MgOdriveInterface::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("mg_wheel_interface"),
+            "Failed to open serial port (Is the odrive plugged in?)");
+        return CallbackReturn::ERROR;
+    }
+    return CallbackReturn::SUCCESS;
+}
+
+
+CallbackReturn mg_wheel_interface::MgOdriveInterface::on_shutdown(const rclcpp_lifecycle::State &)
+{
+    if(odrive_serial_interface.IsOpen()){
+        std::string cmd_left;
+        std::string cmd_right;
+        int hash;
+
+        cmd_left  = "v 1 " + std::to_string(0)  + " ";
+        cmd_right = "v 0 " + std::to_string(0) + " ";
+
+        hash = 0;
+        for(const auto &c: cmd_left) {
+            hash ^= c;
+        }
+        cmd_left += "*"+std::to_string(hash)+"\n";
+
+        hash = 0;
+        for(const auto &c: cmd_right) {
+            hash ^= c;
+        }
+        cmd_right += "*"+std::to_string(hash)+"\n";
+        odrive_serial_interface.Close();
+    }
+    return CallbackReturn::SUCCESS;
+}
+
+
+std::vector<hardware_interface::StateInterface> mg_wheel_interface::MgOdriveInterface::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_wheel_interface::MgOdriveInterface::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_wheel_interface::MgOdriveInterface::read(const rclcpp::Time &time, const rclcpp::Duration &period)
+{
+    std::string resp_l, resp_r;
+    odrive_serial_interface.Write("f 1\n");
+    odrive_serial_interface.ReadLine(resp_l,'\n',10);
+    odrive_serial_interface.Write("f 0\n");
+    odrive_serial_interface.ReadLine(resp_r,'\n',10);
+    left_wheel_pos_state =   std::stod(resp_l);
+    right_wheel_pos_state = -std::stod(resp_r);
+    return hardware_interface::return_type::OK ;
+}
+
+
+hardware_interface::return_type mg_wheel_interface::MgOdriveInterface::write(const rclcpp::Time &time, const rclcpp::Duration &period)
+{
+    std::string cmd_left;
+    std::string cmd_right;
+    int hash;
+
+    cmd_left  = "v 1 " + std::to_string(left_wheel_vel_cmd/M_PI)  + " ";
+    cmd_right = "v 0 " + std::to_string(-right_wheel_vel_cmd/M_PI) + " ";
+
+    hash = 0;
+    for(const auto &c: cmd_left) {
+        hash ^= c;
+    }
+    cmd_left += "*"+std::to_string(hash)+"\n";
+
+    hash = 0;
+    for(const auto &c: cmd_right) {
+        hash ^= c;
+    }
+    cmd_right += "*"+std::to_string(hash)+"\n";
+
+
+    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;
+}

udev/99-usb-serial.rules

@@ -0,0 +1 @@
+SUBSYSTEM=="tty", ATTRS{idVendor}=="1209", ATTRS{idProduct}=="0d32", MODE="0666"