Removed accidental subrepos

src/diffdrive_arduino/CMakeLists.txt

@@ -0,0 +1,102 @@
+cmake_minimum_required(VERSION 3.5)
+project(diffdrive_arduino)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(serial REQUIRED)
+find_package(hardware_interface REQUIRED)
+find_package(controller_manager REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(pluginlib REQUIRED)
+
+
+
+
+
+add_library(diffdrive_arduino SHARED src/diffdrive_arduino.cpp src/wheel.cpp src/arduino_comms.cpp)
+target_include_directories(
+  diffdrive_arduino
+  PRIVATE
+  include
+)
+ament_target_dependencies(
+  diffdrive_arduino
+  hardware_interface
+  controller_manager
+  serial
+  rclcpp
+  rclcpp_lifecycle
+  pluginlib
+)
+
+pluginlib_export_plugin_description_file(hardware_interface robot_hardware.xml)
+
+
+
+add_library(fake_robot SHARED src/fake_robot.cpp src/wheel.cpp)
+set_property(TARGET fake_robot PROPERTY POSITION_INDEPENDENT_CODE ON)
+
+target_include_directories(
+  fake_robot
+  PRIVATE
+  include
+)
+ament_target_dependencies(
+  fake_robot
+  hardware_interface
+  controller_manager
+  rclcpp
+  rclcpp_lifecycle
+  pluginlib
+)
+
+pluginlib_export_plugin_description_file(hardware_interface fake_robot_hardware.xml)
+
+
+
+
+install(
+  TARGETS diffdrive_arduino fake_robot
+  DESTINATION lib
+)
+
+
+
+
+install(
+  DIRECTORY controllers launch/
+  DESTINATION share/${PROJECT_NAME}
+)
+
+
+ament_export_libraries(
+  diffdrive_arduino
+  fake_robot
+)
+ament_export_dependencies(
+  hardware_interface
+  controller_manager
+  serial
+  rclcpp
+  pluginlib
+)
+
+ament_package()
+
+

src/diffdrive_arduino/LICENSE

@@ -0,0 +1,29 @@
+BSD 3-Clause License
+
+Copyright (c) 2020, Josh Newans
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+   list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the documentation
+   and/or other materials provided with the distribution.
+
+3. Neither the name of the copyright holder nor the names of its
+   contributors may be used to endorse or promote products derived from
+   this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

src/diffdrive_arduino/README.md

@@ -0,0 +1,5 @@
+# diffdrive_arduino
+
+**Adds ROS2 Galactic and Humble support**
+
+This node is designed to provide an interface between a `diff_drive_controller` from `ros_control` and an Arduino running firmware from `ros_arduino_bridge`.

src/diffdrive_arduino/controllers/robot_controller_example.yaml

@@ -0,0 +1,43 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 50  # Hz
+
+    diff_controller:
+      type: diff_drive_controller/DiffDriveController
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+diff_controller:
+  ros__parameters:
+    publish_rate: 50.0
+    left_wheel_names: ['left_wheel_joint']
+    right_wheel_names: ['right_wheel_joint']
+    wheels_per_side: 1
+    wheel_separation: 0.3
+    wheel_radius: 0.05
+    base_frame_id: base_link
+    use_stamped_vel: false
+
+    linear.x.has_velocity_limits: false
+    linear.x.has_acceleration_limits: false
+    linear.x.has_jerk_limits: false
+    linear.x.max_velocity: 0.0
+    linear.x.min_velocity: 0.0
+    linear.x.max_acceleration: 0.0
+    linear.x.min_acceleration: 0.0
+    linear.x.max_jerk: 0.0
+    linear.x.min_jerk: 0.0
+
+    angular.z.has_velocity_limits: false
+    angular.z.has_acceleration_limits: false
+    angular.z.has_jerk_limits: false
+    angular.z.max_velocity: 0.0
+    angular.z.min_velocity: 0.0
+    angular.z.max_acceleration: 0.0
+    angular.z.min_acceleration: 0.0
+    angular.z.max_jerk: 0.0
+    angular.z.min_jerk: 0.0
+
+# joint_state_broadcaster:
+#   ros__parameters:

src/diffdrive_arduino/fake_robot_hardware.xml

@@ -0,0 +1,9 @@
+<library path="fake_robot">
+  <class name="fake_robot/FakeRobot"
+         type="FakeRobot"
+         base_class_type="hardware_interface::SystemInterface">
+    <description>
+      The ROS2 Control minimal robot protocol. This is example for start porting the robot from ROS 1.
+    </description>
+  </class>
+</library>

src/diffdrive_arduino/include/diffdrive_arduino/arduino_comms.h

@@ -0,0 +1,60 @@
+#ifndef DIFFDRIVE_ARDUINO_ARDUINO_COMMS_H
+#define DIFFDRIVE_ARDUINO_ARDUINO_COMMS_H
+
+#include <serial/serial.h>
+#include <cstring>
+#include "rclcpp/rclcpp.hpp"
+
+
+typedef enum frametypes {
+	VALUE = 0x00, REQUEST = 0x20, RESPONSE = 0x40
+} U_FrameType;
+
+typedef enum components {
+	DEFAULT_COMPONENT = 0x00, LEFT_MOTOR = 0x10, RIGHT_MOTOR = 0x20, BOTH_MOTORS = 0x30
+} U_Component;
+
+typedef enum requests {
+	DEFAULT_REQUEST, DRIVE_MOTOR, RESET_TIMESTAMP, DRIVE_ENCODER, PID_PARAMETER
+} U_Request;
+
+class ArduinoComms
+{
+
+
+public:
+
+  ArduinoComms()
+  : logger_(rclcpp::get_logger("ArduinoCOMS"))
+  {  }
+
+  ArduinoComms(const std::string &serial_device, int32_t baud_rate, int32_t timeout_ms)
+      : serial_conn_(serial_device, baud_rate, serial::Timeout::simpleTimeout(timeout_ms)), logger_(rclcpp::get_logger("ArduinoCOMS"))
+  {  }
+
+  void setup(const std::string &serial_device, int32_t baud_rate, int32_t timeout_ms);
+  void sendEmptyMsg();
+  void readEncoderValues(double &val_1, double &val_2);
+  void setMotorValues(float val_1, float val_2);
+  void setPidValues(float k_p, float k_d, float k_i, float k_o);
+  const std::vector<uint8_t> buildMsg(uint8_t frame_type, uint8_t frame_component, long time,  std::vector<uint8_t> payload);
+  void readSpeed(double &left_speed, double &right_speed);
+
+  bool connected() const { return serial_conn_.isOpen(); }
+
+  void sendMsg(const std::vector<uint8_t> &msg_to_send, bool print_output = false);
+
+
+private:
+  serial::Serial serial_conn_;  ///< Underlying serial connection 
+  std::vector<uint8_t> marshalling(float number);
+  std::vector<uint8_t> marshalling(uint16_t number);
+  std::vector<uint8_t> marshalling(uint32_t number);
+  void safeRead(std::vector<uint8_t> &buffer, int bytes_to_read);
+  rclcpp::Logger logger_;
+
+
+
+};
+
+#endif // DIFFDRIVE_ARDUINO_ARDUINO_COMMS_H

src/diffdrive_arduino/include/diffdrive_arduino/config.h

@@ -0,0 +1,19 @@
+#ifndef DIFFDRIVE_ARDUINO_CONFIG_H
+#define DIFFDRIVE_ARDUINO_CONFIG_H
+
+#include <string>
+
+
+struct Config
+{
+  std::string left_wheel_name = "left_wheel";
+  std::string right_wheel_name = "right_wheel";
+  float loop_rate = 30;
+  std::string device = "/dev/ttyUSB0";
+  int baud_rate = 115200;
+  int timeout = 1000;
+  int enc_counts_per_rev = 1920;
+};
+
+
+#endif // DIFFDRIVE_ARDUINO_CONFIG_H

src/diffdrive_arduino/include/diffdrive_arduino/diffdrive_arduino.h

@@ -0,0 +1,61 @@
+#ifndef DIFFDRIVE_ARDUINO_REAL_ROBOT_H
+#define DIFFDRIVE_ARDUINO_REAL_ROBOT_H
+
+#include <cstring>
+#include "rclcpp/rclcpp.hpp"
+
+#include <hardware_interface/handle.hpp>
+#include <hardware_interface/hardware_info.hpp>
+#include <hardware_interface/system_interface.hpp>
+#include <rclcpp_lifecycle/state.hpp>
+#include <hardware_interface/types/hardware_interface_return_values.hpp>
+
+#include "config.h"
+#include "wheel.h"
+#include "arduino_comms.h"
+
+using hardware_interface::CallbackReturn;
+using hardware_interface::return_type;
+
+namespace diffdrive_arduino
+{
+
+class DiffDriveArduino : public hardware_interface::SystemInterface
+{
+
+
+public:
+  DiffDriveArduino();
+
+  CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override;
+
+  std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
+
+  std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
+
+  CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
+
+  CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
+
+  return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+  return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+
+
+private:
+
+  Config cfg_;
+  ArduinoComms arduino_;
+
+  Wheel l_wheel_;
+  Wheel r_wheel_;
+
+  rclcpp::Logger logger_;
+
+  std::chrono::time_point<std::chrono::system_clock> time_;
+  
+};
+} // namespace diffdrive_arduino
+
+#endif // DIFFDRIVE_ARDUINO_REAL_ROBOT_H

src/diffdrive_arduino/include/diffdrive_arduino/fake_robot.h

@@ -0,0 +1,55 @@
+#ifndef DIFFDRIVE_ARDUINO_FAKE_ROBOT_H
+#define DIFFDRIVE_ARDUINO_FAKE_ROBOT_H
+
+
+#include "rclcpp/rclcpp.hpp"
+
+#include <hardware_interface/handle.hpp>
+#include <hardware_interface/hardware_info.hpp>
+#include <hardware_interface/system_interface.hpp>
+#include <rclcpp_lifecycle/state.hpp>
+#include <hardware_interface/types/hardware_interface_return_values.hpp>
+
+#include "config.h"
+#include "wheel.h"
+
+using hardware_interface::CallbackReturn;
+using hardware_interface::return_type;
+
+class FakeRobot : public hardware_interface::SystemInterface
+{
+
+
+public:
+  FakeRobot();
+
+  CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override;
+
+  std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
+
+  std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
+
+  CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
+
+  CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
+
+  return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+  return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+
+private:
+
+  Config cfg_;
+
+  Wheel l_wheel_;
+  Wheel r_wheel_;
+
+  rclcpp::Logger logger_;
+
+  std::chrono::time_point<std::chrono::system_clock> time_;
+  
+};
+
+
+#endif // DIFFDRIVE_ARDUINO_FAKE_ROBOT_H

src/diffdrive_arduino/include/diffdrive_arduino/wheel.h

@@ -0,0 +1,34 @@
+#ifndef DIFFDRIVE_ARDUINO_WHEEL_H
+#define DIFFDRIVE_ARDUINO_WHEEL_H
+
+#include <string>
+
+
+
+class Wheel
+{
+    public:
+
+    std::string name = "";
+    int enc = 0;
+    double cmd = 0;
+    double pos = 0;
+    double vel = 0;
+    double eff = 0;
+    double velSetPt = 0;
+    double rads_per_count = 0;
+
+    Wheel() = default;
+
+    Wheel(const std::string &wheel_name, int counts_per_rev);
+    
+    void setup(const std::string &wheel_name, int counts_per_rev);
+
+    double calcEncAngle();
+
+
+
+};
+
+
+#endif // DIFFDRIVE_ARDUINO_WHEEL_H

src/diffdrive_arduino/launch/fake_robot.launch.py

@@ -0,0 +1,52 @@
+# Copyright 2020 ROS2-Control Development Team (2020)
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+import xacro
+
+
+def generate_launch_description():
+
+    # Get URDF via xacro
+    robot_description_path = os.path.join(
+        get_package_share_directory('diffdrive_arduino'),
+        'description',
+        'robot.urdf')
+    robot_description_config = xacro.process_file(robot_description_path)
+    robot_description = {'robot_description': robot_description_config.toxml()}
+
+    test_controller = os.path.join(
+        get_package_share_directory('diffdrive_arduino'),
+        'controllers',
+        'fake_robot_controller.yaml'
+        )
+
+    return LaunchDescription([
+      Node(
+        package='controller_manager',
+        executable='ros2_control_node',
+        parameters=[robot_description, test_controller],
+        output={
+          'stdout': 'screen',
+          'stderr': 'screen',
+          },
+        )
+
+    ])

src/diffdrive_arduino/launch/test_robot.launch.py

@@ -0,0 +1,52 @@
+# Copyright 2020 ROS2-Control Development Team (2020)
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+import xacro
+
+
+def generate_launch_description():
+
+    # Get URDF via xacro
+    robot_description_path = os.path.join(
+        get_package_share_directory('dcaiti_control'),
+        'description',
+        'robot.urdf.xacro')
+    robot_description_config = xacro.process_file(robot_description_path)
+    robot_description = {'robot_description': robot_description_config.toxml()}
+
+    test_controller = os.path.join(
+        get_package_share_directory('diffdrive_arduino'),
+        'controllers',
+        'robot_controller.yaml'
+        )
+
+    return LaunchDescription([
+      Node(
+        package='controller_manager',
+        executable='ros2_control_node',
+        parameters=[robot_description, test_controller],
+        output={
+          'stdout': 'screen',
+          'stderr': 'screen',
+          },
+        )
+
+    ])

src/diffdrive_arduino/package.xml

@@ -0,0 +1,37 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>diffdrive_arduino</name>
+  <version>0.0.1</version>
+  <description>Provides an interface between diff_drive_controller and an Arduino.</description>
+
+  <maintainer email="josh.newans@gmail.com">Josh Newans</maintainer>
+  <author email="josh.newans@gmail.com">Josh Newans</author>
+  <license>BSD</license>
+
+
+  <!-- <url type="website">http://wiki.ros.org/diffdrive_arduino</url> -->
+  <url type="bugtracker">https://github.com/joshnewans/diffdrive_arduino/issues</url>
+  <url type="repository">https://github.com/joshnewans/diffdrive_arduino</url>
+
+
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>hardware_interface</depend>
+  <depend>controller_manager</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>serial</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <!-- <depend>rclcpp_lifecyle</depend> -->
+
+
+<test_depend>ament_add_gmock</test_depend>
+<test_depend>hardware_interface</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+
+  
+</package>

src/diffdrive_arduino/robot_hardware.xml

@@ -0,0 +1,9 @@
+<library path="diffdrive_arduino">
+  <class name="diffdrive_arduino/DiffDriveArduino"
+        type="diffdrive_arduino::DiffDriveArduino"
+        base_class_type="hardware_interface::SystemInterface">
+    <description>
+      Hardware Interface for Differential Drive Arduino controller.
+    </description>
+  </class>
+</library>

src/diffdrive_arduino/src/arduino_comms.cpp

@@ -0,0 +1,169 @@
+#include "diffdrive_arduino/arduino_comms.h"
+// #include <ros/console.h>
+#include <rclcpp/rclcpp.hpp>
+#include <sstream>
+#include <cstdlib>
+#include <arpa/inet.h>
+#include <vector>
+
+
+void ArduinoComms::setup(const std::string &serial_device, int32_t baud_rate, int32_t timeout_ms)
+{  
+    serial_conn_.setPort(serial_device);
+    serial_conn_.setBaudrate(baud_rate);
+    serial::Timeout tt = serial::Timeout::simpleTimeout(timeout_ms);
+    serial_conn_.setTimeout(tt); // This should be inline except setTimeout takes a reference and so needs a variable
+    serial_conn_.open();
+
+}
+
+void ArduinoComms::sendEmptyMsg()
+{
+}
+
+void ArduinoComms::readEncoderValues(double &left_speed, double &right_speed)
+{   
+    std::vector<uint8_t> payload = {DRIVE_ENCODER};
+    std::vector<uint8_t> msg = buildMsg(REQUEST, BOTH_MOTORS, 0, payload);
+    sendMsg(msg);
+    readSpeed(left_speed, right_speed);
+}
+
+void ArduinoComms::setMotorValues(float left_speed, float right_speed)
+{
+    std::vector<uint8_t> payload = {DRIVE_MOTOR};
+    auto bytes_left_speed = marshalling(left_speed);
+    payload.insert(payload.end(), bytes_left_speed.begin(), bytes_left_speed.end());
+    auto bytes_right_speed = marshalling(right_speed);
+    payload.insert(payload.end(), bytes_right_speed.begin(), bytes_right_speed.end());
+
+    std::vector<uint8_t> msg = buildMsg(REQUEST, BOTH_MOTORS, 0, payload);
+
+    sendMsg(msg);
+}
+
+void ArduinoComms::setPidValues(float k_p, float k_d, float k_i, float k_o)
+{
+    std::stringstream ss;
+    ss << "u " << k_p << ":" << k_d << ":" << k_i << ":" << k_o << "\r";
+}
+
+void ArduinoComms::readSpeed(double &left_speed, double &right_speed)
+{
+    std::vector<uint8_t> buffer;
+    int read_bytes = 0;
+    do{
+        buffer.clear();
+        read_bytes = serial_conn_.read(buffer, 1);
+    }
+    while(read_bytes == 0 || buffer.at(0) != 0xAA);
+    
+    // read frame and extract two big-endian floats from payload
+    safeRead(buffer, 1);
+    uint8_t frame_type = buffer.at(0);
+    safeRead(buffer, 1);
+    uint8_t payload_size = buffer.at(0);
+    safeRead(buffer, 1);
+    uint8_t frame_component = buffer.at(0);
+    safeRead(buffer, 4);
+    uint32_t time = *reinterpret_cast<double*>(&buffer[0]);
+
+    safeRead(buffer, payload_size);
+    std::vector<uint8_t> payload(buffer.begin(), buffer.end());
+        // read checksum
+    safeRead(buffer, 2);
+    uint16_t checksum = buffer.at(0);
+    checksum = checksum << 8;
+    checksum = checksum | buffer.at(1);
+
+    // check checksum of payload
+    uint16_t payload_checksum = 0;
+    for (auto it = payload.begin(); it != payload.end(); ++it)
+    {
+        payload_checksum += *it;
+    }
+    if (payload_checksum != checksum)
+    {
+        RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Checksum error");
+    }
+    
+    if (frame_type == VALUE && frame_component == BOTH_MOTORS)
+    {
+            std::vector<uint8_t> left_speed_bytes(payload.begin(), payload.begin() + 4);
+            std::vector<uint8_t> right_speed_bytes(payload.begin() + 4, payload.end());
+            std::reverse(left_speed_bytes.begin(), left_speed_bytes.end());
+            std::reverse(right_speed_bytes.begin(), right_speed_bytes.end());
+            float left_speed_float = *reinterpret_cast<float*>(&left_speed_bytes[0]);
+            float right_speed_float = *reinterpret_cast<float*>(&right_speed_bytes[0]);
+            left_speed = left_speed_float;
+            right_speed = right_speed_float;
+    }
+}
+
+void ArduinoComms::safeRead(std::vector<uint8_t> &buffer, int bytes_to_read){
+    buffer.clear();
+    int read_bytes = 0;
+    do{
+        read_bytes += serial_conn_.read(buffer, bytes_to_read - read_bytes);
+    }
+    while(read_bytes < bytes_to_read);
+}
+
+std::vector<uint8_t> ArduinoComms::marshalling(uint16_t number)
+{
+    number = htons(number);
+    uint8_t array[2];
+    array[0]=number & 0xff;
+    array[1]=(number >> 8);
+    std::vector<uint8_t> vec(array, array + sizeof array / sizeof array[0]);
+    return vec;
+}
+
+std::vector<uint8_t> ArduinoComms::marshalling(uint32_t number)
+{
+    number = htons(number);
+    uint8_t array[4];
+    array[0]=number & 0xff;
+    array[1]=(number >> 8) & 0xff;
+    array[2]=(number >> 16) & 0xff;
+    array[3]=(number >> 24) & 0xff;
+    std::vector<uint8_t> vec(array, array + sizeof array / sizeof array[0]);
+    return vec;
+}
+
+std::vector<uint8_t> ArduinoComms::marshalling(float number)
+{
+    uint32_t int_interpretation = reinterpret_cast<uint32_t &>( number );
+    union {
+    float f;
+    uint8_t b[4];
+    } u;
+    u.f = number;
+    std::vector<uint8_t> vec(u.b, u.b + sizeof u.b / sizeof u.b[0]);
+    std::reverse(vec.begin(), vec.end());
+    return vec;
+}
+
+const std::vector<uint8_t> ArduinoComms::buildMsg(uint8_t frame_type, uint8_t frame_component, long time,  std::vector<uint8_t> payload)
+{
+    std::vector<uint8_t> buffer;
+    buffer.push_back(0xAA);
+    buffer.push_back(frame_type);
+    buffer.push_back(payload.size());
+    buffer.push_back(frame_component);
+    uint32_t time_zero = 0;
+    auto byte_time = marshalling(time_zero);
+    buffer.insert(buffer.end(), byte_time.begin(), byte_time.end());
+    uint16_t checksum = std::accumulate(payload.begin(), payload.end(), 0);
+    buffer.insert(buffer.end(), payload.begin(), payload.end());
+    auto byte_checksum = marshalling(checksum);
+    buffer.insert(buffer.end(), byte_checksum.begin(), byte_checksum.end());
+
+    return buffer;
+}
+
+
+void ArduinoComms::sendMsg(const std::vector<uint8_t> &msg_to_send, bool print_output)
+{
+    serial_conn_.write(msg_to_send);
+}

src/diffdrive_arduino/src/diffdrive_arduino.cpp

@@ -0,0 +1,149 @@
+#include "diffdrive_arduino/diffdrive_arduino.h"
+
+
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+#include "hardware_interface/types/hardware_interface_return_values.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+
+namespace diffdrive_arduino
+{
+
+
+DiffDriveArduino::DiffDriveArduino()
+    : logger_(rclcpp::get_logger("DiffDriveArduino"))
+{}
+
+CallbackReturn DiffDriveArduino::on_init(const hardware_interface::HardwareInfo & info)
+{
+  if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS)
+{
+  return CallbackReturn::ERROR;
+}
+
+  RCLCPP_INFO(logger_, "Configuring...");
+
+  time_ = std::chrono::system_clock::now();
+
+  cfg_.left_wheel_name = info_.hardware_parameters["left_wheel_name"];
+  cfg_.right_wheel_name = info_.hardware_parameters["right_wheel_name"];
+  cfg_.loop_rate = std::stof(info_.hardware_parameters["loop_rate"]);
+  cfg_.device = info_.hardware_parameters["device"];
+  cfg_.baud_rate = std::stoi(info_.hardware_parameters["baud_rate"]);
+  cfg_.timeout = std::stoi(info_.hardware_parameters["timeout"]);
+  cfg_.enc_counts_per_rev = std::stoi(info_.hardware_parameters["enc_counts_per_rev"]);
+
+  // Set up the wheels
+  l_wheel_.setup(cfg_.left_wheel_name, cfg_.enc_counts_per_rev);
+  r_wheel_.setup(cfg_.right_wheel_name, cfg_.enc_counts_per_rev);
+
+  // Set up the Arduino
+  arduino_.setup(cfg_.device, cfg_.baud_rate, cfg_.timeout);  
+
+  RCLCPP_INFO(logger_, "Finished Configuration");
+
+  return CallbackReturn::SUCCESS;
+}
+
+std::vector<hardware_interface::StateInterface> DiffDriveArduino::export_state_interfaces()
+{
+  // We need to set up a position and a velocity interface for each wheel
+
+  std::vector<hardware_interface::StateInterface> state_interfaces;
+
+  state_interfaces.emplace_back(hardware_interface::StateInterface(l_wheel_.name, hardware_interface::HW_IF_VELOCITY, &l_wheel_.vel));
+  state_interfaces.emplace_back(hardware_interface::StateInterface(l_wheel_.name, hardware_interface::HW_IF_POSITION, &l_wheel_.pos));
+  state_interfaces.emplace_back(hardware_interface::StateInterface(r_wheel_.name, hardware_interface::HW_IF_VELOCITY, &r_wheel_.vel));
+  state_interfaces.emplace_back(hardware_interface::StateInterface(r_wheel_.name, hardware_interface::HW_IF_POSITION, &r_wheel_.pos));
+
+  return state_interfaces;
+}
+
+std::vector<hardware_interface::CommandInterface> DiffDriveArduino::export_command_interfaces()
+{
+  // We need to set up a velocity command interface for each wheel
+
+  std::vector<hardware_interface::CommandInterface> command_interfaces;
+
+  command_interfaces.emplace_back(hardware_interface::CommandInterface(l_wheel_.name, hardware_interface::HW_IF_VELOCITY, &l_wheel_.cmd));
+  command_interfaces.emplace_back(hardware_interface::CommandInterface(r_wheel_.name, hardware_interface::HW_IF_VELOCITY, &r_wheel_.cmd));
+
+  return command_interfaces;
+}
+
+
+CallbackReturn DiffDriveArduino::on_activate(const rclcpp_lifecycle::State & /* previous_state */)
+{
+  RCLCPP_INFO(logger_, "Starting Controller...");
+  arduino_.sendEmptyMsg();
+  // arduino.setPidValues(9,7,0,100);
+  // arduino.setPidValues(14,7,0,100);
+  arduino_.setPidValues(30, 20, 0, 100);
+
+  return CallbackReturn::SUCCESS;
+}
+
+CallbackReturn DiffDriveArduino::on_deactivate(const rclcpp_lifecycle::State & /* previous_state */)
+{
+  RCLCPP_INFO(logger_, "Stopping Controller...");
+
+  return CallbackReturn::SUCCESS;
+}
+
+return_type DiffDriveArduino::read(const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */)
+{
+
+  // TODO fix chrono duration
+
+  // Calculate time delta
+  auto new_time = std::chrono::system_clock::now();
+  std::chrono::duration<double> diff = new_time - time_;
+  double deltaSeconds = diff.count();
+  time_ = new_time;
+
+
+  if (!arduino_.connected())
+  {
+    return return_type::ERROR;
+  }
+
+  arduino_.readEncoderValues(l_wheel_.vel , r_wheel_.vel);
+
+    // Force the wheel position
+  l_wheel_.pos = l_wheel_.pos + l_wheel_.vel * deltaSeconds;
+  r_wheel_.pos = r_wheel_.pos + r_wheel_.vel * deltaSeconds;
+
+
+
+  return return_type::OK;
+
+  
+}
+
+return_type DiffDriveArduino::write(const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */)
+{
+
+  if (!arduino_.connected())
+  {
+    return return_type::ERROR;
+  }
+
+  arduino_.setMotorValues(l_wheel_.cmd, r_wheel_.cmd);
+
+
+
+
+  return return_type::OK;
+
+
+  
+}
+
+} // namespace diffdrive_arduino
+
+#include "pluginlib/class_list_macros.hpp"
+
+PLUGINLIB_EXPORT_CLASS(
+  diffdrive_arduino::DiffDriveArduino,
+  hardware_interface::SystemInterface
+)

