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

added in agx_protocol_v2

Browse Source
This commit is contained in:
pinloon.lee
2021-03-31 14:29:55 +08:00
parent ed2f38a7fd
commit e7fdfd3e4a
21 changed files with 1719 additions and 1779 deletions
Download Patch File Download Diff File Expand all files Collapse all files

103
include/ugv_sdk/agilex_base.hpp Normal file
View File

@@ -0,0 +1,103 @@
/*
* 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 "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) {
cmd_thread_period_ms_ = 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();
// 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 */

355
include/ugv_sdk/agilex_message.h Normal file
View File

@@ -0,0 +1,355 @@
/*
* agilex_message.h
*
* Created on: Dec 10, 2020 11:47
* Description:
* all values are using SI units (e.g. meter/second/radian)
*
* Copyright (c) 2020 Ruixiang Du (rdu)
*/
#ifndef AGILEX_MESSAGE_H
#define AGILEX_MESSAGE_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
/***************** Control messages *****************/
// 0x111
typedef struct {
float linear_velocity;
float angular_velocity;
float lateral_velocity;
float steering_angle;
} MotionCommandMessage;
// 0x121
typedef enum {
CONST_OFF = 0x00,
CONST_ON = 0x01,
BREATH = 0x02,
CUSTOM = 0x03
} LightMode;
typedef struct {
LightMode mode;
uint8_t custom_value;
} LightOperation;
typedef struct {
bool cmd_ctrl_allowed;
LightOperation front_light;
LightOperation rear_light;
} LightCommandMessage;
// 0x131
typedef struct {
bool enable_braking;
} BrakingCommandMessage;
/**************** Feedback messages *****************/
// 0x211
typedef enum {
VehicleStateNormal = 0x00,
VehicleStateEStop = 0x01,
VehicleStateException = 0x02
} VehicleState;
typedef enum {
// CONTROL_MODE_STANDBY = 0x00,
CONTROL_MODE_RC = 0x00,
CONTROL_MODE_CAN = 0x01,
CONTROL_MODE_UART = 0x02
} ControlMode;
#define SYSTEM_ERROR_MOTOR_DRIVER_MASK ((uint16_t)0x0100)
#define SYSTEM_ERROR_HL_COMM_MASK ((uint16_t)0x0200)
#define SYSTEM_ERROR_BATTERY_FAULT_MASK ((uint16_t)0x0001)
#define SYSTEM_ERROR_BATTERY_WARNING_MASK ((uint16_t)0x0002)
#define SYSTEM_ERROR_RC_SIGNAL_LOSS_MASK ((uint16_t)0x0004)
#define SYSTEM_ERROR_MOTOR1_COMM_MASK ((uint16_t)0x0008)
#define SYSTEM_ERROR_MOTOR2_COMM_MASK ((uint16_t)0x0010)
#define SYSTEM_ERROR_MOTOR3_COMM_MASK ((uint16_t)0x0020)
#define SYSTEM_ERROR_MOTOR4_COMM_MASK ((uint16_t)0x0040)
#define SYSTEM_ERROR_STEER_ENCODER_MASK ((uint16_t)0x0080)
typedef struct {
VehicleState vehicle_state;
ControlMode control_mode;
float battery_voltage;
uint16_t error_code;
} SystemStateMessage;
// 0x221
typedef struct {
float linear_velocity;
float angular_velocity; // only valid for differential drivering
float lateral_velocity;
float steering_angle; // only valid for ackermann steering
} MotionStateMessage;
// 0x231
typedef LightCommandMessage LightStateMessage;
// 0x241
typedef enum {
RC_SWITCH_UP = 0,
RC_SWITCH_MIDDLE,
RC_SWITCH_DOWN
} RcSwitchState;
typedef struct {
RcSwitchState swa;
RcSwitchState swb;
RcSwitchState swc;
RcSwitchState swd;
int8_t stick_right_v;
int8_t stick_right_h;
int8_t stick_left_v;
int8_t stick_left_h;
int8_t var_a;
} RcStateMessage;
// 0x251 - 0x254
typedef struct {
uint8_t motor_id;
int16_t rpm;
float current;
int32_t pulse_count;
} ActuatorHSStateMessage;
// 0x261 - 0x264
#define DRIVER_STATE_INPUT_VOLTAGE_LOW_MASK ((uint8_t)0x01)
#define DRIVER_STATE_MOTOR_OVERHEAT_MASK ((uint8_t)0x02)
#define DRIVER_STATE_DRIVER_OVERLOAD_MASK ((uint8_t)0x04)
#define DRIVER_STATE_DRIVER_OVERHEAT_MASK ((uint8_t)0x08)
#define DRIVER_STATE_SENSOR_FAULT_MASK ((uint8_t)0x10)
#define DRIVER_STATE_DRIVER_FAULT_MASK ((uint8_t)0x20)
#define DRIVER_STATE_DRIVER_ENABLED_MASK ((uint8_t)0x40)
#define DRIVER_STATE_DRIVER_RESET_MASK ((uint8_t)0x80)
typedef struct {
uint8_t motor_id;
float driver_voltage;
float driver_temperature;
int8_t motor_temperature;
uint8_t driver_state;
} ActuatorLSStateMessage;
/***************** Sensor messages ******************/
// 0x311
typedef struct {
float left_wheel;
float right_wheel;
} OdometryMessage;
// 0x321
typedef struct {
float accel_x;
float accel_y;
float accel_z;
} ImuAccelMessage;
// 0x322
typedef struct {
float gyro_x;
float gyro_y;
float gyro_z;
} ImuGyroMessage;
// 0x323
typedef struct {
float yaw;
float pitch;
float roll;
} ImuEulerMessage;
// 0x331
typedef struct {
uint8_t trigger_state;
} SafetyBumperMessage;
// 0x340 + num
typedef struct {
uint8_t sensor_id;
uint8_t distance[8];
} UltrasonicMessage;
// 0x350 + num
typedef struct {
uint8_t sensor_id;
float relative_distance;
float relative_angle;
bool is_normal;
int8_t channels[3];
} UwbMessage;
// 0x361
typedef struct {
uint8_t battery_soc;
uint8_t battery_soh;
float voltage;
float current;
float temperature;
} BmsBasicMessage;
// 0x362
#define BMS_PROT1_CHARGING_CURRENT_NONZERO_MASK ((uint8_t)0x01)
#define BMS_PROT1_CHARGING_OVERCURRENT_SET_MASK ((uint8_t)0x02)
#define BMS_PROT1_DISCHARGING_CURRENT_NONZERO_MASK ((uint8_t)0x10)
#define BMS_PROT1_DISCHARGING_OVERCURRENT_SET_MASK ((uint8_t)0x20)
#define BMS_PROT1_DISCHARGING_SHORTCIRCUIT_SET_MASK ((uint8_t)0x40)
#define BMS_PROT2_CORE_OPENCIRCUIT_SET_MASK ((uint8_t)0x01)
#define BMS_PROT2_TEMP_SENSOR_OPENCIRCUIT_SET_MASK ((uint8_t)0x02)
#define BMS_PROT2_CORE_OVERVOLTAGE_SET_MASK ((uint8_t)0x10)
#define BMS_PROT2_CORE_UNDERVOLTAGE_SET_MASK ((uint8_t)0x20)
#define BMS_PROT2_TOTAL_OVERVOLTAGE_SET_MASK ((uint8_t)0x40)
#define BMS_PROT2_TOTAL_UNDERVOLTAGE_SET_MASK ((uint8_t)0x80)
#define BMS_PROT3_CHARGING_OVERTEMP_SET_MASK ((uint8_t)0x04)
#define BMS_PROT3_DISCHARGING_OVERTEMP_SET_MASK ((uint8_t)0x08)
#define BMS_PROT3_CHARGING_UNDERTEMP_SET_MASK ((uint8_t)0x10)
#define BMS_PROT3_DISCHARGING_UNDERTEMP_SET_MASK ((uint8_t)0x20)
#define BMS_PROT3_CHARGING_TEMPDIFF_SET_MASK ((uint8_t)0x40)
#define BMS_PROT3_DISCHARGING_TEMPDIFF_SET_MASK ((uint8_t)0x80)
#define BMS_PROT4_CHARGING_MOS_STATE_SET_MASK ((uint8_t)0x01)
#define BMS_PROT4_DISCHARGING_MOS_STATE_SET_MASK ((uint8_t)0x02)
#define BMS_PROT4_CHARGING_MOS_FAILURE_SET_MASK ((uint8_t)0x04)
#define BMS_PROT4_DISCHARGING_MOS_FAILURE_SET_MASK ((uint8_t)0x08)
#define BMS_PROT4_WEAK_SIGNAL_SWITCH_OPEN_SET_MASK ((uint8_t)0x10)
typedef struct {
uint8_t protection_code1;
uint8_t protection_code2;
uint8_t protection_code3;
uint8_t protection_code4;
uint8_t battery_max_teperature;
uint8_t battery_min_teperature;
} BmsExtendedMessage;
/************ Query/config messages ****************/
// 0x411
typedef struct {
bool request;
} VersionRequestMessage;
// 0x41a
typedef struct {
uint16_t controller_hw_version;
uint16_t motor_driver_hw_version;
uint16_t controller_sw_version;
uint16_t motor_driver_sw_version;
} VersionResponseMessage;
// 0x421
typedef struct {
ControlMode mode;
} ControlModeConfigMessage;
// 0x431
typedef struct {
bool set_as_neutral;
} SteerNeutralRequestMessage;
// 0x43a
typedef struct {
bool neutral_set_successful;
} SteerNeutralResponseMessage;
// 0x441
typedef enum {
CLEAR_ALL_FAULT = 0x00,
CLEAR_MOTOR1_FAULT = 0x01,
CLEAR_MOTOR2_FAULT = 0x02,
CLEAR_MOTOR3_FAULT = 0x03,
CLEAR__MOTOR4_FAULT = 0x04
} FaultClearCode;
typedef struct {
uint8_t error_clear_byte;
} StateResetConfigMessage;
//////////////////////////////////////////////////////
typedef enum {
AgxMsgUnkonwn = 0x00,
// command
AgxMsgMotionCommand,
AgxMsgLightCommand,
AgxMsgBrakingCommand,
// state feedback
AgxMsgSystemState,
AgxMsgMotionState,
AgxMsgLightState,
AgxMsgRcState,
AgxMsgActuatorHSState,
AgxMsgActuatorLSState,
// sensor
AgxMsgOdometry,
AgxMsgImuAccel,
AgxMsgImuGyro,
AgxMsgImuEuler,
AgxMsgSafetyBumper,
AgxMsgUltrasonic,
AgxMsgUwb,
AgxMsgBmsBasic,
AgxMsgBmsExtended,
// query/config
AgxMsgVersionRequest,
AgxMsgVersionResponse,
AgxMsgControlModeConfig,
AgxMsgSteerNeutralRequest,
AgxMsgSteerNeutralResponse,
AgxMsgStateResetConfig
} MsgType;
typedef struct {
MsgType type;
union {
// command
MotionCommandMessage motion_command_msg;
LightCommandMessage light_command_msg;
BrakingCommandMessage braking_command_msg;
// state feedback
SystemStateMessage system_state_msg;
MotionStateMessage motion_state_msg;
LightStateMessage light_state_msg;
RcStateMessage rc_state_msg;
ActuatorHSStateMessage actuator_hs_state_msg;
ActuatorLSStateMessage actuator_ls_state_msg;
// sensor
OdometryMessage odometry_msg;
ImuAccelMessage imu_accel_msg;
ImuGyroMessage imu_gyro_msg;
ImuEulerMessage imu_euler_msg;
SafetyBumperMessage safety_bumper_msg;
UltrasonicMessage ultrasonic_msg;
UwbMessage uwb_msg;
BmsBasicMessage bms_basic_msg;
BmsExtendedMessage bms_extended_msg;
// query/config
VersionRequestMessage version_request_msg;
VersionResponseMessage version_response_msg;
ControlModeConfigMessage control_mode_config_msg;
SteerNeutralRequestMessage steer_neutral_request_msg;
SteerNeutralResponseMessage steer_neutral_response_msg;
StateResetConfigMessage state_reset_config_msg;
} body;
} AgxMessage;
#ifdef __cplusplus
}
#endif
#endif /* AGILEX_MESSAGE_H */

41
include/ugv_sdk/details/agilex_msg_parser.h Normal file
View File

@@ -0,0 +1,41 @@
/*
* agilex_msg_parser.h
*
* Created on: Dec 22, 2020 17:11
* Description: public API to encode/decode protocol messages
*
* Copyright (c) 2020 Ruixiang Du (rdu)
*/
#ifndef AGILEX_MSG_PARSER_H
#define AGILEX_MSG_PARSER_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <stdbool.h>
#include <string.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
#include "ugv_sdk/agilex_message.h"
bool DecodeCanFrame(const struct can_frame *rx_frame, AgxMessage *msg);
void EncodeCanFrame(const AgxMessage *msg, struct can_frame *tx_frame);
uint8_t CalcCanFrameChecksum(uint16_t id, uint8_t *data, uint8_t dlc);
#ifdef __cplusplus
}
#endif
#endif /* AGILEX_MSG_PARSER_H */

66
include/ugv_sdk/hunter/hunter_base.hpp
View File

@@ -1,66 +0,0 @@
/*
* hunter_base.hpp
*
* Created on: Apr 01, 2020 09:43
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#ifndef HUNTER_BASE_HPP
#define HUNTER_BASE_HPP
#include <string>
#include <cstdint>
#include <thread>
#include <mutex>
#include "ugv_sdk/mobile_base.hpp"
#include "ugv_sdk/hunter/hunter_protocol.h"
#include "ugv_sdk/hunter/hunter_can_parser.h"
#include "ugv_sdk/hunter/hunter_types.hpp"
namespace westonrobot {
class HunterBase : public MobileBase {
public:
HunterBase() : MobileBase(){};
~HunterBase() = default;
// get robot state
HunterState GetHunterState();
// motion control
void SetMotionCommand(double linear_vel, double angular_vel,
double steering_angle,
HunterMotionCmd::FaultClearFlag fault_clr_flag =
HunterMotionCmd::FaultClearFlag::NO_FAULT);
private:
// cmd/status update related variables
std::mutex hunter_state_mutex_;
std::mutex motion_cmd_mutex_;
std::mutex mode_cmd_mutex_;
std::mutex pack_mode_cmd_mutex_;
HunterState hunter_state_;
HunterMotionCmd current_motion_cmd_;
// internal functions
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 SendModeCtl();
void SetParkMode();
void NewStatusMsgReceivedCallback(const HunterMessage &msg);
public:
static void UpdateHunterState(const HunterMessage &status_msg,
HunterState &state);
};
} // namespace westonrobot
#endif /* HUNTER_BASE_HPP */

87
include/ugv_sdk/hunter/hunter_types.hpp
View File

