saved work

include/ugv_sdk/agilex_base.hpp

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

include/ugv_sdk/details/stopwatch.hpp

@@ -0,0 +1,160 @@
+/*
+ * stopwatch.hpp
+ *
+ * Created on: Jul 12, 2020 12:07
+ * Description:
+ *
+ * Source:
+ * [1] https://github.com/sailormoon/stopwatch
+ * [2] https://github.com/rxdu/stopwatch
+ *
+ * Copyright (c) 2019 sailormoon <http://unlicense.org>
+ * Copyright (c) 2020 Ruixiang Du (rdu)
+ *
+ * License: <http://unlicense.org>
+ */
+
+#ifndef STOPWATCH_HPP
+#define STOPWATCH_HPP
+
+#include <array>
+#include <chrono>
+#include <cstdint>
+#include <algorithm>
+
+#include <thread>
+
+namespace westonrobot {
+// only supported on x86 processors
+#if (defined __x86_64__) || (defined __i386)
+// An implementation of the 'TrivialClock' concept using the rdtscp instruction.
+struct rdtscp_clock {
+  using rep = std::uint64_t;
+  using period = std::ratio<1>;
+  using duration = std::chrono::duration<rep, period>;
+  using time_point = std::chrono::time_point<rdtscp_clock, duration>;
+
+  static time_point now() noexcept {
+    std::uint32_t hi, lo;
+    __asm__ __volatile__("rdtscp" : "=d"(hi), "=a"(lo));
+    return time_point(duration((static_cast<std::uint64_t>(hi) << 32) | lo));
+  }
+};
+
+// A timer using the specified clock.
+template <class Clock = std::chrono::system_clock>
+struct timer {
+  using time_point = typename Clock::time_point;
+  using duration = typename Clock::duration;
+
+  timer(const duration duration) : expiry(Clock::now() + duration) {}
+  timer(const time_point expiry) : expiry(expiry) {}
+  bool done() const { return done(Clock::now()); }
+  bool done(const time_point now) const { return now >= expiry; }
+  duration remaining() const { return remaining(Clock::now()); };
+  duration remaining(const time_point now) const { return expiry - now; }
+  const time_point expiry;
+};
+
+template <class Clock = std::chrono::system_clock>
+constexpr timer<Clock> make_timer(const typename Clock::duration duration) {
+  return timer<Clock>(duration);
+}
+
+// Times how long it takes a function to execute using the specified clock.
+template <class Clock = rdtscp_clock, class Func>
+typename Clock::duration time_func(Func &&function) {
+  const auto start = Clock::now();
+  function();
+  return Clock::now() - start;
+}
+
+// Samples the given function N times using the specified clock.
+template <std::size_t N, class Clock = rdtscp_clock, class Func>
+std::array<typename Clock::duration, N> sample(Func &&function) {
+  std::array<typename Clock::duration, N> samples;
+  for (std::size_t i = 0u; i < N; ++i) {
+    samples[i] = time_func<Clock>(function);
+  }
+  std::sort(samples.begin(), samples.end());
+  return samples;
+}
+#endif /* __x86_64__ or __i386 */
+
+struct StopWatch {
+  using Clock = std::chrono::high_resolution_clock;
+  using time_point = typename Clock::time_point;
+  using duration = typename Clock::duration;
+
+  StopWatch() { tic_point = Clock::now(); };
+
+  time_point tic_point;
+
+  void tic() { tic_point = Clock::now(); };
+
+  double toc() {
+    return std::chrono::duration_cast<std::chrono::microseconds>(Clock::now() -
+                                                                 tic_point)
+               .count() /
+           1000000.0;
+  };
+
+  // toc() in different units
+  double stoc() {
+    return std::chrono::duration_cast<std::chrono::seconds>(Clock::now() -
+                                                            tic_point)
+        .count();
+  };
+
+  double mtoc() {
+    return std::chrono::duration_cast<std::chrono::milliseconds>(Clock::now() -
+                                                                 tic_point)
+        .count();
+  };
+
+  double utoc() {
+    return std::chrono::duration_cast<std::chrono::microseconds>(Clock::now() -
+                                                                 tic_point)
+        .count();
+  };
+
+  double ntoc() {
+    return std::chrono::duration_cast<std::chrono::nanoseconds>(Clock::now() -
+                                                                tic_point)
+        .count();
+  };
+};
+
+struct Timer {
+  using Clock = std::chrono::high_resolution_clock;
+  using time_point = typename Clock::time_point;
+  using duration = typename Clock::duration;
+
+  Timer() { tic_point = Clock::now(); };
+
+  time_point tic_point;
+
+  void reset() { tic_point = Clock::now(); };
+
+  // you have to call reset() before calling sleep functions
+  void sleep_until_ms(int64_t period_ms) {
+    int64_t duration =
+        period_ms - std::chrono::duration_cast<std::chrono::milliseconds>(
+                        Clock::now() - tic_point)
+                        .count();
+    if (duration > 0)
+      std::this_thread::sleep_for(std::chrono::milliseconds(duration));
+  };
+
+  void sleep_until_us(int64_t period_us) {
+    int64_t duration =
+        period_us - std::chrono::duration_cast<std::chrono::microseconds>(
+                        Clock::now() - tic_point)
+                        .count();
+    if (duration > 0)
+      std::this_thread::sleep_for(std::chrono::microseconds(duration));
+  };
+};
+}  // namespace westonrobot
+
+#endif  // STOPWATCH_HPP