src/diffdrive_arduino/src/diffdrive_robot.cpp

@@ -0,0 +1,39 @@
+#include <ros/ros.h>
+#include <controller_manager/controller_manager.h>
+#include "diffdrive_arduino/diffdrive_arduino.h"
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "diffdrive_robot");
+  ros::NodeHandle n("~");
+
+  DiffDriveArduino::Config robot_cfg;
+
+  // Attempt to retrieve parameters. If they don't exist, the default values from the struct will be used
+  n.getParam("left_wheel_name", robot_cfg.left_wheel_name);
+  n.getParam("right_wheel_name", robot_cfg.right_wheel_name);
+  n.getParam("baud_rate", robot_cfg.baud_rate);
+  n.getParam("device", robot_cfg.device);
+  n.getParam("enc_counts_per_rev", robot_cfg.enc_counts_per_rev);
+  n.getParam("robot_loop_rate", robot_cfg.loop_rate);
+  
+
+  DiffDriveArduino robot(robot_cfg);
+  controller_manager::ControllerManager cm(&robot);
+
+  ros::AsyncSpinner spinner(1);
+  spinner.start();
+
+  ros::Time prevTime = ros::Time(0); //ros::Time::now();
+
+  ros::Rate loop_rate(10);
+
+  while (ros::ok())
+  {
+    robot.read();
+    cm.update(robot.get_time(), robot.get_period());
+    robot.write();
+
+    loop_rate.sleep();
+  }
+}

