From e7fdfd3e4abc97ab5ddc6ebbe5e57aef4f796be4 Mon Sep 17 00:00:00 2001 From: "pinloon.lee" Date: Wed, 31 Mar 2021 14:29:55 +0800 Subject: [PATCH 01/11] added in agx_protocol_v2 --- include/ugv_sdk/agilex_base.hpp | 103 ++++ include/ugv_sdk/agilex_message.h | 355 ++++++++++++ include/ugv_sdk/details/agilex_msg_parser.h | 41 ++ include/ugv_sdk/hunter/hunter_base.hpp | 66 --- include/ugv_sdk/hunter/hunter_types.hpp | 87 --- include/ugv_sdk/mobile_base.hpp | 88 --- include/ugv_sdk/proto/agx_msg_parser.h | 83 --- include/ugv_sdk/proto/agx_protocol_v2.h | 342 ------------ include/ugv_sdk/scout/scout_base.hpp | 60 --- include/ugv_sdk/scout/scout_types.hpp | 103 ---- include/ugv_sdk/scout_base.hpp | 83 +++ include/ugv_sdk/tracer/tracer_base.hpp | 59 -- include/ugv_sdk/tracer/tracer_types.hpp | 89 --- include/ugv_sdk/tracer_base.hpp | 78 +++ src/agilex_base.cpp | 155 ++++++ src/agx_msg_parser.c | 565 ++++++++++++++++---- src/agx_protocol_v2.h | 380 +++++++++++++ src/hunter_base.cpp | 239 --------- src/mobile_base.cpp | 107 ---- src/scout_base.cpp | 210 ++------ src/tracer_base.cpp | 205 ++----- 21 files changed, 1719 insertions(+), 1779 deletions(-) create mode 100644 include/ugv_sdk/agilex_base.hpp create mode 100644 include/ugv_sdk/agilex_message.h create mode 100644 include/ugv_sdk/details/agilex_msg_parser.h delete mode 100644 include/ugv_sdk/hunter/hunter_base.hpp delete mode 100644 include/ugv_sdk/hunter/hunter_types.hpp delete mode 100644 include/ugv_sdk/mobile_base.hpp delete mode 100644 include/ugv_sdk/proto/agx_msg_parser.h delete mode 100644 include/ugv_sdk/proto/agx_protocol_v2.h delete mode 100644 include/ugv_sdk/scout/scout_base.hpp delete mode 100644 include/ugv_sdk/scout/scout_types.hpp create mode 100644 include/ugv_sdk/scout_base.hpp delete mode 100644 include/ugv_sdk/tracer/tracer_base.hpp delete mode 100644 include/ugv_sdk/tracer/tracer_types.hpp create mode 100644 include/ugv_sdk/tracer_base.hpp create mode 100644 src/agilex_base.cpp create mode 100644 src/agx_protocol_v2.h delete mode 100644 src/hunter_base.cpp delete mode 100644 src/mobile_base.cpp diff --git a/include/ugv_sdk/agilex_base.hpp b/include/ugv_sdk/agilex_base.hpp new file mode 100644 index 0000000..8d9cf5b --- /dev/null +++ b/include/ugv_sdk/agilex_base.hpp @@ -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 +#include +#include +#include +#include + +#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 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 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 */ diff --git a/include/ugv_sdk/agilex_message.h b/include/ugv_sdk/agilex_message.h new file mode 100644 index 0000000..10e99a6 --- /dev/null +++ b/include/ugv_sdk/agilex_message.h @@ -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 +#include +#include + +/***************** 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 */ diff --git a/include/ugv_sdk/details/agilex_msg_parser.h b/include/ugv_sdk/details/agilex_msg_parser.h new file mode 100644 index 0000000..0c12a27 --- /dev/null +++ b/include/ugv_sdk/details/agilex_msg_parser.h @@ -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 +#include +#include + +#ifdef __linux__ +#include +#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 */ diff --git a/include/ugv_sdk/hunter/hunter_base.hpp b/include/ugv_sdk/hunter/hunter_base.hpp deleted file mode 100644 index 55b2628..0000000 --- a/include/ugv_sdk/hunter/hunter_base.hpp +++ /dev/null @@ -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 -#include -#include -#include - -#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 */ diff --git a/include/ugv_sdk/hunter/hunter_types.hpp b/include/ugv_sdk/hunter/hunter_types.hpp deleted file mode 100644 index 28d9350..0000000 --- a/include/ugv_sdk/hunter/hunter_types.hpp +++ /dev/null @@ -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 -#include - -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 */ diff --git a/include/ugv_sdk/mobile_base.hpp b/include/ugv_sdk/mobile_base.hpp deleted file mode 100644 index e12668e..0000000 --- a/include/ugv_sdk/mobile_base.hpp +++ /dev/null @@ -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 -#include -#include -#include -#include - -#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 can_if_; - std::shared_ptr 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 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 */ diff --git a/include/ugv_sdk/proto/agx_msg_parser.h b/include/ugv_sdk/proto/agx_msg_parser.h deleted file mode 100644 index b654e4e..0000000 --- a/include/ugv_sdk/proto/agx_msg_parser.h +++ /dev/null @@ -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 -#include -#include - -#include "ugv_sdk/proto/agx_protocol_v2.h" - -#ifdef __linux__ -#include -#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 */ diff --git a/include/ugv_sdk/proto/agx_protocol_v2.h b/include/ugv_sdk/proto/agx_protocol_v2.h deleted file mode 100644 index 02e3c33..0000000 --- a/include/ugv_sdk/proto/agx_protocol_v2.h +++ /dev/null @@ -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 - -/*---------------------------- 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 */ diff --git a/include/ugv_sdk/scout/scout_base.hpp b/include/ugv_sdk/scout/scout_base.hpp deleted file mode 100644 index 0d2ac4c..0000000 --- a/include/ugv_sdk/scout/scout_base.hpp +++ /dev/null @@ -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 -#include -#include -#include -#include - -#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 */ diff --git a/include/ugv_sdk/scout/scout_types.hpp b/include/ugv_sdk/scout/scout_types.hpp deleted file mode 100644 index d9b6b29..0000000 --- a/include/ugv_sdk/scout/scout_types.hpp +++ /dev/null @@ -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 -#include - -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 */ diff --git a/include/ugv_sdk/scout_base.hpp b/include/ugv_sdk/scout_base.hpp new file mode 100644 index 0000000..2ca10ae --- /dev/null +++ b/include/ugv_sdk/scout_base.hpp @@ -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 +#include +#include +#include + +#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 */ diff --git a/include/ugv_sdk/tracer/tracer_base.hpp b/include/ugv_sdk/tracer/tracer_base.hpp deleted file mode 100644 index 2f23db4..0000000 --- a/include/ugv_sdk/tracer/tracer_base.hpp +++ /dev/null @@ -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 -#include -#include -#include - -#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 */ diff --git a/include/ugv_sdk/tracer/tracer_types.hpp b/include/ugv_sdk/tracer/tracer_types.hpp deleted file mode 100644 index 6a8e2c8..0000000 --- a/include/ugv_sdk/tracer/tracer_types.hpp +++ /dev/null @@ -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 -#include - -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 */ diff --git a/include/ugv_sdk/tracer_base.hpp b/include/ugv_sdk/tracer_base.hpp new file mode 100644 index 0000000..ac60d64 --- /dev/null +++ b/include/ugv_sdk/tracer_base.hpp @@ -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 +#include +#include +#include + +#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 */ diff --git a/src/agilex_base.cpp b/src/agilex_base.cpp new file mode 100644 index 0000000..f73420a --- /dev/null +++ b/src/agilex_base.cpp @@ -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(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(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 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 \ No newline at end of file diff --git a/src/agx_msg_parser.c b/src/agx_msg_parser.c index 3077aed..7dce7c3 100644 --- a/src/agx_msg_parser.c +++ b/src/agx_msg_parser.c @@ -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) { diff --git a/src/agx_protocol_v2.h b/src/agx_protocol_v2.h new file mode 100644 index 0000000..03df225 --- /dev/null +++ b/src/agx_protocol_v2.h @@ -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 + +// 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 */ diff --git a/src/hunter_base.cpp b/src/hunter_base.cpp deleted file mode 100644 index 5fc1cf0..0000000 --- a/src/hunter_base.cpp +++ /dev/null @@ -1,239 +0,0 @@ -#include "ugv_sdk/hunter/hunter_base.hpp" - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#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 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 guard(motion_cmd_mutex_); - current_motion_cmd_.linear_velocity_height_byte = - static_cast(linear_vel * 1000) >> 8; - current_motion_cmd_.linear_velocity_low_byte = - static_cast(linear_vel * 1000) & 0xff; - current_motion_cmd_.angular_velocity_height_byte = - static_cast(angular_vel * 1000) >> 8; - current_motion_cmd_.angular_velocity_low_byte = - static_cast(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 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( - static_cast(msg.data.status.linear_velocity.low_byte) | - static_cast(msg.data.status.linear_velocity.high_byte) - << 8) / - 1000.0; - state.steering_angle = - static_cast( - static_cast(msg.data.status.angular_velocity.low_byte) | - static_cast(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(msg.data.status.battery_voltage.low_byte) | - static_cast(msg.data.status.battery_voltage.high_byte) - << 8) / - 10.0; - state.fault_code = - (static_cast(msg.data.status.fault_code.low_byte) | - static_cast(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(msg.data.status.current.low_byte) | - static_cast(msg.data.status.current.high_byte) << 8) / - 10.0; - state.motor_hs_state[msg.motor_id].rpm = static_cast( - static_cast(msg.data.status.rpm.low_byte) | - static_cast(msg.data.status.rpm.high_byte) << 8); - state.motor_hs_state[msg.motor_id].motor_pose = static_cast( - static_cast(msg.data.status.moter_pose.lowest) | - static_cast(msg.data.status.moter_pose.sec_lowest) << 8 | - static_cast(msg.data.status.moter_pose.sec_heighest) - << 16 | - static_cast(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(msg.data.status.driver_voltage.low_byte) | - static_cast(msg.data.status.driver_voltage.high_byte) - << 8) / - 10.0; - state.motor_ls_state[msg.motor_id] - .driver_temperature = static_cast( - static_cast(msg.data.status.driver_temperature.low_byte) | - static_cast(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 diff --git a/src/mobile_base.cpp b/src/mobile_base.cpp deleted file mode 100644 index 7394ca8..0000000 --- a/src/mobile_base.cpp +++ /dev/null @@ -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 -#include -#include - -#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(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(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(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 diff --git a/src/scout_base.cpp b/src/scout_base.cpp index c823c15..4799f26 100644 --- a/src/scout_base.cpp +++ b/src/scout_base.cpp @@ -1,6 +1,5 @@ -#include "ugv_sdk/scout/scout_base.hpp" +#include "ugv_sdk/scout_base.hpp" -#include #include #include #include @@ -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(current_motion_cmd_.linear_velocity * 1000); - int16_t angular_cmd = - static_cast(current_motion_cmd_.angular_velocity * 1000); - motion_cmd_mutex_.unlock(); - - // SendControlCmd(); - m_msg.body.motion_command_msg.cmd.linear_velocity.high_byte = - (static_cast(linear_cmd) >> 8) & 0x00ff; - m_msg.body.motion_command_msg.cmd.linear_velocity.low_byte = - (static_cast(linear_cmd) >> 0) & 0x00ff; - m_msg.body.motion_command_msg.cmd.angular_velocity.high_byte = - (static_cast(angular_cmd) >> 8) & 0x00ff; - m_msg.body.motion_command_msg.cmd.angular_velocity.low_byte = - (static_cast(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(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(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 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 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 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 guard(scout_state_mutex_); - UpdateScoutState(msg, scout_state_); + std::lock_guard 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(msg.state.battery_voltage.low_byte) | - static_cast(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( - static_cast(msg.state.linear_velocity.low_byte) | - static_cast(msg.state.linear_velocity.high_byte) << 8) / - 1000.0; - state.angular_velocity = - static_cast( - static_cast(msg.state.angular_velocity.low_byte) | - static_cast(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(msg.data.state.current.low_byte) | - static_cast(msg.data.state.current.high_byte) << 8) / - 10.0; - state.actuator_states[msg.motor_id].motor_rpm = static_cast( - static_cast(msg.data.state.rpm.low_byte) | - static_cast(msg.data.state.rpm.high_byte) << 8); - state.actuator_states[msg.motor_id].motor_pulses = static_cast( - static_cast(msg.data.state.pulse_count.low_byte) | - static_cast(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(msg.data.state.driver_voltage.low_byte) | - static_cast(msg.data.state.driver_voltage.high_byte) - << 8) / - 10.0; - state.actuator_states[msg.motor_id] - .driver_temperature = static_cast( - static_cast(msg.data.state.driver_temperature.low_byte) | - static_cast(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( - (static_cast(msg.state.right_wheel.lsb)) | - (static_cast(msg.state.right_wheel.low_byte) << 8) | - (static_cast(msg.state.right_wheel.high_byte) << 16) | - (static_cast(msg.state.right_wheel.msb) << 24)); - state.left_odometry = static_cast( - (static_cast(msg.state.left_wheel.lsb)) | - (static_cast(msg.state.left_wheel.low_byte) << 8) | - (static_cast(msg.state.left_wheel.high_byte) << 16) | - (static_cast(msg.state.left_wheel.msb) << 24)); + state.odometry = status_msg.body.odometry_msg; } + default: + break; } } } // namespace westonrobot diff --git a/src/tracer_base.cpp b/src/tracer_base.cpp index f22ed15..3041d72 100644 --- a/src/tracer_base.cpp +++ b/src/tracer_base.cpp @@ -1,4 +1,4 @@ -#include "ugv_sdk/tracer/tracer_base.hpp" +#include "ugv_sdk/tracer_base.hpp" #include #include @@ -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(current_motion_cmd_.linear_velocity * 1000); - int16_t angular_cmd = - static_cast(current_motion_cmd_.angular_velocity * 1000); - motion_cmd_mutex_.unlock(); - - // SendControlCmd(); - m_msg.body.motion_command_msg.cmd.linear_velocity.high_byte = - (static_cast(linear_cmd) >> 8) & 0x00ff; - m_msg.body.motion_command_msg.cmd.linear_velocity.low_byte = - (static_cast(linear_cmd) >> 0) & 0x00ff; - m_msg.body.motion_command_msg.cmd.angular_velocity.high_byte = - (static_cast(angular_cmd) >> 8) & 0x00ff; - m_msg.body.motion_command_msg.cmd.angular_velocity.low_byte = - (static_cast(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(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(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 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 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 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 guard(tracer_state_mutex_); - UpdateTracerState(msg, tracer_state_); + std::lock_guard 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(msg.state.battery_voltage.low_byte) | - static_cast(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( - static_cast(msg.state.linear_velocity.low_byte) | - static_cast(msg.state.linear_velocity.high_byte) << 8) / - 1000.0; - state.angular_velocity = - static_cast( - static_cast(msg.state.angular_velocity.low_byte) | - static_cast(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(msg.data.state.current.low_byte) | - static_cast(msg.data.state.current.high_byte) << 8) / - 10.0; - state.actuator_states[msg.motor_id].motor_rpm = static_cast( - static_cast(msg.data.state.rpm.low_byte) | - static_cast(msg.data.state.rpm.high_byte) << 8); - state.actuator_states[msg.motor_id].motor_pulses = static_cast( - static_cast(msg.data.state.pulse_count.low_byte) | - static_cast(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(msg.data.state.driver_voltage.low_byte) | - static_cast(msg.data.state.driver_voltage.high_byte) - << 8) / - 10.0; - state.actuator_states[msg.motor_id] - .driver_temperature = static_cast( - static_cast(msg.data.state.driver_temperature.low_byte) | - static_cast(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( - (static_cast(msg.state.right_wheel.lsb)) | - (static_cast(msg.state.right_wheel.low_byte) << 8) | - (static_cast(msg.state.right_wheel.high_byte) << 16) | - (static_cast(msg.state.right_wheel.msb) << 24)); - state.left_odometry = static_cast( - (static_cast(msg.state.left_wheel.lsb)) | - (static_cast(msg.state.left_wheel.low_byte) << 8) | - (static_cast(msg.state.left_wheel.high_byte) << 16) | - (static_cast(msg.state.left_wheel.msb) << 24)); + state.odometry = status_msg.body.odometry_msg; } } } From 91836551febe05e280e886f2c117cfe070c261ca Mon Sep 17 00:00:00 2001 From: "pinloon.lee" Date: Wed, 31 Mar 2021 14:30:47 +0800 Subject: [PATCH 02/11] replaced wrp_io with async_port --- CMakeLists.txt | 10 ++++++---- package.xml | 4 ++-- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index efaf17d..bf39434 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -67,7 +67,9 @@ if(BUILD_WITHOUT_ROS) set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/${CMAKE_INSTALL_BINDIR}) endif() -if(NOT BUILD_WITHOUT_ROS) +if(BUILD_WITHOUT_ROS) + find_package(async_port REQUIRED) +else() find_package(catkin REQUIRED COMPONENTS async_port) include_directories( include @@ -80,7 +82,7 @@ find_package(Threads REQUIRED) add_library(${PROJECT_NAME} # agx common - src/mobile_base.cpp + src/agilex_base.cpp src/agx_msg_parser.c # robot support src/scout_base.cpp @@ -89,7 +91,7 @@ add_library(${PROJECT_NAME} # src/bunker_base.cpp ) if(BUILD_WITHOUT_ROS) - target_link_libraries(${PROJECT_NAME} wrp_io Threads::Threads) + target_link_libraries(${PROJECT_NAME} westonrobot::async_port Threads::Threads) else() target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} Threads::Threads) endif() @@ -195,7 +197,7 @@ else() # BUILD_WITHOUT_ROS catkin_package( LIBRARIES ${PROJECT_NAME} INCLUDE_DIRS include - CATKIN_DEPENDS wrp_io + CATKIN_DEPENDS async_port # DEPENDS system_lib ) diff --git a/package.xml b/package.xml index 6b8ca5d..e4f2f28 100644 --- a/package.xml +++ b/package.xml @@ -14,7 +14,7 @@ https://github.com/westonrobot/ugv_sdk catkin - wrp_io - wrp_io + async_port + async_port From 062f08f8b056327e1f2d924f31c44006985d1a19 Mon Sep 17 00:00:00 2001 From: "pinloon.lee" Date: Wed, 31 Mar 2021 15:51:13 +0800 Subject: [PATCH 03/11] updated some path for app and demo for scout and tracer --- apps/hunter_demo/hunter_demo.cpp | 2 +- apps/scout_demo/scout_demo.cpp | 65 +++++++++---------- apps/scout_demo/scout_discharge.cpp | 25 +++---- .../include/monitor/scout_monitor.hpp | 2 +- apps/tracer_demo/tracer_demo.cpp | 21 +++--- 5 files changed, 52 insertions(+), 63 deletions(-) diff --git a/apps/hunter_demo/hunter_demo.cpp b/apps/hunter_demo/hunter_demo.cpp index 7c9ff2b..bd8d1c7 100644 --- a/apps/hunter_demo/hunter_demo.cpp +++ b/apps/hunter_demo/hunter_demo.cpp @@ -7,7 +7,7 @@ * Copyright (c) 2019 Ruixiang Du (rdu) */ -#include "ugv_sdk/hunter/hunter_base.hpp" +#include "ugv_sdk/hunter_base.hpp" using namespace westonrobot; diff --git a/apps/scout_demo/scout_demo.cpp b/apps/scout_demo/scout_demo.cpp index 97e4e38..8a16a8c 100644 --- a/apps/scout_demo/scout_demo.cpp +++ b/apps/scout_demo/scout_demo.cpp @@ -7,51 +7,46 @@ * Copyright (c) 2019 Ruixiang Du (rdu) */ -#include "ugv_sdk/scout/scout_base.hpp" +#include "ugv_sdk/scout_base.hpp" using namespace westonrobot; int main(int argc, char **argv) { std::string device_name; - int32_t baud_rate = 0; if (argc == 2) { device_name = {argv[1]}; std::cout << "Specified CAN: " << device_name << std::endl; - } else if (argc == 3) { - device_name = {argv[1]}; - baud_rate = std::stol(argv[2]); - std::cout << "Specified serial: " << device_name << "@" << baud_rate - << std::endl; } else { std::cout << "Usage: app_scout_demo " << std::endl - << "Example 1: ./app_scout_demo can0" << std::endl - << "Example 2: ./app_scout_demo /dev/ttyUSB0 115200" << std::endl; + << "Example 1: ./app_scout_demo can0" << std::endl; return -1; } ScoutBase scout; - scout.Connect(device_name, baud_rate); + scout.Connect(device_name); + + scout.EnableCommandedMode(); // // light control -// std::cout << "Light: const off" << std::endl; -// scout.SetLightCommand({ScoutLightCmd::LightMode::CONST_OFF, 0, -// ScoutLightCmd::LightMode::CONST_OFF, 0}); -// sleep(3); -// std::cout << "Light: const on" << std::endl; -// scout.SetLightCommand({ScoutLightCmd::LightMode::CONST_ON, 0, -// ScoutLightCmd::LightMode::CONST_ON, 0}); -// sleep(3); -// std::cout << "Light: breath" << std::endl; -// scout.SetLightCommand({ScoutLightCmd::LightMode::BREATH, 0, -// ScoutLightCmd::LightMode::BREATH, 0}); -// sleep(3); -// std::cout << "Light: custom 90-80" << std::endl; -// scout.SetLightCommand({ScoutLightCmd::LightMode::CUSTOM, 90, -// ScoutLightCmd::LightMode::CUSTOM, 80}); -// sleep(3); -// std::cout << "Light: diabled cmd control" << std::endl; -// scout.DisableLightCmdControl(); + std::cout << "Light: const off" << std::endl; + scout.SetLightCommand({CONST_OFF, 0, + CONST_OFF, 0}); + sleep(3); + std::cout << "Light: const on" << std::endl; + scout.SetLightCommand({CONST_ON, 0, + CONST_ON, 0}); + sleep(3); + std::cout << "Light: breath" << std::endl; + scout.SetLightCommand({BREATH, 0, + BREATH, 0}); + sleep(3); + std::cout << "Light: custom 90-80" << std::endl; + scout.SetLightCommand({CUSTOM, 90, + CUSTOM, 80}); + sleep(3); + std::cout << "Light: diabled cmd control" << std::endl; + scout.DisableLightControl(); int count = 0; while (true) { @@ -95,8 +90,8 @@ int main(int argc, char **argv) { // { // std::cout << "Motor: 0.0, 0, Light: breath" << std::endl; // scout.SetMotionCommand(0.0, 0.0); - // scout.SetLightCommand({ScoutLightCmd::LightMode::BREATH, 0, - // ScoutLightCmd::LightMode::BREATH, 0}); + // scout.SetLightCommand({BREATH, 0, + // BREATH, 0}); // } if (count < 100) { std::cout << "Motor: 0.2, 0" << std::endl; @@ -106,12 +101,12 @@ int main(int argc, char **argv) { auto state = scout.GetScoutState(); std::cout << "-------------------------------" << std::endl; std::cout << "count: " << count << std::endl; - std::cout << "control mode: " << static_cast(state.control_mode) - << " , base state: " << static_cast(state.base_state) + std::cout << "control mode: " << static_cast(state.system_state.control_mode) + << " , vehicle state: " << static_cast(state.system_state.vehicle_state) << std::endl; - std::cout << "battery voltage: " << state.battery_voltage << std::endl; - std::cout << "velocity (linear, angular): " << state.linear_velocity << ", " - << state.angular_velocity << std::endl; + std::cout << "battery voltage: " << state.system_state.battery_voltage << std::endl; + std::cout << "velocity (linear, angular): " << state.motion_state.linear_velocity << ", " + << state.motion_state.angular_velocity << std::endl; std::cout << "-------------------------------" << std::endl; usleep(20000); diff --git a/apps/scout_demo/scout_discharge.cpp b/apps/scout_demo/scout_discharge.cpp index 03e5728..185b460 100644 --- a/apps/scout_demo/scout_discharge.cpp +++ b/apps/scout_demo/scout_discharge.cpp @@ -7,26 +7,19 @@ * Copyright (c) 2019 Ruixiang Du (rdu) */ -#include "scout_base/scout_base.hpp" +#include "ugv_sdk/scout_base.hpp" using namespace westonrobot; int main(int argc, char **argv) { std::string device_name; - int32_t baud_rate = 0; if (argc == 2) { device_name = {argv[1]}; std::cout << "Specified CAN: " << device_name << std::endl; } - else if (argc == 3) - { - device_name = {argv[1]}; - baud_rate = std::stol(argv[2]); - std::cout << "Specified serial: " << device_name << "@" << baud_rate << std::endl; - } else { std::cout << "Usage: app_scout_monitor " << std::endl @@ -36,34 +29,36 @@ int main(int argc, char **argv) } ScoutBase scout; - scout.Connect(device_name, baud_rate); + scout.Connect(device_name); + + scout.EnableCommandedMode(); // light control std::cout << "Light: const on" << std::endl; - scout.SetLightCommand({ScoutLightCmd::LightMode::CONST_ON, 0, ScoutLightCmd::LightMode::CONST_ON, 0}); + scout.SetLightCommand({CONST_ON, 0, CONST_ON, 0}); int count = 0; while (true) { auto state = scout.GetScoutState(); - if (state.battery_voltage >= 22.5) + if (state.system_state.battery_voltage >= 22.5) { scout.SetMotionCommand(1.35, 0); std::cout << "-------------------------------" << std::endl; std::cout << "-------> discharging <--------" << std::endl; std::cout << "elapsed time: " << count / 60 << " minutes " << count % 60 << " seconds" << std::endl; - std::cout << "control mode: " << static_cast(state.control_mode) << " , base state: " << static_cast(state.base_state) << std::endl; - std::cout << "battery voltage: " << state.battery_voltage << std::endl; - std::cout << "velocity (linear, angular): " << state.linear_velocity << ", " << state.angular_velocity << std::endl; + std::cout << "control mode: " << static_cast(state.system_state.control_mode) << " , vehicle state: " << static_cast(state.system_state.vehicle_state) << std::endl; + std::cout << "battery voltage: " << state.system_state.battery_voltage << std::endl; + std::cout << "velocity (linear, angular): " << state.motion_state.linear_velocity << ", " << state.motion_state.angular_velocity << std::endl; std::cout << "-------------------------------" << std::endl; } else { scout.SetMotionCommand(0, 0); std::cout << "-------------------------------" << std::endl; - std::cout << "discharge stopped at: " << state.battery_voltage << " V" << std::endl; + std::cout << "discharge stopped at: " << state.system_state.battery_voltage << " V" << std::endl; std::cout << "-------------------------------" << std::endl; } sleep(1); diff --git a/apps/scout_monitor/include/monitor/scout_monitor.hpp b/apps/scout_monitor/include/monitor/scout_monitor.hpp index c6fd3be..0916a33 100644 --- a/apps/scout_monitor/include/monitor/scout_monitor.hpp +++ b/apps/scout_monitor/include/monitor/scout_monitor.hpp @@ -10,7 +10,7 @@ #ifndef SCOUT_MONITOR_HPP #define SCOUT_MONITOR_HPP -#include "ugv_sdk/scout/scout_base.hpp" +#include "ugv_sdk/scout_base.hpp" #include diff --git a/apps/tracer_demo/tracer_demo.cpp b/apps/tracer_demo/tracer_demo.cpp index 5917473..8bb5c9a 100644 --- a/apps/tracer_demo/tracer_demo.cpp +++ b/apps/tracer_demo/tracer_demo.cpp @@ -7,13 +7,12 @@ * Copyright (c) 2019 Ruixiang Du (rdu) */ -#include "ugv_sdk/tracer/tracer_base.hpp" +#include "ugv_sdk/tracer_base.hpp" using namespace westonrobot; int main(int argc, char **argv) { std::string device_name; - int32_t baud_rate = 0; if (argc == 2) { device_name = {argv[1]}; @@ -32,19 +31,19 @@ int main(int argc, char **argv) { // light control std::cout << "Light: const off" << std::endl; - tracer.SetLightCommand({TracerLightCmd::LightMode::CONST_OFF, 0}); + tracer.SetLightCommand({CONST_OFF, 0}); // usleep(50000); sleep(3); std::cout << "Light: const on" << std::endl; - tracer.SetLightCommand({TracerLightCmd::LightMode::CONST_ON, 0}); + tracer.SetLightCommand({CONST_ON, 0}); // usleep(50000); sleep(3); std::cout << "Light: breath" << std::endl; - tracer.SetLightCommand({TracerLightCmd::LightMode::BREATH, 0}); + tracer.SetLightCommand({BREATH, 0}); // usleep(50000); sleep(8); std::cout << "Light: custom 90-80" << std::endl; - tracer.SetLightCommand({TracerLightCmd::LightMode::CUSTOM, 90}); + tracer.SetLightCommand({CUSTOM, 90}); // usleep(50000); sleep(3); std::cout << "Light: diabled cmd control" << std::endl; @@ -83,12 +82,12 @@ int main(int argc, char **argv) { auto state = tracer.GetTracerState(); std::cout << "-------------------------------" << std::endl; std::cout << "count: " << count << std::endl; - std::cout << "control mode: " << static_cast(state.control_mode) - << " , base state: " << static_cast(state.base_state) + std::cout << "control mode: " << static_cast(state.system_state.control_mode) + << " , vehicle state: " << static_cast(state.system_state.vehicle_state) << std::endl; - std::cout << "battery voltage: " << state.battery_voltage << std::endl; - std::cout << "velocity (linear, angular): " << state.linear_velocity << ", " - << state.angular_velocity << std::endl; + std::cout << "battery voltage: " << state.system_state.battery_voltage << std::endl; + std::cout << "velocity (linear, angular): " << state.motion_state.linear_velocity << ", " + << state.motion_state.angular_velocity << std::endl; std::cout << "-------------------------------" << std::endl; // usleep(20000); From 5e5c4ec8844e710fb3d16ecdb2cddc3ecdb4e8db Mon Sep 17 00:00:00 2001 From: "pinloon.lee" Date: Wed, 31 Mar 2021 16:01:07 +0800 Subject: [PATCH 04/11] fixed light command ctrl constant --- src/agx_protocol_v2.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/agx_protocol_v2.h b/src/agx_protocol_v2.h index 03df225..afbd522 100644 --- a/src/agx_protocol_v2.h +++ b/src/agx_protocol_v2.h @@ -98,8 +98,8 @@ extern "C" { /*--------------------- Control/State Constants ------------------------*/ -#define LIGHT_CMD_CTRL_ALLOWED ((uint8_t)0x00) -#define LIGHT_CMD_CTRL_DISALLOWED ((uint8_t)0x01) +#define LIGHT_CMD_CTRL_ALLOWED ((uint8_t)0x01) +#define LIGHT_CMD_CTRL_DISALLOWED ((uint8_t)0x00) #define VERSION_REQUEST_VALUE ((uint8_t)0x01) #define STEER_NEUTRAL_REQUEST_VALUE ((uint8_t)0xee) From 91e391c8ddbb4fb65825a0829a083aef1e6243c1 Mon Sep 17 00:00:00 2001 From: "pinloon.lee" Date: Wed, 31 Mar 2021 16:13:24 +0800 Subject: [PATCH 05/11] updated ci for kinetic-catkin-build --- .gitlab-ci.yml | 21 +++++++++------------ 1 file changed, 9 insertions(+), 12 deletions(-) diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index f29298e..9d1d54c 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -3,7 +3,6 @@ xenial-cpp-build: stage: build image: rduweston/ubuntu-ci:16.04 script: - - git submodule update --init --recursive - mkdir build && cd build - cmake -DCMAKE_BUILD_TYPE=Release .. - cmake --build . && cpack @@ -12,7 +11,6 @@ bionic-cpp-build: stage: build image: rduweston/ubuntu-ci:18.04 script: - - git submodule update --init --recursive - mkdir build && cd build - cmake -DCMAKE_BUILD_TYPE=Release .. - cmake --build . && cpack @@ -21,21 +19,20 @@ focal-cpp-build: stage: build image: rduweston/ubuntu-ci:20.04 script: - - git submodule update --init --recursive - mkdir build && cd build - cmake -DCMAKE_BUILD_TYPE=Release .. - cmake --build . && cpack ## Build and test as catkin package -# kinetic-catkin-build: -# stage: build -# image: rduweston/ubuntu-ci:kinetic -# script: -# - mkdir -p /catkin_ws/src && cd /catkin_ws/src && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_init_workspace" -# - cp -r /builds/$CI_PROJECT_PATH /catkin_ws/src/wrp_sdk -# - cd /catkin_ws/src/wrp_sdk && git submodule update --init --recursive -# - cd /catkin_ws/src && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/ros/scout_base.git -# - cd /catkin_ws && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_make" +kinetic-catkin-build: + stage: build + image: rduweston/ubuntu-ci:kinetic + script: + - mkdir -p /catkin_ws/src/ugv_sdk && cd /catkin_ws/src && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_init_workspace" + - cp -r /builds/$CI_PROJECT_PATH /catkin_ws/src/ugv_sdk + - cd /catkin_ws/src/ugv_sdk + - cd /catkin_ws/src && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/public/async_port.git + - cd /catkin_ws && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_make" # kinetic-catkin-isolated-build: # stage: build From 8207428af605f857be26b8d4b680a0987222583a Mon Sep 17 00:00:00 2001 From: "pinloon.lee" Date: Wed, 31 Mar 2021 16:27:15 +0800 Subject: [PATCH 06/11] updated ci for xenial-cpp --- .gitlab-ci.yml | 33 +++++++++++++++++++-------------- 1 file changed, 19 insertions(+), 14 deletions(-) diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index 9d1d54c..2994d94 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -3,25 +3,30 @@ xenial-cpp-build: stage: build image: rduweston/ubuntu-ci:16.04 script: + - cd / && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/public/async_port.git + - cd /async_port && mkdir build && cd build + - cmake .. && cpack + - dkpg async_port_0.1.0_amd64.deb + - cd /ugv_sdk - mkdir build && cd build - cmake -DCMAKE_BUILD_TYPE=Release .. - cmake --build . && cpack -bionic-cpp-build: - stage: build - image: rduweston/ubuntu-ci:18.04 - script: - - mkdir build && cd build - - cmake -DCMAKE_BUILD_TYPE=Release .. - - cmake --build . && cpack +# bionic-cpp-build: +# stage: build +# image: rduweston/ubuntu-ci:18.04 +# script: +# - mkdir build && cd build +# - cmake -DCMAKE_BUILD_TYPE=Release .. +# - cmake --build . && cpack -focal-cpp-build: - stage: build - image: rduweston/ubuntu-ci:20.04 - script: - - mkdir build && cd build - - cmake -DCMAKE_BUILD_TYPE=Release .. - - cmake --build . && cpack +# focal-cpp-build: +# stage: build +# image: rduweston/ubuntu-ci:20.04 +# script: +# - mkdir build && cd build +# - cmake -DCMAKE_BUILD_TYPE=Release .. +# - cmake --build . && cpack ## Build and test as catkin package kinetic-catkin-build: From b207caa1f185ed724f599dc16379317d8579e111 Mon Sep 17 00:00:00 2001 From: "pinloon.lee" Date: Wed, 31 Mar 2021 16:37:59 +0800 Subject: [PATCH 07/11] enabled all ci images --- .gitlab-ci.yml | 134 ++++++++++++++++++++++++++----------------------- 1 file changed, 72 insertions(+), 62 deletions(-) diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index 2994d94..d44a44b 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -6,32 +6,62 @@ xenial-cpp-build: - cd / && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/public/async_port.git - cd /async_port && mkdir build && cd build - cmake .. && cpack - - dkpg async_port_0.1.0_amd64.deb + - apt install ./async_port_0.1.0_amd64.deb - cd /ugv_sdk - mkdir build && cd build - cmake -DCMAKE_BUILD_TYPE=Release .. - cmake --build . && cpack -# bionic-cpp-build: -# stage: build -# image: rduweston/ubuntu-ci:18.04 -# script: -# - mkdir build && cd build -# - cmake -DCMAKE_BUILD_TYPE=Release .. -# - cmake --build . && cpack +bionic-cpp-build: + stage: build + image: rduweston/ubuntu-ci:18.04 + script: + - cd / && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/public/async_port.git + - cd /async_port && mkdir build && cd build + - cmake .. && cpack + - apt install ./async_port_0.1.0_amd64.deb + - cd /ugv_sdk + - mkdir build && cd build + - cmake -DCMAKE_BUILD_TYPE=Release .. + - cmake --build . && cpack -# focal-cpp-build: -# stage: build -# image: rduweston/ubuntu-ci:20.04 -# script: -# - mkdir build && cd build -# - cmake -DCMAKE_BUILD_TYPE=Release .. -# - cmake --build . && cpack +focal-cpp-build: + stage: build + image: rduweston/ubuntu-ci:20.04 + script: + - cd / && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/public/async_port.git + - cd /async_port && mkdir build && cd build + - cmake .. && cpack + - apt install ./async_port_0.1.0_amd64.deb + - cd /ugv_sdk + - mkdir build && cd build + - cmake -DCMAKE_BUILD_TYPE=Release .. + - cmake --build . && cpack ## Build and test as catkin package kinetic-catkin-build: stage: build image: rduweston/ubuntu-ci:kinetic + script: + - mkdir -p /catkin_ws/src/ugv_sdk/ugv_sdk && cd /catkin_ws/src && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_init_workspace" + - cp -r /builds/$CI_PROJECT_PATH /catkin_ws/src/ugv_sdk + - cd /catkin_ws/src/ugv_sdk + - cd /catkin_ws/src && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/public/async_port.git + - cd /catkin_ws && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_make" + +kinetic-catkin-isolated-build: + stage: build + image: rduweston/ubuntu-ci:kinetic + script: + - mkdir -p /catkin_ws/src/ugv_sdk/ugv_sdk && cd /catkin_ws/src && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_init_workspace" + - cp -r /builds/$CI_PROJECT_PATH /catkin_ws/src/ugv_sdk + - cd /catkin_ws/src/ugv_sdk + - cd /catkin_ws/src && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/public/async_port.git + - cd /catkin_ws && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_make_isolated --install" + +melodic-catkin-build: + stage: build + image: rduweston/ubuntu-ci:melodic script: - mkdir -p /catkin_ws/src/ugv_sdk && cd /catkin_ws/src && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_init_workspace" - cp -r /builds/$CI_PROJECT_PATH /catkin_ws/src/ugv_sdk @@ -39,52 +69,32 @@ kinetic-catkin-build: - cd /catkin_ws/src && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/public/async_port.git - cd /catkin_ws && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_make" -# kinetic-catkin-isolated-build: -# stage: build -# image: rduweston/ubuntu-ci:kinetic -# script: -# - mkdir -p /catkin_ws/src && cd /catkin_ws/src && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_init_workspace" -# - cp -r /builds/$CI_PROJECT_PATH /catkin_ws/src/wrp_sdk -# - cd /catkin_ws/src/wrp_sdk && git submodule update --init --recursive -# - cd /catkin_ws/src && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/ros/scout_base.git -# - cd /catkin_ws && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_make_isolated --install" +melodic-catkin-isolated-build: + stage: build + image: rduweston/ubuntu-ci:melodic + script: + - mkdir -p /catkin_ws/src/ugv_sdk && cd /catkin_ws/src && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_init_workspace" + - cp -r /builds/$CI_PROJECT_PATH /catkin_ws/src/ugv_sdk + - cd /catkin_ws/src/ugv_sdk + - cd /catkin_ws/src && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/public/async_port.git + - cd /catkin_ws && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_make_isolated --install" -# melodic-catkin-build: -# stage: build -# image: rduweston/ubuntu-ci:melodic -# script: -# - mkdir -p /catkin_ws/src && cd /catkin_ws/src && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_init_workspace" -# - cp -r /builds/$CI_PROJECT_PATH /catkin_ws/src/wrp_sdk -# - cd /catkin_ws/src/wrp_sdk && git submodule update --init --recursive -# - cd /catkin_ws/src && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/ros/scout_base.git -# - cd /catkin_ws && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_make" +noetic-catkin-build: + stage: build + image: rduweston/ubuntu-ci:noetic + script: + - mkdir -p /catkin_ws/src/ugv_sdk && cd /catkin_ws/src && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_init_workspace" + - cp -r /builds/$CI_PROJECT_PATH /catkin_ws/src/ugv_sdk + - cd /catkin_ws/src/ugv_sdk + - cd /catkin_ws/src && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/public/async_port.git + - cd /catkin_ws && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_make" -# melodic-catkin-isolated-build: -# stage: build -# image: rduweston/ubuntu-ci:melodic -# script: -# - mkdir -p /catkin_ws/src && cd /catkin_ws/src && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_init_workspace" -# - cp -r /builds/$CI_PROJECT_PATH /catkin_ws/src/wrp_sdk -# - cd /catkin_ws/src/wrp_sdk && git submodule update --init --recursive -# - cd /catkin_ws/src && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/ros/scout_base.git -# - cd /catkin_ws && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_make_isolated --install" - -# noetic-catkin-build: -# stage: build -# image: rduweston/ubuntu-ci:noetic -# script: -# - mkdir -p /catkin_ws/src && cd /catkin_ws/src && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_init_workspace" -# - cp -r /builds/$CI_PROJECT_PATH /catkin_ws/src/wrp_sdk -# - cd /catkin_ws/src/wrp_sdk && git submodule update --init --recursive -# - cd /catkin_ws/src && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/ros/scout_base.git -# - cd /catkin_ws && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_make" - -# noetic-catkin-isolated-build: -# stage: build -# image: rduweston/ubuntu-ci:noetic -# script: -# - mkdir -p /catkin_ws/src && cd /catkin_ws/src && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_init_workspace" -# - cp -r /builds/$CI_PROJECT_PATH /catkin_ws/src/wrp_sdk -# - cd /catkin_ws/src/wrp_sdk && git submodule update --init --recursive -# - cd /catkin_ws/src && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/ros/scout_base.git -# - cd /catkin_ws && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_make_isolated --install" \ No newline at end of file +noetic-catkin-isolated-build: + stage: build + image: rduweston/ubuntu-ci:noetic + script: + - mkdir -p /catkin_ws/src/ugv_sdk && cd /catkin_ws/src && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_init_workspace" + - cp -r /builds/$CI_PROJECT_PATH /catkin_ws/src/ugv_sdk + - cd /catkin_ws/src/ugv_sdk + - cd /catkin_ws/src && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/public/async_port.git + - cd /catkin_ws && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_make_isolated --install" \ No newline at end of file From 65ff62435d2ed732aebee87fd025e1a6f52219d0 Mon Sep 17 00:00:00 2001 From: "pinloon.lee" Date: Wed, 31 Mar 2021 16:42:04 +0800 Subject: [PATCH 08/11] resolved path issue in ci for cpp build --- .gitlab-ci.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index d44a44b..5d0891c 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -7,7 +7,7 @@ xenial-cpp-build: - cd /async_port && mkdir build && cd build - cmake .. && cpack - apt install ./async_port_0.1.0_amd64.deb - - cd /ugv_sdk + - cd /$CI_PROJECT_PATH - mkdir build && cd build - cmake -DCMAKE_BUILD_TYPE=Release .. - cmake --build . && cpack @@ -20,7 +20,7 @@ bionic-cpp-build: - cd /async_port && mkdir build && cd build - cmake .. && cpack - apt install ./async_port_0.1.0_amd64.deb - - cd /ugv_sdk + - cd /$CI_PROJECT_PATH - mkdir build && cd build - cmake -DCMAKE_BUILD_TYPE=Release .. - cmake --build . && cpack @@ -33,7 +33,7 @@ focal-cpp-build: - cd /async_port && mkdir build && cd build - cmake .. && cpack - apt install ./async_port_0.1.0_amd64.deb - - cd /ugv_sdk + - cd /$CI_PROJECT_PATH - mkdir build && cd build - cmake -DCMAKE_BUILD_TYPE=Release .. - cmake --build . && cpack From e07d30019dd2b998f6a4df7dc419666981dc4e44 Mon Sep 17 00:00:00 2001 From: "pinloon.lee" Date: Wed, 31 Mar 2021 16:47:18 +0800 Subject: [PATCH 09/11] trying to resolve ci installation of .deb file --- .gitlab-ci.yml | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index 5d0891c..af7c23b 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -6,7 +6,8 @@ xenial-cpp-build: - cd / && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/public/async_port.git - cd /async_port && mkdir build && cd build - cmake .. && cpack - - apt install ./async_port_0.1.0_amd64.deb + - apt-get install -y apt-utils + - apt-get install ./async_port_0.1.0_amd64.deb - cd /$CI_PROJECT_PATH - mkdir build && cd build - cmake -DCMAKE_BUILD_TYPE=Release .. @@ -19,7 +20,8 @@ bionic-cpp-build: - cd / && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/public/async_port.git - cd /async_port && mkdir build && cd build - cmake .. && cpack - - apt install ./async_port_0.1.0_amd64.deb + - apt-get install -y apt-utils + - apt-get install ./async_port_0.1.0_amd64.deb - cd /$CI_PROJECT_PATH - mkdir build && cd build - cmake -DCMAKE_BUILD_TYPE=Release .. @@ -32,7 +34,8 @@ focal-cpp-build: - cd / && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/public/async_port.git - cd /async_port && mkdir build && cd build - cmake .. && cpack - - apt install ./async_port_0.1.0_amd64.deb + - apt-get install -y apt-utils + - apt-get install ./async_port_0.1.0_amd64.deb - cd /$CI_PROJECT_PATH - mkdir build && cd build - cmake -DCMAKE_BUILD_TYPE=Release .. From 36073ef46e136e3f96696539b8aa6b00534a64d6 Mon Sep 17 00:00:00 2001 From: "pinloon.lee" Date: Wed, 31 Mar 2021 16:55:40 +0800 Subject: [PATCH 10/11] changed to make install instead of install .deb --- .gitlab-ci.yml | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index af7c23b..cc4dd6b 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -5,9 +5,8 @@ xenial-cpp-build: script: - cd / && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/public/async_port.git - cd /async_port && mkdir build && cd build - - cmake .. && cpack - - apt-get install -y apt-utils - - apt-get install ./async_port_0.1.0_amd64.deb + - cmake .. && make + - make install - cd /$CI_PROJECT_PATH - mkdir build && cd build - cmake -DCMAKE_BUILD_TYPE=Release .. @@ -19,9 +18,8 @@ bionic-cpp-build: script: - cd / && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/public/async_port.git - cd /async_port && mkdir build && cd build - - cmake .. && cpack - - apt-get install -y apt-utils - - apt-get install ./async_port_0.1.0_amd64.deb + - cmake .. && make + - make install - cd /$CI_PROJECT_PATH - mkdir build && cd build - cmake -DCMAKE_BUILD_TYPE=Release .. @@ -33,9 +31,8 @@ focal-cpp-build: script: - cd / && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/public/async_port.git - cd /async_port && mkdir build && cd build - - cmake .. && cpack - - apt-get install -y apt-utils - - apt-get install ./async_port_0.1.0_amd64.deb + - cmake .. && make + - make install - cd /$CI_PROJECT_PATH - mkdir build && cd build - cmake -DCMAKE_BUILD_TYPE=Release .. From ac67213a0d391f46ca5424bbf88acd00b85cf64c Mon Sep 17 00:00:00 2001 From: "pinloon.lee" Date: Wed, 31 Mar 2021 17:03:35 +0800 Subject: [PATCH 11/11] updated ci project path --- .gitlab-ci.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index cc4dd6b..e00fca8 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -7,7 +7,7 @@ xenial-cpp-build: - cd /async_port && mkdir build && cd build - cmake .. && make - make install - - cd /$CI_PROJECT_PATH + - cd /builds/$CI_PROJECT_PATH - mkdir build && cd build - cmake -DCMAKE_BUILD_TYPE=Release .. - cmake --build . && cpack @@ -20,7 +20,7 @@ bionic-cpp-build: - cd /async_port && mkdir build && cd build - cmake .. && make - make install - - cd /$CI_PROJECT_PATH + - cd /builds/$CI_PROJECT_PATH - mkdir build && cd build - cmake -DCMAKE_BUILD_TYPE=Release .. - cmake --build . && cpack @@ -33,7 +33,7 @@ focal-cpp-build: - cd /async_port && mkdir build && cd build - cmake .. && make - make install - - cd /$CI_PROJECT_PATH + - cd /builds/$CI_PROJECT_PATH - mkdir build && cd build - cmake -DCMAKE_BUILD_TYPE=Release .. - cmake --build . && cpack