merged async_port into ugv_sdk

.gitlab-ci.yml

@@ -3,11 +3,6 @@ xenial-cpp-build:
   stage: build
   image: rduweston/ubuntu-ci:16.04
   script:
-    - cd / && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/public/async_port.git
-    - cd /async_port && mkdir build && cd build
-    - cmake .. && make
-    - make install
-    - cd /builds/$CI_PROJECT_PATH
     - mkdir build && cd build
     - cmake -DCMAKE_BUILD_TYPE=Release ..
     - cmake --build . && cpack
@@ -16,11 +11,6 @@ bionic-cpp-build:
   stage: build
   image: rduweston/ubuntu-ci:18.04
   script:
-    - cd / && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/public/async_port.git
-    - cd /async_port && mkdir build && cd build
-    - cmake .. && make
-    - make install
-    - cd /builds/$CI_PROJECT_PATH
     - mkdir build && cd build
     - cmake -DCMAKE_BUILD_TYPE=Release ..
     - cmake --build . && cpack
@@ -29,11 +19,6 @@ focal-cpp-build:
   stage: build
   image: rduweston/ubuntu-ci:20.04
   script:
-    - cd / && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/public/async_port.git
-    - cd /async_port && mkdir build && cd build
-    - cmake .. && make
-    - make install
-    - cd /builds/$CI_PROJECT_PATH
     - mkdir build && cd build
     - cmake -DCMAKE_BUILD_TYPE=Release ..
     - cmake --build . && cpack
@@ -45,8 +30,6 @@ kinetic-catkin-build:
   script:
     - mkdir -p /catkin_ws/src/ugv_sdk/ugv_sdk && cd /catkin_ws/src && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_init_workspace"
     - cp -r /builds/$CI_PROJECT_PATH /catkin_ws/src/ugv_sdk
-    - cd /catkin_ws/src/ugv_sdk
-    - cd /catkin_ws/src && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/public/async_port.git
     - cd /catkin_ws && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_make"
 
 kinetic-catkin-isolated-build:
@@ -55,8 +38,6 @@ kinetic-catkin-isolated-build:
   script:
     - mkdir -p /catkin_ws/src/ugv_sdk/ugv_sdk && cd /catkin_ws/src && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_init_workspace"
     - cp -r /builds/$CI_PROJECT_PATH /catkin_ws/src/ugv_sdk
-    - cd /catkin_ws/src/ugv_sdk 
-    - cd /catkin_ws/src && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/public/async_port.git
     - cd /catkin_ws && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_make_isolated --install"
 
 melodic-catkin-build:
@@ -65,8 +46,6 @@ melodic-catkin-build:
   script:
     - mkdir -p /catkin_ws/src/ugv_sdk && cd /catkin_ws/src && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_init_workspace"
     - cp -r /builds/$CI_PROJECT_PATH /catkin_ws/src/ugv_sdk
-    - cd /catkin_ws/src/ugv_sdk
-    - cd /catkin_ws/src && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/public/async_port.git
     - cd /catkin_ws && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_make"
 
 melodic-catkin-isolated-build:
@@ -75,8 +54,6 @@ melodic-catkin-isolated-build:
   script:
     - mkdir -p /catkin_ws/src/ugv_sdk && cd /catkin_ws/src && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_init_workspace"
     - cp -r /builds/$CI_PROJECT_PATH /catkin_ws/src/ugv_sdk
-    - cd /catkin_ws/src/ugv_sdk
-    - cd /catkin_ws/src && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/public/async_port.git
     - cd /catkin_ws && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_make_isolated --install"
 
 noetic-catkin-build:
@@ -85,8 +62,6 @@ noetic-catkin-build:
   script:
     - mkdir -p /catkin_ws/src/ugv_sdk && cd /catkin_ws/src && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_init_workspace"
     - cp -r /builds/$CI_PROJECT_PATH /catkin_ws/src/ugv_sdk
-    - cd /catkin_ws/src/ugv_sdk
-    - cd /catkin_ws/src && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/public/async_port.git
     - cd /catkin_ws && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_make"
 
 noetic-catkin-isolated-build:
