From 7fb72d55fd646eb58a65de20ae082c37aeb6d7b6 Mon Sep 17 00:00:00 2001 From: Ruixiang Du Date: Tue, 6 Jul 2021 11:21:45 +0800 Subject: [PATCH] merged async_port into ugv_sdk --- .gitlab-ci.yml | 27 --- CMakeLists.txt | 46 ++---- include/ugv_sdk/agilex_base.hpp | 2 +- .../ugv_sdk/details/async_port/async_can.hpp | 53 ++++++ .../details/async_port/async_port_base.hpp | 60 +++++++ .../details/async_port/async_serial.hpp | 65 ++++++++ .../details/async_port/ring_buffer.hpp | 138 ++++++++++++++++ sample/CMakeLists.txt | 8 + sample/tracer_demo.cpp | 99 +++++++++++ src/async_port/async_can.cpp | 124 ++++++++++++++ src/async_port/async_serial.cpp | 155 ++++++++++++++++++ 11 files changed, 721 insertions(+), 56 deletions(-) create mode 100644 include/ugv_sdk/details/async_port/async_can.hpp create mode 100644 include/ugv_sdk/details/async_port/async_port_base.hpp create mode 100644 include/ugv_sdk/details/async_port/async_serial.hpp create mode 100644 include/ugv_sdk/details/async_port/ring_buffer.hpp create mode 100644 sample/CMakeLists.txt create mode 100644 sample/tracer_demo.cpp create mode 100644 src/async_port/async_can.cpp create mode 100644 src/async_port/async_serial.cpp diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index e00fca8..74fa218 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -3,11 +3,6 @@ xenial-cpp-build: stage: build image: rduweston/ubuntu-ci:16.04 script: - - cd / && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/public/async_port.git - - cd /async_port && mkdir build && cd build - - cmake .. && make - - make install - - cd /builds/$CI_PROJECT_PATH - mkdir build && cd build - cmake -DCMAKE_BUILD_TYPE=Release .. - cmake --build . && cpack @@ -16,11 +11,6 @@ bionic-cpp-build: stage: build image: rduweston/ubuntu-ci:18.04 script: - - cd / && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/public/async_port.git - - cd /async_port && mkdir build && cd build - - cmake .. && make - - make install - - cd /builds/$CI_PROJECT_PATH - mkdir build && cd build - cmake -DCMAKE_BUILD_TYPE=Release .. - cmake --build . && cpack @@ -29,11 +19,6 @@ focal-cpp-build: stage: build image: rduweston/ubuntu-ci:20.04 script: - - cd / && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/public/async_port.git - - cd /async_port && mkdir build && cd build - - cmake .. && make - - make install - - cd /builds/$CI_PROJECT_PATH - mkdir build && cd build - cmake -DCMAKE_BUILD_TYPE=Release .. - cmake --build . && cpack @@ -45,8 +30,6 @@ kinetic-catkin-build: script: - mkdir -p /catkin_ws/src/ugv_sdk/ugv_sdk && cd /catkin_ws/src && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_init_workspace" - cp -r /builds/$CI_PROJECT_PATH /catkin_ws/src/ugv_sdk - - cd /catkin_ws/src/ugv_sdk - - cd /catkin_ws/src && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/public/async_port.git - cd /catkin_ws && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_make" kinetic-catkin-isolated-build: @@ -55,8 +38,6 @@ kinetic-catkin-isolated-build: script: - mkdir -p /catkin_ws/src/ugv_sdk/ugv_sdk && cd /catkin_ws/src && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_init_workspace" - cp -r /builds/$CI_PROJECT_PATH /catkin_ws/src/ugv_sdk - - cd /catkin_ws/src/ugv_sdk - - cd /catkin_ws/src && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/public/async_port.git - cd /catkin_ws && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_make_isolated --install" melodic-catkin-build: @@ -65,8 +46,6 @@ melodic-catkin-build: script: - mkdir -p /catkin_ws/src/ugv_sdk && cd /catkin_ws/src && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_init_workspace" - cp -r /builds/$CI_PROJECT_PATH /catkin_ws/src/ugv_sdk - - cd /catkin_ws/src/ugv_sdk - - cd /catkin_ws/src && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/public/async_port.git - cd /catkin_ws && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_make" melodic-catkin-isolated-build: @@ -75,8 +54,6 @@ melodic-catkin-isolated-build: script: - mkdir -p /catkin_ws/src/ugv_sdk && cd /catkin_ws/src && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_init_workspace" - cp -r /builds/$CI_PROJECT_PATH /catkin_ws/src/ugv_sdk - - cd /catkin_ws/src/ugv_sdk - - cd /catkin_ws/src && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/public/async_port.git - cd /catkin_ws && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_make_isolated --install" noetic-catkin-build: @@ -85,8 +62,6 @@ noetic-catkin-build: script: - mkdir -p /catkin_ws/src/ugv_sdk && cd /catkin_ws/src && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_init_workspace" - cp -r /builds/$CI_PROJECT_PATH /catkin_ws/src/ugv_sdk - - cd /catkin_ws/src/ugv_sdk - - cd /catkin_ws/src && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/public/async_port.git - cd /catkin_ws && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_make" noetic-catkin-isolated-build: @@ -95,6 +70,4 @@ noetic-catkin-isolated-build: script: - mkdir -p /catkin_ws/src/ugv_sdk && cd /catkin_ws/src && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_init_workspace" - cp -r /builds/$CI_PROJECT_PATH /catkin_ws/src/ugv_sdk - - cd /catkin_ws/src/ugv_sdk - - cd /catkin_ws/src && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/public/async_port.git - cd /catkin_ws && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_make_isolated --install" \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 5d96258..f92dba1 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5.1) -project(ugv_sdk VERSION 0.1.5) +project(ugv_sdk VERSION 0.2.0) find_program(CCACHE_PROGRAM ccache) if(CCACHE_PROGRAM) @@ -38,7 +38,6 @@ endif() set(USER_CMAKE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake") list(APPEND CMAKE_MODULE_PATH "${USER_CMAKE_PATH}/modules") list(APPEND CMAKE_PREFIX_PATH "/usr/lib/${CMAKE_SYSTEM_PROCESSOR}-linux-gnu/cmake") -list(APPEND CMAKE_PREFIX_PATH "/opt/weston_robot/lib/cmake") ## Set compiler to use c++ 11 features set(CMAKE_CXX_STANDARD 11) @@ -67,20 +66,14 @@ if(BUILD_WITHOUT_ROS) set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/${CMAKE_INSTALL_BINDIR}) endif() -if(BUILD_WITHOUT_ROS) - find_package(async_port REQUIRED) -else() - find_package(catkin REQUIRED COMPONENTS async_port) - include_directories( - include - ${catkin_INCLUDE_DIRS} - ) -endif() - -# Build libraries +## Build libraries find_package(Threads REQUIRED) -add_library(${PROJECT_NAME} +# add sdk main library +add_library(${PROJECT_NAME} + # async_port from weston robot + src/async_port/async_serial.cpp + src/async_port/async_can.cpp # agx common src/agilex_base.cpp src/agx_msg_parser.c @@ -91,14 +84,11 @@ add_library(${PROJECT_NAME} # src/bunker_base.cpp src/ranger_base.cpp ) -if(BUILD_WITHOUT_ROS) - target_link_libraries(${PROJECT_NAME} westonrobot::async_port Threads::Threads) -else() - target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} Threads::Threads) -endif() +target_link_libraries(${PROJECT_NAME} PUBLIC Threads::Threads) target_include_directories(${PROJECT_NAME} PUBLIC $ - $ + $ + $ PRIVATE src) # Build apps @@ -149,7 +139,7 @@ if(BUILD_WITHOUT_ROS) # BUILD_WITHOUT_ROS endforeach() # targets to install - install(TARGETS ${PROJECT_NAME} + install(TARGETS ${PROJECT_NAME} EXPORT ugv_sdkTargets LIBRARY DESTINATION lib ARCHIVE DESTINATION lib @@ -168,7 +158,7 @@ if(BUILD_WITHOUT_ROS) # BUILD_WITHOUT_ROS install(EXPORT ugv_sdkTargets FILE ugv_sdkTargets.cmake - NAMESPACE westonrobot:: + # NAMESPACE UGVSDK:: DESTINATION lib/cmake/ugv_sdk) configure_file(cmake/ugv_sdkConfig.cmake.in ugv_sdkConfig.cmake @ONLY) @@ -178,18 +168,18 @@ if(BUILD_WITHOUT_ROS) # BUILD_WITHOUT_ROS # Packaging support set(CPACK_PACKAGE_VENDOR "Weston Robot") - set(CPACK_PACKAGE_DESCRIPTION_SUMMARY "Weston Robot Platform - SDK") + set(CPACK_PACKAGE_DESCRIPTION_SUMMARY "UGV SDK for Robots from AgileX/WestonRobot") set(CPACK_PACKAGE_VERSION_MAJOR ${PROJECT_VERSION_MAJOR}) set(CPACK_PACKAGE_VERSION_MINOR ${PROJECT_VERSION_MINOR}) set(CPACK_PACKAGE_VERSION_PATCH ${PROJECT_VERSION_PATCH}) - set(CPACK_PACKAGING_INSTALL_PREFIX "/opt/weston_robot") + # set(CPACK_PACKAGING_INSTALL_PREFIX "/opt/weston_robot") set(CPACK_RESOURCE_FILE_LICENSE "${CMAKE_CURRENT_SOURCE_DIR}/LICENSE") - # set(CPACK_RESOURCE_FILE_README "${CMAKE_CURRENT_SOURCE_DIR}/README.md") + set(CPACK_RESOURCE_FILE_README "${CMAKE_CURRENT_SOURCE_DIR}/README.md") set(CPACK_GENERATOR "DEB") set(CPACK_DEBIAN_FILE_NAME DEB-DEFAULT) set(CPACK_DEBIAN_PACKAGE_MAINTAINER "Ruixiang Du (ruixiang.du@westonrobot.com)") - # set(CPACK_DEBIAN_PACKAGE_DEPENDS "libasio-dev") + set(CPACK_DEBIAN_PACKAGE_DEPENDS "libasio-dev") set(CPACK_SOURCE_IGNORE_FILES .git dist .*build.* /\\\\.DS_Store) include(CPack) @@ -198,8 +188,8 @@ else() # BUILD_WITHOUT_ROS catkin_package( LIBRARIES ${PROJECT_NAME} INCLUDE_DIRS include - CATKIN_DEPENDS async_port - # DEPENDS system_lib + # CATKIN_DEPENDS async_port + # DEPENDS system_lib ) ## Add catkin install targets diff --git a/include/ugv_sdk/agilex_base.hpp b/include/ugv_sdk/agilex_base.hpp index 3cd2731..4c79fe4 100644 --- a/include/ugv_sdk/agilex_base.hpp +++ b/include/ugv_sdk/agilex_base.hpp @@ -22,7 +22,7 @@ #include #include -#include "async_port/async_can.hpp" +#include "ugv_sdk/details/async_port/async_can.hpp" #include "ugv_sdk/agilex_message.h" namespace westonrobot { diff --git a/include/ugv_sdk/details/async_port/async_can.hpp b/include/ugv_sdk/details/async_port/async_can.hpp new file mode 100644 index 0000000..89e2b29 --- /dev/null +++ b/include/ugv_sdk/details/async_port/async_can.hpp @@ -0,0 +1,53 @@ +/* + * async_can.hpp + * + * Created on: Sep 10, 2020 13:22 + * 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 + * into multiple frames and in applications where no frame should be dropped. + * + * Copyright (c) 2020 Weston Robot Pte. Ltd. + */ + +#ifndef ASYNC_CAN_HPP +#define ASYNC_CAN_HPP + +#include + +#include + +#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 { + public: + using ReceiveCallback = std::function; + + public: + AsyncCAN(std::string can_port = "can0"); + + void StopService() override; + + void SetReceiveCallback(ReceiveCallback cb) { rcv_cb_ = cb; } + void SendFrame(const struct can_frame &frame); + + private: + 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); +}; +} // namespace westonrobot + +#endif /* ASYNC_CAN_HPP */ diff --git a/include/ugv_sdk/details/async_port/async_port_base.hpp b/include/ugv_sdk/details/async_port/async_port_base.hpp new file mode 100644 index 0000000..ab3eab0 --- /dev/null +++ b/include/ugv_sdk/details/async_port/async_port_base.hpp @@ -0,0 +1,60 @@ +/* + * 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 << "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 new file mode 100644 index 0000000..ac09a2c --- /dev/null +++ b/include/ugv_sdk/details/async_port/async_serial.hpp @@ -0,0 +1,65 @@ +/* + * async_serial.hpp + * + * Created on: Sep 10, 2020 12:59 + * Description: + * + * Copyright (c) 2020 Weston Robot Pte. Ltd. + */ + +#ifndef ASYNC_SERIAL_HPP +#define ASYNC_SERIAL_HPP + +#include +#include +#include +#include + +#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 { + public: + using ReceiveCallback = + std::function; + + public: + AsyncSerial(std::string port_name, uint32_t baud_rate = 115200); + + void StopService() override; + + bool IsOpened() const override { return serial_port_.is_open(); } + + 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: + 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); +}; +} // namespace westonrobot + +#endif /* ASYNC_SERIAL_HPP */ diff --git a/include/ugv_sdk/details/async_port/ring_buffer.hpp b/include/ugv_sdk/details/async_port/ring_buffer.hpp new file mode 100644 index 0000000..bbe30cc --- /dev/null +++ b/include/ugv_sdk/details/async_port/ring_buffer.hpp @@ -0,0 +1,138 @@ +/* + * 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 Weston Robot Pte. Ltd. + */ + +#ifndef RING_BUFFER_HPP +#define RING_BUFFER_HPP + +#include +#include + +#include +#include + +namespace westonrobot { +template +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 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(buffer_[i]) << std::endl; + } + + private: + std::array 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 */ diff --git a/sample/CMakeLists.txt b/sample/CMakeLists.txt new file mode 100644 index 0000000..f3253eb --- /dev/null +++ b/sample/CMakeLists.txt @@ -0,0 +1,8 @@ +cmake_minimum_required(VERSION 3.10.2) +project(sdk_sample) + +find_package(ugv_sdk REQUIRED) +find_package(Threads REQUIRED) + +add_executable(app_tracer_demo tracer_demo.cpp) +target_link_libraries(app_tracer_demo PRIVATE ugv_sdk Threads::Threads) \ No newline at end of file diff --git a/sample/tracer_demo.cpp b/sample/tracer_demo.cpp new file mode 100644 index 0000000..8bb5c9a --- /dev/null +++ b/sample/tracer_demo.cpp @@ -0,0 +1,99 @@ +/* + * demo_tracer_can.cpp + * + * Created on: Jun 12, 2019 05:03 + * Description: + * + * Copyright (c) 2019 Ruixiang Du (rdu) + */ + +#include "ugv_sdk/tracer_base.hpp" + +using namespace westonrobot; + +int main(int argc, char **argv) { + std::string device_name; + + if (argc == 2) { + device_name = {argv[1]}; + std::cout << "Specified CAN: " << device_name << std::endl; + } else { + std::cout << "Usage: app_tracer_demo " << std::endl + << "Example 1: ./app_tracer_demo can0" << std::endl; + return -1; + } + + TracerBase tracer; + tracer.Connect(device_name); + + tracer.EnableCommandedMode(); + // tracer.DisableTimeout(); + + // light control + std::cout << "Light: const off" << std::endl; + tracer.SetLightCommand({CONST_OFF, 0}); + // usleep(50000); + sleep(3); + std::cout << "Light: const on" << std::endl; + tracer.SetLightCommand({CONST_ON, 0}); + // usleep(50000); + sleep(3); + std::cout << "Light: breath" << std::endl; + tracer.SetLightCommand({BREATH, 0}); + // usleep(50000); + sleep(8); + std::cout << "Light: custom 90-80" << std::endl; + tracer.SetLightCommand({CUSTOM, 90}); + // usleep(50000); + sleep(3); + std::cout << "Light: diabled cmd control" << std::endl; + tracer.SetLightCommand(TracerLightCmd()); + + int count = 0; + while (true) { + // motion control + if (count < 5) { + std::cout << "Motor: 0.2, 0.0" << std::endl; + tracer.SetMotionCommand(0.2, 0.0); + } else if (count < 10) { + std::cout << "Motor: 0.8, 0.3" << std::endl; + tracer.SetMotionCommand(0.8, 0.3); + } else if (count < 15) { + std::cout << "Motor: 1.5, 0.5" << std::endl; + tracer.SetMotionCommand(1.5, 0.5); + } else if (count < 20) { + std::cout << "Motor: 1.0, 0.3" << std::endl; + tracer.SetMotionCommand(1.0, 0.3); + } else if (count < 25) { + std::cout << "Motor: 0.0, 0.0" << std::endl; + tracer.SetMotionCommand(0.0, 0.0); + } else if (count < 30) { + std::cout << "Motor: -0.5, -0.3" << std::endl; + tracer.SetMotionCommand(-0.5, -0.3); + } else if (count < 35) { + std::cout << "Motor: -1.0, -0.5" << std::endl; + tracer.SetMotionCommand(-1.0, -0.5); + } else if (count < 40) { + std::cout << "Motor: 0.0, 0.0," << std::endl; + tracer.SetMotionCommand(0.0, 0.0); + } + // tracer.SetMotionCommand(0.8, 0.8); + + auto state = tracer.GetTracerState(); + std::cout << "-------------------------------" << std::endl; + std::cout << "count: " << count << std::endl; + std::cout << "control mode: " << static_cast(state.system_state.control_mode) + << " , vehicle state: " << static_cast(state.system_state.vehicle_state) + << std::endl; + std::cout << "battery voltage: " << state.system_state.battery_voltage << std::endl; + std::cout << "velocity (linear, angular): " << state.motion_state.linear_velocity << ", " + << state.motion_state.angular_velocity << std::endl; + std::cout << "-------------------------------" << std::endl; + + // usleep(20000); + sleep(1); + ++count; + } + + return 0; +} \ No newline at end of file diff --git a/src/async_port/async_can.cpp b/src/async_port/async_can.cpp new file mode 100644 index 0000000..e0fcd68 --- /dev/null +++ b/src/async_port/async_can.cpp @@ -0,0 +1,124 @@ +/* + * async_can.cpp + * + * Created on: Sep 10, 2020 13:23 + * Description: + * + * Copyright (c) 2020 Weston Robot Pte. Ltd. + */ + +#include "ugv_sdk/details/async_port/async_can.hpp" + +#include +#include +#include +#include +#include +#include + +#include + +namespace westonrobot { +AsyncCAN::AsyncCAN(std::string can_port) + : AsyncPortBase(can_port), socketcan_stream_(io_context_) {} + +bool AsyncCAN::SetupPort() { + try { + const size_t iface_name_size = strlen(port_.c_str()) + 1; + if (iface_name_size > IFNAMSIZ) return false; + + can_fd_ = socket(PF_CAN, SOCK_RAW | SOCK_NONBLOCK, CAN_RAW); + if (can_fd_ < 0) return false; + + struct ifreq ifr; + memset(&ifr, 0, sizeof(ifr)); + memcpy(ifr.ifr_name, port_.c_str(), iface_name_size); + + const int ioctl_result = ioctl(can_fd_, SIOCGIFINDEX, &ifr); + if (ioctl_result < 0) StopService(); + + struct sockaddr_can addr; + memset(&addr, 0, sizeof(addr)); + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + + const int bind_result = + bind(can_fd_, (struct sockaddr *)&addr, sizeof(addr)); + if (bind_result < 0) StopService(); + + port_opened_ = true; + std::cout << "Start listening to port: " << port_ << std::endl; + } catch (std::system_error &e) { + std::cout << e.what(); + return false; + } + + // give some work to io_service to start async io chain + socketcan_stream_.assign(can_fd_); + +#if ASIO_VERSION < 101200L + io_context_.post(std::bind(&AsyncCAN::ReadFromPort, this, + std::ref(rcv_frame_), + std::ref(socketcan_stream_))); +#else + asio::post(io_context_, + std::bind(&AsyncCAN::ReadFromPort, this, std::ref(rcv_frame_), + std::ref(socketcan_stream_))); +#endif + + return true; +} + +void AsyncCAN::StopService() { + // release port fd + const int close_result = ::close(can_fd_); + can_fd_ = -1; + + // stop io thread + io_context_.stop(); + if (io_thread_.joinable()) io_thread_.join(); + io_context_.reset(); + + port_opened_ = false; +} + +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++) + std::cout << std::hex << int(rx_frame->data[i]) << " "; + std::cout << std::dec << std::endl; +} + +void AsyncCAN::ReadFromPort(struct can_frame &rec_frame, + asio::posix::basic_stream_descriptor<> &stream) { + auto sthis = shared_from_this(); + stream.async_read_some( + asio::buffer(&rec_frame, sizeof(rec_frame)), + [sthis](asio::error_code error, size_t bytes_transferred) { + if (error) { + sthis->StopService(); + return; + } + + if (sthis->rcv_cb_ != nullptr) + sthis->rcv_cb_(&sthis->rcv_frame_); + else + sthis->DefaultReceiveCallback(&sthis->rcv_frame_); + + sthis->ReadFromPort(std::ref(sthis->rcv_frame_), + std::ref(sthis->socketcan_stream_)); + }); +} + +void AsyncCAN::SendFrame(const struct can_frame &frame) { + socketcan_stream_.async_write_some( + asio::buffer(&frame, sizeof(frame)), + [](asio::error_code error, size_t bytes_transferred) { + if (error) { + std::cerr << "Failed to send CAN frame" << std::endl; + } + // std::cout << "frame sent" << std::endl; + }); +} + +} // namespace westonrobot \ No newline at end of file diff --git a/src/async_port/async_serial.cpp b/src/async_port/async_serial.cpp new file mode 100644 index 0000000..00d246a --- /dev/null +++ b/src/async_port/async_serial.cpp @@ -0,0 +1,155 @@ +/* + * async_serial.cpp + * + * Created on: Sep 10, 2020 13:00 + * Description: + * + * Copyright (c) 2020 Weston Robot Pte. Ltd. + */ + +#include "ugv_sdk/details/async_port/async_serial.hpp" + +#if defined(__linux__) +#include +#endif + +#include +#include + +namespace westonrobot { + +AsyncSerial::AsyncSerial(std::string port_name, uint32_t baud_rate) + : AsyncPortBase(port_name), + baud_rate_(baud_rate), + serial_port_(io_context_) {} + +void AsyncSerial::SetBaudRate(unsigned baudrate) { + serial_port_.set_option(asio::serial_port_base::baud_rate(baudrate)); +} + +bool AsyncSerial::SetupPort() { + using SPB = asio::serial_port_base; + + try { + serial_port_.open(port_); + + // Set baudrate and 8N1 mode + serial_port_.set_option(SPB::baud_rate(baud_rate_)); + serial_port_.set_option(SPB::character_size(8)); + serial_port_.set_option(SPB::parity(SPB::parity::none)); + serial_port_.set_option(SPB::stop_bits(SPB::stop_bits::one)); + serial_port_.set_option(SPB::flow_control( + (hwflow_) ? SPB::flow_control::hardware : SPB::flow_control::none)); + +#if defined(__linux__) + // Enable low latency mode on Linux + { + int fd = serial_port_.native_handle(); + struct serial_struct ser_info; + ioctl(fd, TIOCGSERIAL, &ser_info); + ser_info.flags |= ASYNC_LOW_LATENCY; + ioctl(fd, TIOCSSERIAL, &ser_info); + } +#endif + + port_opened_ = true; + std::cout << "Start listening to port: " << port_ << "@" << baud_rate_ + << std::endl; + + } catch (std::system_error &e) { + std::cout << e.what(); + return false; + } + + // give some work to io_service to start async io chain +#if ASIO_VERSION < 101200L + io_context_.post(std::bind(&AsyncSerial::ReadFromPort, this)); +#else + asio::post(io_context_, std::bind(&AsyncSerial::ReadFromPort, this)); +#endif + + return true; +} + +void AsyncSerial::StopService() { + if (IsOpened()) { + serial_port_.cancel(); + serial_port_.close(); + } + + // stop io thread + io_context_.stop(); + if (io_thread_.joinable()) io_thread_.join(); + io_context_.reset(); + + port_opened_ = false; +} + +void AsyncSerial::DefaultReceiveCallback(uint8_t *data, const size_t bufsize, + size_t len) {} + +void AsyncSerial::ReadFromPort() { + auto sthis = shared_from_this(); + serial_port_.async_read_some( + asio::buffer(rx_buf_), + [sthis](asio::error_code error, size_t bytes_transferred) { + if (error) { + sthis->StopService(); + return; + } + + if (sthis->rcv_cb_ != nullptr) { + sthis->rcv_cb_(sthis->rx_buf_.data(), sthis->rx_buf_.size(), + bytes_transferred); + } else { + sthis->DefaultReceiveCallback( + sthis->rx_buf_.data(), sthis->rx_buf_.size(), bytes_transferred); + } + sthis->ReadFromPort(); + }); +} + +void AsyncSerial::WriteToPort(bool check_if_busy) { + // do nothing if an async tx has already been initiated + if (check_if_busy && tx_in_progress_) return; + + std::lock_guard lock(tx_mutex_); + if (tx_rbuf_.IsEmpty()) return; + + auto sthis = shared_from_this(); + tx_in_progress_ = true; + auto len = tx_rbuf_.Read(tx_buf_, tx_rbuf_.GetOccupiedSize()); + serial_port_.async_write_some( + asio::buffer(tx_buf_, len), + [sthis](asio::error_code error, size_t bytes_transferred) { + if (error) { + sthis->StopService(); + return; + } + std::lock_guard lock(sthis->tx_mutex_); + if (sthis->tx_rbuf_.IsEmpty()) { + sthis->tx_in_progress_ = false; + return; + } else { + sthis->WriteToPort(false); + } + }); +} + +void AsyncSerial::SendBytes(const uint8_t *bytes, size_t length) { + if (!IsOpened()) { + std::cerr << "Failed to send, port closed" << std::endl; + return; + } + assert(length < rxtx_buffer_size); + std::lock_guard lock(tx_mutex_); + if (tx_rbuf_.GetFreeSize() < length) { + throw std::length_error( + "AsyncSerial::SendBytes: tx buffer overflow, try to slow down sending " + "data"); + } + tx_rbuf_.Write(bytes, length); + io_context_.post( + std::bind(&AsyncSerial::WriteToPort, shared_from_this(), true)); +} +} // namespace westonrobot \ No newline at end of file