diff --git a/demo/scout_demo/scout_mini_omni_demo.cpp b/demo/scout_demo/scout_mini_omni_demo.cpp index c9148b8..0d5d205 100644 --- a/demo/scout_demo/scout_mini_omni_demo.cpp +++ b/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(); diff --git a/demo/tracer_demo/tracer_robot_demo.cpp b/demo/tracer_demo/tracer_robot_demo.cpp index e044725..9b72fbb 100644 --- a/demo/tracer_demo/tracer_robot_demo.cpp +++ b/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(); diff --git a/include/ugv_sdk/details/async_port/async_can.hpp b/include/ugv_sdk/details/async_port/async_can.hpp index 89e2b29..85ec989 100644 --- a/include/ugv_sdk/details/async_port/async_can.hpp +++ b/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 +#include #include +#include +#include +#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 { +class AsyncCAN : public std::enable_shared_from_this { public: using ReceiveCallback = std::function; 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 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); diff --git a/include/ugv_sdk/details/async_port/async_port_base.hpp b/include/ugv_sdk/details/async_port/async_port_base.hpp deleted file mode 100644 index e7a269c..0000000 --- a/include/ugv_sdk/details/async_port/async_port_base.hpp +++ /dev/null @@ -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 -#include -#include -#include - -#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 */ diff --git a/include/ugv_sdk/details/async_port/async_serial.hpp b/include/ugv_sdk/details/async_port/async_serial.hpp index ac09a2c..0578d46 100644 --- a/include/ugv_sdk/details/async_port/async_serial.hpp +++ b/include/ugv_sdk/details/async_port/async_serial.hpp @@ -14,48 +14,63 @@ #include #include #include +#include +#include + +#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 { +class AsyncSerial : public std::enable_shared_from_this { public: using ReceiveCallback = std::function; 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 rx_buf_; - // tx buffer uint8_t tx_buf_[rxtx_buffer_size]; RingBuffer 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); diff --git a/include/ugv_sdk/details/robot_base/agilex_base.hpp b/include/ugv_sdk/details/robot_base/agilex_base.hpp index 893fe4a..18e0809 100644 --- a/include/ugv_sdk/details/robot_base/agilex_base.hpp +++ b/include/ugv_sdk/details/robot_base/agilex_base.hpp @@ -15,6 +15,7 @@ #include #include #include +#include #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 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(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) { diff --git a/src/async_port/async_can.cpp b/src/async_port/async_can.cpp index 7343ee3..b04d012 100644 --- a/src/async_port/async_can.cpp +++ b/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,15 +75,17 @@ bool AsyncCAN::SetupPort() { std::ref(socketcan_stream_))); #endif + // start io thread + io_thread_ = std::thread([this]() { io_context_.run(); }); + return true; } -void AsyncCAN::StopService() { - // stop io thread +void AsyncCAN::Close() { io_context_.stop(); if (io_thread_.joinable()) io_thread_.join(); io_context_.reset(); - + // release port fd const int close_result = ::close(can_fd_); can_fd_ = -1; @@ -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; } diff --git a/src/async_port/async_serial.cpp b/src/async_port/async_serial.cpp index bc1595a..d224452 100644 --- a/src/async_port/async_serial.cpp +++ b/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), - baud_rate_(baud_rate), - serial_port_(io_context_) {} + : port_(port_name), 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,15 +69,17 @@ 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() { - // stop io thread +void AsyncSerial::Close() { io_context_.stop(); if (io_thread_.joinable()) io_thread_.join(); io_context_.reset(); - + if (IsOpened()) { serial_port_.cancel(); serial_port_.close(); @@ -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 lock(sthis->tx_mutex_); diff --git a/src/utilities/protocol_detector.cpp b/src/utilities/protocol_detector.cpp index dd9d402..8b37724 100644 --- a/src/utilities/protocol_detector.cpp +++ b/src/utilities/protocol_detector.cpp @@ -15,7 +15,7 @@ bool ProtocolDectctor::Connect(std::string can_name) { can_ = std::make_shared(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) {