mirror of
https://github.com/westonrobot/ugv_sdk
synced 2023-04-08 06:32:14 +08:00
more code cleanup
This commit is contained in:
@@ -42,7 +42,7 @@
|
||||
// #include "async_io/device_error.hpp"
|
||||
// #include "async_io/msg_buffer.hpp"
|
||||
|
||||
namespace wescore
|
||||
namespace westonrobot
|
||||
{
|
||||
using steady_clock = std::chrono::steady_clock;
|
||||
using lock_guard = std::lock_guard<std::recursive_mutex>;
|
||||
@@ -125,6 +125,6 @@ private:
|
||||
void iostat_tx_add(std::size_t bytes);
|
||||
void iostat_rx_add(std::size_t bytes);
|
||||
};
|
||||
} // namespace wescore
|
||||
} // namespace westonrobot
|
||||
|
||||
#endif /* ASYNC_CAN_HPP */
|
||||
@@ -40,10 +40,10 @@
|
||||
|
||||
#include "asio.hpp"
|
||||
|
||||
#include "wrp_sdk/async_io/device_error.hpp"
|
||||
#include "wrp_sdk/async_io/msg_buffer.hpp"
|
||||
#include "wrp_sdk/asyncio/device_error.hpp"
|
||||
#include "wrp_sdk/asyncio/msg_buffer.hpp"
|
||||
|
||||
namespace wescore
|
||||
namespace westonrobot
|
||||
{
|
||||
using steady_clock = std::chrono::steady_clock;
|
||||
using lock_guard = std::lock_guard<std::recursive_mutex>;
|
||||
@@ -140,6 +140,6 @@ private:
|
||||
void iostat_tx_add(std::size_t bytes);
|
||||
void iostat_rx_add(std::size_t bytes);
|
||||
};
|
||||
} // namespace wescore
|
||||
} // namespace westonrobot
|
||||
|
||||
#endif /* ASYNC_SERIAL_HPP */
|
||||
@@ -19,7 +19,7 @@
|
||||
#include <cstring>
|
||||
#include <stdexcept>
|
||||
|
||||
namespace wescore
|
||||
namespace westonrobot
|
||||
{
|
||||
class DeviceError : public std::runtime_error
|
||||
{
|
||||
@@ -19,7 +19,7 @@
|
||||
#include <cstring>
|
||||
#include <cassert>
|
||||
|
||||
namespace wescore
|
||||
namespace westonrobot
|
||||
{
|
||||
|
||||
/**
|
||||
@@ -66,4 +66,4 @@ struct MsgBuffer
|
||||
return len - pos;
|
||||
}
|
||||
};
|
||||
} // namespace wescore
|
||||
} // namespace westonrobot
|
||||
96
include/wrp_sdk/platforms/common/mobile_base.hpp
Normal file
96
include/wrp_sdk/platforms/common/mobile_base.hpp
Normal file
@@ -0,0 +1,96 @@
|
||||
/*
|
||||
* mobile_base.hpp
|
||||
*
|
||||
* Created on: Jun 17, 2020 11:23
|
||||
* Description:
|
||||
*
|
||||
* Generic mobile base: this class handles the communication
|
||||
* logic that is similar across different mobile platforms
|
||||
*
|
||||
* Copyright (c) 2020 Ruixiang Du (rdu)
|
||||
*/
|
||||
|
||||
#ifndef MOBILE_BASE_HPP
|
||||
#define MOBILE_BASE_HPP
|
||||
|
||||
#include <string>
|
||||
#include <cstdint>
|
||||
#include <thread>
|
||||
#include <mutex>
|
||||
#include <functional>
|
||||
|
||||
#include "wrp_sdk/asyncio/async_can.hpp"
|
||||
#include "wrp_sdk/asyncio/async_serial.hpp"
|
||||
|
||||
namespace westonrobot {
|
||||
class MobileBase {
|
||||
public:
|
||||
MobileBase() = default;
|
||||
virtual ~MobileBase();
|
||||
|
||||
// do not allow copy or assignment
|
||||
MobileBase(const MobileBase &hunter) = delete;
|
||||
MobileBase &operator=(const MobileBase &hunter) = delete;
|
||||
|
||||
// connect to roboot from CAN or serial
|
||||
void Connect(std::string dev_name, int32_t baud_rate = 0);
|
||||
|
||||
// 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;
|
||||
};
|
||||
|
||||
// motion control
|
||||
void SetMotionCommand(double linear_vel, double steering_angle,
|
||||
HunterMotionCmd::FaultClearFlag fault_clr_flag =
|
||||
HunterMotionCmd::FaultClearFlag::NO_FAULT);
|
||||
|
||||
// get robot state
|
||||
HunterState GetHunterState();
|
||||
|
||||
protected:
|
||||
// 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 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);
|
||||
};
|
||||
} // namespace westonrobot
|
||||
|
||||
#endif /* MOBILE_BASE_HPP */
|
||||
@@ -1,11 +1,11 @@
|
||||
/*
|
||||
/*
|
||||
* hunter_base.hpp
|
||||
*
|
||||
*
|
||||
* Created on: Apr 01, 2020 09:43
|
||||
* Description:
|
||||
*
|
||||
* Description:
|
||||
*
|
||||
* Copyright (c) 2019 Ruixiang Du (rdu)
|
||||
*/
|
||||
*/
|
||||
|
||||
#ifndef HUNTER_BASE_HPP
|
||||
#define HUNTER_BASE_HPP
|
||||
@@ -16,84 +16,88 @@
|
||||
#include <mutex>
|
||||
#include <functional>
|
||||
|
||||
#include "wrp_sdk/async_io/async_can.hpp"
|
||||
#include "wrp_sdk/async_io/async_serial.hpp"
|
||||
#include "wrp_sdk/asyncio/async_can.hpp"
|
||||
#include "wrp_sdk/asyncio/async_serial.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 wescore
|
||||
{
|
||||
class HunterBase
|
||||
{
|
||||
public:
|
||||
HunterBase() = default;
|
||||
~HunterBase();
|
||||
namespace westonrobot {
|
||||
class HunterBase {
|
||||
public:
|
||||
HunterBase() = default;
|
||||
~HunterBase();
|
||||
|
||||
// do not allow copy
|
||||
HunterBase(const HunterBase &hunter) = delete;
|
||||
HunterBase &operator=(const HunterBase &hunter) = delete;
|
||||
// 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);
|
||||
public:
|
||||
// connect to roboot from CAN
|
||||
void Connect(std::string dev_name);
|
||||
|
||||
// disconnect from roboot, only valid for serial port
|
||||
void Disconnect();
|
||||
// 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; };
|
||||
// cmd thread runs at 100Hz (10ms) by default
|
||||
void SetCmdThreadPeriodMs(int32_t period_ms) {
|
||||
cmd_thread_period_ms_ = period_ms;
|
||||
};
|
||||
|
||||
// motion control
|
||||
void SetMotionCommand(double linear_vel, double steering_angle,
|
||||
HunterMotionCmd::FaultClearFlag fault_clr_flag = HunterMotionCmd::FaultClearFlag::NO_FAULT);
|
||||
// motion control
|
||||
void SetMotionCommand(double linear_vel, double steering_angle,
|
||||
HunterMotionCmd::FaultClearFlag fault_clr_flag =
|
||||
HunterMotionCmd::FaultClearFlag::NO_FAULT);
|
||||
|
||||
// get robot state
|
||||
HunterState GetHunterState();
|
||||
// get robot state
|
||||
HunterState GetHunterState();
|
||||
|
||||
private:
|
||||
// hardware communication interface
|
||||
std::shared_ptr<ASyncCAN> can_if_;
|
||||
std::shared_ptr<ASyncSerial> serial_if_;
|
||||
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;
|
||||
// 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];
|
||||
// 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_;
|
||||
// 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_;
|
||||
HunterState hunter_state_;
|
||||
HunterMotionCmd current_motion_cmd_;
|
||||
|
||||
int32_t cmd_thread_period_ms_ = 10;
|
||||
bool cmd_thread_started_ = false;
|
||||
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);
|
||||
// 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 StartCmdThread();
|
||||
void ControlLoop(int32_t period_ms);
|
||||
|
||||
void SendMotionCmd(uint8_t count);
|
||||
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 ParseCANFrame(can_frame *rx_frame);
|
||||
void ParseUARTBuffer(uint8_t *buf, const size_t bufsize,
|
||||
size_t bytes_received);
|
||||
|
||||
void NewStatusMsgReceivedCallback(const HunterMessage &msg);
|
||||
void NewStatusMsgReceivedCallback(const HunterMessage &msg);
|
||||
|
||||
public:
|
||||
static void UpdateHunterState(const HunterMessage &status_msg, HunterState &state);
|
||||
public:
|
||||
static void UpdateHunterState(const HunterMessage &status_msg,
|
||||
HunterState &state);
|
||||
};
|
||||
} // namespace wescore
|
||||
} // namespace westonrobot
|
||||
|
||||
#endif /* HUNTER_BASE_HPP */
|
||||
|
||||
@@ -13,7 +13,7 @@
|
||||
#include <cstdint>
|
||||
#include <iostream>
|
||||
|
||||
namespace wescore
|
||||
namespace westonrobot
|
||||
{
|
||||
struct HunterState
|
||||
{
|
||||
@@ -69,6 +69,6 @@ struct HunterMotionCmd
|
||||
static constexpr double max_steering_angle = 0.4622; // 0.4622 rad ~= 26.5 degree
|
||||
static constexpr double min_steering_angle = -0.4622; // -0.4622 rad
|
||||
};
|
||||
} // namespace wescore
|
||||
} // namespace westonrobot
|
||||
|
||||
#endif /* HUNTER_TYPES_HPP */
|
||||
|
||||
@@ -16,8 +16,8 @@
|
||||
#include <mutex>
|
||||
#include <functional>
|
||||
|
||||
#include "wrp_sdk/async_io/async_can.hpp"
|
||||
#include "wrp_sdk/async_io/async_serial.hpp"
|
||||
#include "wrp_sdk/asyncio/async_can.hpp"
|
||||
#include "wrp_sdk/asyncio/async_serial.hpp"
|
||||
|
||||
#include "wrp_sdk/platforms/scout/scout_protocol.h"
|
||||
#include "wrp_sdk/platforms/scout/scout_can_parser.h"
|
||||
@@ -25,7 +25,7 @@
|
||||
|
||||
#include "wrp_sdk/platforms/scout/scout_types.hpp"
|
||||
|
||||
namespace wescore
|
||||
namespace westonrobot
|
||||
{
|
||||
class ScoutBase
|
||||
{
|
||||
@@ -105,6 +105,6 @@ private:
|
||||
public:
|
||||
static void UpdateScoutState(const ScoutMessage &status_msg, ScoutState &state);
|
||||
};
|
||||
} // namespace wescore
|
||||
} // namespace westonrobot
|
||||
|
||||
#endif /* SCOUT_BASE_HPP */
|
||||
|
||||
@@ -13,7 +13,7 @@
|
||||
#include <cstdint>
|
||||
#include <iostream>
|
||||
|
||||
namespace wescore
|
||||
namespace westonrobot
|
||||
{
|
||||
struct ScoutState
|
||||
{
|
||||
@@ -106,6 +106,6 @@ struct ScoutLightCmd
|
||||
LightMode rear_mode;
|
||||
uint8_t rear_custom_value;
|
||||
};
|
||||
} // namespace wescore
|
||||
} // namespace westonrobot
|
||||
|
||||
#endif /* SCOUT_STATE_HPP */
|
||||
|
||||
@@ -24,7 +24,7 @@
|
||||
|
||||
#include "tracer_base/tracer_types.hpp"
|
||||
|
||||
namespace wescore
|
||||
namespace westonrobot
|
||||
{
|
||||
class TracerBase
|
||||
{
|
||||
@@ -104,6 +104,6 @@ private:
|
||||
public:
|
||||
static void UpdateTracerState(const TracerMessage &status_msg, TracerState &state);
|
||||
};
|
||||
} // namespace wescore
|
||||
} // namespace westonrobot
|
||||
|
||||
#endif /* TRACER_BASE_HPP */
|
||||
|
||||
@@ -13,7 +13,7 @@
|
||||
#include <cstdint>
|
||||
#include <iostream>
|
||||
|
||||
namespace wescore
|
||||
namespace westonrobot
|
||||
{
|
||||
struct TracerState
|
||||
{
|
||||
@@ -106,6 +106,6 @@ struct TracerLightCmd
|
||||
LightMode rear_mode;
|
||||
uint8_t rear_custom_value;
|
||||
};
|
||||
} // namespace wescore
|
||||
} // namespace westonrobot
|
||||
|
||||
#endif /* TRACER_TYPES_HPP */
|
||||
|
||||
Reference in New Issue
Block a user