moved ugv_sdk out from sub-folder

include/ugv_sdk/hunter/hunter_base.hpp

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

include/ugv_sdk/hunter/hunter_types.hpp

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

include/ugv_sdk/mobile_base.hpp

@@ -0,0 +1,88 @@
+/*
+ * mobile_base.hpp
+ *
+ * Created on: Jun 17, 2020 11:23
+ * Description:
+ *
+ * Generic mobile base: this class handles the communication
+ * logic that is similar across different mobile platforms
+ *
+ * Copyright (c) 2020 Ruixiang Du (rdu)
+ */
+
+#ifndef MOBILE_BASE_HPP
+#define MOBILE_BASE_HPP
+
+#include <string>
+#include <cstdint>
+#include <thread>
+#include <mutex>
+#include <atomic>
+
+#include "async_port/async_can.hpp"
+#include "async_port/async_serial.hpp"
+
+namespace westonrobot {
+class MobileBase {
+ public:
+  MobileBase() = default;
+  virtual ~MobileBase();
+
+  // do not allow copy or assignment
+  MobileBase(const MobileBase &hunter) = delete;
+  MobileBase &operator=(const MobileBase &hunter) = delete;
+
+  void SetCmdTimeout(bool enable, uint32_t timeout_ms = 100);
+  void DisableTimeout() { enable_timeout_ = false; }
+
+  // connect to roboot from CAN or serial
+  void Connect(std::string dev_name, int32_t baud_rate = 0);
+
+  // disconnect from roboot, only valid for serial port
+  void Disconnect();
+
+  // ask background thread to shutdown properly
+  void Terminate();
+
+  // cmd thread runs at 100Hz (10ms) by default
+  void SetCmdThreadPeriodMs(int32_t period_ms) {
+    cmd_thread_period_ms_ = period_ms;
+  };
+
+ protected:
+  // communication interface
+  bool can_connected_ = false;
+  bool serial_connected_ = false;
+
+  std::shared_ptr<AsyncCAN> can_if_;
+  std::shared_ptr<AsyncSerial> serial_if_;
+
+  // timeout to be implemented in each vehicle
+  bool enable_timeout_ = true;
+  uint32_t timeout_ms_ = 500;
+  uint32_t watchdog_counter_ = 0;
+  void FeedCmdTimeoutWatchdog() { watchdog_counter_ = 0; };
+
+  // command thread
+  std::thread cmd_thread_;
+  int32_t cmd_thread_period_ms_ = 10;
+  bool cmd_thread_started_ = false;
+  std::atomic<bool> keep_running_;
+
+  // internal functions
+  void ConfigureCAN(const std::string &can_if_name = "can0");
+  void ConfigureSerial(const std::string uart_name = "/dev/ttyUSB0",
+                       int32_t baud_rate = 115200);
+
+  void StartCmdThread();
+  void ControlLoop(int32_t period_ms);
+
+  // functions that must/may be implemented by child classes
+  virtual void SendRobotCmd() = 0;
+  virtual void ParseCANFrame(can_frame *rx_frame){};
+  virtual void ParseUARTBuffer(uint8_t *buf, const size_t bufsize,
+                               size_t bytes_received){};
+};
+}  // namespace westonrobot
+
+#endif /* MOBILE_BASE_HPP */

include/ugv_sdk/proto/agx_msg_parser.h

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

include/ugv_sdk/proto/agx_protocol_v2.h

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

include/ugv_sdk/scout/scout_base.hpp

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

include/ugv_sdk/scout/scout_types.hpp

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

include/ugv_sdk/tracer/tracer_base.hpp

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

include/ugv_sdk/tracer/tracer_types.hpp

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