more code cleanup

include/wrp_sdk/asyncio/async_can.hpp

@@ -0,0 +1,130 @@
+/* 
+ * async_can.hpp
+ * 
+ * Created on: Jun 10, 2019 02:16
+ * Description: code based on uavcan and libmavconn
+ * 
+ * Copyright (c) 2019 Ruixiang Du (rdu)
+ */
+
+/*
+ * Copyright (c) 2016 UAVCAN Team
+ *
+ * Distributed under the MIT License, available in the file LICENSE.
+ *
+ */
+
+/*
+ * libmavconn
+ * Copyright 2013,2014,2015,2016 Vladimir Ermakov, All rights reserved.
+ *
+ * This file is part of the mavros package and subject to the license terms
+ * in the top-level LICENSE file of the mavros repository.
+ * https://github.com/mavlink/mavros/tree/master/LICENSE.md
+ */
+
+#ifndef ASYNC_CAN_HPP
+#define ASYNC_CAN_HPP
+
+#include <linux/can.h>
+
+#include <atomic>
+#include <chrono>
+#include <thread>
+#include <mutex>
+#include <deque>
+#include <functional>
+#include <iostream>
+
+#include "asio.hpp"
+#include "asio/posix/basic_stream_descriptor.hpp"
+
+// #include "async_io/device_error.hpp"
+// #include "async_io/msg_buffer.hpp"
+
+namespace westonrobot
+{
+using steady_clock = std::chrono::steady_clock;
+using lock_guard = std::lock_guard<std::recursive_mutex>;
+
+class ASyncCAN : public std::enable_shared_from_this<ASyncCAN>
+{
+public:
+    static constexpr auto DEFAULT_DEVICE = "can1";
+    static constexpr std::size_t MAX_TXQ_SIZE = 1000;
+
+    using ReceiveCallback = std::function<void(can_frame *rx_frame)>;
+    using ClosedCallback = std::function<void(void)>;
+
+    using Ptr = std::shared_ptr<ASyncCAN>;
+    using ConstPtr = std::shared_ptr<ASyncCAN const>;
+    using WeakPtr = std::weak_ptr<ASyncCAN>;
+
+    struct IOStat
+    {
+        std::size_t tx_total_frames; // total bytes transferred
+        std::size_t rx_total_frames; // total bytes received
+        float tx_speed;              // current transfer speed [Frames/s]
+        float rx_speed;              // current receive speed [Frames/s]
+    };
+
+public:
+    ASyncCAN(std::string device = DEFAULT_DEVICE);
+    ~ASyncCAN();
+
+    // do not allow copy
+    ASyncCAN(const ASyncCAN &other) = delete;
+    ASyncCAN &operator=(const ASyncCAN &other) = delete;
+
+    std::size_t conn_id;
+    int can_fd_;
+
+    static Ptr open_url(std::string url);
+    void open(std::string device);
+    void close();
+
+    void set_receive_callback(ReceiveCallback cb) { receive_cb = cb; }
+    void set_closed_callback(ClosedCallback cb) { port_closed_cb = cb; }
+
+    inline bool is_open() { return true; }
+    IOStat get_iostat();
+
+    void send_frame(const can_frame &tx_frame);
+
+private:
+    // monotonic counter (increment only)
+    bool can_interface_opened_ = false;
+    static std::atomic<std::size_t> conn_id_counter;
+
+    struct can_frame rcv_frame;
+
+    // port statistics
+    std::atomic<std::size_t> tx_total_frames;
+    std::atomic<std::size_t> rx_total_frames;
+    std::size_t last_tx_total_frames;
+    std::size_t last_rx_total_frames;
+    std::chrono::time_point<steady_clock> last_iostat;
+    std::recursive_mutex iostat_mutex;
+
+    // io service
+    asio::io_service io_service;
+    asio::posix::basic_stream_descriptor<> stream;
+    std::thread io_thread;
+
+    // callback objects
+    ClosedCallback port_closed_cb;
+    ReceiveCallback receive_cb;
+
+    // internal processing
+    void do_read(struct can_frame &rec_frame, asio::posix::basic_stream_descriptor<> &stream);
+    void do_write(bool check_tx_state);
+
+    void call_receive_callback(can_frame *rx_frame);
+    void default_receive_callback(can_frame *rx_frame);
+
+    void iostat_tx_add(std::size_t bytes);
+    void iostat_rx_add(std::size_t bytes);
+};
+} // namespace westonrobot
+
+#endif /* ASYNC_CAN_HPP */

include/wrp_sdk/asyncio/async_serial.hpp