@@ -1,87 +0,0 @@
/*
* hunter_types.hpp
*
* Created on: Apr 01, 2020 09:43
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#ifndef HUNTER_TYPES_HPP
#define HUNTER_TYPES_HPP
#include <cstdint>
#include <iostream>
namespace westonrobot
{
struct HunterState
{
struct MotorHighSpeedState
{
double current = 0; // in A
double rpm = 0;
double motor_pose = 0;
};
struct MotorLowSpeedState
{
double driver_voltage = 0;
double driver_temperature = 0;
double motor_temperature = 0;
uint8_t driver_state = 0;
};
// base state
uint8_t base_state = 0;
uint8_t control_mode = 0;
uint8_t park_mode = 1;
uint16_t fault_code = 0;
double battery_voltage = 0.0;
uint8_t set_zero_steering = 0;
// motor state
static constexpr uint8_t motor_num = 3;
MotorHighSpeedState motor_hs_state[motor_num];
MotorLowSpeedState motor_ls_state[motor_num];
// motion state
double linear_velocity = 0;
double steering_angle = 0;
};
struct HunterMotionCmd
{
enum class FaultClearFlag
{
NO_FAULT = 0x00,
BAT_UNDER_VOL = 0x01,
BAT_OVER_VOL = 0x02,
MOTOR1_COMM = 0x03,
MOTOR2_COMM = 0x04,
MOTOR3_COMM = 0x05,
MOTOR4_COMM = 0x06,
MOTOR_DRV_OVERHEAT = 0x07,
MOTOR_OVERCURRENT = 0x08
};
HunterMotionCmd(int8_t linear_height_byte = 0, int8_t linear_low_byte = 0, int8_t angular_height_byte = 0, int8_t angular_low_byte = 0,
FaultClearFlag fault_clr_flag = FaultClearFlag::NO_FAULT)
: linear_velocity_height_byte(linear_height_byte),linear_velocity_low_byte(linear_low_byte), angular_velocity_height_byte(angular_height_byte),angular_velocity_low_byte(angular_low_byte),
fault_clear_flag(fault_clr_flag) {}
int8_t linear_velocity_height_byte;
int8_t linear_velocity_low_byte;
int8_t angular_velocity_height_byte;
int8_t angular_velocity_low_byte;
FaultClearFlag fault_clear_flag;
static constexpr double max_linear_velocity = 1.5; // 1.5 m/s
static constexpr double min_linear_velocity = -1.5; // -1.5 m/s
static constexpr double max_steering_angle = 0.576; // 0.576 rad ~= 30.00 degree
static constexpr double min_steering_angle = -0.576; // -0.576 rad
};
} // namespace westonrobot
#endif /* HUNTER_TYPES_HPP */

88
include/ugv_sdk/mobile_base.hpp
View File

@@ -1,88 +0,0 @@
/*
* 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 <atomic>
#include "async_port/async_can.hpp"
#include "async_port/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;
void SetCmdTimeout(bool enable, uint32_t timeout_ms = 100);
void DisableTimeout() { enable_timeout_ = false; }
// 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();
// ask background thread to shutdown properly
void Terminate();
// cmd thread runs at 100Hz (10ms) by default
void SetCmdThreadPeriodMs(int32_t period_ms) {
cmd_thread_period_ms_ = period_ms;
};
protected:
// communication interface
bool can_connected_ = false;
bool serial_connected_ = false;
std::shared_ptr<AsyncCAN> can_if_;
std::shared_ptr<AsyncSerial> serial_if_;
// 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_ = 10;
bool cmd_thread_started_ = false;
std::atomic<bool> keep_running_;
// internal functions
void ConfigureCAN(const std::string &can_if_name = "can0");
void ConfigureSerial(const std::string uart_name = "/dev/ttyUSB0",
int32_t baud_rate = 115200);
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){};
virtual void ParseUARTBuffer(uint8_t *buf, const size_t bufsize,
size_t bytes_received){};
};
} // namespace westonrobot
#endif /* MOBILE_BASE_HPP */

83
include/ugv_sdk/proto/agx_msg_parser.h
View File

@@ -1,83 +0,0 @@
/*
* agx_msg_parser.h
*
* Created on: Nov 04, 2020 13:40
* Description:
*
* Copyright (c) 2020 Ruixiang Du (rdu)
*/
#ifndef AGX_MSG_PARSER_H
#define AGX_MSG_PARSER_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#include "ugv_sdk/proto/agx_protocol_v2.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
#pragma pack(push, 1)
typedef enum {
AgxMsgUnkonwn = 0x00,
// command
AgxMsgMotionCommand = 0x01,
AgxMsgLightCommand = 0x02,
AgxMsgCtrlModeSelect = 0x03,
AgxMsgFaultByteReset = 0x04,
// state feedback
AgxMsgSystemState = 0x21,
AgxMsgMotionState = 0x22,
AgxMsgLightState = 0x23,
AgxMsgRcState = 0x24,
AgxMsgActuatorHSState = 0x25,
AgxMsgActuatorLSState = 0x26,
AgxMsgOdometry = 0x27,
AgxMsgVersionQuery = 0x28,
AgxMsgPlatformVersion = 0x29
} MsgType;
typedef struct {
MsgType type;
union {
// command
MotionCommandMessage motion_command_msg;
LightCommandMessage light_command_msg;
CtrlModeSelectMessage ctrl_mode_select_msg;
StateResetMessage state_reset_msg;
// state feedback
SystemStateMessage system_state_msg;
MotionStateMessage motion_state_msg;
LightStateMessage light_state_msg;
RcStateMessage rc_state_msg;
ActuatorHSStateMessage actuator_hs_state_msg;
ActuatorLSStateMessage actuator_ls_state_msg;
OdometryMessage odometry_msg;
VersionQueryMessage version_query_msg;
PlatformVersionMessage platform_version_msg;
} body;
} AgxMessage;
#pragma pack(pop)
bool DecodeCanFrame(const struct can_frame *rx_frame, AgxMessage *msg);
void EncodeCanFrame(const AgxMessage *msg, struct can_frame *tx_frame);
uint8_t CalcCanFrameChecksum(uint16_t id, uint8_t *data, uint8_t dlc);
#ifdef __cplusplus
}
#endif
#endif /* AGX_PARSER_H */

342
include/ugv_sdk/proto/agx_protocol_v2.h
View File

@@ -1,342 +0,0 @@
/*
* agx_protocol_v2.h
*
* Created on: Nov 04, 2020 13:54
* Description:
*
* Copyright (c) 2020 Ruixiang Du (rdu)
*/
#ifndef AGX_PROTOCOL_V2_H
#define AGX_PROTOCOL_V2_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
/*---------------------------- Motor IDs -------------------------------*/
#define ACTUATOR1_ID ((uint8_t)0x00)
#define ACTUATOR2_ID ((uint8_t)0x01)
#define ACTUATOR3_ID ((uint8_t)0x02)
#define ACTUATOR4_ID ((uint8_t)0x03)
/*--------------------------- Message IDs ------------------------------*/
#define CAN_MSG_MOTION_COMMAND_ID ((uint32_t)0x111)
#define CAN_MSG_LIGHT_COMMAND_ID ((uint32_t)0x121)
#define CAN_MSG_PARK_COMMAND_ID ((uint32_t)0x131)
#define CAN_MSG_SYSTEM_STATE_ID ((uint32_t)0x211)
#define CAN_MSG_MOTION_STATE_ID ((uint32_t)0x221)
#define CAN_MSG_LIGHT_STATE_ID ((uint32_t)0x231)
#define CAN_MSG_RC_STATE_ID ((uint32_t)0x241)
#define CAN_MSG_ACTUATOR1_HS_STATE_ID ((uint32_t)0x251)
#define CAN_MSG_ACTUATOR2_HS_STATE_ID ((uint32_t)0x252)
#define CAN_MSG_ACTUATOR3_HS_STATE_ID ((uint32_t)0x253)
#define CAN_MSG_ACTUATOR4_HS_STATE_ID ((uint32_t)0x254)
#define CAN_MSG_ACTUATOR1_LS_STATE_ID ((uint32_t)0x261)
#define CAN_MSG_ACTUATOR2_LS_STATE_ID ((uint32_t)0x262)
#define CAN_MSG_ACTUATOR3_LS_STATE_ID ((uint32_t)0x263)
#define CAN_MSG_ACTUATOR4_LS_STATE_ID ((uint32_t)0x264)
#define CAN_MSG_ODOMETRY_ID ((uint32_t)0x311)
#define CAN_MSG_VERSION_QUERY_ID ((uint32_t)0x411)
#define CAN_MSG_PLATFORM_VERSION_ID ((uint32_t)0x41a)
#define CAN_MSG_CTRL_MODE_SELECT_ID ((uint32_t)0x421)
#define CAN_MSG_STEER_NEUTRAL_RESET_ID ((uint32_t)0x431)
#define CAN_MSG_STEER_NEUTRAL_RESET_ACK_ID ((uint32_t)0x43a)
#define CAN_MSG_STATE_RESET_ID ((uint32_t)0x441)
/*--------------------- Control/State Constants ------------------------*/
// Control
#define VEHICLE_STATE_NORMAL ((uint8_t)0x00)
#define VEHICLE_STATE_ESTOP ((uint8_t)0x01)
#define VEHICLE_STATE_EXCEPTION ((uint8_t)0x02)
#define FAULT_BATTERY_LOW_ERROR ((uint8_t)0x01)
#define FAULT_BATTERY_LOW_WARN ((uint8_t)0x02)
#define FAULT_RC_SIGNAL_LOSS ((uint8_t)0x04)
#define FAULT_CLR_ALL ((uint8_t)0x00)
#define FAULT_CLR_MOTOR1_COMM ((uint8_t)0x01)
#define FAULT_CLR_MOTOR2_COMM ((uint8_t)0x02)
#define FAULT_CLR_MOTOR3_COMM ((uint8_t)0x03)
#define FAULT_CLR_MOTOR4_COMM ((uint8_t)0x04)
#define CTRL_MODE_RC ((uint8_t)0x00)
#define CTRL_MODE_CMD_CAN ((uint8_t)0x01)
#define CTRL_MODE_CMD_UART ((uint8_t)0x02)
#define QUERY_PLATFORM_VERSION_REQUEST ((uint8_t)0x01)
// Actuator
#define BATTERY_VOLTAGE_LOW ((uint8_t)0x01)
#define MOTOR_OVERHEAT ((uint8_t)0x02)
#define MOTOR_DRIVER_OVERLOAD ((uint8_t)0x04)
#define MOTOR_DRIVER_OVERHEAT ((uint8_t)0x08)
#define MOTOR_SENSOR_FAULT ((uint8_t)0x10)
#define MOTOR_DRIVER_FAULT ((uint8_t)0x20)
#define MOTOR_DRIVER_ENABLED ((uint8_t)0x40)
#define MOTOR_DRIVER_RESERVED0 ((uint8_t)0x80)
#define STEER_NEUTRAL_RESET_ACK ((uint8_t)0xee)
// Light
#define LIGHT_CTRL_DISABLE ((uint8_t)0x00)
#define LIGHT_CTRL_ENABLE ((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)
/*-------------------- Control/Feedback Messages -----------------------*/
/* No padding in the struct */
// reference: https://stackoverflow.com/questions/3318410/pragma-pack-effect
#pragma pack(push, 1)
// Control messages
typedef union {
struct {
struct {
int8_t high_byte;
int8_t low_byte;
} linear_velocity;
struct {
uint8_t high_byte;
uint8_t low_byte;
} angular_velocity;
struct {
uint8_t high_byte;
uint8_t low_byte;
} lateral_velocity;
struct {
uint8_t high_byte;
uint8_t low_byte;
} steering_angle;
} cmd;
uint8_t raw[8];
} MotionCommandMessage;
typedef union {
struct {
uint8_t light_ctrl_enabled;
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 reserved1;
uint8_t count;
} cmd;
uint8_t raw[8];
} LightCommandMessage;
typedef union {
struct {
uint8_t control_mode;
uint8_t reserved0;
uint8_t reserved1;
uint8_t reserved2;
uint8_t reserved3;
uint8_t reserved4;
uint8_t reserved5;
uint8_t reserved6;
} cmd;
uint8_t raw[8];
} CtrlModeSelectMessage;
typedef union {
struct {
uint8_t fault_byte;
uint8_t reserved0;
uint8_t reserved1;
uint8_t reserved2;
uint8_t reserved3;
uint8_t reserved4;
uint8_t reserved5;
uint8_t reserved6;
} cmd;
uint8_t raw[8];
} StateResetMessage;
// State feedback messages
typedef union {
struct {
uint8_t vehicle_state;
uint8_t control_mode;
struct {
uint8_t high_byte;
uint8_t low_byte;
} battery_voltage;
uint8_t fault_code;
uint8_t reserved0;
uint8_t reserved1;
uint8_t count;
} state;
uint8_t raw[8];
} SystemStateMessage;
typedef union {
struct {
struct {
uint8_t high_byte;
uint8_t low_byte;
} linear_velocity;
struct {
uint8_t high_byte;
uint8_t low_byte;
} angular_velocity; // only valid for differential drivering
uint8_t reserved0;
uint8_t reserved1;
struct {
uint8_t high_byte;
uint8_t low_byte;
} steering_angle; // only valid for ackermann steering
} state;
uint8_t raw[8];
} MotionStateMessage;
typedef union {
struct {
uint8_t light_ctrl_enabled;
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 reserved1;
uint8_t count;
} state;
uint8_t raw[8];
} LightStateMessage;
typedef union {
struct {
uint8_t sws;
uint8_t right_stick_left_right;
uint8_t right_stick_up_down;
uint8_t left_stick_left_right;
uint8_t left_stick_up_down;
uint8_t var_a;
uint8_t reserved0;
uint8_t count;
} state;
uint8_t raw[8];
} RcStateMessage;
typedef struct {
uint8_t motor_id;
union {
struct {
struct {
uint8_t high_byte;
uint8_t low_byte;
} rpm;
struct {
uint8_t high_byte;
uint8_t low_byte;
} current;
struct {
int8_t msb;
int8_t high_byte;
int8_t low_byte;
int8_t lsb;
} pulse_count;
} state;
uint8_t raw[8];
} data;
} ActuatorHSStateMessage;
typedef struct {
uint8_t motor_id;
union {
struct {
struct {
uint8_t high_byte;
uint8_t low_byte;
} driver_voltage;
struct {
uint8_t high_byte;
uint8_t low_byte;
} driver_temperature;
int8_t motor_temperature;
uint8_t driver_state;
uint8_t reserved0;
uint8_t reserved1;
} state;
uint8_t raw[8];
} data;
} ActuatorLSStateMessage;
typedef union {
struct {
struct {
uint8_t msb;
uint8_t high_byte;
uint8_t low_byte;
uint8_t lsb;
} left_wheel;
struct {
uint8_t msb;
uint8_t high_byte;
uint8_t low_byte;
uint8_t lsb;
} right_wheel;
} state;
uint8_t raw[8];
} OdometryMessage;
typedef union {
struct {
uint8_t request;
uint8_t reserved0;
uint8_t reserved1;
uint8_t reserved2;
uint8_t reserved3;
uint8_t reserved4;
uint8_t reserved5;
uint8_t reserved6;
} state;
uint8_t raw[8];
} VersionQueryMessage;
typedef union {
struct {
struct {
uint8_t high_byte;
uint8_t low_byte;
} controller_hw_version;
struct {
uint8_t high_byte;
uint8_t low_byte;
} motor_driver_hw_version;
struct {
uint8_t high_byte;
uint8_t low_byte;
} controller_sw_version;
struct {
uint8_t high_byte;
uint8_t low_byte;
} motor_driver_sw_version;
} state;
uint8_t raw[8];
} PlatformVersionMessage;
#pragma pack(pop)
#ifdef __cplusplus
}
#endif
#endif /* AGX_PROTOCOL_V2_H */

60
include/ugv_sdk/scout/scout_base.hpp
View File