src/diffdrive_arduino/src/fake_robot.cpp

@@ -0,0 +1,117 @@
+#include "diffdrive_arduino/fake_robot.h"
+
+
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+#include <string>
+
+
+CallbackReturn FakeRobot::on_init(const hardware_interface::HardwareInfo & info)
+{
+  if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS)
+{
+  return CallbackReturn::ERROR;
+}
+
+  RCLCPP_INFO(logger_, "Configuring...");
+
+  time_ = std::chrono::system_clock::now();
+
+  cfg_.left_wheel_name = info_.hardware_parameters["left_wheel_name"];
+  cfg_.right_wheel_name = info_.hardware_parameters["right_wheel_name"];
+
+
+  // Set up the wheels
+  // Note: It doesn't matter that we haven't set encoder counts per rev
+  // since the fake robot bypasses the encoder code completely
+
+  l_wheel_.setup(cfg_.left_wheel_name, cfg_.enc_counts_per_rev);
+  r_wheel_.setup(cfg_.right_wheel_name, cfg_.enc_counts_per_rev);
+
+  RCLCPP_INFO(logger_, "Finished Configuration");
+
+  return CallbackReturn::SUCCESS;
+}
+
+std::vector<hardware_interface::StateInterface> FakeRobot::export_state_interfaces()
+{
+  // We need to set up a position and a velocity interface for each wheel
+
+  std::vector<hardware_interface::StateInterface> state_interfaces;
+
+  state_interfaces.emplace_back(hardware_interface::StateInterface(l_wheel_.name, hardware_interface::HW_IF_VELOCITY, &l_wheel_.vel));
+  state_interfaces.emplace_back(hardware_interface::StateInterface(l_wheel_.name, hardware_interface::HW_IF_POSITION, &l_wheel_.pos));
+  state_interfaces.emplace_back(hardware_interface::StateInterface(r_wheel_.name, hardware_interface::HW_IF_VELOCITY, &r_wheel_.vel));
+  state_interfaces.emplace_back(hardware_interface::StateInterface(r_wheel_.name, hardware_interface::HW_IF_POSITION, &r_wheel_.pos));
+
+  return state_interfaces;
+}
+
+std::vector<hardware_interface::CommandInterface> FakeRobot::export_command_interfaces()
+{
+  // We need to set up a velocity command interface for each wheel
+
+  std::vector<hardware_interface::CommandInterface> command_interfaces;
+
+  command_interfaces.emplace_back(hardware_interface::CommandInterface(l_wheel_.name, hardware_interface::HW_IF_VELOCITY, &l_wheel_.cmd));
+  command_interfaces.emplace_back(hardware_interface::CommandInterface(r_wheel_.name, hardware_interface::HW_IF_VELOCITY, &r_wheel_.cmd));
+
+  return command_interfaces;
+}
+
+
+
+hardware_interface::return_type FakeRobot::read(const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */)
+{
+
+  // TODO fix chrono duration
+
+  // Calculate time delta
+  auto new_time = std::chrono::system_clock::now();
+  std::chrono::duration<double> diff = new_time - time_;
+  double deltaSeconds = diff.count();
+  time_ = new_time;
+
+
+  // Force the wheel position
+  l_wheel_.pos = l_wheel_.pos + l_wheel_.vel * deltaSeconds;
+  r_wheel_.pos = r_wheel_.pos + r_wheel_.vel * deltaSeconds;
+
+  return return_type::OK;
+
+  
+}
+
+hardware_interface::return_type FakeRobot::write(const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */)
+{
+
+  // Set the wheel velocities to directly match what is commanded
+
+  l_wheel_.vel = l_wheel_.cmd;
+  r_wheel_.vel = r_wheel_.cmd;
+
+
+  return return_type::OK;  
+}
+
+CallbackReturn FakeRobot::on_activate(const rclcpp_lifecycle::State & /* previous_state */)
+{
+  RCLCPP_INFO(logger_, "Starting Controller...");
+
+  return CallbackReturn::SUCCESS;
+}
+
+CallbackReturn FakeRobot::on_deactivate(const rclcpp_lifecycle::State & /* previous_state */)
+{
+  RCLCPP_INFO(logger_, "Stopping Controller...");
+
+  return CallbackReturn::SUCCESS;
+}
+
+#include "pluginlib/class_list_macros.hpp"
+
+PLUGINLIB_EXPORT_CLASS(
+  FakeRobot,
+  hardware_interface::SystemInterface
+)

src/diffdrive_arduino/src/wheel.cpp

@@ -0,0 +1,21 @@
+#include "diffdrive_arduino/wheel.h"
+
+#include <cmath>
+
+
+Wheel::Wheel(const std::string &wheel_name, int counts_per_rev)
+{
+  setup(wheel_name, counts_per_rev);
+}
+
+
+void Wheel::setup(const std::string &wheel_name, int counts_per_rev)
+{
+  name = wheel_name;
+  rads_per_count = (2*M_PI)/counts_per_rev;
+}
+
+double Wheel::calcEncAngle()
+{
+  return enc * rads_per_count;
+}