Ported over odometry and control using new serial communication

toid_control/src/toid_control.cpp

@@ -0,0 +1,87 @@
+#include "toid_control/toid_control.hpp"
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+
+#include <cmath>
+
+CallbackReturn toid::StepperInterface::on_init(const hardware_interface::HardwareComponentInterfaceParams& info) {
+    if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) {
+        return CallbackReturn::ERROR;
+    }
+
+    if (info_.hardware_parameters.find("device_path") != info.hardware_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 toid::StepperInterface::on_configure(const rclcpp_lifecycle::State&) {
+    try {
+        serial_port_.open(serial_port_name_);
+    } catch (const std::exception& er) {
+        RCLCPP_ERROR(rclcpp::get_logger("StepperInterface"),
+                     "Failed to open serial port (Is the stepper driver plugged in)");
+    }
+    return CallbackReturn::SUCCESS;
+}
+
+CallbackReturn toid::StepperInterface::on_shutdown(const rclcpp_lifecycle::State&) {
+    if (serial_port_.is_open()) {
+        boost::system::error_code ec;
+        ec = serial_port_.close(ec);
+    }
+    return CallbackReturn::SUCCESS;
+}
+
+std::vector<hardware_interface::StateInterface> toid::StepperInterface::export_state_interfaces() {
+    std::vector<hardware_interface::StateInterface> state_interfaces;
+    state_interfaces.reserve(2);
+    state_interfaces.emplace_back("drivewhl_l_joint", hardware_interface::HW_IF_POSITION, &left_wheel_pos_state_);
+    state_interfaces.emplace_back("drivewhl_r_joint", hardware_interface::HW_IF_POSITION, &right_wheel_pos_state_);
+
+    return state_interfaces;
+}
+
+std::vector<hardware_interface::CommandInterface> toid::StepperInterface::export_command_interfaces() {
+    std::vector<hardware_interface::CommandInterface> command_interfaces;
+    command_interfaces.reserve(2);
+    command_interfaces.emplace_back("drivewhl_l_joint", hardware_interface::HW_IF_VELOCITY, &left_wheel_vel_cmd_);
+    command_interfaces.emplace_back("drivewhl_r_joint", hardware_interface::HW_IF_VELOCITY, &right_wheel_vel_cmd_);
+
+    return command_interfaces;
+}
+
+hardware_interface::return_type toid::StepperInterface::read(const rclcpp::Time&, const rclcpp::Duration&) {
+    return hardware_interface::return_type::OK;
+}
+
+hardware_interface::return_type toid::StepperInterface::write(const rclcpp::Time&, const rclcpp::Duration&) {
+    using namespace asio::buffer_literals;
+
+    try {
+        double values[] = {
+            -left_wheel_vel_cmd_ / (2 * M_PI),
+            right_wheel_vel_cmd_ / (2 * M_PI),
+        };
+        asio::write(serial_port_, "s;"_buf);
+        asio::write(serial_port_, asio::buffer(values, sizeof(values)));
+    } catch (const std::runtime_error& e) { 
+        RCLCPP_ERROR_THROTTLE(
+            this->get_logger(), 
+            *this->get_clock(), 
+            5000, 
+            "Failed to write to serial port: %s",
+            e.what()
+        );
+        boost::system::error_code ec;
+        ec = serial_port_.close(ec);
+        ec = serial_port_.open(serial_port_name_, ec);
+    }
+    return hardware_interface::return_type::OK;
+}