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:
rdu
2021-07-07 19:03:54 +08:00
parent f0f5b88987
commit 2a67bbceb4
6 changed files with 631 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
CMakeLists.txt
View File

@@ -83,6 +83,9 @@ add_library(${PROJECT_NAME}
src/tracer_base.cpp
# src/bunker_base.cpp
src/ranger_base.cpp
########################
# legacy support
src/legacy/scout_can_parser_v1.c
)
target_link_libraries(${PROJECT_NAME} PUBLIC Threads::Threads)
target_compile_definitions(${PROJECT_NAME} PUBLIC ASIO_ENABLE_OLD_SERVICES)

29
include/ugv_sdk/interface/scout_interface.hpp Normal file
View File

@@ -0,0 +1,29 @@
/*
* scout_interface.hpp
*
* Created on: Jul 07, 2021 09:10
* Description:
*
* Copyright (c) 2021 Weston Robot Pte. Ltd.
*/
#ifndef SCOUT_INTERFACE_HPP
#define SCOUT_INTERFACE_HPP
namespace westonrobot {
class ScoutInterface {
public:
// set up connection
void Connect(std::string dev_name) override;
// robot control
void SetMotionCommand(double linear_vel, double angular_vel);
void SetLightCommand(const ScoutLightCmd &cmd);
void DisableLightCmdControl();
// get robot state
ScoutState GetScoutState();
};
} // namespace westonrobot
#endif /* SCOUT_INTERFACE_HPP */

104
include/ugv_sdk/legacy/agilex_base_v1.hpp Normal file
View File

@@ -0,0 +1,104 @@
/*
* agilex_base.hpp
*
* Created on: Dec 22, 2020 17:14
* Description:
*
* Each robot class derived from this base class should provide implementation
* for the following two functions:
*
* - virtual void Connect(std::string dev_name) = 0;
* - virtual void ParseCANFrame(can_frame *rx_frame) = 0;
*
* Copyright (c) 2020 Ruixiang Du (rdu)
*/
#ifndef AGILEX_BASE_HPP
#define AGILEX_BASE_HPP
#include <string>
#include <cstdint>
#include <thread>
#include <mutex>
#include <atomic>
#include "ugv_sdk/details/async_port/async_can.hpp"
#include "ugv_sdk/agilex_message.h"
namespace westonrobot {
class AgilexBase {
public:
AgilexBase() = default;
virtual ~AgilexBase();
// do not allow copy or assignment
AgilexBase(const AgilexBase &hunter) = delete;
AgilexBase &operator=(const AgilexBase &hunter) = delete;
// any derived robot must implement this method with proper call back defined
virtual void Connect(std::string dev_name) = 0;
// cmd thread runs at 50Hz (20ms) by default
void SetCmdThreadPeriodMs(int32_t period_ms);
// timeout: robot stops if user does not call SetMotionCommand() periodically
void EnableCmdTimeout(uint32_t timeout_ms = 100);
void DisableTimeout() { enable_timeout_ = false; }
// switch to commanded mode
void EnableCommandedMode();
// enforce 50Hz command loop for all AgileX robots internally
void SetMotionCommand(double linear_vel, double angular_vel,
double lateral_velocity, double steering_angle);
// one-shot light command
void SendLightCommand(LightMode front_mode, uint8_t front_custom_value,
LightMode rear_mode, uint8_t rear_custom_value);
void DisableLightControl();
// motion mode change
void SetMotionMode(uint8_t mode);
// reset fault states
void ResetRobotState();
protected:
std::mutex state_mutex_;
std::mutex motion_cmd_mutex_;
MotionCommandMessage current_motion_cmd_;
// communication interface
bool can_connected_ = false;
std::shared_ptr<AsyncCAN> can_;
// timeout to be implemented in each vehicle
bool enable_timeout_ = true;
uint32_t timeout_ms_ = 500;
uint32_t watchdog_counter_ = 0;
void FeedCmdTimeoutWatchdog() { watchdog_counter_ = 0; };
// command thread
std::thread cmd_thread_;
int32_t cmd_thread_period_ms_ = 20;
bool cmd_thread_started_ = false;
std::atomic<bool> keep_running_;
// internal functions
void StartCmdThread();
void ControlLoop(int32_t period_ms);
// connect to roboot from CAN or serial
using CANFrameRxCallback = AsyncCAN::ReceiveCallback;
void Connect(std::string dev_name, CANFrameRxCallback cb);
void Disconnect();
// ask background thread to shutdown properly
void Terminate();
void SendRobotCmd();
virtual void ParseCANFrame(can_frame *rx_frame) = 0;
};
} // namespace westonrobot
#endif /* AGILEX_BASE_HPP */