@@ -0,0 +1,145 @@
+/* 
+ * async_serial.hpp
+ * 
+ * Created on: Nov 23, 2018 22:18
+ * Description: asynchronous serial communication using asio
+ *              adapted from code in libmavconn
+ * 
+ * Main changes: 1. Removed dependency on Boost (asio standalone
+ *                  and C++ STL only)
+ *               2. Removed dependency on console-bridge
+ *               3. Removed mavlink related code
+ *               4. Removed UDP/TCP related code
+ * 
+ * Author: Vladimir Ermakov <vooon341@gmail.com>
+ *         Ruixiang Du <ruixiang.du@gmail.com>
+ * 
+ * Additioanl reference:
+ *  [1] http://www.webalice.it/fede.tft/serial_port/serial_port.html
+ * 
+ */
+
+/*
+ * libmavconn
+ * Copyright 2013,2014,2015,2016 Vladimir Ermakov, All rights reserved.
+ *
+ * This file is part of the mavros package and subject to the license terms
+ * in the top-level LICENSE file of the mavros repository.
+ * https://github.com/mavlink/mavros/tree/master/LICENSE.md
+ */
+
+#ifndef ASYNC_SERIAL_HPP
+#define ASYNC_SERIAL_HPP
+
+#include <atomic>
+#include <chrono>
+#include <thread>
+#include <mutex>
+#include <deque>
+#include <functional>
+
+#include "asio.hpp"
+
+#include "wrp_sdk/asyncio/device_error.hpp"
+#include "wrp_sdk/asyncio/msg_buffer.hpp"
+
+namespace westonrobot
+{
+using steady_clock = std::chrono::steady_clock;
+using lock_guard = std::lock_guard<std::recursive_mutex>;
+
+/// Note: instance of ASyncSerial MUST be managed by a std::shared_ptr<ASyncSerial>
+class ASyncSerial : public std::enable_shared_from_this<ASyncSerial>
+{
+public:
+    static constexpr auto DEFAULT_DEVICE = "/dev/ttyUSB0";
+    static constexpr auto DEFAULT_BAUDRATE = 115200;
+    static constexpr std::size_t MAX_TXQ_SIZE = 1000;
+
+    /**
+     * @param buf: pointer to a buffer
+     * @param bufsize: size of the buffer (the value should be constant in most cases)
+     * @param bytes_received: number of bytes received inside the buffer
+     */
+    using ReceiveCallback = std::function<void(uint8_t *buf, const size_t bufsize, size_t bytes_received)>;
+    using ClosedCallback = std::function<void(void)>;
+
+    using Ptr = std::shared_ptr<ASyncSerial>;
+    using ConstPtr = std::shared_ptr<ASyncSerial const>;
+    using WeakPtr = std::weak_ptr<ASyncSerial>;
+
+    struct IOStat
+    {
+        std::size_t tx_total_bytes; //!< total bytes transferred
+        std::size_t rx_total_bytes; //!< total bytes received
+        float tx_speed;             //!< current transfer speed [B/s]
+        float rx_speed;             //!< current receive speed [B/s]
+    };
+
+public:
+    ASyncSerial(std::string device = DEFAULT_DEVICE, unsigned baudrate = DEFAULT_BAUDRATE, bool hwflow = false);
+    ~ASyncSerial();
+
+    // do not allow copy
+    ASyncSerial(const ASyncSerial &other) = delete;
+    ASyncSerial &operator=(const ASyncSerial &other) = delete;
+
+    std::size_t conn_id;
+
+    static Ptr open_url(std::string url);
+    void open(std::string device = "", unsigned baudrate = 0, bool hwflow = false);
+    void close();
+
+    void set_baud(unsigned baudrate) { serial_dev_.set_option(asio::serial_port_base::baud_rate(baudrate)); }
+
+    void send_bytes(const uint8_t *bytes, size_t length);
+    void set_receive_callback(ReceiveCallback cb) { receive_cb = cb; }
+    void set_closed_callback(ClosedCallback cb) { port_closed_cb = cb; }
+
+    inline bool is_open() { return serial_dev_.is_open(); }
+    IOStat get_iostat();
+
+private:
+    // monotonic counter (increment only)
+    static std::atomic<std::size_t> conn_id_counter;
+
+    // port properties
+    std::string device_ = DEFAULT_DEVICE;
+    unsigned baudrate_ = DEFAULT_BAUDRATE;
+    bool hwflow_ = false;
+
+    // port statistics
+    std::atomic<std::size_t> tx_total_bytes;
+    std::atomic<std::size_t> rx_total_bytes;
+    std::size_t last_tx_total_bytes;
+    std::size_t last_rx_total_bytes;
+    std::chrono::time_point<steady_clock> last_iostat;
+    std::recursive_mutex iostat_mutex;
+
+    // io service
+    asio::io_service io_service;
+    std::thread io_thread;
+    asio::serial_port serial_dev_;
+
+    std::atomic<bool> tx_in_progress;
+    std::deque<MsgBuffer> tx_q;
+    std::array<uint8_t, MsgBuffer::MAX_SIZE> rx_buf;
+    std::recursive_mutex mutex;
+
+    // callback objects
+    ClosedCallback port_closed_cb;
+    ReceiveCallback receive_cb;
+
+    // internal processing
+    void do_read();
+    void do_write(bool check_tx_state);
+
+    void call_receive_callback(uint8_t *buf, const std::size_t bufsize, std::size_t bytes_received);
+    void default_receive_callback(uint8_t *buf, const std::size_t bufsize, std::size_t bytes_received);
+
+    void iostat_tx_add(std::size_t bytes);
+    void iostat_rx_add(std::size_t bytes);
+};
+} // namespace westonrobot
+
+#endif /* ASYNC_SERIAL_HPP */

