Add ros2_control hardware interface

firmware/arm/README.md

@@ -0,0 +1,19 @@
+# Arm Firmware
+
+## Getting Started
+
+#### Install Arduino CLI and dependencies
+```bash
+sudo adduser $USER dialout
+curl -fsSL https://raw.githubusercontent.com/arduino/arduino-cli/master/install.sh | BINDIR=~/.local/bin sh
+echo 'export PATH="$PATH:~/.local/bin"' >> ~/.bashrc && source ~/.bashrc
+sudo apt remove brltty
+arduino-cli core update-index
+arduino-cli core install arduino:avr
+arduino-cli lib install circularbuffer servo
+```
+
+#### Compile and upload
+```bash
+arduino-cli compile --upload --port /dev/ttyUSB0 -b arduino:avr:mega
+```

firmware/arm/arm.ino

@@ -0,0 +1,92 @@
+#include <CircularBuffer.h>
+#include <Servo.h>
+
+const int N_SERVOS = 6;
+const int START_BYTE = 0x29;
+const int MAX_MESSAGE_SIZE = 10;
+const int CMD_WRITE_POSITION = 0x01;
+
+Servo servos[N_SERVOS];
+
+CircularBuffer<byte, 50> incomming_buffer;
+enum State
+{
+    STATE_SEARCH_START_BYTE = 0,
+    STATE_WAIT_FOR_MESSAGE
+};
+State state = STATE_SEARCH_START_BYTE;
+
+void setup()
+{
+    Serial.begin(115200);
+
+    servos[0].attach(2);
+    servos[1].attach(3);
+    servos[2].attach(4);
+    servos[3].attach(5);
+    servos[4].attach(6);
+    servos[5].attach(7);
+}
+
+void loop()
+{
+    update_communication();
+}
+
+int16_t unpack_int16(byte *buffer)
+{
+    int16_t value = 0;
+    value |= ((uint32_t)buffer[2] << 8);
+    value |= ((uint32_t)buffer[3] << 0);
+    return value;
+}
+
+void update_communication()
+{
+    // Add data to the buffer
+    while (Serial.available() > 0)
+        incomming_buffer.push(Serial.read());
+
+    // Find the start byte
+    switch (state)
+    {
+    case STATE_SEARCH_START_BYTE:
+        if (!incomming_buffer.isEmpty() && incomming_buffer.first() != START_BYTE)
+            incomming_buffer.shift();
+        else
+            state = STATE_WAIT_FOR_MESSAGE;
+        break;
+    case STATE_WAIT_FOR_MESSAGE:
+        if (incomming_buffer.size() >= 2 && incomming_buffer.size() >= incomming_buffer[1])
+        {
+            on_full_message_received();
+            state = STATE_SEARCH_START_BYTE;
+        }
+        if (incomming_buffer.size() >= 2 && (uint8_t)incomming_buffer[1] > MAX_MESSAGE_SIZE)
+        {
+            // Message too big, something is wrong.
+            incomming_buffer.shift();
+            incomming_buffer.shift();
+            state = STATE_SEARCH_START_BYTE;
+        }
+        break;
+    }
+}
+
+void on_full_message_received()
+{
+    incomming_buffer.shift(); // start byte
+    incomming_buffer.shift(); // message size
+    incomming_buffer.shift(); // command type
+
+    for (uint8_t i = 0; i < N_SERVOS; i++)
+    {
+        byte buffer[2];
+        buffer[0] = incomming_buffer.shift();
+        buffer[1] = incomming_buffer.shift();
+
+        const int16_t target_position = unpack_int16(buffer);
+
+        servos[i].writeMicroseconds(target_position);
+    }
+}

spesbot_hardware/CMakeLists.txt