@@ -1,60 +0,0 @@
/*
* scout_base.hpp
*
* Created on: Jun 04, 2019 01:22
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#ifndef SCOUT_BASE_HPP
#define SCOUT_BASE_HPP
#include <string>
#include <cstdint>
#include <thread>
#include <mutex>
#include <functional>
#include "ugv_sdk/mobile_base.hpp"
#include "ugv_sdk/proto/agx_msg_parser.h"
#include "ugv_sdk/scout/scout_types.hpp"
namespace westonrobot {
class ScoutBase : public MobileBase {
public:
ScoutBase() : MobileBase(){};
~ScoutBase() = default;
// get robot state
ScoutState GetScoutState();
void EnableCommandedMode();
// motion control
void SetMotionCommand(double linear_vel, double angular_vel);
// light control
void SetLightCommand(const ScoutLightCmd &cmd);
private:
// cmd/status update related variables
std::mutex scout_state_mutex_;
std::mutex motion_cmd_mutex_;
ScoutState scout_state_;
ScoutMotionCmd current_motion_cmd_;
// internal functions
void SendMotionCmd(uint8_t count);
void SendLightCmd(const ScoutLightCmd &cmd, uint8_t count);
void SendRobotCmd() override;
void ParseCANFrame(can_frame *rx_frame) override;
void NewStatusMsgReceivedCallback(const AgxMessage &msg);
static void UpdateScoutState(const AgxMessage &status_msg, ScoutState &state);
};
} // namespace westonrobot
#endif /* SCOUT_BASE_HPP */

103
include/ugv_sdk/scout/scout_types.hpp
View File

@@ -1,103 +0,0 @@
/*
* scout_state.hpp
*
* Created on: Jun 11, 2019 08:48
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#ifndef SCOUT_STATE_HPP
#define SCOUT_STATE_HPP
#include <cstdint>
#include <iostream>
namespace westonrobot {
struct ScoutState {
enum MotorID {
FRONT_RIGHT = 0,
FRONT_LEFT = 1,
REAR_LEFT = 2,
REAR_RIGHT = 3
};
struct ActuatorState {
double motor_current = 0; // in A
uint16_t motor_rpm = 0;
uint16_t motor_pulses = 0;
double motor_temperature = 0;
double driver_voltage = 0;
double driver_temperature = 0;
uint8_t driver_state = 0;
};
struct LightState {
uint8_t mode = 0;
uint8_t custom_value = 0;
};
// base state
uint8_t base_state = 0;
uint8_t control_mode = 0;
uint8_t fault_code = 0;
double battery_voltage = 0.0;
// motor state
static constexpr uint8_t motor_num = 4;
ActuatorState actuator_states[motor_num];
// light state
bool light_control_enabled = false;
LightState front_light_state;
LightState rear_light_state;
// motion state
double linear_velocity = 0;
double angular_velocity = 0;
// odometer state
double left_odometry = 0;
double right_odometry = 0;
};
struct ScoutMotionCmd {
ScoutMotionCmd(double linear = 0.0, double angular = 0.0)
: linear_velocity(linear), angular_velocity(angular) {}
double linear_velocity;
double angular_velocity;
static constexpr double max_linear_velocity = 1.5; // 1.5 m/s
static constexpr double min_linear_velocity = -1.5; // -1.5 m/s
static constexpr double max_angular_velocity = 0.5235; // 0.5235 rad/s
static constexpr double min_angular_velocity = -0.5235; // -0.5235 rad/s
};
struct ScoutLightCmd {
enum class LightMode {
CONST_OFF = 0x00,
CONST_ON = 0x01,
BREATH = 0x02,
CUSTOM = 0x03
};
ScoutLightCmd() = default;
ScoutLightCmd(LightMode f_mode, uint8_t f_value, LightMode r_mode,
uint8_t r_value)
: enable_ctrl(true),
front_mode(f_mode),
front_custom_value(f_value),
rear_mode(r_mode),
rear_custom_value(r_value) {}
bool enable_ctrl = false;
LightMode front_mode;
uint8_t front_custom_value;
LightMode rear_mode;
uint8_t rear_custom_value;
};
} // namespace westonrobot
#endif /* SCOUT_STATE_HPP */

83
include/ugv_sdk/scout_base.hpp Normal file
View File

@@ -0,0 +1,83 @@
/*
* scout_base.hpp
*
* Created on: Dec 23, 2020 14:39
* Description:
*
* Copyright (c) 2020 Ruixiang Du (rdu)
*/
#ifndef SCOUT_BASE_HPP
#define SCOUT_BASE_HPP
#include <string>
#include <cstdint>
#include <thread>
#include <mutex>
#include "ugv_sdk/agilex_base.hpp"
namespace westonrobot {
struct ScoutState {
// system state
SystemStateMessage system_state;
MotionStateMessage motion_state;
LightStateMessage light_state;
RcStateMessage rc_state;
ActuatorHSStateMessage actuator_hs_state[4];
ActuatorLSStateMessage actuator_ls_state[4];
// sensor data
OdometryMessage odometry;
};
struct ScoutMotionCmd {
double linear_velocity;
double angular_velocity;
};
struct ScoutLightCmd {
ScoutLightCmd() = default;
ScoutLightCmd(LightMode f_mode, uint8_t f_value, LightMode r_mode,
uint8_t r_value)
: cmd_ctrl_allowed(true),
front_mode(f_mode),
front_custom_value(f_value),
rear_mode(r_mode),
rear_custom_value(r_value) {}
bool cmd_ctrl_allowed = false;
LightMode front_mode;
uint8_t front_custom_value;
LightMode rear_mode;
uint8_t rear_custom_value;
};
/////////////////////////////////////////////////////////////////////////
class ScoutBase : public AgilexBase {
public:
ScoutBase() : AgilexBase(){};
~ScoutBase() = default;
// 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);
// get robot state
ScoutState GetScoutState();
private:
ScoutState scout_state_;
void ParseCANFrame(can_frame *rx_frame) override;
void UpdateScoutState(const AgxMessage &status_msg, ScoutState &state);
};
} // namespace westonrobot
#endif /* SCOUT_BASE_HPP */

59
include/ugv_sdk/tracer/tracer_base.hpp
View File

@@ -1,59 +0,0 @@
/*
* tracer_base.hpp
*
* Created on: Apr 14, 2020 10:21
* Description:
*
* Copyright (c) 2020 Ruixiang Du (rdu)
*/
#ifndef TRACER_BASE_HPP
#define TRACER_BASE_HPP
#include <string>
#include <cstdint>
#include <thread>
#include <mutex>
#include "ugv_sdk/mobile_base.hpp"
#include "ugv_sdk/proto/agx_msg_parser.h"
#include "ugv_sdk/tracer/tracer_types.hpp"
namespace westonrobot {
class TracerBase : public MobileBase {
public:
TracerBase() : MobileBase(){};
~TracerBase() = default;
// get robot state
TracerState GetTracerState();
void EnableCommandedMode();
// motion control
void SetMotionCommand(double linear_vel, double angular_vel);
// light control
void SetLightCommand(const TracerLightCmd &cmd);
private:
// cmd/status update related variables
std::mutex tracer_state_mutex_;
std::mutex motion_cmd_mutex_;
TracerState tracer_state_;
TracerMotionCmd current_motion_cmd_;
void SendMotionCmd(uint8_t count);
void SendLightCmd(const TracerLightCmd &cmd, uint8_t count);
void SendRobotCmd() override;
void ParseCANFrame(can_frame *rx_frame) override;
void NewStatusMsgReceivedCallback(const AgxMessage &msg);
static void UpdateTracerState(const AgxMessage &status_msg,
TracerState &state);
};
} // namespace westonrobot
#endif /* TRACER_BASE_HPP */

89
include/ugv_sdk/tracer/tracer_types.hpp
View File

@@ -1,89 +0,0 @@
/*
* tracer_types.hpp
*
* Created on: Apr 14, 2020 10:22
* Description:
*
* Copyright (c) 2020 Ruixiang Du (rdu)
*/
#ifndef TRACER_TYPES_HPP
#define TRACER_TYPES_HPP
#include <cstdint>
#include <iostream>
namespace westonrobot {
struct TracerState {
struct ActuatorState {
double motor_current = 0; // in A
uint16_t motor_rpm = 0;
uint16_t motor_pulses = 0;
double motor_temperature = 0;
double driver_voltage = 0;
double driver_temperature = 0;
uint8_t driver_state = 0;
};
struct LightState {
uint8_t mode = 0;
uint8_t custom_value = 0;
};
// base state
uint8_t base_state = 0;
uint8_t control_mode = 0;
uint8_t fault_code = 0;
double battery_voltage = 0.0;
// actuator state
ActuatorState actuator_states[2];
// light state
bool light_control_enabled = false;
LightState front_light_state;
// motion state
double linear_velocity = 0;
double angular_velocity = 0;
// odometer state
double left_odometry = 0;
double right_odometry = 0;
};
struct TracerMotionCmd {
TracerMotionCmd(double linear = 0.0, double angular = 0.0)
: linear_velocity(linear), angular_velocity(angular) {}
double linear_velocity;
double angular_velocity;
static constexpr double max_linear_velocity = 1.8; // m/s
static constexpr double min_linear_velocity = -1.8;
static constexpr double max_angular_velocity = 1.0; // rad/s
static constexpr double min_angular_velocity = -1.0;
};
struct TracerLightCmd {
enum class LightMode {
CONST_OFF = 0x00,
CONST_ON = 0x01,
BREATH = 0x02,
CUSTOM = 0x03
};
TracerLightCmd() = default;
TracerLightCmd(LightMode f_mode, uint8_t f_value)
: enable_ctrl(true), front_mode(f_mode), front_custom_value(f_value) {}
bool enable_ctrl = false;
LightMode front_mode;
uint8_t front_custom_value;
LightMode rear_mode = LightMode::CONST_OFF;
uint8_t rear_custom_value = 0;
};
} // namespace westonrobot
#endif /* TRACER_TYPES_HPP */

78
include/ugv_sdk/tracer_base.hpp Normal file
View File

@@ -0,0 +1,78 @@
/*
* tracer_base.hpp
*
* Created on: Apr 14, 2020 10:21
* Description:
*
* Copyright (c) 2020 Weston Robot Pte. Ltd.
*/
#ifndef TRACER_BASE_HPP
#define TRACER_BASE_HPP
#include <string>
#include <cstdint>
#include <thread>
#include <mutex>
#include "ugv_sdk/agilex_base.hpp"
namespace westonrobot {
struct TracerState {
// system state
SystemStateMessage system_state;
MotionStateMessage motion_state;
LightStateMessage light_state;
RcStateMessage rc_state;
ActuatorHSStateMessage actuator_hs_state[2];
ActuatorLSStateMessage actuator_ls_state[2];
// sensor data
OdometryMessage odometry;
};
struct TracerMotionCmd {
double linear_velocity;
double angular_velocity;
};
struct TracerLightCmd {
TracerLightCmd() = default;
TracerLightCmd(LightMode f_mode, uint8_t f_value)
: cmd_ctrl_allowed(true),
front_mode(f_mode),
front_custom_value(f_value) {}
bool cmd_ctrl_allowed = false;
LightMode front_mode;
uint8_t front_custom_value;
};
/////////////////////////////////////////////////////////////////////////
class TracerBase : public AgilexBase {
public:
TracerBase() : AgilexBase(){};
~TracerBase() = default;
// set up connection
void Connect(std::string dev_name) override;
// robot control
void SetMotionCommand(double linear_vel, double angular_vel);
void SetLightCommand(const TracerLightCmd &cmd);
// get robot state
TracerState GetTracerState();
private:
TracerState tracer_state_;
void ParseCANFrame(can_frame *rx_frame) override;
void UpdateTracerState(const AgxMessage &status_msg, TracerState &state);
};
} // namespace westonrobot
#endif /* TRACER_BASE_HPP */

155
src/agilex_base.cpp Normal file
View File

@@ -0,0 +1,155 @@
/*
* agilex_base.cpp
*
* Created on: Dec 22, 2020 17:20
* Description:
*
* Copyright (c) 2020 Ruixiang Du (rdu)
*/
#include "ugv_sdk/agilex_base.hpp"
#include "ugv_sdk/details/agilex_msg_parser.h"
#include "stopwatch.hpp"
namespace westonrobot {
AgilexBase::~AgilexBase() {
// release resource if occupied
Disconnect();
// joint cmd thread
if (cmd_thread_.joinable()) cmd_thread_.join();
}
void AgilexBase::Connect(std::string dev_name, CANFrameRxCallback cb) {
can_ = std::make_shared<AsyncCAN>(dev_name);
can_->SetReceiveCallback(cb);
can_->StartListening();
can_connected_ = true;
}
void AgilexBase::Disconnect() {
if (can_connected_) can_->StopService();
}
void AgilexBase::Terminate() {
keep_running_ = false;
std::terminate();
}
void AgilexBase::EnableCmdTimeout(uint32_t timeout_ms) {
enable_timeout_ = true;
timeout_ms_ = timeout_ms;
}
void AgilexBase::StartCmdThread() {
keep_running_ = true;
cmd_thread_ = std::thread(
std::bind(&AgilexBase::ControlLoop, this, cmd_thread_period_ms_));
cmd_thread_started_ = true;
}
void AgilexBase::ControlLoop(int32_t period_ms) {
uint32_t timeout_iter_num;
if (enable_timeout_) {
if (timeout_ms_ < period_ms) timeout_ms_ = period_ms;
timeout_iter_num = static_cast<uint32_t>(timeout_ms_ / period_ms);
// std::cout << "Timeout iteration number: " << timeout_iter_num <<
// std::endl;
}
Timer tm;
while (keep_running_) {
tm.reset();
if (enable_timeout_) {
if (watchdog_counter_ < timeout_iter_num) {
SendRobotCmd();
++watchdog_counter_;
}
// else {
// std::cout << "Warning: cmd timeout, no cmd sent to robot" <<
// std::endl;
// }
} else {
SendRobotCmd();
}
tm.sleep_until_ms(period_ms);
}
}
void AgilexBase::EnableCommandedMode() {
// construct message
AgxMessage msg;
msg.type = AgxMsgControlModeConfig;
msg.body.control_mode_config_msg.mode = CONTROL_MODE_CAN;
// encode msg to can frame and send to bus
can_frame frame;
EncodeCanFrame(&msg, &frame);
can_->SendFrame(frame);
}
void AgilexBase::SetMotionCommand(double linear_vel, double angular_vel,
double lateral_velocity,
double steering_angle) {
// make sure cmd thread is started before attempting to send commands
if (!cmd_thread_started_) StartCmdThread();
std::lock_guard<std::mutex> guard(motion_cmd_mutex_);
current_motion_cmd_.linear_velocity = linear_vel;
current_motion_cmd_.angular_velocity = angular_vel;
current_motion_cmd_.lateral_velocity = lateral_velocity;
current_motion_cmd_.steering_angle = steering_angle;
FeedCmdTimeoutWatchdog();
}
void AgilexBase::SendRobotCmd() {
if (can_connected_) {
// motion control message
AgxMessage msg;
msg.type = AgxMsgMotionCommand;
motion_cmd_mutex_.lock();
msg.body.motion_command_msg = current_motion_cmd_;
motion_cmd_mutex_.unlock();
// send to can bus
can_frame frame;
EncodeCanFrame(&msg, &frame);
can_->SendFrame(frame);
}
}
void AgilexBase::SendLightCommand(LightMode front_mode,
uint8_t front_custom_value,
LightMode rear_mode,
uint8_t rear_custom_value) {
AgxMessage msg;
msg.type = AgxMsgLightCommand;
msg.body.light_command_msg.cmd_ctrl_allowed = true;
msg.body.light_command_msg.front_light.mode = front_mode;
msg.body.light_command_msg.front_light.custom_value = front_custom_value;
msg.body.light_command_msg.rear_light.mode = rear_mode;
msg.body.light_command_msg.rear_light.custom_value = rear_custom_value;
// send to can bus
can_frame frame;
EncodeCanFrame(&msg, &frame);
can_->SendFrame(frame);
}
void AgilexBase::DisableLightControl() {
AgxMessage msg;
msg.type = AgxMsgLightCommand;
msg.body.light_command_msg.cmd_ctrl_allowed = false;
// send to can bus
can_frame frame;
EncodeCanFrame(&msg, &frame);
can_->SendFrame(frame);
}
} // namespace westonrobot

