Merge pull request #17 from westonrobot/devel

async_port: updated latest async_port impl from wrp_sdk

demo/scout_demo/scout_mini_omni_demo.cpp

@@ -76,7 +76,7 @@ int main(int argc, char **argv) {
   while (true) {
     // motion control
     std::cout << "Setting motion command: 0.0, 0,0, 0.8" << std::endl;
-    scout->SetMotionCommand(0.0, 0.0, 0.8);
+    scout->SetMotionCommand(0.0, 0.3, 0.0);
 
     // get robot state
     auto state = scout->GetRobotState();

demo/tracer_demo/tracer_robot_demo.cpp

@@ -59,7 +59,7 @@ int main(int argc, char **argv) {
   while (true) {
     // motion control
     std::cout << "Motor: 1.0, 0" << std::endl;
-    tracer->SetMotionCommand(1.0, 0.0);
+    tracer->SetMotionCommand(0.0, 0.0);
 
     // get robot state
     auto state = tracer->GetRobotState();

include/ugv_sdk/details/async_port/async_can.hpp

@@ -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,
+class AsyncCAN : public std::enable_shared_from_this<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,
+class AsyncSerial : public std::enable_shared_from_this<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_->Open();
-    return can_connected_;
   }
 
   void DisconnectPort() {
-    if (can_connected_) can_->StopService();
+    if (can_->IsOpened()) can_->Close();
   }
 
   void SetBrakeMode(AgxBrakeMode mode) {

src/async_port/async_can.cpp

@@ -20,9 +20,11 @@
 
 namespace westonrobot {
 AsyncCAN::AsyncCAN(std::string can_port)
-    : AsyncPortBase(can_port), socketcan_stream_(io_context_) {}
+    : port_(can_port), socketcan_stream_(io_context_) {}
 
-bool AsyncCAN::SetupPort() {
+AsyncCAN::~AsyncCAN() { Close(); }
+
+bool AsyncCAN::Open() {
   try {
     const size_t iface_name_size = strlen(port_.c_str()) + 1;
     if (iface_name_size > IFNAMSIZ) return false;
@@ -36,7 +38,7 @@ bool AsyncCAN::SetupPort() {
 
     const int ioctl_result = ioctl(can_fd_, SIOCGIFINDEX, &ifr);
     if (ioctl_result < 0) {
-      StopService();
+      Close();
       return false;
     }
 
@@ -48,13 +50,14 @@ bool AsyncCAN::SetupPort() {
     const int bind_result =
         bind(can_fd_, (struct sockaddr *)&addr, sizeof(addr));
     if (bind_result < 0) {
-      StopService();
+      Close();
       return false;
     }
 
     port_opened_ = true;
     std::cout << "Start listening to port: " << port_ << std::endl;
   } catch (std::system_error &e) {
+    port_opened_ = false;
     std::cout << e.what() << std::endl;
     return false;
   }
@@ -72,11 +75,13 @@ bool AsyncCAN::SetupPort() {
                        std::ref(socketcan_stream_)));
 #endif
 
+  // start io thread
+  io_thread_ = std::thread([this]() { io_context_.run(); });
+
   return true;
 }
 
-void AsyncCAN::StopService() {
+void AsyncCAN::Close() {
-  // stop io thread
   io_context_.stop();
   if (io_thread_.joinable()) io_thread_.join();
   io_context_.reset();
@@ -88,6 +93,8 @@ void AsyncCAN::StopService() {
   port_opened_ = false;
 }
 
+bool AsyncCAN::IsOpened() const { return port_opened_; }
+
 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++)
@@ -102,7 +109,7 @@ void AsyncCAN::ReadFromPort(struct can_frame &rec_frame,
       asio::buffer(&rec_frame, sizeof(rec_frame)),
       [sthis](asio::error_code error, size_t bytes_transferred) {
         if (error) {
-          sthis->StopService();
+          sthis->Close();
           return;
         }
 

src/async_port/async_serial.cpp

@@ -19,15 +19,17 @@
 namespace westonrobot {
 
 AsyncSerial::AsyncSerial(std::string port_name, uint32_t baud_rate)
-    : AsyncPortBase(port_name),
+    : port_(port_name), baud_rate_(baud_rate), serial_port_(io_context_) {}
-      baud_rate_(baud_rate),
+
-      serial_port_(io_context_) {}
+AsyncSerial::~AsyncSerial() { Close(); }
 
 void AsyncSerial::SetBaudRate(unsigned baudrate) {
   serial_port_.set_option(asio::serial_port_base::baud_rate(baudrate));
 }
 
-bool AsyncSerial::SetupPort() {
+void AsyncSerial::SetHardwareFlowControl(bool enabled) { hwflow_ = enabled; }
+
+bool AsyncSerial::Open() {
   using SPB = asio::serial_port_base;
 
   try {
@@ -67,11 +69,13 @@ bool AsyncSerial::SetupPort() {
   asio::post(io_context_, std::bind(&AsyncSerial::ReadFromPort, this));
 #endif
 
+  // start io thread
+  io_thread_ = std::thread([this]() { io_context_.run(); });
+
   return true;
 }
 
-void AsyncSerial::StopService() {
+void AsyncSerial::Close() {
-  // stop io thread
   io_context_.stop();
   if (io_thread_.joinable()) io_thread_.join();
   io_context_.reset();
@@ -84,6 +88,8 @@ void AsyncSerial::StopService() {
   port_opened_ = false;
 }
 
+bool AsyncSerial::IsOpened() const { return serial_port_.is_open(); }
+
 void AsyncSerial::DefaultReceiveCallback(uint8_t *data, const size_t bufsize,
                                          size_t len) {}
 
@@ -93,7 +99,7 @@ void AsyncSerial::ReadFromPort() {
       asio::buffer(rx_buf_),
       [sthis](asio::error_code error, size_t bytes_transferred) {
         if (error) {
-          sthis->StopService();
+          sthis->Close();
           return;
         }
 
@@ -122,7 +128,7 @@ void AsyncSerial::WriteToPort(bool check_if_busy) {
       asio::buffer(tx_buf_, len),
       [sthis](asio::error_code error, size_t bytes_transferred) {
         if (error) {
-          sthis->StopService();
+          sthis->Close();
           return;
         }
         std::lock_guard<std::recursive_mutex> lock(sthis->tx_mutex_);

src/utilities/protocol_detector.cpp

@@ -15,7 +15,7 @@ bool ProtocolDectctor::Connect(std::string can_name) {
   can_ = std::make_shared<AsyncCAN>(can_name);
   can_->SetReceiveCallback(
       std::bind(&ProtocolDectctor::ParseCANFrame, this, std::placeholders::_1));
-  return can_->StartListening();
+  return can_->Open();
 }
 
 ProtocolVersion ProtocolDectctor::DetectProtocolVersion(uint32_t timeout_sec) {