@@ -95,6 +70,4 @@ noetic-catkin-isolated-build:
   script:
     - mkdir -p /catkin_ws/src/ugv_sdk && cd /catkin_ws/src && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_init_workspace"
     - cp -r /builds/$CI_PROJECT_PATH /catkin_ws/src/ugv_sdk
-    - cd /catkin_ws/src/ugv_sdk
-    - cd /catkin_ws/src && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/public/async_port.git
     - cd /catkin_ws && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_make_isolated --install"

CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.5.1)
-project(ugv_sdk VERSION 0.1.5)
+project(ugv_sdk VERSION 0.2.0)
 
 find_program(CCACHE_PROGRAM ccache)
 if(CCACHE_PROGRAM)
@@ -38,7 +38,6 @@ endif()
 set(USER_CMAKE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake")
 list(APPEND CMAKE_MODULE_PATH "${USER_CMAKE_PATH}/modules")
 list(APPEND CMAKE_PREFIX_PATH "/usr/lib/${CMAKE_SYSTEM_PROCESSOR}-linux-gnu/cmake")
-list(APPEND CMAKE_PREFIX_PATH "/opt/weston_robot/lib/cmake")
 
 ## Set compiler to use c++ 11 features
 set(CMAKE_CXX_STANDARD 11)
@@ -67,20 +66,14 @@ if(BUILD_WITHOUT_ROS)
     set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/${CMAKE_INSTALL_BINDIR})
 endif()
 
-if(BUILD_WITHOUT_ROS)
-    find_package(async_port REQUIRED)
-else()
-    find_package(catkin REQUIRED COMPONENTS async_port)
-    include_directories(
-        include
-        ${catkin_INCLUDE_DIRS}
-    )
-endif()
-
-# Build libraries
+## Build libraries
 find_package(Threads REQUIRED)
 
-add_library(${PROJECT_NAME} 
+# add sdk main library
+add_library(${PROJECT_NAME}
+    # async_port from weston robot
+    src/async_port/async_serial.cpp
+    src/async_port/async_can.cpp
     # agx common
     src/agilex_base.cpp
     src/agx_msg_parser.c
@@ -91,14 +84,11 @@ add_library(${PROJECT_NAME}
     # src/bunker_base.cpp
     src/ranger_base.cpp
 )
-if(BUILD_WITHOUT_ROS)
-    target_link_libraries(${PROJECT_NAME} westonrobot::async_port Threads::Threads)
-else()
-    target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} Threads::Threads)
-endif() 
+target_link_libraries(${PROJECT_NAME} PUBLIC Threads::Threads)
 target_include_directories(${PROJECT_NAME} PUBLIC
 	$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-	$<INSTALL_INTERFACE:include>
+    $<INSTALL_INTERFACE:include/ugv_sdk/details/asio/include>
+    $<INSTALL_INTERFACE:include>
     PRIVATE src)
     
 # Build apps
@@ -149,7 +139,7 @@ if(BUILD_WITHOUT_ROS) # BUILD_WITHOUT_ROS
     endforeach()
 
     # targets to install