565
src/agx_msg_parser.c
View File

@@ -4,82 +4,164 @@
* Created on: Aug 31, 2019 04:25
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
* Copyright (c) 2019 Weston Robot Pte. Ltd.
*/
#include "ugv_sdk/proto/agx_msg_parser.h"
#include "ugv_sdk/details/agilex_msg_parser.h"
#include "agx_protocol_v2.h"
#include "string.h"
#include "stdio.h"
bool DecodeCanFrame(const struct can_frame *rx_frame, AgxMessage *msg) {
msg->type = AgxMsgUnkonwn;
switch (rx_frame->can_id) {
// command frame
/***************** command frame *****************/
case CAN_MSG_MOTION_COMMAND_ID: {
msg->type = AgxMsgMotionCommand;
memcpy(msg->body.motion_command_msg.raw, rx_frame->data,
rx_frame->can_dlc * sizeof(uint8_t));
// parse frame buffer to message
MotionCommandFrame *frame = (MotionCommandFrame *)(rx_frame->data);
msg->body.motion_command_msg.linear_velocity =
(int16_t)((uint16_t)(frame->linear_velocity.low_byte) |
(uint16_t)(frame->linear_velocity.high_byte) << 8) /
1000.0;
msg->body.motion_command_msg.angular_velocity =
(int16_t)((uint16_t)(frame->angular_velocity.low_byte) |
(uint16_t)(frame->angular_velocity.high_byte) << 8) /
1000.0;
msg->body.motion_command_msg.lateral_velocity =
(int16_t)((uint16_t)(frame->lateral_velocity.low_byte) |
(uint16_t)(frame->lateral_velocity.high_byte) << 8) /
1000.0;
msg->body.motion_command_msg.steering_angle =
(int16_t)((uint16_t)(frame->steering_angle.low_byte) |
(uint16_t)(frame->steering_angle.high_byte) << 8) /
1000.0;
break;
}
case CAN_MSG_LIGHT_COMMAND_ID: {
msg->type = AgxMsgLightCommand;
memcpy(msg->body.light_command_msg.raw, rx_frame->data,
rx_frame->can_dlc * sizeof(uint8_t));
// parse frame buffer to message
LightCommandFrame *frame = (LightCommandFrame *)(rx_frame->data);
msg->body.light_command_msg.cmd_ctrl_allowed =
(frame->cmd_ctrl_allowed != 0) ? true : false;
msg->body.light_command_msg.front_light.mode = frame->front_light_mode;
msg->body.light_command_msg.front_light.custom_value =
frame->front_light_custom;
msg->body.light_command_msg.rear_light.mode = frame->rear_light_mode;
msg->body.light_command_msg.rear_light.custom_value =
frame->rear_light_custom;
break;
}
case CAN_MSG_RC_STATE_ID: {
msg->type = AgxMsgRcState;
memcpy(msg->body.rc_state_msg.raw, rx_frame->data,
rx_frame->can_dlc * sizeof(uint8_t));
case CAN_MSG_BRAKING_COMMAND_ID: {
msg->type = AgxMsgBrakingCommand;
// TODO
break;
}
case CAN_MSG_CTRL_MODE_SELECT_ID: {
msg->type = AgxMsgCtrlModeSelect;
memcpy(msg->body.ctrl_mode_select_msg.raw, rx_frame->data,
rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_STATE_RESET_ID: {
msg->type = AgxMsgFaultByteReset;
memcpy(msg->body.state_reset_msg.raw, rx_frame->data,
rx_frame->can_dlc * sizeof(uint8_t));
break;
}
// state feedback frame
/***************** feedback frame ****************/
case CAN_MSG_SYSTEM_STATE_ID: {
msg->type = AgxMsgSystemState;
memcpy(msg->body.system_state_msg.raw, rx_frame->data,
rx_frame->can_dlc * sizeof(uint8_t));
SystemStateFrame *frame = (SystemStateFrame *)(rx_frame->data);
msg->body.system_state_msg.vehicle_state = frame->vehicle_state;
msg->body.system_state_msg.control_mode = frame->control_mode;
msg->body.system_state_msg.battery_voltage =
(int16_t)((uint16_t)(frame->battery_voltage.low_byte) |
(uint16_t)(frame->battery_voltage.high_byte) << 8) *
0.1;
msg->body.system_state_msg.error_code =
(uint16_t)(frame->error_code.low_byte) |
(uint16_t)(frame->error_code.high_byte) << 8;
break;
}
case CAN_MSG_MOTION_STATE_ID: {
msg->type = AgxMsgMotionState;
memcpy(msg->body.motion_state_msg.raw, rx_frame->data,
rx_frame->can_dlc * sizeof(uint8_t));
MotionStateFrame *frame = (MotionStateFrame *)(rx_frame->data);
msg->body.motion_command_msg.linear_velocity =
(int16_t)((uint16_t)(frame->linear_velocity.low_byte) |
(uint16_t)(frame->linear_velocity.high_byte) << 8) /
1000.0;
msg->body.motion_command_msg.angular_velocity =
(int16_t)((uint16_t)(frame->angular_velocity.low_byte) |
(uint16_t)(frame->angular_velocity.high_byte) << 8) /
1000.0;
msg->body.motion_command_msg.lateral_velocity =
(int16_t)((uint16_t)(frame->lateral_velocity.low_byte) |
(uint16_t)(frame->lateral_velocity.high_byte) << 8) /
1000.0;
msg->body.motion_command_msg.steering_angle =
(int16_t)((uint16_t)(frame->steering_angle.low_byte) |
(uint16_t)(frame->steering_angle.high_byte) << 8) /
1000.0;
break;
}
case CAN_MSG_LIGHT_STATE_ID: {
msg->type = AgxMsgLightState;
memcpy(msg->body.light_state_msg.raw, rx_frame->data,
rx_frame->can_dlc * sizeof(uint8_t));
LightStateFrame *frame = (LightStateFrame *)(rx_frame->data);
msg->body.light_command_msg.cmd_ctrl_allowed =
(frame->cmd_ctrl_allowed != 0) ? true : false;
msg->body.light_command_msg.front_light.mode = frame->front_light_mode;
msg->body.light_command_msg.front_light.custom_value =
frame->front_light_custom;
msg->body.light_command_msg.rear_light.mode = frame->rear_light_mode;
msg->body.light_command_msg.rear_light.custom_value =
frame->rear_light_custom;
break;
}
case CAN_MSG_ODOMETRY_ID: {
msg->type = AgxMsgOdometry;
memcpy(msg->body.odometry_msg.raw, rx_frame->data,
rx_frame->can_dlc * sizeof(uint8_t));
case CAN_MSG_RC_STATE_ID: {
msg->type = AgxMsgRcState;
RcStateFrame *frame = (RcStateFrame *)(rx_frame->data);
// switch a
if ((frame->sws & RC_SWA_MASK) == RC_SWA_UP_MASK)
msg->body.rc_state_msg.swa = RC_SWITCH_UP;
else if ((frame->sws & RC_SWA_MASK) == RC_SWA_DOWN_MASK)
msg->body.rc_state_msg.swa = RC_SWITCH_DOWN;
// switch b
if ((frame->sws & RC_SWB_MASK) == RC_SWB_UP_MASK)
msg->body.rc_state_msg.swb = RC_SWITCH_UP;
else if ((frame->sws & RC_SWB_MASK) == RC_SWB_MIDDLE_MASK)
msg->body.rc_state_msg.swb = RC_SWITCH_MIDDLE;
else if ((frame->sws & RC_SWB_MASK) == RC_SWB_DOWN_MASK)
msg->body.rc_state_msg.swb = RC_SWITCH_DOWN;
// switch c
if ((frame->sws & RC_SWC_MASK) == RC_SWC_UP_MASK)
msg->body.rc_state_msg.swc = RC_SWITCH_UP;
else if ((frame->sws & RC_SWC_MASK) == RC_SWC_MIDDLE_MASK)
msg->body.rc_state_msg.swc = RC_SWITCH_MIDDLE;
else if ((frame->sws & RC_SWC_MASK) == RC_SWC_DOWN_MASK)
msg->body.rc_state_msg.swc = RC_SWITCH_DOWN;
// switch d
if ((frame->sws & RC_SWD_MASK) == RC_SWD_UP_MASK)
msg->body.rc_state_msg.swd = RC_SWITCH_UP;
else if ((frame->sws & RC_SWD_MASK) == RC_SWD_DOWN_MASK)
msg->body.rc_state_msg.swd = RC_SWITCH_DOWN;
msg->body.rc_state_msg.stick_right_v = frame->stick_right_v;
msg->body.rc_state_msg.stick_right_h = frame->stick_right_h;
msg->body.rc_state_msg.stick_left_v = frame->stick_left_v;
msg->body.rc_state_msg.stick_left_h = frame->stick_left_h;
msg->body.rc_state_msg.var_a = frame->var_a;
break;
}
case CAN_MSG_ACTUATOR1_HS_STATE_ID:
case CAN_MSG_ACTUATOR2_HS_STATE_ID:
case CAN_MSG_ACTUATOR3_HS_STATE_ID:
case CAN_MSG_ACTUATOR4_HS_STATE_ID: {
msg->type = AgxMsgActuatorLSState;
msg->type = AgxMsgActuatorHSState;
ActuatorHSStateFrame *frame = (ActuatorHSStateFrame *)(rx_frame->data);
msg->body.actuator_hs_state_msg.motor_id =
(uint8_t)(rx_frame->can_id - CAN_MSG_ACTUATOR1_HS_STATE_ID);
memcpy(msg->body.actuator_hs_state_msg.data.raw, rx_frame->data,
rx_frame->can_dlc * sizeof(uint8_t));
rx_frame->can_id - CAN_MSG_ACTUATOR1_HS_STATE_ID;
msg->body.actuator_hs_state_msg.rpm =
(int16_t)((uint16_t)(frame->rpm.low_byte) |
(uint16_t)(frame->rpm.high_byte) << 8);
msg->body.actuator_hs_state_msg.current =
((uint16_t)(frame->current.low_byte) |
(uint16_t)(frame->current.high_byte) << 8) *
0.1;
msg->body.actuator_hs_state_msg.pulse_count =
(int32_t)((uint32_t)(frame->pulse_count.lsb) |
(uint32_t)(frame->pulse_count.low_byte) << 8 |
(uint32_t)(frame->pulse_count.high_byte) << 16 |
(uint32_t)(frame->pulse_count.msb) << 24);
break;
}
case CAN_MSG_ACTUATOR1_LS_STATE_ID:
@@ -87,10 +169,120 @@ bool DecodeCanFrame(const struct can_frame *rx_frame, AgxMessage *msg) {
case CAN_MSG_ACTUATOR3_LS_STATE_ID:
case CAN_MSG_ACTUATOR4_LS_STATE_ID: {
msg->type = AgxMsgActuatorLSState;
msg->body.actuator_ls_state_msg.motor_id =
(uint8_t)(rx_frame->can_id - CAN_MSG_ACTUATOR1_LS_STATE_ID);
memcpy(msg->body.actuator_ls_state_msg.data.raw, rx_frame->data,
rx_frame->can_dlc * sizeof(uint8_t));
ActuatorLSStateFrame *frame = (ActuatorLSStateFrame *)(rx_frame->data);
msg->body.actuator_hs_state_msg.motor_id =
rx_frame->can_id - CAN_MSG_ACTUATOR1_LS_STATE_ID;
msg->body.actuator_ls_state_msg.driver_voltage =
((uint16_t)(frame->driver_voltage.low_byte) |
(uint16_t)(frame->driver_voltage.high_byte) << 8) *
0.1;
msg->body.actuator_ls_state_msg.driver_temperature =
(int16_t)((uint16_t)(frame->driver_temperature.low_byte) |
(uint16_t)(frame->driver_temperature.high_byte) << 8);
msg->body.actuator_ls_state_msg.motor_temperature =
frame->motor_temperature;
msg->body.actuator_ls_state_msg.driver_state = frame->driver_state;
break;
}
/****************** sensor frame *****************/
case CAN_MSG_ODOMETRY_ID: {
msg->type = AgxMsgOdometry;
OdometryFrame *frame = (OdometryFrame *)(rx_frame->data);
msg->body.odometry_msg.left_wheel =
(int32_t)((uint32_t)(frame->left_wheel.lsb) |
(uint32_t)(frame->left_wheel.low_byte) << 8 |
(uint32_t)(frame->left_wheel.high_byte) << 16 |
(uint32_t)(frame->left_wheel.msb) << 24);
msg->body.odometry_msg.right_wheel =
(int32_t)((uint32_t)(frame->right_wheel.lsb) |
(uint32_t)(frame->right_wheel.low_byte) << 8 |
(uint32_t)(frame->right_wheel.high_byte) << 16 |
(uint32_t)(frame->right_wheel.msb) << 24);
break;
}
case CAN_MSG_IMU_ACCEL_ID: {
msg->type = AgxMsgImuAccel;
// TODO
break;
}
case CAN_MSG_IMU_GYRO_ID: {
msg->type = AgxMsgImuGyro;
// TODO
break;
}
case CAN_MSG_IMU_EULER_ID: {
msg->type = AgxMsgImuEuler;
// TODO
break;
}
case CAN_MSG_SAFETY_BUMPER_ID: {
msg->type = AgxMsgSafetyBumper;
// TODO
break;
}
case CAN_MSG_ULTRASONIC_1_ID:
case CAN_MSG_ULTRASONIC_2_ID:
case CAN_MSG_ULTRASONIC_3_ID:
case CAN_MSG_ULTRASONIC_4_ID:
case CAN_MSG_ULTRASONIC_5_ID:
case CAN_MSG_ULTRASONIC_6_ID:
case CAN_MSG_ULTRASONIC_7_ID:
case CAN_MSG_ULTRASONIC_8_ID: {
msg->type = AgxMsgUltrasonic;
// TODO
break;
}
case CAN_MSG_UWB_1_ID:
case CAN_MSG_UWB_2_ID:
case CAN_MSG_UWB_3_ID:
case CAN_MSG_UWB_4_ID: {
msg->type = AgxMsgUwb;
// TODO
break;
}
case CAN_MSG_BMS_BASIC_ID: {
msg->type = AgxMsgBmsBasic;
// TODO
break;
}
case CAN_MSG_BMS_EXTENDED_ID: {
msg->type = AgxMsgBmsExtended;
// TODO
break;
}
/*************** query/config frame **************/
case CAN_MSG_VERSION_REQUEST_ID: {
msg->type = AgxMsgVersionRequest;
// TODO
break;
}
case CAN_MSG_VERSION_RESPONSE_ID: {
msg->type = AgxMsgVersionResponse;
// TODO
break;
}
case CAN_MSG_CTRL_MODE_CONFIG_ID: {
msg->type = AgxMsgControlModeConfig;
ControlModeConfigFrame *frame =
(ControlModeConfigFrame *)(rx_frame->data);
msg->body.control_mode_config_msg.mode = frame->mode;
break;
}
case CAN_MSG_STEER_NEUTRAL_REQUEST_ID: {
msg->type = AgxMsgSteerNeutralRequest;
// TODO
break;
}
case CAN_MSG_STEER_NEUTRAL_RESPONSE_ID: {
msg->type = AgxMsgSteerNeutralResponse;
// TODO
break;
}
case CAN_MSG_STATE_RESET_CONFIG_ID: {
msg->type = AgxMsgStateResetConfig;
StateResetConfigFrame *frame = (StateResetConfigFrame *)(rx_frame->data);
msg->body.state_reset_config_msg.error_clear_byte =
frame->error_clear_byte;
break;
}
default:
@@ -102,113 +294,272 @@ bool DecodeCanFrame(const struct can_frame *rx_frame, AgxMessage *msg) {
void EncodeCanFrame(const AgxMessage *msg, struct can_frame *tx_frame) {
switch (msg->type) {
// command frame
/***************** command frame *****************/
case AgxMsgMotionCommand: {
tx_frame->can_id = CAN_MSG_MOTION_COMMAND_ID;
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->body.motion_command_msg.raw,
tx_frame->can_dlc);
MotionCommandFrame frame;
int16_t linear_cmd =
(int16_t)(msg->body.motion_command_msg.linear_velocity * 1000);
int16_t angular_cmd =
(int16_t)(msg->body.motion_command_msg.angular_velocity * 1000);
int16_t lateral_cmd =
(int16_t)(msg->body.motion_command_msg.lateral_velocity * 1000);
int16_t steering_cmd =
(int16_t)(msg->body.motion_command_msg.steering_angle * 1000);
frame.linear_velocity.high_byte = (uint8_t)(linear_cmd >> 8);
frame.linear_velocity.low_byte = (uint8_t)(linear_cmd & 0x00ff);
frame.angular_velocity.high_byte = (uint8_t)(angular_cmd >> 8);
frame.angular_velocity.low_byte = (uint8_t)(angular_cmd & 0x00ff);
frame.lateral_velocity.high_byte = (uint8_t)(lateral_cmd >> 8);
frame.lateral_velocity.low_byte = (uint8_t)(lateral_cmd & 0x00ff);
frame.steering_angle.high_byte = (uint8_t)(steering_cmd >> 8);
frame.steering_angle.low_byte = (uint8_t)(steering_cmd & 0x00ff);
memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc);
break;
}
case AgxMsgLightCommand: {
static uint8_t count = 0;
tx_frame->can_id = CAN_MSG_LIGHT_COMMAND_ID;
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->body.light_command_msg.raw,
tx_frame->can_dlc);
LightCommandFrame frame;
if (msg->body.light_command_msg.cmd_ctrl_allowed) {
frame.cmd_ctrl_allowed = LIGHT_CMD_CTRL_ALLOWED;
frame.front_light_mode = msg->body.light_command_msg.front_light.mode;
frame.front_light_custom =
msg->body.light_command_msg.front_light.custom_value;
frame.rear_light_mode = msg->body.light_command_msg.rear_light.mode;
frame.rear_light_custom =
msg->body.light_command_msg.rear_light.custom_value;
} else {
frame.cmd_ctrl_allowed = LIGHT_CMD_CTRL_DISALLOWED;
frame.front_light_mode = 0;
frame.front_light_custom = 0;
frame.rear_light_mode = 0;
frame.rear_light_custom = 0;
}
frame.reserved0 = 0;
frame.reserved1 = 0;
frame.count = count++;
memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc);
break;
}
case AgxMsgCtrlModeSelect: {
tx_frame->can_id = CAN_MSG_CTRL_MODE_SELECT_ID;
case AgxMsgBrakingCommand: {
tx_frame->can_id = CAN_MSG_BRAKING_COMMAND_ID;
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->body.ctrl_mode_select_msg.raw,
tx_frame->can_dlc);
BrakingCommandFrame frame;
// TODO
memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc);
break;
}
case AgxMsgFaultByteReset: {
tx_frame->can_id = CAN_MSG_STATE_RESET_ID;
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->body.state_reset_msg.raw, tx_frame->can_dlc);
break;
}
// state feedback frame
/***************** feedback frame ****************/
case AgxMsgSystemState: {
tx_frame->can_id = CAN_MSG_SYSTEM_STATE_ID;
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->body.system_state_msg.raw, tx_frame->can_dlc);
SystemStateFrame frame;
frame.vehicle_state = msg->body.system_state_msg.vehicle_state;
frame.control_mode = msg->body.system_state_msg.control_mode;
uint16_t battery =
(uint16_t)(msg->body.system_state_msg.battery_voltage * 10);
frame.battery_voltage.high_byte = (uint8_t)(battery >> 8);
frame.battery_voltage.low_byte = (uint8_t)(battery & 0x00ff);
frame.error_code.high_byte =
(uint8_t)(msg->body.system_state_msg.error_code >> 8);
frame.error_code.low_byte =
(uint8_t)(msg->body.system_state_msg.error_code & 0x00ff);
memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc);
break;
}
case AgxMsgMotionState: {
tx_frame->can_id = CAN_MSG_MOTION_STATE_ID;
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->body.motion_state_msg.raw, tx_frame->can_dlc);
MotionStateFrame frame;
int16_t linear =
(int16_t)(msg->body.motion_state_msg.linear_velocity * 1000);
int16_t angular =
(int16_t)(msg->body.motion_state_msg.angular_velocity * 1000);
int16_t lateral =
(int16_t)(msg->body.motion_state_msg.lateral_velocity * 1000);
int16_t steering =
(int16_t)(msg->body.motion_state_msg.steering_angle * 1000);
frame.linear_velocity.high_byte = (uint8_t)(linear >> 8);
frame.linear_velocity.low_byte = (uint8_t)(linear & 0x00ff);
frame.angular_velocity.high_byte = (uint8_t)(angular >> 8);
frame.angular_velocity.low_byte = (uint8_t)(angular & 0x00ff);
frame.lateral_velocity.high_byte = (uint8_t)(lateral >> 8);
frame.lateral_velocity.low_byte = (uint8_t)(lateral & 0x00ff);
frame.steering_angle.high_byte = (uint8_t)(steering >> 8);
frame.steering_angle.low_byte = (uint8_t)(steering & 0x00ff);
memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc);
break;
}
case AgxMsgLightState: {
tx_frame->can_id = CAN_MSG_LIGHT_STATE_ID;
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->body.light_state_msg.raw, tx_frame->can_dlc);
LightStateFrame frame;
// TODO
memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc);
break;
}
case AgxMsgOdometry: {
tx_frame->can_id = CAN_MSG_ODOMETRY_ID;
case AgxMsgRcState: {
tx_frame->can_id = CAN_MSG_RC_STATE_ID;
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->body.odometry_msg.raw, tx_frame->can_dlc);
RcStateFrame frame;
// TODO
memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc);
break;
}
case AgxMsgActuatorHSState: {
switch (msg->body.actuator_hs_state_msg.motor_id) {
case ACTUATOR1_ID: {
tx_frame->can_id = CAN_MSG_ACTUATOR1_HS_STATE_ID;
break;
}
case ACTUATOR2_ID: {
tx_frame->can_id = CAN_MSG_ACTUATOR2_HS_STATE_ID;
break;
}
case ACTUATOR3_ID: {
tx_frame->can_id = CAN_MSG_ACTUATOR3_HS_STATE_ID;
break;
}
case ACTUATOR4_ID: {
tx_frame->can_id = CAN_MSG_ACTUATOR4_HS_STATE_ID;
break;
}
}
tx_frame->can_id = msg->body.actuator_hs_state_msg.motor_id +
CAN_MSG_ACTUATOR1_HS_STATE_ID;
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->body.actuator_hs_state_msg.data.raw,
tx_frame->can_dlc);
ActuatorHSStateFrame frame;
// TODO
memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc);
break;
}
case AgxMsgActuatorLSState: {
switch (msg->body.actuator_ls_state_msg.motor_id) {
case ACTUATOR1_ID: {
tx_frame->can_id = CAN_MSG_ACTUATOR1_LS_STATE_ID;
break;
}
case ACTUATOR2_ID: {
tx_frame->can_id = CAN_MSG_ACTUATOR2_LS_STATE_ID;
break;
}
case ACTUATOR3_ID: {
tx_frame->can_id = CAN_MSG_ACTUATOR3_LS_STATE_ID;
break;
}
case ACTUATOR4_ID: {
tx_frame->can_id = CAN_MSG_ACTUATOR4_LS_STATE_ID;
break;
}
}
tx_frame->can_id = msg->body.actuator_ls_state_msg.motor_id +
CAN_MSG_ACTUATOR1_LS_STATE_ID;
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->body.actuator_ls_state_msg.data.raw,
tx_frame->can_dlc);
ActuatorLSStateFrame frame;
// TODO
memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc);
break;
}
/****************** sensor frame *****************/
case AgxMsgOdometry: {
tx_frame->can_id = CAN_MSG_ODOMETRY_ID;
tx_frame->can_dlc = 8;
OdometryFrame frame;
// TODO
memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc);
break;
}
case AgxMsgImuAccel: {
tx_frame->can_id = CAN_MSG_IMU_ACCEL_ID;
tx_frame->can_dlc = 8;
ImuAccelFrame frame;
// TODO
memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc);
break;
}
case AgxMsgImuGyro: {
tx_frame->can_id = CAN_MSG_IMU_GYRO_ID;
tx_frame->can_dlc = 8;
ImuGyroFrame frame;
// TODO
memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc);
break;
}
case AgxMsgImuEuler: {
tx_frame->can_id = CAN_MSG_IMU_EULER_ID;
tx_frame->can_dlc = 8;
ImuEulerFrame frame;
// TODO
memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc);
break;
}
case AgxMsgSafetyBumper: {
tx_frame->can_id = CAN_MSG_SAFETY_BUMPER_ID;
tx_frame->can_dlc = 8;
SafetyBumperFrame frame;
// TODO
memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc);
break;
}
case AgxMsgUltrasonic: {
tx_frame->can_id =
msg->body.ultrasonic_msg.sensor_id + CAN_MSG_ULTRASONIC_1_ID;
tx_frame->can_dlc = 8;
UltrasonicFrame frame;
// TODO
memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc);
break;
}
case AgxMsgUwb: {
tx_frame->can_id = msg->body.uwb_msg.sensor_id + CAN_MSG_UWB_1_ID;
tx_frame->can_dlc = 8;
UwbFrame frame;
// TODO
memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc);
break;
}
case AgxMsgBmsBasic: {
tx_frame->can_id = CAN_MSG_BMS_BASIC_ID;
tx_frame->can_dlc = 8;
BmsBasicFrame frame;
// TODO
memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc);
break;
}
case AgxMsgBmsExtended: {
tx_frame->can_id = CAN_MSG_BMS_EXTENDED_ID;
tx_frame->can_dlc = 8;
BmsExtendedFrame frame;
// TODO
memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc);
break;
}
/*************** query/config frame **************/
case AgxMsgVersionRequest: {
tx_frame->can_id = CAN_MSG_VERSION_REQUEST_ID;
tx_frame->can_dlc = 8;
VersionRequestFrame frame;
// TODO
memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc);
break;
}
case AgxMsgVersionResponse: {
tx_frame->can_id = CAN_MSG_VERSION_RESPONSE_ID;
tx_frame->can_dlc = 8;
VersionResponseFrame frame;
// TODO
memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc);
break;
}
case AgxMsgControlModeConfig: {
tx_frame->can_id = CAN_MSG_CTRL_MODE_CONFIG_ID;
tx_frame->can_dlc = 8;
ControlModeConfigFrame frame;
frame.mode = msg->body.control_mode_config_msg.mode;
frame.reserved0 = 0;
frame.reserved1 = 0;
frame.reserved2 = 0;
frame.reserved3 = 0;
frame.reserved4 = 0;
frame.reserved5 = 0;
frame.reserved6 = 0;
memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc);
break;
}
case AgxMsgSteerNeutralRequest: {
tx_frame->can_id = CAN_MSG_STEER_NEUTRAL_REQUEST_ID;
tx_frame->can_dlc = 8;
SteerNeutralRequestFrame frame;
// TODO
memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc);
break;
}
case AgxMsgSteerNeutralResponse: {
tx_frame->can_id = CAN_MSG_STEER_NEUTRAL_RESPONSE_ID;
tx_frame->can_dlc = 8;
SteerNeutralResponseFrame frame;
// TODO
memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc);
break;
}
case AgxMsgStateResetConfig: {
tx_frame->can_id = CAN_MSG_STATE_RESET_CONFIG_ID;
tx_frame->can_dlc = 8;
StateResetConfigFrame frame;
// TODO
memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc);
break;
}
default:
break;
}
// tx_frame->data[7] =
// CalcCanFrameChecksum(tx_frame->can_id, tx_frame->data,
// tx_frame->can_dlc);
}
uint8_t CalcCanFrameChecksum(uint16_t id, uint8_t *data, uint8_t dlc) {

380
src/agx_protocol_v2.h Normal file
View File

@@ -0,0 +1,380 @@
/*
* agx_protocol_v2.h
*
* Created on: Nov 04, 2020 13:54
* Description:
*
* Copyright (c) 2020 Weston Robot Pte. Ltd.
*/
#ifndef AGX_PROTOCOL_V2_H
#define AGX_PROTOCOL_V2_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
// define endianess of the platform
#if (!defined(USE_LITTLE_ENDIAN) && !defined(USE_BIG_ENDIAN))
#define USE_LITTLE_ENDIAN
#endif
#ifdef USE_BIG_ENDIAN
#error "BIG ENDIAN IS CURRENTLY NOT SUPPORTED"
#endif
/*---------------------------- Motor IDs -------------------------------*/
#define ACTUATOR1_ID ((uint8_t)0x00)
#define ACTUATOR2_ID ((uint8_t)0x01)
#define ACTUATOR3_ID ((uint8_t)0x02)
#define ACTUATOR4_ID ((uint8_t)0x03)
#define ACTUATOR5_ID ((uint8_t)0x04)
#define ACTUATOR6_ID ((uint8_t)0x05)
#define ACTUATOR7_ID ((uint8_t)0x06)
#define ACTUATOR8_ID ((uint8_t)0x07)
/*--------------------------- Message IDs ------------------------------*/
// control group: 0x1
#define CAN_MSG_MOTION_COMMAND_ID ((uint32_t)0x111)
#define CAN_MSG_LIGHT_COMMAND_ID ((uint32_t)0x121)
#define CAN_MSG_BRAKING_COMMAND_ID ((uint32_t)0x131)
// state feedback group: 0x2
#define CAN_MSG_SYSTEM_STATE_ID ((uint32_t)0x211)
#define CAN_MSG_MOTION_STATE_ID ((uint32_t)0x221)
#define CAN_MSG_LIGHT_STATE_ID ((uint32_t)0x231)
#define CAN_MSG_RC_STATE_ID ((uint32_t)0x241)
#define CAN_MSG_ACTUATOR1_HS_STATE_ID ((uint32_t)0x251)
#define CAN_MSG_ACTUATOR2_HS_STATE_ID ((uint32_t)0x252)
#define CAN_MSG_ACTUATOR3_HS_STATE_ID ((uint32_t)0x253)
#define CAN_MSG_ACTUATOR4_HS_STATE_ID ((uint32_t)0x254)
#define CAN_MSG_ACTUATOR1_LS_STATE_ID ((uint32_t)0x261)
#define CAN_MSG_ACTUATOR2_LS_STATE_ID ((uint32_t)0x262)
#define CAN_MSG_ACTUATOR3_LS_STATE_ID ((uint32_t)0x263)
#define CAN_MSG_ACTUATOR4_LS_STATE_ID ((uint32_t)0x264)
// sensor data group: 0x3
#define CAN_MSG_ODOMETRY_ID ((uint32_t)0x311)
#define CAN_MSG_IMU_ACCEL_ID ((uint32_t)0x321)
#define CAN_MSG_IMU_GYRO_ID ((uint32_t)0x322)
#define CAN_MSG_IMU_EULER_ID ((uint32_t)0x323)
#define CAN_MSG_SAFETY_BUMPER_ID ((uint32_t)0x331)
#define CAN_MSG_ULTRASONIC_1_ID ((uint32_t)0x341)
#define CAN_MSG_ULTRASONIC_2_ID ((uint32_t)0x342)
#define CAN_MSG_ULTRASONIC_3_ID ((uint32_t)0x343)
#define CAN_MSG_ULTRASONIC_4_ID ((uint32_t)0x344)
#define CAN_MSG_ULTRASONIC_5_ID ((uint32_t)0x345)
#define CAN_MSG_ULTRASONIC_6_ID ((uint32_t)0x346)
#define CAN_MSG_ULTRASONIC_7_ID ((uint32_t)0x347)
#define CAN_MSG_ULTRASONIC_8_ID ((uint32_t)0x348)
#define CAN_MSG_UWB_1_ID ((uint32_t)0x351)
#define CAN_MSG_UWB_2_ID ((uint32_t)0x352)
#define CAN_MSG_UWB_3_ID ((uint32_t)0x353)
#define CAN_MSG_UWB_4_ID ((uint32_t)0x354)
#define CAN_MSG_BMS_BASIC_ID ((uint32_t)0x361)
#define CAN_MSG_BMS_EXTENDED_ID ((uint32_t)0x362)
// query/config group: 0x4
#define CAN_MSG_VERSION_REQUEST_ID ((uint32_t)0x411)
#define CAN_MSG_VERSION_RESPONSE_ID ((uint32_t)0x41a)
#define CAN_MSG_CTRL_MODE_CONFIG_ID ((uint32_t)0x421)
#define CAN_MSG_STEER_NEUTRAL_REQUEST_ID ((uint32_t)0x431)
#define CAN_MSG_STEER_NEUTRAL_RESPONSE_ID ((uint32_t)0x43a)
#define CAN_MSG_STATE_RESET_CONFIG_ID ((uint32_t)0x441)
/*--------------------- Control/State Constants ------------------------*/
#define LIGHT_CMD_CTRL_ALLOWED ((uint8_t)0x00)
#define LIGHT_CMD_CTRL_DISALLOWED ((uint8_t)0x01)
#define VERSION_REQUEST_VALUE ((uint8_t)0x01)
#define STEER_NEUTRAL_REQUEST_VALUE ((uint8_t)0xee)
#define STEER_NEUTRAL_RESPONSE_SUCCESS_VALUE ((uint8_t)0xee)
#define STEER_NEUTRAL_RESPONSE_FAILURE_VALUE ((uint8_t)0xff)
/*------------------------ Frame Memory Layout -------------------------*/
/* No padding in the struct */
// reference: https://stackoverflow.com/questions/3318410/pragma-pack-effect
#pragma pack(push, 1)
#ifdef USE_LITTLE_ENDIAN
typedef struct {
uint8_t high_byte;
uint8_t low_byte;
} struct16_t;
typedef struct {
uint8_t msb;
uint8_t high_byte;
uint8_t low_byte;
uint8_t lsb;
} struct32_t;
#elif defined(USE_BIG_ENDIAN)
typedef struct {
uint8_t low_byte;
uint8_t high_byte;
} struct16_t;
typedef struct {
uint8_t lsb;
uint8_t low_byte;
uint8_t high_byte;
uint8_t msb;
} struct32_t;
#endif
// Control messages
typedef struct {
struct16_t linear_velocity;
struct16_t angular_velocity;
struct16_t lateral_velocity;
struct16_t steering_angle;
} MotionCommandFrame;
typedef struct {
uint8_t cmd_ctrl_allowed;
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 reserved1;
uint8_t count;
} LightCommandFrame;
typedef struct {
uint8_t cmd_ctrl_allowed;
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 reserved1;
uint8_t count;
} BrakingCommandFrame;
// State feedback messages
typedef struct {
uint8_t vehicle_state;
uint8_t control_mode;
struct16_t battery_voltage;
struct16_t error_code;
uint8_t reserved0;
uint8_t count;
} SystemStateFrame;
typedef struct {
struct16_t linear_velocity;
struct16_t angular_velocity;
struct16_t lateral_velocity;
struct16_t steering_angle;
} MotionStateFrame;
typedef struct {
uint8_t cmd_ctrl_allowed;
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 reserved1;
uint8_t count;
} LightStateFrame;
#define RC_SWA_MASK ((uint8_t)0b00000011)
#define RC_SWA_UP_MASK ((uint8_t)0b00000010)
#define RC_SWA_DOWN_MASK ((uint8_t)0b00000011)
#define RC_SWB_MASK ((uint8_t)0b00001100)
#define RC_SWB_UP_MASK ((uint8_t)0b00001000)
#define RC_SWB_MIDDLE_MASK ((uint8_t)0b00000100)
#define RC_SWB_DOWN_MASK ((uint8_t)0b00001100)
#define RC_SWC_MASK ((uint8_t)0b00110000)
#define RC_SWC_UP_MASK ((uint8_t)0b00100000)
#define RC_SWC_MIDDLE_MASK ((uint8_t)0b00010000)
#define RC_SWC_DOWN_MASK ((uint8_t)0b00110000)
#define RC_SWD_MASK ((uint8_t)0b11000000)
#define RC_SWD_UP_MASK ((uint8_t)0b10000000)
#define RC_SWD_DOWN_MASK ((uint8_t)0b11000000)
typedef struct {
uint8_t sws;
int8_t stick_right_h;
int8_t stick_right_v;
int8_t stick_left_v;
int8_t stick_left_h;
int8_t var_a;
uint8_t reserved0;
uint8_t count;
} RcStateFrame;
typedef struct {
struct16_t rpm;
struct16_t current;
struct32_t pulse_count;
} ActuatorHSStateFrame;
typedef struct {
struct16_t driver_voltage;
struct16_t driver_temperature;
int8_t motor_temperature;
uint8_t driver_state;
uint8_t reserved0;
uint8_t reserved1;
} ActuatorLSStateFrame;
// sensors
typedef struct {
struct32_t left_wheel;
struct32_t right_wheel;
} OdometryFrame;
typedef struct {
struct16_t accel_x;
struct16_t accel_y;
struct16_t accel_z;
uint8_t reserverd0;
uint8_t count;
} ImuAccelFrame;
typedef struct {
struct16_t gyro_x;
struct16_t gyro_y;
struct16_t gyro_z;
uint8_t reserverd0;
uint8_t count;
} ImuGyroFrame;
typedef struct {
struct16_t yaw;
struct16_t pitch;
struct16_t roll;
uint8_t reserverd0;
uint8_t count;
} ImuEulerFrame;
typedef struct {
uint8_t trigger_state;
uint8_t reserved0;
uint8_t reserved1;
uint8_t reserved2;
uint8_t reserved3;
uint8_t reserved4;
uint8_t reserved5;
uint8_t reserved6;
} SafetyBumperFrame;
typedef struct {
uint8_t distance[8];
} UltrasonicFrame;
typedef struct {
struct16_t relative_distance;
struct16_t relative_angle;
uint8_t is_normal;
int8_t channels[3];
} UwbFrame;
typedef struct {
uint8_t battery_soc;
uint8_t battery_soh;
struct16_t voltage;
struct16_t current;
struct16_t temperature;
} BmsBasicFrame;
typedef struct {
uint8_t protection_code1;
uint8_t protection_code2;
uint8_t protection_code3;
uint8_t protection_code4;
uint8_t battery_max_teperature;
uint8_t battery_min_teperature;
struct16_t count;
} BmsExtendedFrame;
// query/config
typedef struct {
uint8_t request;
uint8_t reserved0;
uint8_t reserved1;
uint8_t reserved2;
uint8_t reserved3;
uint8_t reserved4;
uint8_t reserved5;
uint8_t reserved6;
} VersionRequestFrame;
typedef struct {
struct16_t controller_hw_version;
struct16_t motor_driver_hw_version;
struct16_t controller_sw_version;
struct16_t motor_driver_sw_version;
} VersionResponseFrame;
typedef struct {
uint8_t mode;
uint8_t reserved0;
uint8_t reserved1;
uint8_t reserved2;
uint8_t reserved3;
uint8_t reserved4;
uint8_t reserved5;
uint8_t reserved6;
} ControlModeConfigFrame;
typedef struct {
uint8_t set_as_neutral;
uint8_t reserved0;
uint8_t reserved1;
uint8_t reserved2;
uint8_t reserved3;
uint8_t reserved4;
uint8_t reserved5;
uint8_t reserved6;
} SteerNeutralRequestFrame;
typedef struct {
uint8_t neutral_set_successful;
uint8_t reserved0;
uint8_t reserved1;
uint8_t reserved2;
uint8_t reserved3;
uint8_t reserved4;
uint8_t reserved5;
uint8_t reserved6;
} SteerNeutralResponseFrame;
typedef struct {
uint8_t error_clear_byte;
uint8_t reserved0;
uint8_t reserved1;
uint8_t reserved2;
uint8_t reserved3;
uint8_t reserved4;
uint8_t reserved5;
uint8_t reserved6;
} StateResetConfigFrame;
#pragma pack(pop)
#ifdef __cplusplus
}
#endif
#endif /* AGX_PROTOCOL_V2_H */

