mirrors/ugv_sdk
1
0
Fork 0
mirror of https://github.com/westonrobot/ugv_sdk synced 2023-04-08 06:32:14 +08:00
Code Issues Packages Projects Releases Wiki Activity

updated to use updated asyncio implementation

Browse Source
This commit is contained in:
Ruixiang Du
2020-09-14 15:15:39 +08:00
parent 1cb41c4eb4
commit f01ab6415d
4876 changed files with 619410 additions and 996 deletions
Download Patch File Download Diff File Expand all files Collapse all files

135
include/wrp_sdk/asyncio/async_can.hpp
View File

@@ -1,26 +1,14 @@
/*
/*
* 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.
* Created on: Sep 10, 2020 13:22
* Description:
*
*/
/*
* libmavconn
* Copyright 2013,2014,2015,2016 Vladimir Ermakov, All rights reserved.
* 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.
*
* 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
* Copyright (c) 2020 Ruixiang Du (rdu)
*/
#ifndef ASYNC_CAN_HPP
@@ -28,103 +16,38 @@
#include <linux/can.h>
#include <atomic>
#include <chrono>
#include <thread>
#include <mutex>
#include <deque>
#include <functional>
#include <iostream>
#include <memory>
#include "asio.hpp"
#include "asio/posix/basic_stream_descriptor.hpp"
// #include "async_io/device_error.hpp"
// #include "async_io/msg_buffer.hpp"
#include "wrp_sdk/asyncio/async_listener.hpp"
namespace westonrobot
{
using steady_clock = std::chrono::steady_clock;
using lock_guard = std::lock_guard<std::recursive_mutex>;
namespace westonrobot {
class AsyncCAN : public AsyncListener,
public std::enable_shared_from_this<AsyncCAN> {
public:
using ReceiveCallback = std::function<void(can_frame *rx_frame)>;
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;
public:
AsyncCAN(std::string can_port = "can0");
using ReceiveCallback = std::function<void(can_frame *rx_frame)>;
using ClosedCallback = std::function<void(void)>;
void StopService() override;
using Ptr = std::shared_ptr<ASyncCAN>;
using ConstPtr = std::shared_ptr<ASyncCAN const>;
using WeakPtr = std::weak_ptr<ASyncCAN>;
void SetReceiveCallback(ReceiveCallback cb) { rcv_cb_ = cb; }
void SendFrame(const can_frame &frame);
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]
};
private:
int can_fd_;
asio::posix::basic_stream_descriptor<> socketcan_stream_;
public:
ASyncCAN(std::string device = DEFAULT_DEVICE);
~ASyncCAN();
struct can_frame rcv_frame_;
ReceiveCallback rcv_cb_ = nullptr;
// 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);
bool SetupPort();
void DefaultReceiveCallback(can_frame *rx_frame);
void ReadFromPort(struct can_frame &rec_frame,
asio::posix::basic_stream_descriptor<> &stream);
};
} // namespace westonrobot
} // namespace westonrobot
#endif /* ASYNC_CAN_HPP */

56
include/wrp_sdk/asyncio/async_listener.hpp Normal file
View File

