Ported over odometry and control using new serial communication

toid_control/include/toid_control/toid_control.hpp

@@ -0,0 +1,45 @@
+#ifndef MG_WHEEL_INTERFACE_HPP_
+#define MG_WHEEL_INTERFACE_HPP_
+#include <vector>
+#include <string>
+
+#include "boost/asio.hpp"
+#include "hardware_interface/handle.hpp"
+#include "hardware_interface/system_interface.hpp"
+#include "hardware_interface/types/hardware_interface_return_values.hpp"
+#include "pluginlib/class_list_macros.hpp"
+
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
+namespace toid {
+    namespace asio = boost::asio;
+
+    class StepperInterface : public hardware_interface::SystemInterface {
+      public:
+
+        CallbackReturn on_init(const hardware_interface::HardwareComponentInterfaceParams& 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_;
+        asio::io_context io_context_;
+        asio::serial_port serial_port_ = asio::serial_port(io_context_);
+
+        double left_wheel_vel_cmd_    = 0;
+        double left_wheel_pos_state_  = 0;
+        double right_wheel_vel_cmd_   = 0;
+        double right_wheel_pos_state_ = 0;
+    };
+}
+
+PLUGINLIB_EXPORT_CLASS(toid::StepperInterface, hardware_interface::SystemInterface)
+
+#endif