239
src/hunter_base.cpp
View File

@@ -1,239 +0,0 @@
#include "ugv_sdk/hunter/hunter_base.hpp"
#include <algorithm>
#include <array>
#include <chrono>
#include <cstdint>
#include <cstring>
#include <iostream>
#include <ratio>
#include <string>
#include <thread>
#include "stopwatch.hpp"
namespace westonrobot {
void HunterBase::SendRobotCmd() {
static uint8_t cmd_count = 0;
SendModeCtl();
SetParkMode();
SendMotionCmd(cmd_count++);
}
void HunterBase::SendMotionCmd(uint8_t count) {
// motion control message
HunterMessage m_msg;
m_msg.type = HunterMotionControlMsg;
/*if (can_connected_)
m_msg.body.motion_control_msg.data.cmd.control_mode = CTRL_MODE_CMD_CAN;
else if (serial_connected_)
m_msg.body.motion_cmd_msg.data.cmd.control_mode = CTRL_MODE_CMD_UART*/
;
motion_cmd_mutex_.lock();
m_msg.body.motion_control_msg.data.cmd.linear_velocity_cmd.high_byte =
current_motion_cmd_.linear_velocity_height_byte;
m_msg.body.motion_control_msg.data.cmd.linear_velocity_cmd.low_byte =
current_motion_cmd_.linear_velocity_low_byte;
m_msg.body.motion_control_msg.data.cmd.angular_velocity_cmd.high_byte =
current_motion_cmd_.angular_velocity_height_byte;
m_msg.body.motion_control_msg.data.cmd.angular_velocity_cmd.low_byte =
current_motion_cmd_.angular_velocity_low_byte;
motion_cmd_mutex_.unlock();
m_msg.body.motion_control_msg.data.cmd.reserved0 = 0;
m_msg.body.motion_control_msg.data.cmd.reserved1 = 0;
m_msg.body.motion_control_msg.data.cmd.reserved2 = 0;
m_msg.body.motion_control_msg.data.cmd.reserved3 = 0;
// send to can bus
if (can_connected_) {
can_frame m_frame;
EncodeHunterMsgToCAN(&m_msg, &m_frame);
can_if_->SendFrame(m_frame);
}
}
HunterState HunterBase::GetHunterState() {
std::lock_guard<std::mutex> guard(hunter_state_mutex_);
return hunter_state_;
}
void HunterBase::SetMotionCommand(
double linear_vel, double angular_vel, double steering_angle,
HunterMotionCmd::FaultClearFlag fault_clr_flag) {
// make sure cmd thread is started before attempting to send commands
if (!cmd_thread_started_) StartCmdThread();
if (linear_vel < HunterMotionCmd::min_linear_velocity)
linear_vel = HunterMotionCmd::min_linear_velocity;
if (linear_vel > HunterMotionCmd::max_linear_velocity)
linear_vel = HunterMotionCmd::max_linear_velocity;
if (steering_angle < HunterMotionCmd::min_steering_angle)
steering_angle = HunterMotionCmd::min_steering_angle;
if (steering_angle > HunterMotionCmd::max_steering_angle)
steering_angle = HunterMotionCmd::max_steering_angle;
std::lock_guard<std::mutex> guard(motion_cmd_mutex_);
current_motion_cmd_.linear_velocity_height_byte =
static_cast<int16_t>(linear_vel * 1000) >> 8;
current_motion_cmd_.linear_velocity_low_byte =
static_cast<int16_t>(linear_vel * 1000) & 0xff;
current_motion_cmd_.angular_velocity_height_byte =
static_cast<int16_t>(angular_vel * 1000) >> 8;
current_motion_cmd_.angular_velocity_low_byte =
static_cast<int16_t>(angular_vel * 1000) & 0xff;
current_motion_cmd_.fault_clear_flag = fault_clr_flag;
FeedCmdTimeoutWatchdog();
}
void HunterBase::SendModeCtl() {
HunterMessage m_msg;
m_msg.type = HunterControlModeMsg;
mode_cmd_mutex_.lock();
m_msg.body.mode_cmd_msg.data.cmd.mode_control = 0x01;
mode_cmd_mutex_.unlock();
m_msg.body.mode_cmd_msg.data.cmd.reserved0 = 0;
m_msg.body.mode_cmd_msg.data.cmd.reserved1 = 0;
m_msg.body.mode_cmd_msg.data.cmd.reserved2 = 0;
m_msg.body.mode_cmd_msg.data.cmd.reserved3 = 0;
m_msg.body.mode_cmd_msg.data.cmd.reserved4 = 0;
m_msg.body.mode_cmd_msg.data.cmd.reserved5 = 0;
m_msg.body.mode_cmd_msg.data.cmd.reserved6 = 0;
if (can_connected_) {
// send to can bus
can_frame m_frame;
EncodeHunterMsgToCAN(&m_msg, &m_frame);
can_if_->SendFrame(m_frame);
} else {
}
}
void HunterBase::SetParkMode() {
HunterMessage m_msg;
m_msg.type = HunterParkControlMsg;
bool flag = current_motion_cmd_.linear_velocity_height_byte ||
current_motion_cmd_.linear_velocity_low_byte;
if (flag) {
pack_mode_cmd_mutex_.lock();
m_msg.body.park_control_msg.data.cmd.packing_mode = 0x00;
pack_mode_cmd_mutex_.unlock();
} else {
pack_mode_cmd_mutex_.lock();
m_msg.body.park_control_msg.data.cmd.packing_mode = 0x01;
pack_mode_cmd_mutex_.unlock();
}
m_msg.body.park_control_msg.data.cmd.reserved0 = 0;
m_msg.body.park_control_msg.data.cmd.reserved1 = 0;
m_msg.body.park_control_msg.data.cmd.reserved2 = 0;
m_msg.body.park_control_msg.data.cmd.reserved3 = 0;
m_msg.body.park_control_msg.data.cmd.reserved4 = 0;
m_msg.body.park_control_msg.data.cmd.reserved5 = 0;
m_msg.body.park_control_msg.data.cmd.reserved6 = 0;
if (can_connected_) {
// send to can bus
can_frame m_frame;
EncodeHunterMsgToCAN(&m_msg, &m_frame);
can_if_->SendFrame(m_frame);
} else {
}
}
void HunterBase::ParseCANFrame(can_frame *rx_frame) {
// update robot state with new frame
HunterMessage status_msg;
DecodeHunterMsgFromCAN(rx_frame, &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_);
UpdateHunterState(msg, hunter_state_);
}
void HunterBase::UpdateHunterState(const HunterMessage &status_msg,
HunterState &state) {
switch (status_msg.type) {
case HunterMotionStatusMsg: {
// std::cout << "motion control feedback received" << std::endl;
const MotionStatusMessage &msg = status_msg.body.motion_status_msg;
state.linear_velocity =
static_cast<int16_t>(
static_cast<uint16_t>(msg.data.status.linear_velocity.low_byte) |
static_cast<uint16_t>(msg.data.status.linear_velocity.high_byte)
<< 8) /
1000.0;
state.steering_angle =
static_cast<int16_t>(
static_cast<uint16_t>(msg.data.status.angular_velocity.low_byte) |
static_cast<uint16_t>(msg.data.status.angular_velocity.high_byte)
<< 8) /
1000.0;
break;
}
case HunterSystemStatusMsg: {
// std::cout << "system status feedback received" << std::endl;
const SystemStatusMessage &msg = status_msg.body.system_status_msg;
state.control_mode = msg.data.status.control_mode;
state.park_mode = msg.data.status.park_mode;
state.base_state = msg.data.status.base_state;
state.battery_voltage =
(static_cast<uint16_t>(msg.data.status.battery_voltage.low_byte) |
static_cast<uint16_t>(msg.data.status.battery_voltage.high_byte)
<< 8) /
10.0;
state.fault_code =
(static_cast<uint16_t>(msg.data.status.fault_code.low_byte) |
static_cast<uint16_t>(msg.data.status.fault_code.high_byte) << 8);
break;
}
case HunterMotorDriverHeightSpeedStatusMsg: {
// std::cout << "motor driver height speed feedback received" <<
// std::endl;
const MotorDriverHeightSpeedStatusMessage &msg =
status_msg.body.motor_driver_height_speed_status_msg;
for (int i = 0; i < HunterState::motor_num; ++i) {
state.motor_hs_state[msg.motor_id].current =
(static_cast<uint16_t>(msg.data.status.current.low_byte) |
static_cast<uint16_t>(msg.data.status.current.high_byte) << 8) /
10.0;
state.motor_hs_state[msg.motor_id].rpm = static_cast<int16_t>(
static_cast<uint16_t>(msg.data.status.rpm.low_byte) |
static_cast<uint16_t>(msg.data.status.rpm.high_byte) << 8);
state.motor_hs_state[msg.motor_id].motor_pose = static_cast<int32_t>(
static_cast<uint32_t>(msg.data.status.moter_pose.lowest) |
static_cast<uint32_t>(msg.data.status.moter_pose.sec_lowest) << 8 |
static_cast<uint32_t>(msg.data.status.moter_pose.sec_heighest)
<< 16 |
static_cast<uint32_t>(msg.data.status.moter_pose.heighest) << 24);
}
break;
}
case HunterMotorDriverLowSpeedStatusMsg: {
// std::cout << "motor driver low speed feedback received" << std::endl;
const MotorDriverLowSpeedStatusMessage &msg =
status_msg.body.motor_driver_low_speed_status_msg;
for (int i = 0; i < HunterState::motor_num; ++i) {
state.motor_ls_state[msg.motor_id].driver_voltage =
(static_cast<uint16_t>(msg.data.status.driver_voltage.low_byte) |
static_cast<uint16_t>(msg.data.status.driver_voltage.high_byte)
<< 8) /
10.0;
state.motor_ls_state[msg.motor_id]
.driver_temperature = static_cast<int16_t>(
static_cast<uint16_t>(msg.data.status.driver_temperature.low_byte) |
static_cast<uint16_t>(msg.data.status.driver_temperature.high_byte)
<< 8);
state.motor_ls_state[msg.motor_id].motor_temperature =
msg.data.status.motor_temperature;
state.motor_ls_state[msg.motor_id].driver_state =
msg.data.status.driver_status;
}
}
default:
break;
}
}
} // namespace westonrobot

