updated scout base to use class MobileBase

include/wrp_sdk/platforms/hunter/hunter_base.hpp

@@ -36,10 +36,6 @@ class HunterBase : public MobileBase {
                             HunterMotionCmd::FaultClearFlag::NO_FAULT);
 
  private:
-  // serial port related variables
-  uint8_t tx_cmd_len_;
-  uint8_t tx_buffer_[HUNTER_CMD_BUF_LEN];
-
   // cmd/status update related variables
   std::mutex hunter_state_mutex_;
   std::mutex motion_cmd_mutex_;

include/wrp_sdk/platforms/scout/scout_base.hpp

@@ -1,9 +1,9 @@
-/* 
+/*
  * scout_base.hpp
- * 
+ *
  * Created on: Jun 04, 2019 01:22
- * Description: 
+ * Description:
- * 
+ *
  * Copyright (c) 2019 Ruixiang Du (rdu)
  */
 
@@ -16,95 +16,67 @@
 #include <mutex>
 #include <functional>
 
-#include "wrp_sdk/asyncio/async_can.hpp"
+#include "wrp_sdk/platforms/common/mobile_base.hpp"
-#include "wrp_sdk/asyncio/async_serial.hpp"
 
 #include "wrp_sdk/platforms/scout/scout_protocol.h"
 #include "wrp_sdk/platforms/scout/scout_can_parser.h"
 #include "wrp_sdk/platforms/scout/scout_uart_parser.h"
-
 #include "wrp_sdk/platforms/scout/scout_types.hpp"
 
