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

saved work

Browse Source
This commit is contained in:
Ruixiang Du
2020-06-17 14:47:37 +08:00
parent 741c521863
commit 19197facc0
5 changed files with 52 additions and 184 deletions
Download Patch File Download Diff File Expand all files Collapse all files

88
CMakeLists.txt
View File

@@ -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
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
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)
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})

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

@@ -17,7 +17,6 @@
#include <cstdint>
#include <thread>
#include <mutex>
#include <functional>
#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

54
include/wrp_sdk/platforms/hunter/hunter_base.hpp
View File

@@ -14,84 +14,50 @@
#include <cstdint>
#include <thread>
#include <mutex>
#include <functional>
#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<ASyncCAN> can_if_;
std::shared_ptr<ASyncSerial> 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:

84
src/platforms/hunter_base.cpp
View File

@@ -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<ASyncCAN>(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<ASyncSerial>(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<std::mutex> 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<std::mutex> guard(hunter_state_mutex_);

2
src/platforms/scout_base.cpp
View File

@@ -335,4 +335,4 @@ void ScoutBase::UpdateScoutState(const ScoutMessage &status_msg, ScoutState &sta
}
}
}
} // namespace wescore
} // namespace westonrobot