async_port: updated latest async_port impl from wrp_sdk

include/ugv_sdk/details/async_port/async_can.hpp

@@ -5,7 +5,7 @@
  * 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 
+ *  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.
@@ -16,34 +16,53 @@
 
 #include <linux/can.h>
 
+#include <atomic>
 #include <memory>
+#include <thread>
+#include <functional>
 
+#include "asio.hpp"
 #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> {
+class AsyncCAN : public std::enable_shared_from_this<AsyncCAN> {
  public:
   using ReceiveCallback = std::function<void(can_frame *rx_frame)>;
 
  public:
   AsyncCAN(std::string can_port = "can0");
+  ~AsyncCAN();
 
-  void StopService() override;
+  // do not allow copy
+  AsyncCAN(const AsyncCAN &) = delete;
+  AsyncCAN &operator=(const AsyncCAN &) = delete;
+
+  // Public API
+  bool Open();
+  void Close();
+  bool IsOpened() const;
 
   void SetReceiveCallback(ReceiveCallback cb) { rcv_cb_ = cb; }
   void SendFrame(const struct can_frame &frame);
 
  private:
+  std::string port_;
+  std::atomic<bool> port_opened_{false};
+
+#if ASIO_VERSION < 101200L
+  asio::io_service io_context_;
+#else
+  asio::io_context io_context_;
+#endif
+
+  std::thread io_thread_;
+
   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);

include/ugv_sdk/details/async_port/async_port_base.hpp

@@ -1,61 +0,0 @@
-/*
- * 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
-        << "[ERROR] 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

@@ -14,48 +14,63 @@
 #include <memory>
 #include <array>
 #include <mutex>
+#include <thread>
+#include <functional>
+
+#include "asio.hpp"
 
-#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> {
+class AsyncSerial : 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);
+  ~AsyncSerial();
 
-  void StopService() override;
+  // do not allow copy
+  AsyncSerial(const AsyncSerial &) = delete;
+  AsyncSerial &operator=(const AsyncSerial &) = delete;
 
-  bool IsOpened() const override { return serial_port_.is_open(); }
+  // Public API
+  void SetBaudRate(unsigned baudrate);
+  void SetHardwareFlowControl(bool enabled);
+
+  bool Open();
+  void Close();
+  bool IsOpened() const;
 
   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:
+  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_;
+
+  // serial port
   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);

include/ugv_sdk/details/robot_base/agilex_base.hpp

@@ -15,6 +15,7 @@
 #include <thread>
 #include <mutex>
 #include <atomic>
+#include <iostream>
 
 #include "ugv_sdk/details/async_port/async_can.hpp"
 #include "ugv_sdk/details/interface/robot_common_interface.hpp"
@@ -60,7 +61,7 @@ class AgilexBase : public RobotCommonInterface {
   // must be called at a frequency >= 50Hz
   void SendMotionCommand(double linear_vel, double angular_vel,
                          double lateral_vel, double steering_angle) {
-    if (can_connected_) {
+    if (can_->IsOpened()) {
       // motion control message
       AgxMessage msg;
       if (parser_.GetParserProtocolVersion() == ProtocolVersion::AGX_V1) {
@@ -94,7 +95,7 @@ class AgilexBase : public RobotCommonInterface {
   // one-shot light command
   void SendLightCommand(AgxLightMode front_mode, uint8_t front_custom_value,
                         AgxLightMode rear_mode, uint8_t rear_custom_value) {
-    if (can_connected_) {
+    if (can_->IsOpened()) {
       AgxMessage msg;
       msg.type = AgxMsgLightCommand;
       msg.body.light_command_msg.enable_cmd_ctrl = true;
@@ -110,7 +111,7 @@ class AgilexBase : public RobotCommonInterface {
   }
 
   void DisableLightControl() {
-    if (can_connected_) {
+    if (can_->IsOpened()) {
       AgxMessage msg;
       msg.type = AgxMsgLightCommand;
 
@@ -124,7 +125,7 @@ class AgilexBase : public RobotCommonInterface {
 
   // motion mode change
   void SetMotionMode(uint8_t mode) {
-    if (can_connected_) {
+    if (can_->IsOpened()) {
       AgxMessage msg;
       msg.type = AgxMsgSetMotionModeCommand;
       msg.body.motion_mode_msg.motion_mode = mode;
@@ -169,7 +170,6 @@ class AgilexBase : public RobotCommonInterface {
   /* feedback group 3: common sensor */
 
   // communication interface
-  bool can_connected_ = false;
   std::shared_ptr<AsyncCAN> can_;
 
   // connect to roboot from CAN or serial
@@ -177,12 +177,11 @@ class AgilexBase : public RobotCommonInterface {
   bool ConnectPort(std::string dev_name, CANFrameRxCallback cb) {
     can_ = std::make_shared<AsyncCAN>(dev_name);
     can_->SetReceiveCallback(cb);
-    can_connected_ = can_->StartListening();
-    return can_connected_;
+    return can_->Open();
   }
 
   void DisconnectPort() {
-    if (can_connected_) can_->StopService();
+    if (can_->IsOpened()) can_->Close();
   }
 
   void SetBrakeMode(AgxBrakeMode mode) {