107
src/mobile_base.cpp
View File

@@ -1,107 +0,0 @@
/*
* mobile_base.cpp
*
* Created on: Jun 17, 2020 11:26
* Description:
*
* Copyright (c) 2020 Ruixiang Du (rdu)
*/
#include "ugv_sdk/mobile_base.hpp"
#include <cstring>
#include <iostream>
#include <algorithm>
#include "stopwatch.hpp"
namespace westonrobot {
MobileBase::~MobileBase() {
// release resource if occupied
Disconnect();
// joint cmd thread
if (cmd_thread_.joinable()) cmd_thread_.join();
}
void MobileBase::Connect(std::string dev_name, int32_t baud_rate) {
if (baud_rate == 0) {
ConfigureCAN(dev_name);
} else {
ConfigureSerial(dev_name, baud_rate);
if (!serial_connected_)
std::cerr << "ERROR: Failed to connect to serial port" << std::endl;
}
}
void MobileBase::Disconnect() {
if (can_connected_) can_if_->StopService();
if (serial_connected_ && serial_if_->IsOpened()) {
serial_if_->StopService();
}
}
void MobileBase::Terminate() {
keep_running_ = false;
std::terminate();
}
void MobileBase::ConfigureCAN(const std::string &can_if_name) {
can_if_ = std::make_shared<AsyncCAN>(can_if_name);
can_if_->SetReceiveCallback(
std::bind(&MobileBase::ParseCANFrame, this, std::placeholders::_1));
can_if_->StartListening();
can_connected_ = true;
}
void MobileBase::ConfigureSerial(const std::string uart_name,
int32_t baud_rate) {
serial_if_ = std::make_shared<AsyncSerial>(uart_name, baud_rate);
serial_if_->SetReceiveCallback(
std::bind(&MobileBase::ParseUARTBuffer, this, std::placeholders::_1,
std::placeholders::_2, std::placeholders::_3));
serial_if_->StartListening();
if (serial_if_->IsOpened()) serial_connected_ = true;
}
void MobileBase::SetCmdTimeout(bool enable, uint32_t timeout_ms) {
enable_timeout_ = true;
timeout_ms_ = timeout_ms;
}
void MobileBase::StartCmdThread() {
keep_running_ = true;
cmd_thread_ = std::thread(
std::bind(&MobileBase::ControlLoop, this, cmd_thread_period_ms_));
cmd_thread_started_ = true;
}
void MobileBase::ControlLoop(int32_t period_ms) {
uint32_t timeout_iter_num;
if (enable_timeout_) {
if (timeout_ms_ < period_ms) timeout_ms_ = period_ms;
timeout_iter_num = static_cast<uint32_t>(timeout_ms_ / period_ms);
// std::cout << "Timeout iteration number: " << timeout_iter_num <<
// std::endl;
}
Timer tm;
while (keep_running_) {
tm.reset();
if (enable_timeout_) {
if (watchdog_counter_ < timeout_iter_num) {
SendRobotCmd();
++watchdog_counter_;
}
// else {
// std::cout << "Warning: cmd timeout, no cmd sent to robot" <<
// std::endl;
// }
} else {
SendRobotCmd();
}
tm.sleep_until_ms(period_ms);
}
}
} // namespace westonrobot