include/wrp_sdk/asyncio/device_error.hpp

@@ -0,0 +1,57 @@
+/**
+ * @brief MAVConn device error class
+ * @file device_error.hpp
+ * @author Vladimir Ermakov <vooon341@gmail.com>
+ */
+/*
+ * libmavconn
+ * Copyright 2013,2014,2015,2016 Vladimir Ermakov, All rights reserved.
+ *
+ * This file is part of the mavros package and subject to the license terms
+ * in the top-level LICENSE file of the mavros repository.
+ * https://github.com/mavlink/mavros/tree/master/LICENSE.md
+ */
+
+#ifndef DEVICE_ERROR_HPP
+#define DEVICE_ERROR_HPP
+
+#include <string>
+#include <cstring>
+#include <stdexcept>
+
+namespace westonrobot
+{
+class DeviceError : public std::runtime_error
+{
+  public:
+    template <typename T>
+    DeviceError(const char *module, T msg) : std::runtime_error(make_message(module, msg))
+    {
+    }
+
+    template <typename T>
+    static std::string make_message(const char *module, T msg)
+    {
+        std::ostringstream ss;
+        ss << "DeviceError:" << module << ":" << msg_to_string(msg);
+        return ss.str();
+    }
+
+    static std::string msg_to_string(const char *description)
+    {
+        return description;
+    }
+
+    static std::string msg_to_string(int errnum)
+    {
+        return std::strerror(errnum);
+    }
+
+    static std::string msg_to_string(std::system_error &err)
+    {
+        return err.what();
+    }
+};
+} // namespace mavconn
+
+#endif /* DEVICE_ERROR_HPP */

include/wrp_sdk/asyncio/msg_buffer.hpp

@@ -0,0 +1,69 @@
+/**
+ * @brief MAVConn message buffer class (internal)
+ * @file msgbuffer.hpp
+ * @author Vladimir Ermakov <vooon341@gmail.com>
+ */
+/*
+ * libmavconn
+ * Copyright 2014,2015,2016 Vladimir Ermakov, All rights reserved.
+ *
+ * This file is part of the mavros package and subject to the license terms
+ * in the top-level LICENSE file of the mavros repository.
+ * https://github.com/mavlink/mavros/tree/master/LICENSE.md
+ */
+
+#pragma once
+
+#include <sys/types.h>
+#include <cstdint>
+#include <cstring>
+#include <cassert>
+
+namespace westonrobot
+{
+
+/**
+ * @brief Message buffer for internal use in libmavconn
+ */
+struct MsgBuffer
+{
+    //! Maximum buffer size with padding for CRC bytes (280 + padding)
+    static constexpr ssize_t MAX_PACKET_LEN = 255;
+    static constexpr ssize_t MAX_SIZE = MAX_PACKET_LEN + 16;
+    uint8_t data[MAX_SIZE];
+    ssize_t len;
+    ssize_t pos;
+
+    MsgBuffer() : pos(0),
+                  len(0)
+    {
+    }
+    
+    /**
+	 * @brief Buffer constructor for send_bytes()
+	 * @param[in] nbytes should be less than MAX_SIZE
+	 */
+    MsgBuffer(const uint8_t *bytes, ssize_t nbytes) : pos(0),
+                                                      len(nbytes)
+    {
+        assert(0 < nbytes && nbytes < MAX_SIZE);
+        std::memcpy(data, bytes, nbytes);
+    }
+
+    virtual ~MsgBuffer()
+    {
+        pos = 0;
+        len = 0;
+    }
+
+    uint8_t *dpos()
+    {
+        return data + pos;
+    }
+
+    ssize_t nbytes()
+    {
+        return len - pos;
+    }
+};
+} // namespace westonrobot