42
include/ugv_sdk/legacy/scout_can_parser_v1.h Normal file
View File

@@ -0,0 +1,42 @@
/*
* scout_can_parser.h
*
* Created on: Aug 31, 2019 04:23
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#ifndef SCOUT_CAN_PARSER_H
#define SCOUT_CAN_PARSER_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <stdbool.h>
#include "ugv_sdk/legacy/scout_protocol_v1.h"
#ifdef __linux__
#include <linux/can.h>
#else
struct can_frame
{
uint32_t can_id;
uint8_t can_dlc;
uint8_t data[8]__attribute__((aligned(8)));
};
#endif
bool DecodeScoutMsgFromCAN(const struct can_frame *rx_frame, ScoutMessage *msg);
void EncodeScoutMsgToCAN(const ScoutMessage *msg, struct can_frame *tx_frame);
uint8_t CalcScoutCANChecksum(uint16_t id, uint8_t *data, uint8_t dlc);
#ifdef __cplusplus
}
#endif
#endif /* SCOUT_CAN_PARSER_H */

275
include/ugv_sdk/legacy/scout_protocol_v1.h Normal file
View File

@@ -0,0 +1,275 @@
/*
* scout_protocol.h
*
* Created on: Aug 07, 2019 21:49
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#ifndef SCOUT_PROTOCOL_H
#define SCOUT_PROTOCOL_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#define SCOUT_CMD_BUF_LEN 32
#define SCOUT_STATUS_BUF_LEN 32
#define SCOUT_FRAME_SIZE 13
#define SCOUT_MOTOR1_ID ((uint8_t)0x00)
#define SCOUT_MOTOR2_ID ((uint8_t)0x01)
#define SCOUT_MOTOR3_ID ((uint8_t)0x02)
#define SCOUT_MOTOR4_ID ((uint8_t)0x03)
// UART Definitions
#define UART_FRAME_SYSTEM_STATUS_ID ((uint8_t)0x01)
#define UART_FRAME_MOTION_STATUS_ID ((uint8_t)0x02)
#define UART_FRAME_MOTOR1_DRIVER_STATUS_ID ((uint8_t)0x03)
#define UART_FRAME_MOTOR2_DRIVER_STATUS_ID ((uint8_t)0x04)
#define UART_FRAME_MOTOR3_DRIVER_STATUS_ID ((uint8_t)0x05)
#define UART_FRAME_MOTOR4_DRIVER_STATUS_ID ((uint8_t)0x06)
#define UART_FRAME_LIGHT_STATUS_ID ((uint8_t)0x07)
#define UART_FRAME_MOTION_CONTROL_ID ((uint8_t)0x01)
#define UART_FRAME_LIGHT_CONTROL_ID ((uint8_t)0x02)
// CAN Definitions
#define CAN_MSG_MOTION_CONTROL_CMD_ID ((uint32_t)0x130)
#define CAN_MSG_MOTION_CONTROL_STATUS_ID ((uint32_t)0x131)
#define CAN_MSG_LIGHT_CONTROL_CMD_ID ((uint32_t)0x140)
#define CAN_MSG_LIGHT_CONTROL_STATUS_ID ((uint32_t)0x141)
#define CAN_MSG_SYSTEM_STATUS_STATUS_ID ((uint32_t)0x151)
#define CAN_MSG_MOTOR1_DRIVER_STATUS_ID ((uint32_t)0x200)
#define CAN_MSG_MOTOR2_DRIVER_STATUS_ID ((uint32_t)0x201)
#define CAN_MSG_MOTOR3_DRIVER_STATUS_ID ((uint32_t)0x202)
#define CAN_MSG_MOTOR4_DRIVER_STATUS_ID ((uint32_t)0x203)
/*--------------------- Control/State Constants ------------------------*/
// Motion Control
#define CTRL_MODE_REMOTE ((uint8_t)0x00)
#define CTRL_MODE_CMD_CAN ((uint8_t)0x01)
#define CTRL_MODE_CMD_UART ((uint8_t)0x02)
#define CTRL_MODE_COMMANDED ((uint8_t)0x03)
#define FAULT_CLR_NONE ((uint8_t)0x00)
#define FAULT_CLR_BAT_UNDER_VOL ((uint8_t)0x01)
#define FAULT_CLR_BAT_OVER_VOL ((uint8_t)0x02)
#define FAULT_CLR_MOTOR1_COMM ((uint8_t)0x03)
#define FAULT_CLR_MOTOR2_COMM ((uint8_t)0x04)
#define FAULT_CLR_MOTOR3_COMM ((uint8_t)0x05)
#define FAULT_CLR_MOTOR4_COMM ((uint8_t)0x06)
#define FAULT_CLR_MOTOR_DRV_OVERHEAT ((uint8_t)0x07)
#define FAULT_CLR_MOTOR_OVERCURRENT ((uint8_t)0x08)
// Light Control
#define LIGHT_DISABLE_CTRL ((uint8_t)0x00)
#define LIGHT_ENABLE_CTRL ((uint8_t)0x01)
#define LIGHT_MODE_CONST_OFF ((uint8_t)0x00)
#define LIGHT_MODE_CONST_ON ((uint8_t)0x01)
#define LIGHT_MODE_BREATH ((uint8_t)0x02)
#define LIGHT_MODE_CUSTOM ((uint8_t)0x03)
// System Status Feedback
#define BASE_STATE_NORMAL ((uint8_t)0x00)
#define BASE_STATE_ESTOP ((uint8_t)0x01)
#define BASE_STATE_EXCEPTION ((uint8_t)0x02)
#define FAULT_CAN_CHECKSUM_ERROR ((uint16_t)0x0100)
#define FAULT_MOTOR_DRV_OVERHEAT_W ((uint16_t)0x0200)
#define FAULT_MOTOR_OVERCURRENT_W ((uint16_t)0x0400)
#define FAULT_BAT_UNDER_VOL_W ((uint16_t)0x0800)
#define FAULT_RC_SIGNAL_LOSS ((uint16_t)0x1000)
#define FAULT_HIGH_BYTE_RESERVED2 ((uint16_t)0x2000)
#define FAULT_HIGH_BYTE_RESERVED3 ((uint16_t)0x4000)
#define FAULT_HIGH_BYTE_RESERVED4 ((uint16_t)0x8000)
#define FAULT_BAT_UNDER_VOL_F ((uint16_t)0x0001)
#define FAULT_BAT_OVER_VOL_F ((uint16_t)0x0002)
#define FAULT_MOTOR1_COMM_F ((uint16_t)0x0004)
#define FAULT_MOTOR2_COMM_F ((uint16_t)0x0008)
#define FAULT_MOTOR3_COMM_F ((uint16_t)0x0010)
#define FAULT_MOTOR4_COMM_F ((uint16_t)0x0020)
#define FAULT_MOTOR_DRV_OVERHEAT_F ((uint16_t)0x0040)
#define FAULT_MOTOR_OVERCURRENT_F ((uint16_t)0x0080)
/*-------------------- Control/Feedback Messages -----------------------*/
/* No padding in the struct */
// reference: https://stackoverflow.com/questions/3318410/pragma-pack-effect
#pragma pack(push, 1)
// Note: id could be different for UART and CAN protocol
// Motion Control
typedef struct {
union
{
struct
{
uint8_t control_mode;
uint8_t fault_clear_flag;
int8_t linear_velocity_cmd;
int8_t angular_velocity_cmd;
uint8_t reserved0;
uint8_t reserved1;
uint8_t count;
uint8_t checksum;
} cmd;
uint8_t raw[8];
} data;
} MotionControlMessage;
typedef struct {
union
{
struct
{
struct
{
uint8_t high_byte;
uint8_t low_byte;
} linear_velocity;
struct
{
uint8_t high_byte;
uint8_t low_byte;
} angular_velocity;
uint8_t reserved0;
uint8_t reserved1;
uint8_t count;
uint8_t checksum;
} status;
uint8_t raw[8];
} data;
} MotionStatusMessage;
// System Status Feedback
typedef struct {
union
{
struct
{
uint8_t base_state;
uint8_t control_mode;
struct
{
uint8_t high_byte;
uint8_t low_byte;
} battery_voltage;
struct
{
uint8_t high_byte;
uint8_t low_byte;
} fault_code;
uint8_t count;
uint8_t checksum;
} status;
uint8_t raw[8];
} data;
} SystemStatusMessage;
// Light Control
typedef struct {
union
{
struct
{
uint8_t light_ctrl_enable;
uint8_t front_light_mode;
uint8_t front_light_custom;
uint8_t rear_light_mode;
uint8_t rear_light_custom;
uint8_t reserved0;
uint8_t count;
uint8_t checksum;
} cmd;
uint8_t raw[8];
} data;
} LightControlMessage;
typedef struct {
union
{
struct
{
uint8_t light_ctrl_enable;
uint8_t front_light_mode;
uint8_t front_light_custom;
uint8_t rear_light_mode;
uint8_t rear_light_custom;
uint8_t reserved0;
uint8_t count;
uint8_t checksum;
} status;
uint8_t raw[8];
} data;
} LightStatusMessage;
// Motor Driver Feedback
typedef struct
{
uint8_t motor_id;
union {
struct
{
struct
{
uint8_t high_byte;
uint8_t low_byte;
} current;
struct
{
uint8_t high_byte;
uint8_t low_byte;
} rpm;
int8_t temperature;
uint8_t reserved0;
uint8_t count;
uint8_t checksum;
} status;
uint8_t raw[8];
} data;
} MotorDriverStatusMessage;
// For convenience to access status/control message
typedef enum
{
ScoutMsgNone = 0x00,
// status messages
ScoutMotionStatusMsg = 0x01,
ScoutLightStatusMsg = 0x02,
ScoutSystemStatusMsg = 0x03,
ScoutMotorDriverStatusMsg = 0x04,
// control messages
ScoutMotionControlMsg = 0x21,
ScoutLightControlMsg = 0x22
} ScoutMsgType;
typedef struct
{
ScoutMsgType type;
union {
// status messages
MotionStatusMessage motion_status_msg;
LightStatusMessage light_status_msg;
SystemStatusMessage system_status_msg;
MotorDriverStatusMessage motor_driver_status_msg;
// control messages
MotionControlMessage motion_control_msg;
LightControlMessage light_control_msg;
} body;
} ScoutMessage;
#pragma pack(pop)
#ifdef __cplusplus
}
#endif
#endif /* SCOUT_PROTOCOL_H */

