merged async_port into ugv_sdk

include/ugv_sdk/agilex_base.hpp

@@ -22,7 +22,7 @@
 #include <mutex>
 #include <atomic>
 
-#include "async_port/async_can.hpp"
+#include "ugv_sdk/details/async_port/async_can.hpp"
 #include "ugv_sdk/agilex_message.h"
 
 namespace westonrobot {

include/ugv_sdk/details/async_port/async_can.hpp

@@ -0,0 +1,53 @@
+/*
+ * async_can.hpp
+ *
+ * Created on: Sep 10, 2020 13:22
+ * Description:
+ *
+ * Note: CAN TX is not buffered and only the latest frame will be transmitted.
+ *  Buffered transmission will need to be added if a message has to be divided 
+ *  into multiple frames and in applications where no frame should be dropped.
+ *
+ * Copyright (c) 2020 Weston Robot Pte. Ltd.
+ */
+
+#ifndef ASYNC_CAN_HPP
+#define ASYNC_CAN_HPP
+
+#include <linux/can.h>
+
+#include <memory>
+
+#include "asio/posix/basic_stream_descriptor.hpp"
+
+#include "ugv_sdk/details/async_port/async_port_base.hpp"
+
+namespace westonrobot {
+class AsyncCAN : public AsyncPortBase,
+                 public std::enable_shared_from_this<AsyncCAN> {
+ public:
+  using ReceiveCallback = std::function<void(can_frame *rx_frame)>;
+
+ public:
+  AsyncCAN(std::string can_port = "can0");
+
+  void StopService() override;
+
+  void SetReceiveCallback(ReceiveCallback cb) { rcv_cb_ = cb; }
+  void SendFrame(const struct can_frame &frame);
+
+ private:
+  int can_fd_;
+  asio::posix::basic_stream_descriptor<> socketcan_stream_;
+
+  struct can_frame rcv_frame_;
+  ReceiveCallback rcv_cb_ = nullptr;
+
+  bool SetupPort();
+  void DefaultReceiveCallback(can_frame *rx_frame);
+  void ReadFromPort(struct can_frame &rec_frame,
+                    asio::posix::basic_stream_descriptor<> &stream);
+};
+}  // namespace westonrobot
+
+#endif /* ASYNC_CAN_HPP */

include/ugv_sdk/details/async_port/async_port_base.hpp

@@ -0,0 +1,60 @@
+/*
+ * async_port_base.hpp
+ *
+ * Created on: Jun 22, 2021 16:19
+ * Description:
+ *
+ * Copyright (c) 2021 Weston Robot Pte. Ltd.
+ */
+
+#ifndef ASYNC_PORT_BASE_HPP
+#define ASYNC_PORT_BASE_HPP
+
+#include <thread>
+#include <mutex>
+#include <functional>
+#include <iostream>
+
+#include "asio.hpp"
+
+namespace westonrobot {
+class AsyncPortBase {
+ public:
+  AsyncPortBase(std::string port) : port_(port) {}
+  virtual ~AsyncPortBase(){};
+
+  // do not allow copy
+  AsyncPortBase() = delete;
+  AsyncPortBase(const AsyncPortBase& other) = delete;
+
+  virtual bool IsOpened() const { return port_opened_; }
+
+  bool StartListening() {
+    if (SetupPort()) {
+      io_thread_ = std::thread([this]() { io_context_.run(); });
+      return true;
+    }
+    std::cerr << "Failed to setup port, please check if specified port exits "
+                 "or if you have proper permissions to access it"
+              << std::endl;
+    return false;
+  };
+  virtual void StopService() {}
+
+ protected:
+  std::string port_;
+  bool port_opened_ = false;
+
+#if ASIO_VERSION < 101200L
+  asio::io_service io_context_;
+#else
+  asio::io_context io_context_;
+#endif
+
+  std::thread io_thread_;
+
+  virtual bool SetupPort() = 0;
+};
+}  // namespace westonrobot
+
+#endif /* ASYNC_PORT_BASE_HPP */

include/ugv_sdk/details/async_port/async_serial.hpp

@@ -0,0 +1,65 @@
+/*
+ * async_serial.hpp
+ *
+ * Created on: Sep 10, 2020 12:59
+ * Description:
+ *
+ * Copyright (c) 2020 Weston Robot Pte. Ltd.
+ */
+
+#ifndef ASYNC_SERIAL_HPP
+#define ASYNC_SERIAL_HPP
+
+#include <cstdint>
+#include <memory>
+#include <array>
+#include <mutex>
+
+#include "ugv_sdk/details/async_port/async_port_base.hpp"
+#include "ugv_sdk/details/async_port/ring_buffer.hpp"
+
+namespace westonrobot {
+class AsyncSerial : public AsyncPortBase,
+                    public std::enable_shared_from_this<AsyncSerial> {
+ public:
+  using ReceiveCallback =
+      std::function<void(uint8_t *data, const size_t bufsize, size_t len)>;
+
+ public:
+  AsyncSerial(std::string port_name, uint32_t baud_rate = 115200);
+
+  void StopService() override;
+
+  bool IsOpened() const override { return serial_port_.is_open(); }
+
+  void SetReceiveCallback(ReceiveCallback cb) { rcv_cb_ = cb; }
+  void SendBytes(const uint8_t *bytes, size_t length);
+
+  void SetBaudRate(unsigned baudrate);
+  void SetHardwareFlowControl(bool enabled) { hwflow_ = enabled; }
+
+ private:
+  asio::serial_port serial_port_;
+  uint32_t baud_rate_ = 115200;
+  bool hwflow_ = false;
+  ReceiveCallback rcv_cb_ = nullptr;
+
+
+  // tx/rx buffering
+  static constexpr uint32_t rxtx_buffer_size = 1024 * 8;
+  // rx buffer
+  std::array<uint8_t, rxtx_buffer_size> rx_buf_;
+  // tx buffer
+  uint8_t tx_buf_[rxtx_buffer_size];
+  RingBuffer<uint8_t, rxtx_buffer_size> tx_rbuf_;
+  std::recursive_mutex tx_mutex_;
+  bool tx_in_progress_ = false;
+
+  bool SetupPort();
+  void DefaultReceiveCallback(uint8_t *data, const size_t bufsize, size_t len);
+  void ReadFromPort();
+  void WriteToPort(bool check_if_busy);
+};
+}  // namespace westonrobot
+
+#endif /* ASYNC_SERIAL_HPP */

include/ugv_sdk/details/async_port/ring_buffer.hpp

@@ -0,0 +1,138 @@
+/*
+ * ring_buffer.hpp
+ *
+ * Created on: Dec 08, 2019 22:22
+ * Description:
+ *
+ * Requirements:
+ *  1. Size of buffer must be a power of 2
+ *  2. Maximum buffer size is 2^(number_of_bits_of(size_t)-1)
+ *
+ * Implementation details:
+ *
+ * - Initial state (empty)
+ * [0][1][2][3]...[N]
+ *  ^
+ * R/W
+ *
+ * - Add one element
+ * [D][1][2][3]...[N]
+ *  ^  ^
+ *  R  W
+ *
+ * - Buffer gets full when last element X is inserted
+ * [X][D][D][D]...[D]
+ *     ^
+ *    W/R (W>R)
+ *
+ * - Buffer data overwritten by new element Y after getting full
+ * [X][Y][D][D]...[D]
+ *        ^
+ *       W/R (W>R)
+ *
+ * Reference:
+ *  [1] https://embeddedartistry.com/blog/2017/05/17/creating-a-circular-buffer-in-c-and-c/
+ *  [2] https://stackoverflow.com/questions/10527581/why-must-a-ring-buffer-size-be-a-power-of-2
+ *  [3] https://stackoverflow.com/questions/9718116/improving-c-circular-buffer-efficiency
+ *  [4] https://www.snellman.net/blog/archive/2016-12-13-ring-buffers/
+ *  [5] tttp://www.trytoprogram.com/c-examples/c-program-to-test-if-a-number-is-a-power-of-2/
+ *
+ * Copyright (c) 2019 Weston Robot Pte. Ltd.
+ */
+
+#ifndef RING_BUFFER_HPP
+#define RING_BUFFER_HPP
+
+#include <cstdint>
+#include <cstddef>
+
+#include <array>
+#include <iostream>
+
+namespace westonrobot {
+template <typename T = uint8_t, std::size_t N = 1024>
+class RingBuffer {
+ public:
+  // Init and reset of buffer
+  RingBuffer() {
+    // assert size is a power of 2
+    static_assert((N != 0) && ((N & (N - 1)) == 0), 
+        "Size of ring buffer has to be 2^n, where n is a positive integer");
+
+    size_ = N;
+    size_mask_ = size_ - 1;
+    write_index_ = 0;
+    read_index_ = 0;
+  }
+
+  void Reset() {
+    write_index_ = 0;
+    read_index_ = 0;
+  }
+
+  // Buffer size information
+  bool IsEmpty() { return (read_index_ == write_index_); }
+  bool IsFull() { return (size_ == GetOccupiedSize()); }
+
+  std::size_t GetFreeSize() const { return (size_ - GetOccupiedSize()); }
+  std::size_t GetOccupiedSize() const { return (write_index_ - read_index_); };
+
+  std::array<T, N> GetBuffer() const { return buffer_; }
+
+  // Read/Write functions
+  std::size_t Read(T data[], std::size_t btr) {
+    for (int i = 0; i < btr; ++i) {
+      if (ReadByte(&data[i]) == 0) return i;
+    }
+    return btr;
+  }
+  std::size_t Peek(T data[], std::size_t btp) {
+    for (int i = 0; i < btp; ++i) {
+      if (PeekByteAt(&data[i], i) == 0) return i;
+    }
+    return btp;
+  }
+  std::size_t Write(const T new_data[], std::size_t btw) {
+    for (int i = 0; i < btw; ++i) {
+      if (WriteByte(new_data[i]) == 0) return i;
+    }
+    return btw;
+  }
+
+  void PrintBuffer() const {
+    std::cout << "read index: " << read_index_
+              << " , write index: " << write_index_ << std::endl;
+    std::cout << "buffer content: " << std::endl;
+    for (int i = 0; i < N; ++i)
+      std::cout << "[" << i << "]"
+                << " " << static_cast<int>(buffer_[i]) << std::endl;
+  }
+
+ private:
+  std::array<T, N> buffer_;  // buffer memory to store data
+  std::size_t size_;         // size of allocated memory area
+  std::size_t size_mask_;    // for internal indexing management
+  std::size_t write_index_;  // place new data to be written
+  std::size_t read_index_;   // place earliest written data to be read from
+
+  size_t ReadByte(T *data) {
+    if (IsEmpty()) return 0;
+    *data = buffer_[read_index_++ & size_mask_];
+    return 1;
+  }
+
+  size_t PeekByteAt(T *data, size_t n) {
+    if (n >= GetOccupiedSize()) return 0;
+    *data = buffer_[(read_index_ + n) & size_mask_];
+    return 1;
+  }
+
+  size_t WriteByte(const T &new_data) {
+    if (GetOccupiedSize() == size_) return 0;
+    buffer_[(write_index_++) & size_mask_] = new_data;
+    return 1;
+  }
+};
+}  // namespace westonrobot
+
+#endif /* RING_BUFFER_HPP */