210
src/scout_base.cpp
View File

@@ -1,6 +1,5 @@
#include "ugv_sdk/scout/scout_base.hpp"
#include "ugv_sdk/scout_base.hpp"
#include <stdio.h>
#include <string>
#include <cstring>
#include <iostream>
@@ -13,219 +12,78 @@
#include "stopwatch.hpp"
#include "ugv_sdk/details/agilex_msg_parser.h"
namespace westonrobot {
void ScoutBase::SendRobotCmd() {
static uint8_t cmd_count = 0;
// EnableCommandedMode();
if (can_connected_) {
SendMotionCmd(cmd_count++);
}
}
void ScoutBase::EnableCommandedMode() {
AgxMessage c_msg;
c_msg.type = AgxMsgCtrlModeSelect;
memset(c_msg.body.ctrl_mode_select_msg.raw, 0, 8);
c_msg.body.ctrl_mode_select_msg.cmd.control_mode = CTRL_MODE_CMD_CAN;
// send to can bus
can_frame c_frame;
EncodeCanFrame(&c_msg, &c_frame);
can_if_->SendFrame(c_frame);
}
void ScoutBase::SendMotionCmd(uint8_t count) {
// motion control message
AgxMessage m_msg;
m_msg.type = AgxMsgMotionCommand;
memset(m_msg.body.motion_command_msg.raw, 0, 8);
motion_cmd_mutex_.lock();
int16_t linear_cmd =
static_cast<int16_t>(current_motion_cmd_.linear_velocity * 1000);
int16_t angular_cmd =
static_cast<int16_t>(current_motion_cmd_.angular_velocity * 1000);
motion_cmd_mutex_.unlock();
// SendControlCmd();
m_msg.body.motion_command_msg.cmd.linear_velocity.high_byte =
(static_cast<uint16_t>(linear_cmd) >> 8) & 0x00ff;
m_msg.body.motion_command_msg.cmd.linear_velocity.low_byte =
(static_cast<uint16_t>(linear_cmd) >> 0) & 0x00ff;
m_msg.body.motion_command_msg.cmd.angular_velocity.high_byte =
(static_cast<uint16_t>(angular_cmd) >> 8) & 0x00ff;
m_msg.body.motion_command_msg.cmd.angular_velocity.low_byte =
(static_cast<uint16_t>(angular_cmd) >> 0) & 0x00ff;
// send to can bus
can_frame m_frame;
EncodeCanFrame(&m_msg, &m_frame);
can_if_->SendFrame(m_frame);
}
void ScoutBase::SendLightCmd(const ScoutLightCmd &lcmd, uint8_t count) {
AgxMessage l_msg;
l_msg.type = AgxMsgLightCommand;
memset(l_msg.body.light_command_msg.raw, 0, 8);
if (lcmd.enable_ctrl) {
l_msg.body.light_command_msg.cmd.light_ctrl_enabled = LIGHT_CTRL_ENABLE;
l_msg.body.light_command_msg.cmd.front_light_mode =
static_cast<uint8_t>(lcmd.front_mode);
l_msg.body.light_command_msg.cmd.front_light_custom =
lcmd.front_custom_value;
l_msg.body.light_command_msg.cmd.rear_light_mode =
static_cast<uint8_t>(lcmd.rear_mode);
l_msg.body.light_command_msg.cmd.rear_light_custom = lcmd.rear_custom_value;
} else {
l_msg.body.light_command_msg.cmd.light_ctrl_enabled = LIGHT_CTRL_DISABLE;
}
l_msg.body.light_command_msg.cmd.count = count;
// send to can bus
can_frame l_frame;
EncodeCanFrame(&l_msg, &l_frame);
can_if_->SendFrame(l_frame);
}
ScoutState ScoutBase::GetScoutState() {
std::lock_guard<std::mutex> guard(scout_state_mutex_);
return scout_state_;
void ScoutBase::Connect(std::string dev_name) {
AgilexBase::Connect(dev_name, std::bind(&ScoutBase::ParseCANFrame, this,
std::placeholders::_1));
}
void ScoutBase::SetMotionCommand(double linear_vel, double angular_vel) {
// make sure cmd thread is started before attempting to send commands
if (!cmd_thread_started_) StartCmdThread();
if (linear_vel < ScoutMotionCmd::min_linear_velocity)
linear_vel = ScoutMotionCmd::min_linear_velocity;
if (linear_vel > ScoutMotionCmd::max_linear_velocity)
linear_vel = ScoutMotionCmd::max_linear_velocity;
if (angular_vel < ScoutMotionCmd::min_angular_velocity)
angular_vel = ScoutMotionCmd::min_angular_velocity;
if (angular_vel > ScoutMotionCmd::max_angular_velocity)
angular_vel = ScoutMotionCmd::max_angular_velocity;
std::lock_guard<std::mutex> guard(motion_cmd_mutex_);
current_motion_cmd_.linear_velocity = linear_vel;
current_motion_cmd_.angular_velocity = angular_vel;
FeedCmdTimeoutWatchdog();
AgilexBase::SetMotionCommand(linear_vel, angular_vel, 0.0, 0.0);
}
void ScoutBase::SetLightCommand(const ScoutLightCmd &cmd) {
static uint8_t light_cmd_count = 0;
SendLightCmd(cmd, light_cmd_count++);
if (cmd.cmd_ctrl_allowed) {
AgilexBase::SendLightCommand(cmd.front_mode, cmd.front_custom_value,
LightMode::CONST_OFF, 0);
}
}
ScoutState ScoutBase::GetScoutState() {
std::lock_guard<std::mutex> guard(state_mutex_);
return scout_state_;
}
void ScoutBase::ParseCANFrame(can_frame *rx_frame) {
AgxMessage status_msg;
DecodeCanFrame(rx_frame, &status_msg);
NewStatusMsgReceivedCallback(status_msg);
}
void ScoutBase::NewStatusMsgReceivedCallback(const AgxMessage &msg) {
// std::cout << "new status msg received" << std::endl;
std::lock_guard<std::mutex> guard(scout_state_mutex_);
UpdateScoutState(msg, scout_state_);
std::lock_guard<std::mutex> guard(state_mutex_);
UpdateScoutState(status_msg, scout_state_);
}
void ScoutBase::UpdateScoutState(const AgxMessage &status_msg,
ScoutState &state) {
switch (status_msg.type) {
case AgxMsgSystemState: {
// std::cout << "system status feedback received" << std::endl;
const SystemStateMessage &msg = status_msg.body.system_state_msg;
state.control_mode = msg.state.control_mode;
state.base_state = msg.state.vehicle_state;
state.battery_voltage =
(static_cast<uint16_t>(msg.state.battery_voltage.low_byte) |
static_cast<uint16_t>(msg.state.battery_voltage.high_byte) << 8) /
10.0;
state.fault_code = msg.state.fault_code;
// std::cout << "system status feedback received" << std::endl;
state.system_state = status_msg.body.system_state_msg;
break;
}
case AgxMsgMotionState: {
// std::cout << "motion control feedback received" << std::endl;
const MotionStateMessage &msg = status_msg.body.motion_state_msg;
state.linear_velocity =
static_cast<int16_t>(
static_cast<uint16_t>(msg.state.linear_velocity.low_byte) |
static_cast<uint16_t>(msg.state.linear_velocity.high_byte) << 8) /
1000.0;
state.angular_velocity =
static_cast<int16_t>(
static_cast<uint16_t>(msg.state.angular_velocity.low_byte) |
static_cast<uint16_t>(msg.state.angular_velocity.high_byte)
<< 8) /
1000.0;
state.motion_state = status_msg.body.motion_state_msg;
break;
}
case AgxMsgLightState: {
// std::cout << "light control feedback received" << std::endl;
const LightStateMessage &msg = status_msg.body.light_state_msg;
if (msg.state.light_ctrl_enabled == LIGHT_CTRL_DISABLE)
state.light_control_enabled = false;
else
state.light_control_enabled = true;
state.front_light_state.mode = msg.state.front_light_mode;
state.front_light_state.custom_value = msg.state.front_light_custom;
state.rear_light_state.mode = msg.state.rear_light_mode;
state.rear_light_state.custom_value = msg.state.rear_light_custom;
state.light_state = status_msg.body.light_state_msg;
break;
}
case AgxMsgRcState: {
state.rc_state = status_msg.body.rc_state_msg;
break;
}
case AgxMsgActuatorHSState: {
// std::cout << "actuator hs feedback received" << std::endl;
const ActuatorHSStateMessage &msg = status_msg.body.actuator_hs_state_msg;
state.actuator_states[msg.motor_id].motor_current =
(static_cast<uint16_t>(msg.data.state.current.low_byte) |
static_cast<uint16_t>(msg.data.state.current.high_byte) << 8) /
10.0;
state.actuator_states[msg.motor_id].motor_rpm = static_cast<int16_t>(
static_cast<uint16_t>(msg.data.state.rpm.low_byte) |
static_cast<uint16_t>(msg.data.state.rpm.high_byte) << 8);
state.actuator_states[msg.motor_id].motor_pulses = static_cast<int16_t>(
static_cast<uint16_t>(msg.data.state.pulse_count.low_byte) |
static_cast<uint16_t>(msg.data.state.pulse_count.high_byte) << 8);
state.actuator_hs_state[status_msg.body.actuator_hs_state_msg.motor_id] =
status_msg.body.actuator_hs_state_msg;
break;
}
case AgxMsgActuatorLSState: {
// std::cout << "actuator ls feedback received" << std::endl;
const ActuatorLSStateMessage &msg = status_msg.body.actuator_ls_state_msg;
for (int i = 0; i < 2; ++i) {
state.actuator_states[msg.motor_id].driver_voltage =
(static_cast<uint16_t>(msg.data.state.driver_voltage.low_byte) |
static_cast<uint16_t>(msg.data.state.driver_voltage.high_byte)
<< 8) /
10.0;
state.actuator_states[msg.motor_id]
.driver_temperature = static_cast<int16_t>(
static_cast<uint16_t>(msg.data.state.driver_temperature.low_byte) |
static_cast<uint16_t>(msg.data.state.driver_temperature.high_byte)
<< 8);
state.actuator_states[msg.motor_id].motor_temperature =
msg.data.state.motor_temperature;
state.actuator_states[msg.motor_id].driver_state =
msg.data.state.driver_state;
}
state.actuator_ls_state[status_msg.body.actuator_ls_state_msg.motor_id] =
status_msg.body.actuator_ls_state_msg;
break;
}
/* sensor feedback */
case AgxMsgOdometry: {
// std::cout << "Odometer msg feedback received" << std::endl;
const OdometryMessage &msg = status_msg.body.odometry_msg;
state.right_odometry = static_cast<int32_t>(
(static_cast<uint32_t>(msg.state.right_wheel.lsb)) |
(static_cast<uint32_t>(msg.state.right_wheel.low_byte) << 8) |
(static_cast<uint32_t>(msg.state.right_wheel.high_byte) << 16) |
(static_cast<uint32_t>(msg.state.right_wheel.msb) << 24));
state.left_odometry = static_cast<int32_t>(
(static_cast<uint32_t>(msg.state.left_wheel.lsb)) |
(static_cast<uint32_t>(msg.state.left_wheel.low_byte) << 8) |
(static_cast<uint32_t>(msg.state.left_wheel.high_byte) << 16) |
(static_cast<uint32_t>(msg.state.left_wheel.msb) << 24));
state.odometry = status_msg.body.odometry_msg;
}
default:
break;
}
}
} // namespace westonrobot

205
src/tracer_base.cpp
View File

@@ -1,4 +1,4 @@
#include "ugv_sdk/tracer/tracer_base.hpp"
#include "ugv_sdk/tracer_base.hpp"
#include <string>
#include <cstring>
@@ -12,216 +12,75 @@
#include "stopwatch.hpp"
#include "ugv_sdk/details/agilex_msg_parser.h"
namespace westonrobot {
void TracerBase::SendRobotCmd() {
static uint8_t cmd_count = 0;
// EnableCommandedMode();
if (can_connected_) {
SendMotionCmd(cmd_count++);
}
}
void TracerBase::EnableCommandedMode() {
AgxMessage c_msg;
c_msg.type = AgxMsgCtrlModeSelect;
memset(c_msg.body.ctrl_mode_select_msg.raw, 0, 8);
c_msg.body.ctrl_mode_select_msg.cmd.control_mode = CTRL_MODE_CMD_CAN;
// send to can bus
can_frame c_frame;
EncodeCanFrame(&c_msg, &c_frame);
can_if_->SendFrame(c_frame);
}
void TracerBase::SendMotionCmd(uint8_t count) {
// motion control message
AgxMessage m_msg;
m_msg.type = AgxMsgMotionCommand;
memset(m_msg.body.motion_command_msg.raw, 0, 8);
motion_cmd_mutex_.lock();
int16_t linear_cmd =
static_cast<int16_t>(current_motion_cmd_.linear_velocity * 1000);
int16_t angular_cmd =
static_cast<int16_t>(current_motion_cmd_.angular_velocity * 1000);
motion_cmd_mutex_.unlock();
// SendControlCmd();
m_msg.body.motion_command_msg.cmd.linear_velocity.high_byte =
(static_cast<uint16_t>(linear_cmd) >> 8) & 0x00ff;
m_msg.body.motion_command_msg.cmd.linear_velocity.low_byte =
(static_cast<uint16_t>(linear_cmd) >> 0) & 0x00ff;
m_msg.body.motion_command_msg.cmd.angular_velocity.high_byte =
(static_cast<uint16_t>(angular_cmd) >> 8) & 0x00ff;
m_msg.body.motion_command_msg.cmd.angular_velocity.low_byte =
(static_cast<uint16_t>(angular_cmd) >> 0) & 0x00ff;
// send to can bus
can_frame m_frame;
EncodeCanFrame(&m_msg, &m_frame);
can_if_->SendFrame(m_frame);
}
void TracerBase::SendLightCmd(const TracerLightCmd &lcmd, uint8_t count) {
AgxMessage l_msg;
l_msg.type = AgxMsgLightCommand;
memset(l_msg.body.light_command_msg.raw, 0, 8);
if (lcmd.enable_ctrl) {
l_msg.body.light_command_msg.cmd.light_ctrl_enabled = LIGHT_CTRL_ENABLE;
l_msg.body.light_command_msg.cmd.front_light_mode =
static_cast<uint8_t>(lcmd.front_mode);
l_msg.body.light_command_msg.cmd.front_light_custom =
lcmd.front_custom_value;
l_msg.body.light_command_msg.cmd.rear_light_mode =
static_cast<uint8_t>(lcmd.rear_mode);
l_msg.body.light_command_msg.cmd.rear_light_custom = lcmd.rear_custom_value;
} else {
l_msg.body.light_command_msg.cmd.light_ctrl_enabled = LIGHT_CTRL_DISABLE;
}
l_msg.body.light_command_msg.cmd.count = count;
// send to can bus
can_frame l_frame;
EncodeCanFrame(&l_msg, &l_frame);
can_if_->SendFrame(l_frame);
}
TracerState TracerBase::GetTracerState() {
std::lock_guard<std::mutex> guard(tracer_state_mutex_);
return tracer_state_;
void TracerBase::Connect(std::string dev_name) {
AgilexBase::Connect(dev_name, std::bind(&TracerBase::ParseCANFrame, this,
std::placeholders::_1));
}
void TracerBase::SetMotionCommand(double linear_vel, double angular_vel) {
// make sure cmd thread is started before attempting to send commands
if (!cmd_thread_started_) StartCmdThread();
if (linear_vel < TracerMotionCmd::min_linear_velocity)
linear_vel = TracerMotionCmd::min_linear_velocity;
if (linear_vel > TracerMotionCmd::max_linear_velocity)
linear_vel = TracerMotionCmd::max_linear_velocity;
if (angular_vel < TracerMotionCmd::min_angular_velocity)
angular_vel = TracerMotionCmd::min_angular_velocity;
if (angular_vel > TracerMotionCmd::max_angular_velocity)
angular_vel = TracerMotionCmd::max_angular_velocity;
std::lock_guard<std::mutex> guard(motion_cmd_mutex_);
current_motion_cmd_.linear_velocity = linear_vel;
current_motion_cmd_.angular_velocity = angular_vel;
FeedCmdTimeoutWatchdog();
AgilexBase::SetMotionCommand(linear_vel, angular_vel, 0.0, 0.0);
}
void TracerBase::SetLightCommand(const TracerLightCmd &cmd) {
static uint8_t light_cmd_count = 0;
SendLightCmd(cmd, light_cmd_count++);
if (cmd.cmd_ctrl_allowed) {
AgilexBase::SendLightCommand(cmd.front_mode, cmd.front_custom_value,
LightMode::CONST_OFF, 0);
}
}
TracerState TracerBase::GetTracerState() {
std::lock_guard<std::mutex> guard(state_mutex_);
return tracer_state_;
}
void TracerBase::ParseCANFrame(can_frame *rx_frame) {
AgxMessage status_msg;
DecodeCanFrame(rx_frame, &status_msg);
NewStatusMsgReceivedCallback(status_msg);
}
void TracerBase::NewStatusMsgReceivedCallback(const AgxMessage &msg) {
// std::cout << "new status msg received" << std::endl;
std::lock_guard<std::mutex> guard(tracer_state_mutex_);
UpdateTracerState(msg, tracer_state_);
std::lock_guard<std::mutex> guard(state_mutex_);
UpdateTracerState(status_msg, tracer_state_);
}
void TracerBase::UpdateTracerState(const AgxMessage &status_msg,
TracerState &state) {
switch (status_msg.type) {
case AgxMsgSystemState: {
// std::cout << "system status feedback received" << std::endl;
const SystemStateMessage &msg = status_msg.body.system_state_msg;
state.control_mode = msg.state.control_mode;
state.base_state = msg.state.vehicle_state;
state.battery_voltage =
(static_cast<uint16_t>(msg.state.battery_voltage.low_byte) |
static_cast<uint16_t>(msg.state.battery_voltage.high_byte) << 8) /
10.0;
state.fault_code = msg.state.fault_code;
// std::cout << "system status feedback received" << std::endl;
state.system_state = status_msg.body.system_state_msg;
break;
}
case AgxMsgMotionState: {
// std::cout << "motion control feedback received" << std::endl;
const MotionStateMessage &msg = status_msg.body.motion_state_msg;
state.linear_velocity =
static_cast<int16_t>(
static_cast<uint16_t>(msg.state.linear_velocity.low_byte) |
static_cast<uint16_t>(msg.state.linear_velocity.high_byte) << 8) /
1000.0;
state.angular_velocity =
static_cast<int16_t>(
static_cast<uint16_t>(msg.state.angular_velocity.low_byte) |
static_cast<uint16_t>(msg.state.angular_velocity.high_byte)
<< 8) /
1000.0;
state.motion_state = status_msg.body.motion_state_msg;
break;
}
case AgxMsgLightState: {
// std::cout << "light control feedback received" << std::endl;
const LightStateMessage &msg = status_msg.body.light_state_msg;
if (msg.state.light_ctrl_enabled == LIGHT_CTRL_DISABLE)
state.light_control_enabled = false;
else
state.light_control_enabled = true;
state.front_light_state.mode = msg.state.front_light_mode;
state.front_light_state.custom_value = msg.state.front_light_custom;
state.light_state = status_msg.body.light_state_msg;
break;
}
case AgxMsgRcState: {
state.rc_state = status_msg.body.rc_state_msg;
break;
}
case AgxMsgActuatorHSState: {
// std::cout << "actuator hs feedback received" << std::endl;
const ActuatorHSStateMessage &msg = status_msg.body.actuator_hs_state_msg;
state.actuator_states[msg.motor_id].motor_current =
(static_cast<uint16_t>(msg.data.state.current.low_byte) |
static_cast<uint16_t>(msg.data.state.current.high_byte) << 8) /
10.0;
state.actuator_states[msg.motor_id].motor_rpm = static_cast<int16_t>(
static_cast<uint16_t>(msg.data.state.rpm.low_byte) |
static_cast<uint16_t>(msg.data.state.rpm.high_byte) << 8);
state.actuator_states[msg.motor_id].motor_pulses = static_cast<int16_t>(
static_cast<uint16_t>(msg.data.state.pulse_count.low_byte) |
static_cast<uint16_t>(msg.data.state.pulse_count.high_byte) << 8);
state.actuator_hs_state[status_msg.body.actuator_hs_state_msg.motor_id] =
status_msg.body.actuator_hs_state_msg;
break;
}
case AgxMsgActuatorLSState: {
// std::cout << "actuator ls feedback received" << std::endl;
const ActuatorLSStateMessage &msg = status_msg.body.actuator_ls_state_msg;
for (int i = 0; i < 2; ++i) {
state.actuator_states[msg.motor_id].driver_voltage =
(static_cast<uint16_t>(msg.data.state.driver_voltage.low_byte) |
static_cast<uint16_t>(msg.data.state.driver_voltage.high_byte)
<< 8) /
10.0;
state.actuator_states[msg.motor_id]
.driver_temperature = static_cast<int16_t>(
static_cast<uint16_t>(msg.data.state.driver_temperature.low_byte) |
static_cast<uint16_t>(msg.data.state.driver_temperature.high_byte)
<< 8);
state.actuator_states[msg.motor_id].motor_temperature =
msg.data.state.motor_temperature;
state.actuator_states[msg.motor_id].driver_state =
msg.data.state.driver_state;
}
state.actuator_ls_state[status_msg.body.actuator_ls_state_msg.motor_id] =
status_msg.body.actuator_ls_state_msg;
break;
}
/* sensor feedback */
case AgxMsgOdometry: {
// std::cout << "Odometer msg feedback received" << std::endl;
const OdometryMessage &msg = status_msg.body.odometry_msg;
state.right_odometry = static_cast<int32_t>(
(static_cast<uint32_t>(msg.state.right_wheel.lsb)) |
(static_cast<uint32_t>(msg.state.right_wheel.low_byte) << 8) |
(static_cast<uint32_t>(msg.state.right_wheel.high_byte) << 16) |
(static_cast<uint32_t>(msg.state.right_wheel.msb) << 24));
state.left_odometry = static_cast<int32_t>(
(static_cast<uint32_t>(msg.state.left_wheel.lsb)) |
(static_cast<uint32_t>(msg.state.left_wheel.low_byte) << 8) |
(static_cast<uint32_t>(msg.state.left_wheel.high_byte) << 16) |
(static_cast<uint32_t>(msg.state.left_wheel.msb) << 24));
state.odometry = status_msg.body.odometry_msg;
}
}
}