@@ -3,6 +3,37 @@ project(spesbot_hardware)
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
+find_package(hardware_interface REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(Boost REQUIRED)
+
+# Serial Hardware Interface
+add_library(
+  serial_hardware_interface
+  SHARED
+  src/serial_hardware_interface.cpp
+)
+target_include_directories(
+  serial_hardware_interface
+  PRIVATE
+  include
+  ${Boost_INCLUDE_DIRS}
+)
+ament_target_dependencies(
+  serial_hardware_interface
+  hardware_interface
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+)
+target_link_libraries(serial_hardware_interface ${Boost_LIBRARIES})
+pluginlib_export_plugin_description_file(hardware_interface serial_hardware_interface.xml)
+install(
+  TARGETS serial_hardware_interface
+  DESTINATION lib
+)
 
 # resources
 install(

spesbot_hardware/include/spesbot_hardware/serial_hardware_interface.hpp

@@ -0,0 +1,45 @@
+#ifndef SPESBOT_HARDWARE__SERIAL_HARDWARE_INTERFACE_HPP_
+#define SPESBOT_HARDWARE__SERIAL_HARDWARE_INTERFACE_HPP_
+
+#include <vector>
+#include <boost/asio.hpp>
+
+#include "hardware_interface/handle.hpp"
+#include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/system_interface.hpp"
+#include "hardware_interface/types/hardware_interface_return_values.hpp"
+
+namespace spesbot_hardware
+{
+    class Joint
+    {
+    public:
+        double position_command;
+        std::string name;
+        uint8_t index;
+        double scale;
+        double offset;
+    };
+
+    class SerialHardwareInterface : public hardware_interface::SystemInterface
+    {
+    public:
+        SerialHardwareInterface();
+        rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_init(const hardware_interface::HardwareInfo &info) override;
+        rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate(const rclcpp_lifecycle::State &) override;
+        rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate(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::vector<Joint> joints_;
+        boost::asio::io_service io_;
+        boost::asio::serial_port serial_;
+
+        uint8_t max_position_command_index_{0};
+    };
+} // namespace spesbot_hardware
+
+#endif // SPESBOT_HARDWARE__SERIAL_HARDWARE_INTERFACE_HPP_

spesbot_hardware/serial_hardware_interface.xml

@@ -0,0 +1,3 @@
+<library path="serial_hardware_interface">
+    <class type="spesbot_hardware::SerialHardwareInterface" base_class_type="hardware_interface::SystemInterface"></class>
+</library>

spesbot_hardware/src/serial_hardware_interface.cpp

@@ -0,0 +1,120 @@
+#include "spesbot_hardware/serial_hardware_interface.hpp"
+
+#include <vector>
+#include <boost/array.hpp>
+
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+namespace spesbot_hardware
+{
+    const uint8_t START_BYTE = 0x29;
+    const uint8_t HEADER_SIZE = 3;
+    const uint8_t POSITION_N_BYTES = 2;
+    const uint8_t CMD_WRITE_POSITION = 0x01;
+
+    inline void pack_int16(uint8_t *buffer, int16_t value)
+    {
+        buffer[2] = (value >> 8) & 0xFF;
+        buffer[3] = value & 0xFF;
+    }
+
+    SerialHardwareInterface::SerialHardwareInterface() : serial_(io_)
+    {
+    }
+
+    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn SerialHardwareInterface::on_init(const hardware_interface::HardwareInfo &info)
+    {
+        if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS)
+        {
+            return CallbackReturn::ERROR;
+        }
+
+        for (hardware_interface::ComponentInfo component : info.joints)
+        {
+            Joint joint;
+            joint.name = component.name;
+            joint.index = std::stoi(component.parameters.at("index"));
+            joint.scale = std::stof(component.parameters.at("scale"));
+            joint.offset = std::stof(component.parameters.at("offset"));
+            joints_.push_back(joint);
+
+            if (joint.index > max_position_command_index_)
+                max_position_command_index_ = joint.index;
+        }
+
+        return CallbackReturn::SUCCESS;
+    }
+
+    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn SerialHardwareInterface::on_activate(const rclcpp_lifecycle::State &)
+    {
+        const std::string port = info_.hardware_parameters["port"];
+        const int baudrate_value = std::stod(info_.hardware_parameters["baudrate"]);
+
+        boost::system::error_code error_code;
+        serial_.open(port, error_code);
+        if (!error_code)
+        {
+            boost::asio::serial_port_base::baud_rate baudrate_option(baudrate_value);
+            serial_.set_option(baudrate_option);
+
+            // Sleep for some time, so the Arduino has time to initialize
+            std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+        }
+        else
+        {
+            std::cerr << "ERROR: Cannot connect to " << port << " with error code " << error_code << "\n";
+            return CallbackReturn::ERROR;
+        }
+
+        return CallbackReturn::SUCCESS;
+    }
+
+    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn SerialHardwareInterface::on_deactivate(const rclcpp_lifecycle::State &)
+    {
+        return CallbackReturn::SUCCESS;
+    }
+
+    std::vector<hardware_interface::StateInterface> SerialHardwareInterface::export_state_interfaces()
+    {
+        std::vector<hardware_interface::StateInterface> interfaces;
+        return interfaces;
+    }
+
+    std::vector<hardware_interface::CommandInterface>
+    SerialHardwareInterface::export_command_interfaces()
+    {
+        std::vector<hardware_interface::CommandInterface> interfaces;
+        for (Joint &joint : joints_)
+        {
+            interfaces.emplace_back(hardware_interface::CommandInterface(joint.name, hardware_interface::HW_IF_POSITION, &joint.position_command));
+        }
+        return interfaces;
+    }
+
+    hardware_interface::return_type SerialHardwareInterface::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
+    {
+        return hardware_interface::return_type::OK;
+    }
+
+    hardware_interface::return_type SerialHardwareInterface::write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
+    {
+        const int message_length = HEADER_SIZE + (max_position_command_index_ + 1) * POSITION_N_BYTES;
+        uint8_t position_command_buffer[message_length];
+
+        position_command_buffer[0] = START_BYTE;
+        position_command_buffer[1] = message_length;
+        position_command_buffer[2] = CMD_WRITE_POSITION;
+
+        for (Joint &joint : joints_)
+        {
+            int16_t position = joint.position_command * joint.scale + joint.offset;
+            pack_int16(&position_command_buffer[HEADER_SIZE + joint.index * POSITION_N_BYTES], position);
+            boost::asio::write(serial_, boost::asio::buffer(position_command_buffer, message_length));
+        }
+        return hardware_interface::return_type::OK;
+    }
+} // namespace spesbot_hardware
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(spesbot_hardware::SerialHardwareInterface, hardware_interface::SystemInterface)