-namespace westonrobot
+namespace westonrobot {
-{
+class ScoutBase : public MobileBase {
-class ScoutBase
+ public:
-{
+  ScoutBase() : MobileBase(){};
-public:
+  ~ScoutBase() = default;
-    ScoutBase() = default;
-    ~ScoutBase();
 
-    // do not allow copy
+ public:
-    ScoutBase(const ScoutBase &scout) = delete;
+  // motion control
-    ScoutBase &operator=(const ScoutBase &scout) = delete;
+  void SetMotionCommand(double linear_vel, double angular_vel,
+                        ScoutMotionCmd::FaultClearFlag fault_clr_flag =
+                            ScoutMotionCmd::FaultClearFlag::NO_FAULT);
 
-public:
+  // light control
-    // connect to roboot from CAN or serial
+  void SetLightCommand(ScoutLightCmd cmd);
-    void Connect(std::string dev_name, int32_t baud_rate = 0);
+  void DisableLightCmdControl();
 
-    // disconnect from roboot, only valid for serial port
+  // get robot state
-    void Disconnect();
+  ScoutState GetScoutState();
 
-    // cmd thread runs at 100Hz (10ms) by default
+ private:
-    void SetCmdThreadPeriodMs(int32_t period_ms) { cmd_thread_period_ms_ = period_ms; };
+  // serial port buffer
+  uint8_t tx_cmd_len_;
+  uint8_t tx_buffer_[SCOUT_CMD_BUF_LEN];
 
-    // motion control
+  // cmd/status update related variables
-    void SetMotionCommand(double linear_vel, double angular_vel,
+  std::thread cmd_thread_;
-                          ScoutMotionCmd::FaultClearFlag fault_clr_flag = ScoutMotionCmd::FaultClearFlag::NO_FAULT);
+  std::mutex scout_state_mutex_;
+  std::mutex motion_cmd_mutex_;
+  std::mutex light_cmd_mutex_;
 
-    // light control
+  ScoutState scout_state_;
-    void SetLightCommand(ScoutLightCmd cmd);
+  ScoutMotionCmd current_motion_cmd_;
-    void DisableLightCmdControl();
+  ScoutLightCmd current_light_cmd_;
 
-    // get robot state
+  int32_t cmd_thread_period_ms_ = 10;
-    ScoutState GetScoutState();
+  bool cmd_thread_started_ = false;
 
-private:
+  bool light_ctrl_enabled_ = false;
-    // hardware communication interface
+  bool light_ctrl_requested_ = false;
-    std::shared_ptr<ASyncCAN> can_if_;
-    std::shared_ptr<ASyncSerial> serial_if_;
 
-    // CAN priority higher than serial if both connected
+  // internal functions
-    bool can_connected_ = false;
+  void SendRobotCmd() override;
-    bool serial_connected_ = false;
+  void ParseCANFrame(can_frame *rx_frame);
+  void ParseUARTBuffer(uint8_t *buf, const size_t bufsize,
+                       size_t bytes_received);
 
-    // serial port related variables
+  void SendMotionCmd(uint8_t count);
-    uint8_t tx_cmd_len_;
+  void SendLightCmd(uint8_t count);
-    uint8_t tx_buffer_[SCOUT_CMD_BUF_LEN];
+  void NewStatusMsgReceivedCallback(const ScoutMessage &msg);
 
-    // cmd/status update related variables
+ public:
-    std::thread cmd_thread_;
+  static void UpdateScoutState(const ScoutMessage &status_msg,
-    std::mutex scout_state_mutex_;
+                               ScoutState &state);
-    std::mutex motion_cmd_mutex_;
-    std::mutex light_cmd_mutex_;
-
-    ScoutState scout_state_;
-    ScoutMotionCmd current_motion_cmd_;
-    ScoutLightCmd current_light_cmd_;
-
-    int32_t cmd_thread_period_ms_ = 10;
-    bool cmd_thread_started_ = false;
-
-    bool light_ctrl_enabled_ = false;
-    bool light_ctrl_requested_ = false;
-
-    // internal functions
-    void ConfigureCANBus(const std::string &can_if_name = "can1");
-    void ConfigureSerial(const std::string uart_name = "/dev/ttyUSB0", int32_t baud_rate = 115200);
-
-    void StartCmdThread();
-    void ControlLoop(int32_t period_ms);
-
-    void SendMotionCmd(uint8_t count);
-    void SendLightCmd(uint8_t count);
-
-    void ParseCANFrame(can_frame *rx_frame);
-    void ParseUARTBuffer(uint8_t *buf, const size_t bufsize, size_t bytes_received);
-
-    void NewStatusMsgReceivedCallback(const ScoutMessage &msg);
-
-public:
-    static void UpdateScoutState(const ScoutMessage &status_msg, ScoutState &state);
 };
-} // namespace westonrobot
+}  // namespace westonrobot
 
 #endif /* SCOUT_BASE_HPP */

src/platforms/scout_base.cpp

@@ -12,327 +12,270 @@
 
 #include "stopwatch.h"
 
-namespace westonrobot
+namespace westonrobot {
-{
+void ScoutBase::SendRobotCmd() {
-ScoutBase::~ScoutBase()
+  static uint8_t cmd_count = 0;
-{
+  static uint8_t light_cmd_count = 0;
-    if (serial_connected_)
+  SendMotionCmd(cmd_count++);
-        serial_if_->close();
+  if (light_ctrl_requested_) SendLightCmd(light_cmd_count++);
-
-    if (cmd_thread_.joinable())
-        cmd_thread_.join();
 }
 
-void ScoutBase::Connect(std::string dev_name, int32_t baud_rate)
+void ScoutBase::SendMotionCmd(uint8_t count) {
-{
+  // motion control message
-    if (baud_rate == 0)
+  ScoutMessage m_msg;
-    {
+  m_msg.type = ScoutMotionControlMsg;
-        ConfigureCANBus(dev_name);
-    }
-    else
-    {
-        ConfigureSerial(dev_name, baud_rate);
 
-        if (!serial_connected_)
+  if (can_connected_)
-            std::cerr << "ERROR: Failed to connect to serial port" << std::endl;
+    m_msg.body.motion_control_msg.data.cmd.control_mode = CTRL_MODE_CMD_CAN;
-    }
+  else if (serial_connected_)
+    m_msg.body.motion_control_msg.data.cmd.control_mode = CTRL_MODE_CMD_UART;
+
+  motion_cmd_mutex_.lock();
+  m_msg.body.motion_control_msg.data.cmd.fault_clear_flag =
+      static_cast<uint8_t>(current_motion_cmd_.fault_clear_flag);
+  m_msg.body.motion_control_msg.data.cmd.linear_velocity_cmd =
+      current_motion_cmd_.linear_velocity;
+  m_msg.body.motion_control_msg.data.cmd.angular_velocity_cmd =
+      current_motion_cmd_.angular_velocity;
+  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.count = count;
+
+  if (can_connected_)
+    m_msg.body.motion_control_msg.data.cmd.checksum =
+        CalcScoutCANChecksum(CAN_MSG_MOTION_CONTROL_CMD_ID,
+                             m_msg.body.motion_control_msg.data.raw, 8);
+  // serial_connected_: checksum will be calculated later when packed into a
+  // complete serial frame
+
+  if (can_connected_) {
+    // send to can bus
+    can_frame m_frame;
+    EncodeScoutMsgToCAN(&m_msg, &m_frame);
+    can_if_->send_frame(m_frame);
+  } else {
+    // send to serial port
+    EncodeScoutMsgToUART(&m_msg, tx_buffer_, &tx_cmd_len_);
+    serial_if_->send_bytes(tx_buffer_, tx_cmd_len_);
+  }
 }
 
-void ScoutBase::Disconnect()
+void ScoutBase::SendLightCmd(uint8_t count) {
-{
+  ScoutMessage l_msg;
-    if (serial_connected_)
+  l_msg.type = ScoutLightControlMsg;
-    {
+
-        if (serial_if_->is_open())
+  light_cmd_mutex_.lock();
-            serial_if_->close();
+  if (light_ctrl_enabled_) {
-    }
+    l_msg.body.light_control_msg.data.cmd.light_ctrl_enable = LIGHT_ENABLE_CTRL;
+
+    l_msg.body.light_control_msg.data.cmd.front_light_mode =
+        static_cast<uint8_t>(current_light_cmd_.front_mode);
+    l_msg.body.light_control_msg.data.cmd.front_light_custom =
+        current_light_cmd_.front_custom_value;
+    l_msg.body.light_control_msg.data.cmd.rear_light_mode =
+        static_cast<uint8_t>(current_light_cmd_.rear_mode);
+    l_msg.body.light_control_msg.data.cmd.rear_light_custom =
+        current_light_cmd_.rear_custom_value;
+
+    // std::cout << "cmd: " << l_msg.data.cmd.front_light_mode << " , " <<
+    // l_msg.data.cmd.front_light_custom << " , "
+    //           << l_msg.data.cmd.rear_light_mode << " , " <<
+    //           l_msg.data.cmd.rear_light_custom << std::endl;
+    // std::cout << "light cmd generated" << std::endl;
+  } else {
+    l_msg.body.light_control_msg.data.cmd.light_ctrl_enable =
+        LIGHT_DISABLE_CTRL;
+
+    l_msg.body.light_control_msg.data.cmd.front_light_mode =
+        LIGHT_MODE_CONST_OFF;
+    l_msg.body.light_control_msg.data.cmd.front_light_custom = 0;
+    l_msg.body.light_control_msg.data.cmd.rear_light_mode =
+        LIGHT_MODE_CONST_OFF;
+    l_msg.body.light_control_msg.data.cmd.rear_light_custom = 0;
+  }
+  light_ctrl_requested_ = false;
+  light_cmd_mutex_.unlock();
+
+  l_msg.body.light_control_msg.data.cmd.reserved0 = 0;
+  l_msg.body.light_control_msg.data.cmd.count = count;
+
+  if (can_connected_)
+    l_msg.body.light_control_msg.data.cmd.checksum = CalcScoutCANChecksum(
+        CAN_MSG_LIGHT_CONTROL_CMD_ID, l_msg.body.light_control_msg.data.raw, 8);
+  // serial_connected_: checksum will be calculated later when packed into a
+  // complete serial frame
+
+  if (can_connected_) {
+    // send to can bus
+    can_frame l_frame;
+    EncodeScoutMsgToCAN(&l_msg, &l_frame);
+
+    can_if_->send_frame(l_frame);
+  } else {
+    // send to serial port
+    EncodeScoutMsgToUART(&l_msg, tx_buffer_, &tx_cmd_len_);
+    serial_if_->send_bytes(tx_buffer_, tx_cmd_len_);
+  }
+
+  // std::cout << "cmd: " << static_cast<int>(l_msg.data.cmd.front_light_mode)
+  // << " , " << static_cast<int>(l_msg.data.cmd.front_light_custom) << " , "
+  //           << static_cast<int>(l_msg.data.cmd.rear_light_mode) << " , " <<
+  //           static_cast<int>(l_msg.data.cmd.rear_light_custom) << std::endl;
+  // std::cout << "can: ";
+  // for (int i = 0; i < 8; ++i)
+  //     std::cout << static_cast<int>(l_frame.data[i]) << " ";
+  // std::cout << "uart: ";
+  // for (int i = 0; i < tx_cmd_len_; ++i)
+  //     std::cout << static_cast<int>(tx_buffer_[i]) << " ";
+  // std::cout << std::endl;
 }
 
-void ScoutBase::ConfigureCANBus(const std::string &can_if_name)
+ScoutState ScoutBase::GetScoutState() {
-{
+  std::lock_guard<std::mutex> guard(scout_state_mutex_);
-    can_if_ = std::make_shared<ASyncCAN>(can_if_name);
+  return scout_state_;
-
-    can_if_->set_receive_callback(std::bind(&ScoutBase::ParseCANFrame, this, std::placeholders::_1));
-
-    can_connected_ = true;
 }
 
-void ScoutBase::ConfigureSerial(const std::string uart_name, int32_t baud_rate)
+void ScoutBase::SetMotionCommand(
-{
+    double linear_vel, double angular_vel,
-    serial_if_ = std::make_shared<ASyncSerial>(uart_name, baud_rate);
+    ScoutMotionCmd::FaultClearFlag fault_clr_flag) {
-    serial_if_->open();
+  // make sure cmd thread is started before attempting to send commands
+  if (!cmd_thread_started_) StartCmdThread();
 
-    if (serial_if_->is_open())
+  if (linear_vel < ScoutMotionCmd::min_linear_velocity)
-        serial_connected_ = true;
+    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;
 
-    serial_if_->set_receive_callback(std::bind(&ScoutBase::ParseUARTBuffer, this,
+  std::lock_guard<std::mutex> guard(motion_cmd_mutex_);
-                                               std::placeholders::_1,
+  current_motion_cmd_.linear_velocity = static_cast<int8_t>(
-                                               std::placeholders::_2,
+      linear_vel / ScoutMotionCmd::max_linear_velocity * 100.0);
-                                               std::placeholders::_3));
+  current_motion_cmd_.angular_velocity = static_cast<int8_t>(
+      angular_vel / ScoutMotionCmd::max_angular_velocity * 100.0);
+  current_motion_cmd_.fault_clear_flag = fault_clr_flag;
 }
 
-void ScoutBase::StartCmdThread()
+void ScoutBase::SetLightCommand(ScoutLightCmd cmd) {
-{
+  if (!cmd_thread_started_) StartCmdThread();
-    current_motion_cmd_.linear_velocity = 0;
-    current_motion_cmd_.angular_velocity = 0;
-    current_motion_cmd_.fault_clear_flag = ScoutMotionCmd::FaultClearFlag::NO_FAULT;
 
-    cmd_thread_ = std::thread(std::bind(&ScoutBase::ControlLoop, this, cmd_thread_period_ms_));
+  std::lock_guard<std::mutex> guard(light_cmd_mutex_);
-    cmd_thread_started_ = true;
+  current_light_cmd_ = cmd;
+  light_ctrl_enabled_ = true;
+  light_ctrl_requested_ = true;
 }
 
-void ScoutBase::SendMotionCmd(uint8_t count)
+void ScoutBase::DisableLightCmdControl() {
-{
+  std::lock_guard<std::mutex> guard(light_cmd_mutex_);
-    // motion control message
+  light_ctrl_enabled_ = false;
-    ScoutMessage m_msg;
+  light_ctrl_requested_ = true;
-    m_msg.type = ScoutMotionControlMsg;
-
-    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_control_msg.data.cmd.control_mode = CTRL_MODE_CMD_UART;
-
-    motion_cmd_mutex_.lock();
-    m_msg.body.motion_control_msg.data.cmd.fault_clear_flag = static_cast<uint8_t>(current_motion_cmd_.fault_clear_flag);
-    m_msg.body.motion_control_msg.data.cmd.linear_velocity_cmd = current_motion_cmd_.linear_velocity;
-    m_msg.body.motion_control_msg.data.cmd.angular_velocity_cmd = current_motion_cmd_.angular_velocity;
-    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.count = count;
-
-    if (can_connected_)
-        m_msg.body.motion_control_msg.data.cmd.checksum = CalcScoutCANChecksum(CAN_MSG_MOTION_CONTROL_CMD_ID, m_msg.body.motion_control_msg.data.raw, 8);
-    // serial_connected_: checksum will be calculated later when packed into a complete serial frame
-
-    if (can_connected_)
-    {
-        // send to can bus
-        can_frame m_frame;
-        EncodeScoutMsgToCAN(&m_msg, &m_frame);
-        can_if_->send_frame(m_frame);
-    }
-    else
-    {
-        // send to serial port
-        EncodeScoutMsgToUART(&m_msg, tx_buffer_, &tx_cmd_len_);
-        serial_if_->send_bytes(tx_buffer_, tx_cmd_len_);
-    }
 }
 
-void ScoutBase::SendLightCmd(uint8_t count)
+void ScoutBase::ParseCANFrame(can_frame *rx_frame) {
-{
+  // validate checksum, discard frame if fails
-    ScoutMessage l_msg;
+  if (!rx_frame->data[7] == CalcScoutCANChecksum(rx_frame->can_id,
-    l_msg.type = ScoutLightControlMsg;
+                                                 rx_frame->data,
+                                                 rx_frame->can_dlc)) {
+    std::cerr << "ERROR: checksum mismatch, discard frame with id "
+              << rx_frame->can_id << std::endl;
+    return;
+  }
 
-    light_cmd_mutex_.lock();
+  // otherwise, update robot state with new frame
-    if (light_ctrl_enabled_)
+  ScoutMessage status_msg;
-    {
+  DecodeScoutMsgFromCAN(rx_frame, &status_msg);
-        l_msg.body.light_control_msg.data.cmd.light_ctrl_enable = LIGHT_ENABLE_CTRL;
+  NewStatusMsgReceivedCallback(status_msg);
-
-        l_msg.body.light_control_msg.data.cmd.front_light_mode = static_cast<uint8_t>(current_light_cmd_.front_mode);
-        l_msg.body.light_control_msg.data.cmd.front_light_custom = current_light_cmd_.front_custom_value;
-        l_msg.body.light_control_msg.data.cmd.rear_light_mode = static_cast<uint8_t>(current_light_cmd_.rear_mode);
-        l_msg.body.light_control_msg.data.cmd.rear_light_custom = current_light_cmd_.rear_custom_value;
-
-        // std::cout << "cmd: " << l_msg.data.cmd.front_light_mode << " , " << l_msg.data.cmd.front_light_custom << " , "
-        //           << l_msg.data.cmd.rear_light_mode << " , " << l_msg.data.cmd.rear_light_custom << std::endl;
-        // std::cout << "light cmd generated" << std::endl;
-    }
-    else
-    {
-        l_msg.body.light_control_msg.data.cmd.light_ctrl_enable = LIGHT_DISABLE_CTRL;
-
-        l_msg.body.light_control_msg.data.cmd.front_light_mode = LIGHT_MODE_CONST_OFF;
-        l_msg.body.light_control_msg.data.cmd.front_light_custom = 0;
-        l_msg.body.light_control_msg.data.cmd.rear_light_mode = LIGHT_MODE_CONST_OFF;
-        l_msg.body.light_control_msg.data.cmd.rear_light_custom = 0;
-    }
-    light_ctrl_requested_ = false;
-    light_cmd_mutex_.unlock();
-
-    l_msg.body.light_control_msg.data.cmd.reserved0 = 0;
-    l_msg.body.light_control_msg.data.cmd.count = count;
-
-    if (can_connected_)
-        l_msg.body.light_control_msg.data.cmd.checksum = CalcScoutCANChecksum(CAN_MSG_LIGHT_CONTROL_CMD_ID, l_msg.body.light_control_msg.data.raw, 8);
-    // serial_connected_: checksum will be calculated later when packed into a complete serial frame
-
-    if (can_connected_)
-    {
-        // send to can bus
-        can_frame l_frame;
-        EncodeScoutMsgToCAN(&l_msg, &l_frame);
-
-        can_if_->send_frame(l_frame);
-    }
-    else
-    {
-        // send to serial port
-        EncodeScoutMsgToUART(&l_msg, tx_buffer_, &tx_cmd_len_);
-        serial_if_->send_bytes(tx_buffer_, tx_cmd_len_);
-    }
-
-    // std::cout << "cmd: " << static_cast<int>(l_msg.data.cmd.front_light_mode) << " , " << static_cast<int>(l_msg.data.cmd.front_light_custom) << " , "
-    //           << static_cast<int>(l_msg.data.cmd.rear_light_mode) << " , " << static_cast<int>(l_msg.data.cmd.rear_light_custom) << std::endl;
-    // std::cout << "can: ";
-    // for (int i = 0; i < 8; ++i)
-    //     std::cout << static_cast<int>(l_frame.data[i]) << " ";
-    // std::cout << "uart: ";
-    // for (int i = 0; i < tx_cmd_len_; ++i)
-    //     std::cout << static_cast<int>(tx_buffer_[i]) << " ";
-    // std::cout << std::endl;
 }
 
-void ScoutBase::ControlLoop(int32_t period_ms)
+void ScoutBase::ParseUARTBuffer(uint8_t *buf, const size_t bufsize,
-{
+                                size_t bytes_received) {
-    StopWatch ctrl_sw;
+  // std::cout << "bytes received from serial: " << bytes_received << std::endl;
-    uint8_t cmd_count = 0;
+  // serial_parser_.PrintStatistics();
-    uint8_t light_cmd_count = 0;
+  // serial_parser_.ParseBuffer(buf, bytes_received);
-    while (true)
+  ScoutMessage status_msg;
-    {
+  for (int i = 0; i < bytes_received; ++i) {
-        ctrl_sw.tic();
+    if (DecodeScoutMsgFromUART(buf[i], &status_msg))
+      NewStatusMsgReceivedCallback(status_msg);
+  }
+}
 
-        // motion control message
+void ScoutBase::NewStatusMsgReceivedCallback(const ScoutMessage &msg) {
-        SendMotionCmd(cmd_count++);
+  // std::cout << "new status msg received" << std::endl;
+  std::lock_guard<std::mutex> guard(scout_state_mutex_);
+  UpdateScoutState(msg, scout_state_);
+}
 
-        // check if there is request for light control
+void ScoutBase::UpdateScoutState(const ScoutMessage &status_msg,
-        if (light_ctrl_requested_)
+                                 ScoutState &state) {
-            SendLightCmd(light_cmd_count++);
+  switch (status_msg.type) {
-
+    case ScoutMotionStatusMsg: {
-        ctrl_sw.sleep_until_ms(period_ms);
+      // std::cout << "motion control feedback received" << std::endl;
-        // std::cout << "control loop update frequency: " << 1.0 / ctrl_sw.toc() << std::endl;
+      const MotionStatusMessage &msg = status_msg.body.motion_status_msg;
+      state.linear_velocity =
+          static_cast<int16_t>(
+              static_cast<uint16_t>(msg.data.status.linear_velocity.low_byte) |
+              static_cast<uint16_t>(msg.data.status.linear_velocity.high_byte)
+                  << 8) /
+          1000.0;
+      state.angular_velocity =
+          static_cast<int16_t>(
+              static_cast<uint16_t>(msg.data.status.angular_velocity.low_byte) |
+              static_cast<uint16_t>(msg.data.status.angular_velocity.high_byte)
+                  << 8) /
+          1000.0;
+      break;
     }
-}
+    case ScoutLightStatusMsg: {
-
+      // std::cout << "light control feedback received" << std::endl;
-ScoutState ScoutBase::GetScoutState()
+      const LightStatusMessage &msg = status_msg.body.light_status_msg;
-{
+      if (msg.data.status.light_ctrl_enable == LIGHT_DISABLE_CTRL)
-    std::lock_guard<std::mutex> guard(scout_state_mutex_);
+        state.light_control_enabled = false;
-    return scout_state_;
+      else
-}
+        state.light_control_enabled = true;
-
+      state.front_light_state.mode = msg.data.status.front_light_mode;
-void ScoutBase::SetMotionCommand(double linear_vel, double angular_vel, ScoutMotionCmd::FaultClearFlag fault_clr_flag)
+      state.front_light_state.custom_value = msg.data.status.front_light_custom;
-{
+      state.rear_light_state.mode = msg.data.status.rear_light_mode;
-    // make sure cmd thread is started before attempting to send commands
+      state.rear_light_state.custom_value = msg.data.status.rear_light_custom;
-    if (!cmd_thread_started_)
+      break;
-        StartCmdThread();
-
-    if (linear_vel < ScoutMotionCmd::min_linear_velocity)
-        linear_vel = ScoutMotionCmd::min_linear_velocity;
-    if (linear_vel > ScoutMotionCmd::max_linear_velocity)
-        linear_vel = ScoutMotionCmd::max_linear_velocity;
-    if (angular_vel < ScoutMotionCmd::min_angular_velocity)
-        angular_vel = ScoutMotionCmd::min_angular_velocity;
-    if (angular_vel > ScoutMotionCmd::max_angular_velocity)
-        angular_vel = ScoutMotionCmd::max_angular_velocity;
-
-    std::lock_guard<std::mutex> guard(motion_cmd_mutex_);
-    current_motion_cmd_.linear_velocity = static_cast<int8_t>(linear_vel / ScoutMotionCmd::max_linear_velocity * 100.0);
-    current_motion_cmd_.angular_velocity = static_cast<int8_t>(angular_vel / ScoutMotionCmd::max_angular_velocity * 100.0);
-    current_motion_cmd_.fault_clear_flag = fault_clr_flag;
-}
-
-void ScoutBase::SetLightCommand(ScoutLightCmd cmd)
-{
-    if (!cmd_thread_started_)
-        StartCmdThread();
-
-    std::lock_guard<std::mutex> guard(light_cmd_mutex_);
-    current_light_cmd_ = cmd;
-    light_ctrl_enabled_ = true;
-    light_ctrl_requested_ = true;
-}
-
-void ScoutBase::DisableLightCmdControl()
-{
-    std::lock_guard<std::mutex> guard(light_cmd_mutex_);
-    light_ctrl_enabled_ = false;
-    light_ctrl_requested_ = true;
-}
-
-void ScoutBase::ParseCANFrame(can_frame *rx_frame)
-{
-    // validate checksum, discard frame if fails
-    if (!rx_frame->data[7] == CalcScoutCANChecksum(rx_frame->can_id, rx_frame->data, rx_frame->can_dlc))
-    {
-        std::cerr << "ERROR: checksum mismatch, discard frame with id " << rx_frame->can_id << std::endl;
-        return;
     }
-
+    case ScoutSystemStatusMsg: {
-    // otherwise, update robot state with new frame
+      // std::cout << "system status feedback received" << std::endl;
-    ScoutMessage status_msg;
+      const SystemStatusMessage &msg = status_msg.body.system_status_msg;
-    DecodeScoutMsgFromCAN(rx_frame, &status_msg);
+      state.control_mode = msg.data.status.control_mode;
-    NewStatusMsgReceivedCallback(status_msg);
+      state.base_state = msg.data.status.base_state;
+      state.battery_voltage =
+          (static_cast<uint16_t>(msg.data.status.battery_voltage.low_byte) |
+           static_cast<uint16_t>(msg.data.status.battery_voltage.high_byte)
+               << 8) /
+          10.0;
+      state.fault_code =
+          (static_cast<uint16_t>(msg.data.status.fault_code.low_byte) |
+           static_cast<uint16_t>(msg.data.status.fault_code.high_byte) << 8);
+      break;
+    }
+    case ScoutMotorDriverStatusMsg: {
+      // std::cout << "motor 1 driver feedback received" << std::endl;
+      const MotorDriverStatusMessage &msg =
+          status_msg.body.motor_driver_status_msg;
+      for (int i = 0; i < ScoutState::motor_num; ++i) {
+        state.motor_states[status_msg.body.motor_driver_status_msg.motor_id]
+            .current =
+            (static_cast<uint16_t>(msg.data.status.current.low_byte) |
+             static_cast<uint16_t>(msg.data.status.current.high_byte) << 8) /
+            10.0;
+        state.motor_states[status_msg.body.motor_driver_status_msg.motor_id]
+            .rpm = static_cast<int16_t>(
+            static_cast<uint16_t>(msg.data.status.rpm.low_byte) |
+            static_cast<uint16_t>(msg.data.status.rpm.high_byte) << 8);
+        state.motor_states[status_msg.body.motor_driver_status_msg.motor_id]
+            .temperature = msg.data.status.temperature;
+      }
+      break;
+    }
+  }
 }
-
+}  // namespace westonrobot
-void ScoutBase::ParseUARTBuffer(uint8_t *buf, const size_t bufsize, size_t bytes_received)
-{
-    // std::cout << "bytes received from serial: " << bytes_received << std::endl;
-    // serial_parser_.PrintStatistics();
-    // serial_parser_.ParseBuffer(buf, bytes_received);
-    ScoutMessage status_msg;
-    for (int i = 0; i < bytes_received; ++i)
-    {
-        if (DecodeScoutMsgFromUART(buf[i], &status_msg))
-            NewStatusMsgReceivedCallback(status_msg);
-    }
-}
-
-void ScoutBase::NewStatusMsgReceivedCallback(const ScoutMessage &msg)
-{
-    // std::cout << "new status msg received" << std::endl;
-    std::lock_guard<std::mutex> guard(scout_state_mutex_);
-    UpdateScoutState(msg, scout_state_);
-}
-
-void ScoutBase::UpdateScoutState(const ScoutMessage &status_msg, ScoutState &state)
-{
-    switch (status_msg.type)
-    {
-    case ScoutMotionStatusMsg:
-    {
-        // std::cout << "motion control feedback received" << std::endl;
-        const MotionStatusMessage &msg = status_msg.body.motion_status_msg;
-        state.linear_velocity = static_cast<int16_t>(static_cast<uint16_t>(msg.data.status.linear_velocity.low_byte) | static_cast<uint16_t>(msg.data.status.linear_velocity.high_byte) << 8) / 1000.0;
-        state.angular_velocity = static_cast<int16_t>(static_cast<uint16_t>(msg.data.status.angular_velocity.low_byte) | static_cast<uint16_t>(msg.data.status.angular_velocity.high_byte) << 8) / 1000.0;
-        break;
-    }
-    case ScoutLightStatusMsg:
-    {
-        // std::cout << "light control feedback received" << std::endl;
-        const LightStatusMessage &msg = status_msg.body.light_status_msg;
-        if (msg.data.status.light_ctrl_enable == LIGHT_DISABLE_CTRL)
-            state.light_control_enabled = false;
-        else
-            state.light_control_enabled = true;
-        state.front_light_state.mode = msg.data.status.front_light_mode;
-        state.front_light_state.custom_value = msg.data.status.front_light_custom;
-        state.rear_light_state.mode = msg.data.status.rear_light_mode;
-        state.rear_light_state.custom_value = msg.data.status.rear_light_custom;
-        break;
-    }
-    case ScoutSystemStatusMsg:
-    {
-        // 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.base_state = msg.data.status.base_state;
-        state.battery_voltage = (static_cast<uint16_t>(msg.data.status.battery_voltage.low_byte) | static_cast<uint16_t>(msg.data.status.battery_voltage.high_byte) << 8) / 10.0;
-        state.fault_code = (static_cast<uint16_t>(msg.data.status.fault_code.low_byte) | static_cast<uint16_t>(msg.data.status.fault_code.high_byte) << 8);
-        break;
-    }
-    case ScoutMotorDriverStatusMsg:
-    {
-        // std::cout << "motor 1 driver feedback received" << std::endl;
-        const MotorDriverStatusMessage &msg = status_msg.body.motor_driver_status_msg;
-        for (int i = 0; i < ScoutState::motor_num; ++i)
-        {
-            state.motor_states[status_msg.body.motor_driver_status_msg.motor_id].current = (static_cast<uint16_t>(msg.data.status.current.low_byte) | static_cast<uint16_t>(msg.data.status.current.high_byte) << 8) / 10.0;
-            state.motor_states[status_msg.body.motor_driver_status_msg.motor_id].rpm = static_cast<int16_t>(static_cast<uint16_t>(msg.data.status.rpm.low_byte) | static_cast<uint16_t>(msg.data.status.rpm.high_byte) << 8);
-            state.motor_states[status_msg.body.motor_driver_status_msg.motor_id].temperature = msg.data.status.temperature;
-        }
-        break;
-    }
-    }
-}
-} // namespace westonrobot