cleaned up folder

include/ugv_sdk/details/interface/agilex_message.h

@@ -0,0 +1,370 @@
+/*
+ * 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 <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+
+/***************** Control messages *****************/
+
+// 0x111
+typedef struct {
+  float linear_velocity;
+  float angular_velocity;
+  float lateral_velocity;
+  float steering_angle;
+} MotionCommandMessage;
+
+// 0x121
+typedef enum {
+  CONST_OFF = 0x00,
+  CONST_ON = 0x01,
+  BREATH = 0x02,
+  CUSTOM = 0x03
+} LightMode;
+
+typedef struct {
+  LightMode mode;
+  uint8_t custom_value;
+} LightOperation;
+
+typedef struct {
+  bool cmd_ctrl_allowed;
+  LightOperation front_light;
+  LightOperation rear_light;
+} LightCommandMessage;
+
+// 0x131
+typedef struct {
+  bool enable_braking;
+} BrakingCommandMessage;
+
+// 0x141
+typedef struct {
+  uint8_t motion_mode;
+} MotionModeMessage;
+
+/**************** 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 - 0x258
+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)
+
+// 0x291
+typedef struct {
+  uint8_t motion_mode;
+  uint8_t mode_changing;
+} MotionModeFeedbackMessage;
+
+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,
+  AgxMsgSetMotionMode,
+  // state feedback
+  AgxMsgSystemState,
+  AgxMsgMotionState,
+  AgxMsgLightState,
+  AgxMsgRcState,
+  AgxMsgActuatorHSState,
+  AgxMsgActuatorLSState,
+  AgxMsgMotionModeState,
+  // 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;
+    MotionModeMessage motion_mode_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;
+    MotionModeFeedbackMessage motion_mode_feedback_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 */

include/ugv_sdk/details/interface/parser_interface.hpp

@@ -0,0 +1,52 @@
+/*
+ * parser_interface.hpp
+ *
+ * Created on: Jul 08, 2021 14:43
+ * Description:
+ *
+ * Copyright (c) 2021 Weston Robot Pte. Ltd.
+ */
+
+#ifndef PASER_INTERFACE_HPP
+#define PASER_INTERFACE_HPP
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <string.h>
+
+#ifdef __linux__
+#include <linux/can.h>
+#else
+struct can_frame {
+  uint32_t can_id;
+  uint8_t can_dlc;
+  uint8_t data[8] __attribute__((aligned(8)));
+};
+#endif
+
+#include "ugv_sdk/details/interface/agilex_message.h"
+
+struct ParserInterface {
+  // CAN support
+  virtual bool DecodeMessage(const struct can_frame *rx_frame,
+                             AgxMessage *msg) = 0;
+  virtual void EncodeMessage(const AgxMessage *msg,
+                             struct can_frame *tx_frame) = 0;
+  virtual uint8_t CalculateChecksum(uint16_t id, uint8_t *data,
+                                    uint8_t dlc) = 0;
+
+  // UART support
+  virtual bool DecodeMessage(uint8_t *data, uint8_t dlc, AgxMessage *msg) {
+    // throw exception
+    return false;
+  };
+  virtual void EncodeMessage(const AgxMessage *msg, uint8_t *buf, uint8_t *len){
+      // throw exception
+  };
+  virtual uint8_t CalculateChecksum(uint8_t *buf, uint8_t len) {
+    // throw exception
+    return 0;
+  };
+};
+
+#endif /* PASER_INTERFACE_HPP */

include/ugv_sdk/details/interface/ranger_interface.hpp