include/ugv_sdk/interface/parser_interface.hpp

@@ -0,0 +1,50 @@
+/*
+ * 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/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
+  };
+  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
+  };
+};
+
+#endif /* PASER_INTERFACE_HPP */

include/ugv_sdk/interface/robot_interface.hpp

@@ -0,0 +1,40 @@
+/*
+ * 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/interface/agilex_message.h"
+
+namespace westonrobot {
+struct RobotInterface {
+  // functions to be implemented by class AgilexBase
+  virtual void EnableCommandedMode() = 0;
+
+  virtual void SetMotionMode(uint8_t mode){};
+
+  virtual void SendMotionCommand(double linear_vel, double angular_vel,
+                                 double lateral_velocity,
+                                 double steering_angle) = 0;
+  virtual void SendLightCommand(LightMode front_mode,
+                                uint8_t front_custom_value, LightMode rear_mode,
+                                uint8_t rear_custom_value) = 0;
+  virtual void DisableLightControl() = 0;
+
+  // functions to be implemented by each robot class
+  virtual void Connect(std::string can_name) = 0;
+  virtual void Connect(std::string uart_name, uint32_t baudrate) = 0;
+
+  virtual void ResetRobotState() = 0;
+};
+}  // namespace westonrobot
+
+#endif /* ROBOT_INTERFACE_HPP */

include/ugv_sdk/interface/scout_interface.hpp

@@ -1,8 +1,8 @@
 /*
  * scout_interface.hpp
  *
- * Created on: Jul 07, 2021 09:10
- * Description:
+ * Created on: Jul 08, 2021 12:02
+ * Description: Scout-specific interface
  *
  * Copyright (c) 2021 Weston Robot Pte. Ltd.
  */
@@ -10,19 +10,33 @@
 #ifndef SCOUT_INTERFACE_HPP
 #define SCOUT_INTERFACE_HPP
 
-namespace westonrobot {
-class ScoutInterface {
- public:
-  // set up connection
-  void Connect(std::string dev_name) override;
+#include <string>
 
-  // robot control
-  void SetMotionCommand(double linear_vel, double angular_vel);
-  void SetLightCommand(const ScoutLightCmd &cmd);
-  void DisableLightCmdControl();
+#include "ugv_sdk/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 {
+  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
-  ScoutState GetScoutState();
+  virtual ScoutState GetRobotState() = 0;
 };
 }  // namespace westonrobot
 

include/ugv_sdk/legacy/agilex_base_v1.hpp

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

include/ugv_sdk/mobile_base/agilex_base.hpp