-    install(TARGETS ${PROJECT_NAME}
+    install(TARGETS ${PROJECT_NAME} 
         EXPORT ugv_sdkTargets
         LIBRARY DESTINATION lib
         ARCHIVE DESTINATION lib
@@ -168,7 +158,7 @@ if(BUILD_WITHOUT_ROS) # BUILD_WITHOUT_ROS
 
     install(EXPORT ugv_sdkTargets
         FILE ugv_sdkTargets.cmake
-        NAMESPACE westonrobot::
+        # NAMESPACE UGVSDK::
         DESTINATION lib/cmake/ugv_sdk)
 
     configure_file(cmake/ugv_sdkConfig.cmake.in ugv_sdkConfig.cmake @ONLY)
@@ -178,18 +168,18 @@ if(BUILD_WITHOUT_ROS) # BUILD_WITHOUT_ROS
 
     # Packaging support
     set(CPACK_PACKAGE_VENDOR "Weston Robot")
-    set(CPACK_PACKAGE_DESCRIPTION_SUMMARY "Weston Robot Platform - SDK")
+    set(CPACK_PACKAGE_DESCRIPTION_SUMMARY "UGV SDK for Robots from AgileX/WestonRobot")
     set(CPACK_PACKAGE_VERSION_MAJOR ${PROJECT_VERSION_MAJOR})
     set(CPACK_PACKAGE_VERSION_MINOR ${PROJECT_VERSION_MINOR})
     set(CPACK_PACKAGE_VERSION_PATCH ${PROJECT_VERSION_PATCH})
-    set(CPACK_PACKAGING_INSTALL_PREFIX "/opt/weston_robot")
+    # set(CPACK_PACKAGING_INSTALL_PREFIX "/opt/weston_robot")
     set(CPACK_RESOURCE_FILE_LICENSE "${CMAKE_CURRENT_SOURCE_DIR}/LICENSE")
-    # set(CPACK_RESOURCE_FILE_README "${CMAKE_CURRENT_SOURCE_DIR}/README.md")
+    set(CPACK_RESOURCE_FILE_README "${CMAKE_CURRENT_SOURCE_DIR}/README.md")
 
     set(CPACK_GENERATOR "DEB")
     set(CPACK_DEBIAN_FILE_NAME DEB-DEFAULT)
     set(CPACK_DEBIAN_PACKAGE_MAINTAINER "Ruixiang Du (ruixiang.du@westonrobot.com)") 
-    # set(CPACK_DEBIAN_PACKAGE_DEPENDS "libasio-dev")
+    set(CPACK_DEBIAN_PACKAGE_DEPENDS "libasio-dev")
     set(CPACK_SOURCE_IGNORE_FILES .git dist .*build.* /\\\\.DS_Store)
     include(CPack)
 
@@ -198,8 +188,8 @@ else() # BUILD_WITHOUT_ROS
     catkin_package(
         LIBRARIES ${PROJECT_NAME}
         INCLUDE_DIRS include 
-         CATKIN_DEPENDS async_port
-        #  DEPENDS system_lib
+        # CATKIN_DEPENDS async_port
+        # DEPENDS system_lib
     )
 
     ## Add catkin install targets

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 */

sample/CMakeLists.txt

@@ -0,0 +1,8 @@
+cmake_minimum_required(VERSION 3.10.2)
+project(sdk_sample)
+
+find_package(ugv_sdk REQUIRED)
+find_package(Threads REQUIRED)
+
+add_executable(app_tracer_demo tracer_demo.cpp)
+target_link_libraries(app_tracer_demo PRIVATE ugv_sdk Threads::Threads)

sample/tracer_demo.cpp

@@ -0,0 +1,99 @@
+/*
+ * demo_tracer_can.cpp
+ *
+ * Created on: Jun 12, 2019 05:03
+ * Description:
+ *
+ * Copyright (c) 2019 Ruixiang Du (rdu)
+ */
+
+#include "ugv_sdk/tracer_base.hpp"
+
+using namespace westonrobot;
+
+int main(int argc, char **argv) {
+  std::string device_name;
+
+  if (argc == 2) {
+    device_name = {argv[1]};
+    std::cout << "Specified CAN: " << device_name << std::endl;
+  } else {
+    std::cout << "Usage: app_tracer_demo <interface>" << std::endl
+              << "Example 1: ./app_tracer_demo can0" << std::endl;
+    return -1;
+  }
+
+  TracerBase tracer;
+  tracer.Connect(device_name);
+
+  tracer.EnableCommandedMode();
+  //   tracer.DisableTimeout();
+
+  // light control
+  std::cout << "Light: const off" << std::endl;
+  tracer.SetLightCommand({CONST_OFF, 0});
+  //   usleep(50000);
+  sleep(3);
+  std::cout << "Light: const on" << std::endl;
+  tracer.SetLightCommand({CONST_ON, 0});
+  //   usleep(50000);
+  sleep(3);
+  std::cout << "Light: breath" << std::endl;
+  tracer.SetLightCommand({BREATH, 0});
+  //   usleep(50000);
+  sleep(8);
+  std::cout << "Light: custom 90-80" << std::endl;
+  tracer.SetLightCommand({CUSTOM, 90});
+  //   usleep(50000);
+  sleep(3);
+  std::cout << "Light: diabled cmd control" << std::endl;
+  tracer.SetLightCommand(TracerLightCmd());
+
+  int count = 0;
+  while (true) {
+    // motion control
+    if (count < 5) {
+      std::cout << "Motor: 0.2, 0.0" << std::endl;
+      tracer.SetMotionCommand(0.2, 0.0);
+    } else if (count < 10) {
+      std::cout << "Motor: 0.8, 0.3" << std::endl;
+      tracer.SetMotionCommand(0.8, 0.3);
+    } else if (count < 15) {
+      std::cout << "Motor: 1.5, 0.5" << std::endl;
+      tracer.SetMotionCommand(1.5, 0.5);
+    } else if (count < 20) {
+      std::cout << "Motor: 1.0, 0.3" << std::endl;
+      tracer.SetMotionCommand(1.0, 0.3);
+    } else if (count < 25) {
+      std::cout << "Motor: 0.0, 0.0" << std::endl;
+      tracer.SetMotionCommand(0.0, 0.0);
+    } else if (count < 30) {
+      std::cout << "Motor: -0.5, -0.3" << std::endl;
+      tracer.SetMotionCommand(-0.5, -0.3);
+    } else if (count < 35) {
+      std::cout << "Motor: -1.0, -0.5" << std::endl;
+      tracer.SetMotionCommand(-1.0, -0.5);
+    } else if (count < 40) {
+      std::cout << "Motor: 0.0, 0.0," << std::endl;
+      tracer.SetMotionCommand(0.0, 0.0);
+    }
+    // tracer.SetMotionCommand(0.8, 0.8);
+
+    auto state = tracer.GetTracerState();
+    std::cout << "-------------------------------" << std::endl;
+    std::cout << "count: " << count << std::endl;
+    std::cout << "control mode: " << static_cast<int>(state.system_state.control_mode)
+              << " , vehicle state: " << static_cast<int>(state.system_state.vehicle_state)
+              << std::endl;
+    std::cout << "battery voltage: " << state.system_state.battery_voltage << std::endl;
+    std::cout << "velocity (linear, angular): " << state.motion_state.linear_velocity << ", "
+              << state.motion_state.angular_velocity << std::endl;
+    std::cout << "-------------------------------" << std::endl;
+
+    // usleep(20000);
+    sleep(1);
+    ++count;
+  }
+
+  return 0;
+}

src/async_port/async_can.cpp

@@ -0,0 +1,124 @@
+/*
+ * async_can.cpp
+ *
+ * Created on: Sep 10, 2020 13:23
+ * Description:
+ *
+ * Copyright (c) 2020 Weston Robot Pte. Ltd.
+ */
+
+#include "ugv_sdk/details/async_port/async_can.hpp"
+
+#include <net/if.h>
+#include <poll.h>
+#include <string.h>
+#include <unistd.h>
+#include <sys/ioctl.h>
+#include <linux/can.h>
+
+#include <iostream>
+
+namespace westonrobot {
+AsyncCAN::AsyncCAN(std::string can_port)
+    : AsyncPortBase(can_port), socketcan_stream_(io_context_) {}
+
+bool AsyncCAN::SetupPort() {
+  try {
+    const size_t iface_name_size = strlen(port_.c_str()) + 1;
+    if (iface_name_size > IFNAMSIZ) return false;
+
+    can_fd_ = socket(PF_CAN, SOCK_RAW | SOCK_NONBLOCK, CAN_RAW);
+    if (can_fd_ < 0) return false;
+
+    struct ifreq ifr;
+    memset(&ifr, 0, sizeof(ifr));
+    memcpy(ifr.ifr_name, port_.c_str(), iface_name_size);
+
+    const int ioctl_result = ioctl(can_fd_, SIOCGIFINDEX, &ifr);
+    if (ioctl_result < 0) StopService();
+
+    struct sockaddr_can addr;
+    memset(&addr, 0, sizeof(addr));
+    addr.can_family = AF_CAN;
+    addr.can_ifindex = ifr.ifr_ifindex;
+
+    const int bind_result =
+        bind(can_fd_, (struct sockaddr *)&addr, sizeof(addr));
+    if (bind_result < 0) StopService();
+
+    port_opened_ = true;
+    std::cout << "Start listening to port: " << port_ << std::endl;
+  } catch (std::system_error &e) {
+    std::cout << e.what();
+    return false;
+  }
+
+  // give some work to io_service to start async io chain
+  socketcan_stream_.assign(can_fd_);
+
+#if ASIO_VERSION < 101200L
+  io_context_.post(std::bind(&AsyncCAN::ReadFromPort, this,
+                             std::ref(rcv_frame_),
+                             std::ref(socketcan_stream_)));
+#else
+  asio::post(io_context_,
+             std::bind(&AsyncCAN::ReadFromPort, this, std::ref(rcv_frame_),
+                       std::ref(socketcan_stream_)));
+#endif
+
+  return true;
+}
+
+void AsyncCAN::StopService() {
+  // release port fd
+  const int close_result = ::close(can_fd_);
+  can_fd_ = -1;
+
+  // stop io thread
+  io_context_.stop();
+  if (io_thread_.joinable()) io_thread_.join();
+  io_context_.reset();
+
+  port_opened_ = false;
+}
+
+void AsyncCAN::DefaultReceiveCallback(can_frame *rx_frame) {
+  std::cout << std::hex << rx_frame->can_id << "  ";
+  for (int i = 0; i < rx_frame->can_dlc; i++)
+    std::cout << std::hex << int(rx_frame->data[i]) << " ";
+  std::cout << std::dec << std::endl;
+}
+
+void AsyncCAN::ReadFromPort(struct can_frame &rec_frame,
+                            asio::posix::basic_stream_descriptor<> &stream) {
+  auto sthis = shared_from_this();
+  stream.async_read_some(
+      asio::buffer(&rec_frame, sizeof(rec_frame)),
+      [sthis](asio::error_code error, size_t bytes_transferred) {
+        if (error) {
+          sthis->StopService();
+          return;
+        }
+
+        if (sthis->rcv_cb_ != nullptr)
+          sthis->rcv_cb_(&sthis->rcv_frame_);
+        else
+          sthis->DefaultReceiveCallback(&sthis->rcv_frame_);
+
+        sthis->ReadFromPort(std::ref(sthis->rcv_frame_),
+                            std::ref(sthis->socketcan_stream_));
+      });
+}
+
+void AsyncCAN::SendFrame(const struct can_frame &frame) {
+  socketcan_stream_.async_write_some(
+      asio::buffer(&frame, sizeof(frame)),
+      [](asio::error_code error, size_t bytes_transferred) {
+        if (error) {
+          std::cerr << "Failed to send CAN frame" << std::endl;
+        }
+        // std::cout << "frame sent" << std::endl;
+      });
+}
+
+}  // namespace westonrobot

src/async_port/async_serial.cpp

@@ -0,0 +1,155 @@
+/*
+ * async_serial.cpp
+ *
+ * Created on: Sep 10, 2020 13:00
+ * Description:
+ *
+ * Copyright (c) 2020 Weston Robot Pte. Ltd.
+ */
+
+#include "ugv_sdk/details/async_port/async_serial.hpp"
+
+#if defined(__linux__)
+#include <linux/serial.h>
+#endif
+
+#include <cstring>
+#include <iostream>
+
+namespace westonrobot {
+
+AsyncSerial::AsyncSerial(std::string port_name, uint32_t baud_rate)
+    : AsyncPortBase(port_name),
+      baud_rate_(baud_rate),
+      serial_port_(io_context_) {}
+
+void AsyncSerial::SetBaudRate(unsigned baudrate) {
+  serial_port_.set_option(asio::serial_port_base::baud_rate(baudrate));
+}
+
+bool AsyncSerial::SetupPort() {
+  using SPB = asio::serial_port_base;
+
+  try {
+    serial_port_.open(port_);
+
+    // Set baudrate and 8N1 mode
+    serial_port_.set_option(SPB::baud_rate(baud_rate_));
+    serial_port_.set_option(SPB::character_size(8));
+    serial_port_.set_option(SPB::parity(SPB::parity::none));
+    serial_port_.set_option(SPB::stop_bits(SPB::stop_bits::one));
+    serial_port_.set_option(SPB::flow_control(
+        (hwflow_) ? SPB::flow_control::hardware : SPB::flow_control::none));
+
+#if defined(__linux__)
+    // Enable low latency mode on Linux
+    {
+      int fd = serial_port_.native_handle();
+      struct serial_struct ser_info;
+      ioctl(fd, TIOCGSERIAL, &ser_info);
+      ser_info.flags |= ASYNC_LOW_LATENCY;
+      ioctl(fd, TIOCSSERIAL, &ser_info);
+    }
+#endif
+
+    port_opened_ = true;
+    std::cout << "Start listening to port: " << port_ << "@" << baud_rate_
+              << std::endl;
+
+  } catch (std::system_error &e) {
+    std::cout << e.what();
+    return false;
+  }
+
+  // give some work to io_service to start async io chain
+#if ASIO_VERSION < 101200L
+  io_context_.post(std::bind(&AsyncSerial::ReadFromPort, this));
+#else
+  asio::post(io_context_, std::bind(&AsyncSerial::ReadFromPort, this));
+#endif
+
+  return true;
+}
+
+void AsyncSerial::StopService() {
+  if (IsOpened()) {
+    serial_port_.cancel();
+    serial_port_.close();
+  }
+
+  // stop io thread
+  io_context_.stop();
+  if (io_thread_.joinable()) io_thread_.join();
+  io_context_.reset();
+
+  port_opened_ = false;
+}
+
+void AsyncSerial::DefaultReceiveCallback(uint8_t *data, const size_t bufsize,
+                                         size_t len) {}
+
+void AsyncSerial::ReadFromPort() {
+  auto sthis = shared_from_this();
+  serial_port_.async_read_some(
+      asio::buffer(rx_buf_),
+      [sthis](asio::error_code error, size_t bytes_transferred) {
+        if (error) {
+          sthis->StopService();
+          return;
+        }
+
+        if (sthis->rcv_cb_ != nullptr) {
+          sthis->rcv_cb_(sthis->rx_buf_.data(), sthis->rx_buf_.size(),
+                         bytes_transferred);
+        } else {
+          sthis->DefaultReceiveCallback(
+              sthis->rx_buf_.data(), sthis->rx_buf_.size(), bytes_transferred);
+        }
+        sthis->ReadFromPort();
+      });
+}
+
+void AsyncSerial::WriteToPort(bool check_if_busy) {
+  // do nothing if an async tx has already been initiated
+  if (check_if_busy && tx_in_progress_) return;
+
+  std::lock_guard<std::recursive_mutex> lock(tx_mutex_);
+  if (tx_rbuf_.IsEmpty()) return;
+
+  auto sthis = shared_from_this();
+  tx_in_progress_ = true;
+  auto len = tx_rbuf_.Read(tx_buf_, tx_rbuf_.GetOccupiedSize());
+  serial_port_.async_write_some(
+      asio::buffer(tx_buf_, len),
+      [sthis](asio::error_code error, size_t bytes_transferred) {
+        if (error) {
+          sthis->StopService();
+          return;
+        }
+        std::lock_guard<std::recursive_mutex> lock(sthis->tx_mutex_);
+        if (sthis->tx_rbuf_.IsEmpty()) {
+          sthis->tx_in_progress_ = false;
+          return;
+        } else {
+          sthis->WriteToPort(false);
+        }
+      });
+}
+
+void AsyncSerial::SendBytes(const uint8_t *bytes, size_t length) {
+  if (!IsOpened()) {
+    std::cerr << "Failed to send, port closed" << std::endl;
+    return;
+  }
+  assert(length < rxtx_buffer_size);
+  std::lock_guard<std::recursive_mutex> lock(tx_mutex_);
+  if (tx_rbuf_.GetFreeSize() < length) {
+    throw std::length_error(
+        "AsyncSerial::SendBytes: tx buffer overflow, try to slow down sending "
+        "data");
+  }
+  tx_rbuf_.Write(bytes, length);
+  io_context_.post(
+      std::bind(&AsyncSerial::WriteToPort, shared_from_this(), true));
+}
+}  // namespace westonrobot