@@ -0,0 +1,72 @@
+/*
+ * ranger_interface.hpp
+ *
+ * Created on: Jul 08, 2021 09:40
+ * Description:
+ *
+ * Copyright (c) 2021 Weston Robot Pte. Ltd.
+ */
+
+#ifndef RANGER_INTERFACE_HPP
+#define RANGER_INTERFACE_HPP
+
+#include <string>
+
+#include "ugv_sdk/interface/agilex_message.h"
+
+namespace westonrobot {
+struct RangerState {
+  // system state
+  SystemStateMessage system_state;
+  MotionStateMessage motion_state;
+  LightStateMessage light_state;
+
+  RcStateMessage rc_state;
+
+  ActuatorHSStateMessage actuator_hs_state[8];
+  ActuatorLSStateMessage actuator_ls_state[8];
+  MotionModeFeedbackMessage current_motion_mode;
+
+  // sensor data
+  OdometryMessage odometry;
+};
+
+struct RangerMotionCmd {
+  double linear_velocity;
+  double angular_velocity;
+};
+
+struct RangerLightCmd {
+  RangerLightCmd() = default;
+  RangerLightCmd(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;
+};
+
+/////////////////////////////////////////////////////////////////////////
+
+struct RangerInterface {
+  virtual void Connect(std::string dev_name) = 0;
+
+  // robot control
+  virtual void SetMotionCommand(double linear_vel, double steer_angle,
+                                double lateral_vel, double angular_vel) = 0;
+  virtual void SetLightCommand(const RangerLightCmd &cmd) = 0;
+  virtual void SetMotionMode(uint8_t mode) = 0;
+
+  // get robot state
+  virtual RangerState GetRangerState() = 0;
+};
+}  // namespace westonrobot
+
+#endif /* RANGER_INTERFACE_HPP */

include/ugv_sdk/details/interface/robot_interface.hpp

@@ -0,0 +1,56 @@
+/*
+ * robot_interface.hpp
+ *
+ * Created on: Jul 08, 2021 11:48
+ * Description:
+ *
+ * Copyright (c) 2021 Weston Robot Pte. Ltd.
+ */
+
+#ifndef ROBOT_INTERFACE_HPP
+#define ROBOT_INTERFACE_HPP
+
+#include <string>
+
+#include "ugv_sdk/details/interface/agilex_message.h"
+
+namespace westonrobot {
+enum class ProtocolType { AGX_V1, AGX_V2 };
+
+class RobotInterface {
+ public:
+  ~RobotInterface() = default;
+
+  // functions to be implemented by class AgilexBase
+  virtual void EnableCommandedMode() = 0;
+  virtual void DisableLightControl() = 0;
+
+  // functions to be implemented by each robot class
+  virtual void Connect(std::string can_name) = 0;
+  // functions to be implemented by each robot class
+  virtual void Connect(std::string uart_name, uint32_t baudrate){
+      // use derived version
+  };
+  virtual void ResetRobotState() = 0;
+
+ protected:
+  /****** functions not available/valid to all robots ******/
+  // functions to be implemented by class AgilexBase
+  virtual void SetMotionMode(uint8_t mode){};
+
+    // any specific robot will use a specialized version of the two functions
+  virtual void SendMotionCommand(double linear_vel, double angular_vel,
+                                 double lateral_velocity,
+                                 double steering_angle){
+      // use derived version
+  };
+
+  virtual void SendLightCommand(LightMode front_mode,
+                                uint8_t front_custom_value, LightMode rear_mode,
+                                uint8_t rear_custom_value){
+      // use derived version
+  };
+};
+}  // namespace westonrobot
+
+#endif /* ROBOT_INTERFACE_HPP */

include/ugv_sdk/details/interface/scout_interface.hpp

@@ -0,0 +1,45 @@
+/*
+ * scout_interface.hpp
+ *
+ * Created on: Jul 08, 2021 12:02
+ * Description: Scout-specific interface
+ *
+ * Copyright (c) 2021 Weston Robot Pte. Ltd.
+ */
+
+#ifndef SCOUT_INTERFACE_HPP
+#define SCOUT_INTERFACE_HPP
+
+#include <string>
+
+#include "ugv_sdk/details/interface/agilex_message.h"
+
+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 ScoutInterface {
+  ~ScoutInterface() = default;
+
+  virtual void SetMotionCommand(double linear_vel, double angular_vel) = 0;
+  virtual void SetLightCommand(LightMode f_mode, uint8_t f_value,
+                               LightMode r_mode, uint8_t r_value) = 0;
+
+  // get robot state
+  virtual ScoutState GetRobotState() = 0;
+};
+}  // namespace westonrobot
+
+#endif /* SCOUT_INTERFACE_HPP */

include/ugv_sdk/details/interface/tracer_interface.hpp

@@ -0,0 +1,66 @@
+/*
+ * tracer_interface.hpp
+ *
+ * Created on: Jul 08, 2021 09:36
+ * Description:
+ *
+ * Copyright (c) 2021 Weston Robot Pte. Ltd.
+ */
+
+#ifndef TRACER_INTERFACE_HPP
+#define TRACER_INTERFACE_HPP
+
+#include <string>
+
+#include "ugv_sdk/interface/agilex_message.h"
+
+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;
+};
+
+/////////////////////////////////////////////////////////////////////////
+
+struct TracerInterface {
+  // set up connection
+  virtual void Connect(std::string can_name) = 0;
+  virtual void Connect(std::string uart_name, uint32_t baudrate) = 0;
+
+  // robot control
+  virtual void SetMotionCommand(double linear_vel, double angular_vel) = 0;
+  virtual void SetLightCommand(const TracerLightCmd &cmd) = 0;
+
+  // get robot state
+  virtual TracerState GetTracerState() = 0;
+};
+}  // namespace westonrobot
+
+#endif /* TRACER_INTERFACE_HPP */