@@ -0,0 +1,151 @@
+/*
+ * agilex_base.hpp
+ *
+ * Created on: Dec 22, 2020 17:14
+ * Description:
+ *
+ * Each robot class derived from this base class should provide implementation
+ * for the following two functions:
+ *
+ * - virtual void Connect(std::string dev_name) = 0;
+ * - virtual void ParseCANFrame(can_frame *rx_frame) = 0;
+ *
+ * Copyright (c) 2020 Ruixiang Du (rdu)
+ */
+
+#ifndef AGILEX_BASE_HPP
+#define AGILEX_BASE_HPP
+
+#include <string>
+#include <cstdint>
+#include <thread>
+#include <mutex>
+#include <atomic>
+
+#include "ugv_sdk/details/stopwatch.hpp"
+#include "ugv_sdk/details/async_port/async_can.hpp"
+
+#include "ugv_sdk/interface/robot_interface.hpp"
+#include "ugv_sdk/protocol_v2/agilex_msg_parser.h"
+
+namespace westonrobot {
+enum class AgilexProtocol { V1, V2 };
+
+template <typename ParserType>
+class AgilexBase : public RobotInterface {
+ public:
+  AgilexBase() = default;
+  virtual ~AgilexBase() { DisconnectPort(); }
+
+  // do not allow copy or assignment
+  AgilexBase(const AgilexBase &hunter) = delete;
+  AgilexBase &operator=(const AgilexBase &hunter) = delete;
+
+  // switch to commanded mode
+  void 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;
+    parser_.EncodeMessage(&msg, &frame);
+    can_->SendFrame(frame);
+  }
+
+  // must be called at a frequency >= 50Hz
+  void SendMotionCommand(double linear_vel, double angular_vel,
+                         double lateral_velocity, double steering_angle) {
+    if (can_connected_) {
+      // motion control message
+      AgxMessage msg;
+      msg.type = AgxMsgMotionCommand;
+      msg.body.motion_command_msg.linear_velocity = linear_vel;
+      msg.body.motion_command_msg.angular_velocity = angular_vel;
+      msg.body.motion_command_msg.lateral_velocity = lateral_velocity;
+      msg.body.motion_command_msg.steering_angle = steering_angle;
+
+      // send to can bus
+      can_frame frame;
+      parser_.EncodeMessage(&msg, &frame);
+      can_->SendFrame(frame);
+    }
+  }
+
+  // one-shot light command
+  void SendLightCommand(LightMode front_mode, uint8_t front_custom_value,
+                        LightMode rear_mode, uint8_t rear_custom_value) {
+    if (can_connected_) {
+      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;
+      parser_.EncodeMessage(&msg, &frame);
+      can_->SendFrame(frame);
+    }
+  }
+
+  void DisableLightControl() {
+    if (can_connected_) {
+      AgxMessage msg;
+      msg.type = AgxMsgLightCommand;
+
+      msg.body.light_command_msg.cmd_ctrl_allowed = false;
+
+      // send to can bus
+      can_frame frame;
+      parser_.EncodeMessage(&msg, &frame);
+      can_->SendFrame(frame);
+    }
+  }
+
+  // motion mode change
+  void SetMotionMode(uint8_t mode) {
+    if (can_connected_) {
+      AgxMessage msg;
+      msg.type = AgxMsgSetMotionMode;
+      msg.body.motion_mode_msg.motion_mode = mode;
+
+      // send to can bus
+      can_frame frame;
+      parser_.EncodeMessage(&msg, &frame);
+      can_->SendFrame(frame);
+    }
+  }
+
+ protected:
+  std::mutex state_mutex_;
+  //   std::mutex motion_cmd_mutex_;
+  MotionCommandMessage current_motion_cmd_;
+
+  ParserType parser_;
+
+  // communication interface
+  bool can_connected_ = false;
+  std::shared_ptr<AsyncCAN> can_;
+
+  // connect to roboot from CAN or serial
+  using CANFrameRxCallback = AsyncCAN::ReceiveCallback;
+  void ConnectPort(std::string dev_name, CANFrameRxCallback cb) {
+    can_ = std::make_shared<AsyncCAN>(dev_name);
+    can_->SetReceiveCallback(cb);
+    can_->StartListening();
+    can_connected_ = true;
+  }
+
+  void DisconnectPort() {
+    if (can_connected_) can_->StopService();
+  }
+
+  virtual void ParseCANFrame(can_frame *rx_frame) = 0;
+};
+}  // namespace westonrobot
+
+#endif /* AGILEX_BASE_HPP */