178
src/legacy/scout_can_parser_v1.c Normal file
View File

@@ -0,0 +1,178 @@
/*
* scout_can_parser.c
*
* Created on: Aug 31, 2019 04:25
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#include "ugv_sdk/legacy/scout_can_parser_v1.h"
#include "string.h"
static void EncodeScoutMotionControlMsgToCAN(const MotionControlMessage *msg, struct can_frame *tx_frame);
static void EncodeScoutLightControlMsgToCAN(const LightControlMessage *msg, struct can_frame *tx_frame);
bool DecodeScoutMsgFromCAN(const struct can_frame *rx_frame, ScoutMessage *msg)
{
msg->type = ScoutMsgNone;
switch (rx_frame->can_id)
{
// in the current implementation, both MsgType and can_frame include 8 * uint8_t
case CAN_MSG_MOTION_CONTROL_STATUS_ID:
{
msg->type = ScoutMotionStatusMsg;
// msg->motion_status_msg.id = CAN_MSG_MOTION_CONTROL_STATUS_ID;
memcpy(msg->body.motion_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_LIGHT_CONTROL_STATUS_ID:
{
msg->type = ScoutLightStatusMsg;
// msg->light_status_msg.id = CAN_MSG_LIGHT_CONTROL_STATUS_ID;
memcpy(msg->body.light_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_SYSTEM_STATUS_STATUS_ID:
{
msg->type = ScoutSystemStatusMsg;
// msg->system_status_msg.id = CAN_MSG_SYSTEM_STATUS_STATUS_ID;
memcpy(msg->body.system_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_MOTOR1_DRIVER_STATUS_ID:
{
msg->type = ScoutMotorDriverStatusMsg;
// msg->motor_driver_status_msg.id = CAN_MSG_MOTOR1_DRIVER_STATUS_ID;
msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR1_ID;
memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_MOTOR2_DRIVER_STATUS_ID:
{
msg->type = ScoutMotorDriverStatusMsg;
// msg->motor_driver_status_msg.id = CAN_MSG_MOTOR2_DRIVER_STATUS_ID;
msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR2_ID;
memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_MOTOR3_DRIVER_STATUS_ID:
{
msg->type = ScoutMotorDriverStatusMsg;
// msg->motor_driver_status_msg.id = CAN_MSG_MOTOR3_DRIVER_STATUS_ID;
msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR3_ID;
memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_MOTOR4_DRIVER_STATUS_ID:
{
msg->type = ScoutMotorDriverStatusMsg;
// msg->motor_driver_status_msg.id = CAN_MSG_MOTOR4_DRIVER_STATUS_ID;
msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR4_ID;
memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
// in the current implementation, both MsgType and can_frame include 8 * uint8_t
case CAN_MSG_MOTION_CONTROL_CMD_ID:
{
msg->type = ScoutMotionControlMsg;
// msg->motion_control_msg.id = CAN_MSG_MOTION_CONTROL_CMD_ID;
memcpy(msg->body.motion_control_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_LIGHT_CONTROL_CMD_ID:
{
msg->type = ScoutLightControlMsg;
// msg->light_control_msg.id = CAN_MSG_LIGHT_CONTROL_STATUS_ID;
memcpy(msg->body.light_control_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
default:
break;
}
return true;
}
void EncodeScoutMsgToCAN(const ScoutMessage *msg, struct can_frame *tx_frame)
{
switch (msg->type)
{
// in the current implementation, both MsgType and can_frame include 8 * uint8_t
case ScoutMotionStatusMsg:
{
tx_frame->can_id = CAN_MSG_MOTION_CONTROL_STATUS_ID;
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->body.motion_status_msg.data.raw, tx_frame->can_dlc);
break;
}
case ScoutLightStatusMsg:
{
tx_frame->can_id = CAN_MSG_LIGHT_CONTROL_STATUS_ID;
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->body.light_status_msg.data.raw, tx_frame->can_dlc);
break;
}
case ScoutSystemStatusMsg:
{
tx_frame->can_id = CAN_MSG_SYSTEM_STATUS_STATUS_ID;
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->body.system_status_msg.data.raw, tx_frame->can_dlc);
break;
}
case ScoutMotorDriverStatusMsg:
{
if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR1_ID)
tx_frame->can_id = CAN_MSG_MOTOR1_DRIVER_STATUS_ID;
else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR2_ID)
tx_frame->can_id = CAN_MSG_MOTOR2_DRIVER_STATUS_ID;
else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR3_ID)
tx_frame->can_id = CAN_MSG_MOTOR3_DRIVER_STATUS_ID;
else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR4_ID)
tx_frame->can_id = CAN_MSG_MOTOR4_DRIVER_STATUS_ID;
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->body.motor_driver_status_msg.data.raw, tx_frame->can_dlc);
break;
}
case ScoutMotionControlMsg:
{
EncodeScoutMotionControlMsgToCAN(&(msg->body.motion_control_msg), tx_frame);
break;
}
case ScoutLightControlMsg:
{
EncodeScoutLightControlMsgToCAN(&(msg->body.light_control_msg), tx_frame);
break;
}
default:
break;
}
tx_frame->data[7] = CalcScoutCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
}
void EncodeScoutMotionControlMsgToCAN(const MotionControlMessage *msg, struct can_frame *tx_frame)
{
tx_frame->can_id = CAN_MSG_MOTION_CONTROL_CMD_ID;
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->data.raw, tx_frame->can_dlc);
tx_frame->data[7] = CalcScoutCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
}
void EncodeScoutLightControlMsgToCAN(const LightControlMessage *msg, struct can_frame *tx_frame)
{
tx_frame->can_id = CAN_MSG_LIGHT_CONTROL_CMD_ID;
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->data.raw, tx_frame->can_dlc);
tx_frame->data[7] = CalcScoutCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
}
uint8_t CalcScoutCANChecksum(uint16_t id, uint8_t *data, uint8_t dlc)
{
uint8_t checksum = 0x00;
checksum = (uint8_t)(id & 0x00ff) + (uint8_t)(id >> 8) + dlc;
for (int i = 0; i < (dlc - 1); ++i)
checksum += data[i];
return checksum;
}