From 19197facc0100f1fb7e88119ffda86d44414c032 Mon Sep 17 00:00:00 2001 From: Ruixiang Du Date: Wed, 17 Jun 2020 14:47:37 +0800 Subject: [PATCH] saved work --- CMakeLists.txt | 90 ++++++++----------- .../wrp_sdk/platforms/common/mobile_base.hpp | 6 +- .../wrp_sdk/platforms/hunter/hunter_base.hpp | 54 +++-------- src/platforms/hunter_base.cpp | 84 +---------------- src/platforms/scout_base.cpp | 2 +- 5 files changed, 52 insertions(+), 184 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ae0f1d9..db90456 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,8 +1,8 @@ cmake_minimum_required(VERSION 3.1.0) project(wrp_sdk) -# # Find catkin -# find_package(catkin REQUIRED) +# Find catkin +find_package(catkin REQUIRED) # generate symbols for IDE indexer set(CMAKE_EXPORT_COMPILE_COMMANDS ON) @@ -14,12 +14,12 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) ## Put all binary files into /bin and libraries into /lib -set(EXECUTABLE_OUTPUT_PATH ${CMAKE_BINARY_DIR}/bin) -set(LIBRARY_OUTPUT_PATH ${CMAKE_BINARY_DIR}/lib) -set(ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib) +# set(EXECUTABLE_OUTPUT_PATH ${CMAKE_BINARY_DIR}/bin) +# set(LIBRARY_OUTPUT_PATH ${CMAKE_BINARY_DIR}/lib) +# set(ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib) ## Chosse build type -set(CMAKE_BUILD_TYPE Release) +# set(CMAKE_BUILD_TYPE Release) # set(CMAKE_BUILD_TYPE Debug) set(default_build_type "Release") @@ -33,7 +33,7 @@ if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) endif() ## Optionally built modules: ON/OFF -set(BUILD_TESTS ON) +set(BUILD_TESTS OFF) set(BUILD_MONITOR ON) # Disable monitor if ncurses library is not found @@ -46,7 +46,20 @@ if(BUILD_MONITOR AND NOT CURSES_FOUND) endif() # Add libraries -set(WRP_SDK_SRC +# set(WRP_SDK_SRC +# src/async_serial.cpp +# src/async_can.cpp +# src/asyncio_utils.cpp +# # agilex mobile platforms +# src/platforms/mobile_base.cpp +# src/platforms/hunter_base.cpp +# src/platforms/hunter_can_parser.c +# src/platforms/scout_base.cpp +# src/platforms/scout_can_parser.c +# src/platforms/scout_uart_parser.c +# ) +# add_library(${PROJECT_NAME} STATIC ${WRP_SDK_SRC}) +add_library(${PROJECT_NAME} src/async_serial.cpp src/async_can.cpp src/asyncio_utils.cpp @@ -58,65 +71,36 @@ set(WRP_SDK_SRC src/platforms/scout_can_parser.c src/platforms/scout_uart_parser.c ) -add_library(wrpsdk STATIC ${WRP_SDK_SRC}) -target_compile_definitions(wrpsdk PUBLIC "-DASIO_ENABLE_OLD_SERVICES -DASIO_HAS_POSIX_STREAM_DESCRIPTOR") -target_link_libraries(wrpsdk pthread) -target_include_directories(wrpsdk PUBLIC +target_compile_definitions(${PROJECT_NAME} PUBLIC "-DASIO_ENABLE_OLD_SERVICES -DASIO_HAS_POSIX_STREAM_DESCRIPTOR") +target_link_libraries(${PROJECT_NAME} pthread) +target_include_directories(${PROJECT_NAME} PUBLIC $ $ PRIVATE src) -# install(TARGETS asyncio -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) - -# install(DIRECTORY include asio -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) - # Add executables if(BUILD_TESTS) - add_subdirectory(tests) + add_subdirectory(tests) + add_subdirectory(unit_tests) endif() - -if(BUILD_TESTS) -add_subdirectory(unit_tests) -endif() +## Export catkin targets +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} +# CATKIN_DEPENDS hunter_msgs roscpp +# DEPENDS system_lib +) ## Use GNUInstallDirs to install libraries into correct # locations on all platforms. include(GNUInstallDirs) -# ## Export catkin targets -# set(EXPORTED_LIBS -# asyncio -# stopwatch -# scout_protocol -# scoutbase -# hunter_protocol -# hunterbase -# ) -# set(INCLUDE_DIRS -# src/common/async_io/asio -# src/common/async_io/include -# src/common/utilities/stopwatch/include -# src/scout_sdk/scout_protocol/include -# src/scout_sdk/scout_base/include -# src/hunter_sdk/hunter_protocol/include -# src/hunter_sdk/hunter_base/include -# ) -# catkin_package( -# LIBRARIES ${EXPORTED_LIBS} -# INCLUDE_DIRS ${INCLUDE_DIRS} -# # DEPENDS rt pthread -# ) - ## Add catkin install targets -# install(TARGETS ${EXPORTED_LIBS} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) +install(TARGETS ${PROJECT_NAME} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) # install(DIRECTORY include/${PROJECT_NAME}/ # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) diff --git a/include/wrp_sdk/platforms/common/mobile_base.hpp b/include/wrp_sdk/platforms/common/mobile_base.hpp index 2dfb976..bcf34d4 100644 --- a/include/wrp_sdk/platforms/common/mobile_base.hpp +++ b/include/wrp_sdk/platforms/common/mobile_base.hpp @@ -17,7 +17,6 @@ #include #include #include -#include #include "wrp_sdk/asyncio/async_can.hpp" #include "wrp_sdk/asyncio/async_serial.hpp" @@ -64,10 +63,11 @@ class MobileBase { void StartCmdThread(); void ControlLoop(int32_t period_ms); + // functions that must/may be implemented by child classes virtual void SendRobotCmd() = 0; - virtual void ParseCANFrame(can_frame *rx_frame) = 0; + virtual void ParseCANFrame(can_frame *rx_frame){}; virtual void ParseUARTBuffer(uint8_t *buf, const size_t bufsize, - size_t bytes_received) = 0; + size_t bytes_received){}; }; } // namespace westonrobot diff --git a/include/wrp_sdk/platforms/hunter/hunter_base.hpp b/include/wrp_sdk/platforms/hunter/hunter_base.hpp index 1fd654b..296b305 100644 --- a/include/wrp_sdk/platforms/hunter/hunter_base.hpp +++ b/include/wrp_sdk/platforms/hunter/hunter_base.hpp @@ -14,84 +14,50 @@ #include #include #include -#include -#include "wrp_sdk/asyncio/async_can.hpp" -#include "wrp_sdk/asyncio/async_serial.hpp" +#include "wrp_sdk/platforms/common/mobile_base.hpp" #include "wrp_sdk/platforms/hunter/hunter_protocol.h" #include "wrp_sdk/platforms/hunter/hunter_can_parser.h" - #include "wrp_sdk/platforms/hunter/hunter_types.hpp" namespace westonrobot { -class HunterBase { +class HunterBase : public MobileBase { public: - HunterBase() = default; - ~HunterBase(); + HunterBase() : MobileBase(){}; + ~HunterBase() = default; - // do not allow copy - HunterBase(const HunterBase &hunter) = delete; - HunterBase &operator=(const HunterBase &hunter) = delete; - - public: - // connect to roboot from CAN - void Connect(std::string dev_name); - - // disconnect from roboot, only valid for serial port - void Disconnect(); - - // cmd thread runs at 100Hz (10ms) by default - void SetCmdThreadPeriodMs(int32_t period_ms) { - cmd_thread_period_ms_ = period_ms; - }; + // get robot state + HunterState GetHunterState(); // motion control void SetMotionCommand(double linear_vel, double steering_angle, HunterMotionCmd::FaultClearFlag fault_clr_flag = HunterMotionCmd::FaultClearFlag::NO_FAULT); - // get robot state - HunterState GetHunterState(); - private: - // hardware communication interface - std::shared_ptr can_if_; - std::shared_ptr serial_if_; - - // CAN priority higher than serial if both connected - bool can_connected_ = false; - bool serial_connected_ = false; - // serial port related variables uint8_t tx_cmd_len_; uint8_t tx_buffer_[HUNTER_CMD_BUF_LEN]; // cmd/status update related variables - std::thread cmd_thread_; std::mutex hunter_state_mutex_; std::mutex motion_cmd_mutex_; HunterState hunter_state_; HunterMotionCmd current_motion_cmd_; - int32_t cmd_thread_period_ms_ = 10; - bool cmd_thread_started_ = false; - // internal functions void ConfigureCANBus(const std::string &can_if_name = "can1"); void ConfigureSerial(const std::string uart_name = "/dev/ttyUSB0", int32_t baud_rate = 115200); - void StartCmdThread(); - void ControlLoop(int32_t period_ms); + void SendRobotCmd() override; + void ParseCANFrame(can_frame *rx_frame) override; + void ParseUARTBuffer(uint8_t *buf, const size_t bufsize, + size_t bytes_received) override{}; void SendMotionCmd(uint8_t count); - - void ParseCANFrame(can_frame *rx_frame); - void ParseUARTBuffer(uint8_t *buf, const size_t bufsize, - size_t bytes_received); - void NewStatusMsgReceivedCallback(const HunterMessage &msg); public: diff --git a/src/platforms/hunter_base.cpp b/src/platforms/hunter_base.cpp index 98397d0..c2378ef 100644 --- a/src/platforms/hunter_base.cpp +++ b/src/platforms/hunter_base.cpp @@ -13,60 +13,8 @@ #include "stopwatch.h" namespace westonrobot { -HunterBase::~HunterBase() { - if (serial_connected_) serial_if_->close(); - if (cmd_thread_.joinable()) cmd_thread_.join(); -} - -void HunterBase::Connect(std::string dev_name) { - // if (baud_rate == 0) { - ConfigureCANBus(dev_name); - // } else { - // ConfigureSerial(dev_name, baud_rate); - - // if (!serial_connected_) - // std::cerr << "ERROR: Failed to connect to serial port" << std::endl; - // } -} - -void HunterBase::Disconnect() { - if (serial_connected_) { - if (serial_if_->is_open()) serial_if_->close(); - } -} - -void HunterBase::ConfigureCANBus(const std::string &can_if_name) { - can_if_ = std::make_shared(can_if_name); - - can_if_->set_receive_callback( - std::bind(&HunterBase::ParseCANFrame, this, std::placeholders::_1)); - - can_connected_ = true; -} - -void HunterBase::ConfigureSerial(const std::string uart_name, - int32_t baud_rate) { - serial_if_ = std::make_shared(uart_name, baud_rate); - serial_if_->open(); - - if (serial_if_->is_open()) serial_connected_ = true; - - serial_if_->set_receive_callback( - std::bind(&HunterBase::ParseUARTBuffer, this, std::placeholders::_1, - std::placeholders::_2, std::placeholders::_3)); -} - -void HunterBase::StartCmdThread() { - current_motion_cmd_.linear_velocity = 0; - current_motion_cmd_.angular_velocity = 0; - current_motion_cmd_.fault_clear_flag = - HunterMotionCmd::FaultClearFlag::NO_FAULT; - - cmd_thread_ = std::thread( - std::bind(&HunterBase::ControlLoop, this, cmd_thread_period_ms_)); - cmd_thread_started_ = true; -} +void HunterBase::SendRobotCmd() {} void HunterBase::SendMotionCmd(uint8_t count) { // motion control message @@ -110,22 +58,6 @@ void HunterBase::SendMotionCmd(uint8_t count) { } } -void HunterBase::ControlLoop(int32_t period_ms) { - StopWatch ctrl_sw; - uint8_t cmd_count = 0; - uint8_t light_cmd_count = 0; - while (true) { - ctrl_sw.tic(); - - // motion control message - SendMotionCmd(cmd_count++); - - ctrl_sw.sleep_until_ms(period_ms); - // std::cout << "control loop update frequency: " << 1.0 / ctrl_sw.toc() << - // std::endl; - } -} - HunterState HunterBase::GetHunterState() { std::lock_guard guard(hunter_state_mutex_); return hunter_state_; @@ -170,20 +102,6 @@ void HunterBase::ParseCANFrame(can_frame *rx_frame) { NewStatusMsgReceivedCallback(status_msg); } -void HunterBase::ParseUARTBuffer(uint8_t *buf, const size_t bufsize, - size_t bytes_received) { - // std::cout << "bytes received from serial: " << bytes_received << std::endl; - // serial_parser_.PrintStatistics(); - // serial_parser_.ParseBuffer(buf, bytes_received); - - // TODO - // HunterMessage status_msg; - // for (int i = 0; i < bytes_received; ++i) { - // if (DecodeHunterMsgFromUART(buf[i], &status_msg)) - // NewStatusMsgReceivedCallback(status_msg); - // } -} - void HunterBase::NewStatusMsgReceivedCallback(const HunterMessage &msg) { // std::cout << "new status msg received" << std::endl; std::lock_guard guard(hunter_state_mutex_); diff --git a/src/platforms/scout_base.cpp b/src/platforms/scout_base.cpp index ac924df..ea60bbe 100644 --- a/src/platforms/scout_base.cpp +++ b/src/platforms/scout_base.cpp @@ -335,4 +335,4 @@ void ScoutBase::UpdateScoutState(const ScoutMessage &status_msg, ScoutState &sta } } } -} // namespace wescore +} // namespace westonrobot