include/ugv_sdk/mobile_base/ranger_interface.hpp

@@ -1,21 +1,18 @@
-/**
- * @Kit       : Qt-Creator: Desktop
- * @Author    : Wang Zhe
- * @Date      : 2021-04-19  19:12:52
- * @FileName  : ranger_base.hpp
- * @Mail      : wangzheqie@qq.com
- * Copyright  : AgileX Robotics
- **/
+/*
+ * ranger_interface.hpp
+ *
+ * Created on: Jul 08, 2021 09:40
+ * Description:
+ *
+ * Copyright (c) 2021 Weston Robot Pte. Ltd.
+ */
 
-#ifndef RANGER_BASE_HPP
-#define RANGER_BASE_HPP
+#ifndef RANGER_INTERFACE_HPP
+#define RANGER_INTERFACE_HPP
 
-#include <cstdint>
-#include <mutex>
 #include <string>
-#include <thread>
 
-#include "ugv_sdk/agilex_base.hpp"
+#include "ugv_sdk/interface/agilex_message.h"
 
 namespace westonrobot {
 struct RangerState {
@@ -24,8 +21,6 @@ struct RangerState {
   MotionStateMessage motion_state;
   LightStateMessage light_state;
 
-
-
   RcStateMessage rc_state;
 
   ActuatorHSStateMessage actuator_hs_state[8];
@@ -60,28 +55,18 @@ struct RangerLightCmd {
 
 /////////////////////////////////////////////////////////////////////////
 
-class RangerBase : public AgilexBase {
- public:
-  RangerBase() : AgilexBase(){};
-  ~RangerBase() = default;
-
-  // set up connection
-  void Connect(std::string dev_name) override;
+struct RangerInterface {
+  virtual void Connect(std::string dev_name) = 0;
 
   // robot control
-  void SetMotionCommand(double linear_vel, double steer_angle,
-                        double lateral_vel = 0.0, double angular_vel = 0.0);
-  void SetLightCommand(const RangerLightCmd &cmd);
-  void SetMotionMode(uint8_t mode);
+  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
-  RangerState GetRangerState();
-
- private:
-  RangerState ranger_state_;
-
-  void ParseCANFrame(can_frame *rx_frame) override;
-  void UpdateRangerState(const AgxMessage &status_msg, RangerState &state);
+  virtual RangerState GetRangerState() = 0;
 };
 }  // namespace westonrobot
-#endif  // RANGER_BASE_HPP
+
+#endif /* RANGER_INTERFACE_HPP */

include/ugv_sdk/mobile_base/scout_robot.hpp

@@ -0,0 +1,27 @@
+/*
+ * scout_robot.hpp
+ *
+ * Created on: Jul 08, 2021 10:59
+ * Description:
+ *
+ * Copyright (c) 2021 Weston Robot Pte. Ltd.
+ */
+
+#ifndef SCOUT_ROBOT_HPP
+#define SCOUT_ROBOT_HPP
+
+#include <memory>
+
+#include "ugv_sdk/mobile_base/common.hpp"
+
+namespace westonrobot {
+class ScoutRobot {
+ public:
+  ScoutRobot(AgilexProtocol protocol);
+
+ private:
+  std::unique_ptr<ScoutInterface> robot_;
+};
+}  // namespace westonrobot
+
+#endif /* SCOUT_ROBOT_HPP */

include/ugv_sdk/mobile_base/tracer_interface.hpp

@@ -1,21 +1,18 @@
 /*
- * tracer_base.hpp
+ * tracer_interface.hpp
  *
- * Created on: Apr 14, 2020 10:21
+ * Created on: Jul 08, 2021 09:36
  * Description:
  *
- * Copyright (c) 2020 Weston Robot Pte. Ltd.
+ * Copyright (c) 2021 Weston Robot Pte. Ltd.
  */
 
-#ifndef TRACER_BASE_HPP
-#define TRACER_BASE_HPP
+#ifndef TRACER_INTERFACE_HPP
+#define TRACER_INTERFACE_HPP
 
 #include <string>
-#include <cstdint>
-#include <thread>
-#include <mutex>
 
-#include "ugv_sdk/agilex_base.hpp"
+#include "ugv_sdk/interface/agilex_message.h"
 
 namespace westonrobot {
 struct TracerState {
@@ -52,27 +49,18 @@ struct TracerLightCmd {
 
 /////////////////////////////////////////////////////////////////////////
 
-class TracerBase : public AgilexBase {
- public:
-  TracerBase() : AgilexBase(){};
-  ~TracerBase() = default;
-
+struct TracerInterface {
   // set up connection
-  void Connect(std::string dev_name) override;
+  virtual void Connect(std::string can_name) = 0;
+  virtual void Connect(std::string uart_name, uint32_t baudrate) = 0;
 
   // robot control
-  void SetMotionCommand(double linear_vel, double angular_vel);
-  void SetLightCommand(const TracerLightCmd &cmd);
+  virtual void SetMotionCommand(double linear_vel, double angular_vel) = 0;
+  virtual void SetLightCommand(const TracerLightCmd &cmd) = 0;
 
   // get robot state
-  TracerState GetTracerState();
-
- private:
-  TracerState tracer_state_;
-
-  void ParseCANFrame(can_frame *rx_frame) override;
-  void UpdateTracerState(const AgxMessage &status_msg, TracerState &state);
+  virtual TracerState GetTracerState() = 0;
 };
 }  // namespace westonrobot
 
-#endif /* TRACER_BASE_HPP */
+#endif /* TRACER_INTERFACE_HPP */

include/ugv_sdk/protocol_v1/scout_can_parser_v1.h

@@ -17,7 +17,7 @@ extern "C" {
 #include <stdint.h>
 #include <stdbool.h>
 
-#include "ugv_sdk/legacy/scout_protocol_v1.h"
+#include "ugv_sdk/protocol_v1/scout_protocol_v1.h"
 
 #ifdef __linux__
 #include <linux/can.h>

include/ugv_sdk/protocol_v2/agilex_msg_parser.h

@@ -28,7 +28,7 @@ struct can_frame {
 };
 #endif
 
-#include "ugv_sdk/agilex_message.h"
+#include "ugv_sdk/interface/agilex_message.h"
 
 bool DecodeCanFrame(const struct can_frame *rx_frame, AgxMessage *msg);
 void EncodeCanFrame(const AgxMessage *msg, struct can_frame *tx_frame);

include/ugv_sdk/protocol_v2/protocol_v2_parser.hpp

@@ -0,0 +1,26 @@
+/*
+ * protocol_v2_parser.hpp
+ *
+ * Created on: Jul 08, 2021 14:51
+ * Description:
+ *
+ * Copyright (c) 2021 Weston Robot Pte. Ltd.
+ */
+
+#ifndef PROTOCOL_V2_PARSER_HPP
+#define PROTOCOL_V2_PARSER_HPP
+
+#include "ugv_sdk/interface/parser_interface.hpp"
+
+namespace westonrobot {
+class ProtocolV2Parser : public ParserInterface {
+ public:
+  bool DecodeMessage(const struct can_frame *rx_frame,
+                     AgxMessage *msg) override;
+  void EncodeMessage(const AgxMessage *msg,
+                     struct can_frame *tx_frame) override;
+  uint8_t CalculateChecksum(uint16_t id, uint8_t *data, uint8_t dlc) override;
+};
+}  // namespace westonrobot
+
+#endif /* PROTOCOL_V2_PARSER_HPP */

include/ugv_sdk/protocol_v2/ranger_base.hpp

@@ -0,0 +1,46 @@
+/**
+ * @Kit       : Qt-Creator: Desktop
+ * @Author    : Wang Zhe
+ * @Date      : 2021-04-19  19:12:52
+ * @FileName  : ranger_base.hpp
+ * @Mail      : wangzheqie@qq.com
+ * Copyright  : AgileX Robotics
+ **/
+
+#ifndef RANGER_BASE_HPP
+#define RANGER_BASE_HPP
+
+#include <cstdint>
+#include <mutex>
+#include <string>
+#include <thread>
+
+#include "ugv_sdk/interface/ranger_interface.hpp"
+#include "ugv_sdk/protocol_v2/agilex_base.hpp"
+
+namespace westonrobot {
+class RangerBase : public AgilexBase, public RangerInterface {
+ public:
+  RangerBase() : AgilexBase(){};
+  ~RangerBase() = default;
+
+  // set up connection
+  void Connect(std::string dev_name) override;
+
+  // robot control
+  void SetMotionCommand(double linear_vel, double steer_angle,
+                        double lateral_vel = 0.0, double angular_vel = 0.0);
+  void SetLightCommand(const RangerLightCmd &cmd);
+  void SetMotionMode(uint8_t mode);
+
+  // get robot state
+  RangerState GetRangerState();
+
+ private:
+  RangerState ranger_state_;
+
+  void ParseCANFrame(can_frame *rx_frame) override;
+  void UpdateRangerState(const AgxMessage &status_msg, RangerState &state);
+};
+}  // namespace westonrobot
+#endif  // RANGER_BASE_HPP

include/ugv_sdk/protocol_v2/scout_base_v2.hpp

@@ -0,0 +1,51 @@
+/*
+ * scout_base.hpp
+ *
+ * Created on: Dec 23, 2020 14:39
+ * Description:
+ *
+ * Copyright (c) 2020 Ruixiang Du (rdu)
+ */
+
+#ifndef SCOUT_BASE_HPP
+#define SCOUT_BASE_HPP
+
+#include <string>
+#include <cstdint>
+#include <thread>
+#include <mutex>
+
+#include "ugv_sdk/interface/scout_interface.hpp"
+
+#include "ugv_sdk/mobile_base/agilex_base.hpp"
+#include "ugv_sdk/protocol_v2/protocol_v2_parser.hpp"
+
+namespace westonrobot {
+class ScoutBase : public AgilexBase<ProtocolV2Parser>, public ScoutInterface {
+ public:
+  ScoutBase() : AgilexBase(){};
+  ~ScoutBase() = default;
+
+  // set up connection
+  void Connect(std::string can_name) override;
+  void Connect(std::string uart_name, uint32_t baudrate) override;
+
+  // robot control
+  void SetMotionCommand(double linear_vel, double angular_vel) override;
+  void SetLightCommand(LightMode f_mode, uint8_t f_value, LightMode r_mode,
+                       uint8_t r_value) override;
+
+  // get robot state
+  ScoutState GetRobotState() override;
+
+  void ResetRobotState() override {}
+
+ 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 */

include/ugv_sdk/protocol_v2/tracer_base.hpp

@@ -0,0 +1,46 @@
+/*
+ * tracer_base.hpp
+ *
+ * Created on: Apr 14, 2020 10:21
+ * Description:
+ *
+ * Copyright (c) 2020 Weston Robot Pte. Ltd.
+ */
+
+#ifndef TRACER_BASE_HPP
+#define TRACER_BASE_HPP
+
+#include <string>
+#include <cstdint>
+#include <thread>
+#include <mutex>
+
+#include "ugv_sdk/interface/tracer_interface.hpp"
+#include "ugv_sdk/protocol_v2/agilex_base.hpp"
+
+namespace westonrobot {
+class TracerBase : public AgilexBase, public TracerInterface {
+ public:
+  TracerBase() : AgilexBase(){};
+  ~TracerBase() = default;
+
+  // set up connection
+  void Connect(std::string can_name) override;
+  void Connect(std::string uart_name, uint32_t baudrate) 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 */

include/ugv_sdk/scout_base.hpp

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