@@ -0,0 +1,56 @@
/*
* async_listener.hpp
*
* Created on: Sep 10, 2020 11:50
* Description:
*
* Copyright (c) 2020 Ruixiang Du (rdu)
*/
#ifndef ASYNC_LISTENER_HPP
#define ASYNC_LISTENER_HPP
#include <thread>
#include <mutex>
#include <functional>
#include <iostream>
#include "asio.hpp"
namespace westonrobot {
class AsyncListener {
public:
AsyncListener(std::string port) : port_(port) {}
virtual ~AsyncListener(){};
// do not allow copy
AsyncListener() = delete;
AsyncListener(const AsyncListener& 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;
asio::io_context io_context_;
std::thread io_thread_;
virtual bool SetupPort() = 0;
};
} // namespace westonrobot
#endif /* ASYNC_LISTENER_HPP */

166
include/wrp_sdk/asyncio/async_serial.hpp
View File

@@ -1,145 +1,65 @@
/*
* 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.
* async_serial.hpp
*
* 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
* Created on: Sep 10, 2020 12:59
* Description:
*
* Copyright (c) 2020 Ruixiang Du (rdu)
*/
#ifndef ASYNC_SERIAL_HPP
#define ASYNC_SERIAL_HPP
#include <atomic>
#include <chrono>
#include <thread>
#include <cstdint>
#include <memory>
#include <array>
#include <mutex>
#include <deque>
#include <functional>
#include "asio.hpp"
#include "wrp_sdk/asyncio/async_listener.hpp"
#include "wrp_sdk/asyncio/ring_buffer.hpp"
#include "wrp_sdk/asyncio/device_error.hpp"
#include "wrp_sdk/asyncio/msg_buffer.hpp"
namespace westonrobot {
class AsyncSerial : public AsyncListener,
public std::enable_shared_from_this<AsyncSerial> {
public:
using ReceiveCallback =
std::function<void(uint8_t *data, size_t len, const size_t bufsize)>;
namespace westonrobot
{
using steady_clock = std::chrono::steady_clock;
using lock_guard = std::lock_guard<std::recursive_mutex>;
public:
AsyncSerial(std::string port_name, uint32_t baud_rate = 115200);
/// 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;
void StopService() override;
/**
* @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)>;
bool IsOpened() const override { return serial_port_.is_open(); }
using Ptr = std::shared_ptr<ASyncSerial>;
using ConstPtr = std::shared_ptr<ASyncSerial const>;
using WeakPtr = std::weak_ptr<ASyncSerial>;
void SetReceiveCallback(ReceiveCallback cb) { rcv_cb_ = cb; }
void SendBytes(const uint8_t *bytes, size_t length);
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]
};
void SetBaudRate(unsigned baudrate);
void SetHardwareFlowControl(bool enabled) { hwflow_ = enabled; }
public:
ASyncSerial(std::string device = DEFAULT_DEVICE, unsigned baudrate = DEFAULT_BAUDRATE, bool hwflow = false);
~ASyncSerial();
private:
asio::serial_port serial_port_;
uint32_t baud_rate_ = 115200;
bool hwflow_ = false;
ReceiveCallback rcv_cb_ = nullptr;
// do not allow copy
ASyncSerial(const ASyncSerial &other) = delete;
ASyncSerial &operator=(const ASyncSerial &other) = delete;
std::size_t conn_id;
// 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;
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);
bool SetupPort();
void DefaultReceiveCallback(uint8_t *data, size_t len, const size_t bufsize);
void ReadFromPort();
void WriteToPort(bool check_if_busy);
};
} // namespace westonrobot
} // namespace westonrobot
#endif /* ASYNC_SERIAL_HPP */

57
include/wrp_sdk/asyncio/device_error.hpp
View File

@@ -1,57 +0,0 @@
/**
* @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 */

69
include/wrp_sdk/asyncio/msg_buffer.hpp
View File

@@ -1,69 +0,0 @@
/**
* @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

137
include/wrp_sdk/asyncio/ring_buffer.hpp Normal file
View File

@@ -0,0 +1,137 @@
/*
* 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 Ruixiang Du (rdu)
*/
#ifndef RING_BUFFER_HPP
#define RING_BUFFER_HPP
#include <cstdint>
#include <cstddef>
#include <array>
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 */

4
include/wrp_sdk/platforms/common/mobile_base.hpp
View File

@@ -47,8 +47,8 @@ class MobileBase {
bool can_connected_ = false;
bool serial_connected_ = false;
std::shared_ptr<ASyncCAN> can_if_;
std::shared_ptr<ASyncSerial> serial_if_;
std::shared_ptr<AsyncCAN> can_if_;
std::shared_ptr<AsyncSerial> serial_if_;
// command thread
std::thread cmd_thread_;

4
include/wrp_sdk/platforms/tracer/tracer_base.hpp
View File

@@ -59,8 +59,8 @@ public:
private:
// hardware communication interface
std::shared_ptr<ASyncCAN> can_if_;
std::shared_ptr<ASyncSerial> serial_if_;
std::shared_ptr<AsyncCAN> can_if_;
std::shared_ptr<AsyncSerial> serial_if_;
// CAN priority higher than serial if both connected
bool can_connected_ = false;