more code cleanup

README.md

@@ -5,6 +5,7 @@ Copyright (c) 2020 [WestonRobot](https://www.westonrobot.com/)
 ## Introduction
 
 Supported platforms
+
 * **Scout**: skid-steer mobile base
 * **Hunter**: ackermann mobile base
 

apps/hunter_demo/hunter_demo.cpp

@@ -9,7 +9,7 @@
 
 #include "hunter_base/hunter_base.hpp"
 
-using namespace wescore;
+using namespace westonrobot;
 
 int main(int argc, char **argv)
 {

apps/scout_demo/scout_demo.cpp

@@ -9,7 +9,7 @@
 
 #include "scout_base/scout_base.hpp"
 
-using namespace wescore;
+using namespace westonrobot;
 
 int main(int argc, char **argv)
 {

apps/scout_demo/scout_discharge.cpp

@@ -9,7 +9,7 @@
 
 #include "scout_base/scout_base.hpp"
 
-using namespace wescore;
+using namespace westonrobot;
 
 int main(int argc, char **argv)
 {

apps/scout_monitor/app/app_scout_monitor.cpp

@@ -7,7 +7,7 @@
 
 #include "monitor/scout_monitor.hpp"
 
-using namespace wescore;
+using namespace westonrobot;
 
 int main(int argc, char **argv)
 {

apps/scout_monitor/include/monitor/ncolors.hpp

@@ -17,7 +17,7 @@
 
 #include <ncurses.h>
 
-namespace wescore
+namespace westonrobot
 {
 struct NColors
 {
@@ -62,6 +62,6 @@ struct NColors
     static void WSetColor(WINDOW *win, int fg, int bg = BLACK);
     static void WUnsetColor(WINDOW *win, int fg, int bg = BLACK);
 };
-} // namespace wescore
+} // namespace westonrobot
 
 #endif /* NCOLORS_HPP */

apps/scout_monitor/include/monitor/nshapes.hpp

@@ -12,13 +12,13 @@
 
 #include <ncurses.h>
 
-namespace wescore
+namespace westonrobot
 {
 struct NShapes
 {
     static void DrawRectangle(int tl_y, int tl_x, int br_y, int br_x);
     static void WDrawRectangle(WINDOW *win, int tl_y, int tl_x, int br_y, int br_x);
 };
-} // namespace wescore
+} // namespace westonrobot
 
 #endif /* NSHAPES_HPP */

apps/scout_monitor/include/monitor/scout_monitor.hpp

@@ -14,7 +14,7 @@
 
 #include <ncurses.h>
 
-namespace wescore
+namespace westonrobot
 {
 class ScoutMonitor
 {
@@ -73,6 +73,6 @@ private:
     void UpdateScoutSystemInfo();
     void UpdateScoutCmdWindow();
 };
-} // namespace wescore
+} // namespace westonrobot
 
 #endif /* SCOUT_MONITOR_HPP */

apps/scout_monitor/src/ncolors.cpp

@@ -58,7 +58,7 @@ short CursorColor(int fg)
 }
 } // namespace
 
-namespace wescore
+namespace westonrobot
 {
 void NColors::InitColors()
 {
@@ -111,4 +111,4 @@ void NColors::WUnsetColor(WINDOW *win, int fg, int bg)
     if (IsBold(fg))
         wattroff(win, A_BOLD);
 }
-} // namespace wescore
+} // namespace westonrobot

apps/scout_monitor/src/nshapes.cpp

@@ -9,7 +9,7 @@
 
 #include "monitor/nshapes.hpp"
 
-namespace wescore
+namespace westonrobot
 {
 void NShapes::DrawRectangle(int tl_y, int tl_x, int br_y, int br_x)
 {
@@ -38,4 +38,4 @@ void NShapes::WDrawRectangle(WINDOW *win, int tl_y, int tl_x, int br_y, int br_x
         mvwprintw(win, br_y, i, "-");
     }
 }
-} // namespace wescore
+} // namespace westonrobot

apps/scout_monitor/src/scout_monitor.cpp

@@ -111,7 +111,7 @@ struct StopWatch
 };
 } // namespace
 
-namespace wescore
+namespace westonrobot
 {
 ScoutMonitor::ScoutMonitor()
 {
@@ -655,4 +655,4 @@ void ScoutMonitor::ShowMotorInfo(int y, int x, double cur, int rpm, int temp, bo
     std::string temp_str = "TMP:" + ConvertFloatToString(temp, 0);
     mvwprintw(body_info_win_, y + 3, col_title, temp_str.c_str());
 }
-} // namespace wescore
+} // namespace westonrobot

apps/scout_monitor/tests/test_ncolor2.cpp

@@ -1,6 +1,6 @@
 #include "monitor/ncolors.hpp"
 
-using namespace wescore;
+using namespace westonrobot;
 
 int main(void)
 {

apps/scout_monitor/tests/test_scout_monitor.cpp

@@ -7,7 +7,7 @@
 
 #include "monitor/scout_monitor.hpp"
 
-using namespace wescore;
+using namespace westonrobot;
 
 ScoutMonitor monitor;
 

apps/scout_monitor/tests/test_scout_monitor_virtual.cpp

@@ -7,7 +7,7 @@
 
 #include "monitor/scout_monitor.hpp"
 
-using namespace wescore;
+using namespace westonrobot;
 
 ScoutMonitor monitor;
 

apps/tracer_demo/tracer_demo.cpp

@@ -9,7 +9,7 @@
 
 #include "tracer_base/tracer_base.hpp"
 
-using namespace wescore;
+using namespace westonrobot;
 
 int main(int argc, char **argv)
 {

include/wrp_sdk/asyncio/async_can.hpp

@@ -42,7 +42,7 @@
 // #include "async_io/device_error.hpp"
 // #include "async_io/msg_buffer.hpp"
 
-namespace wescore
+namespace westonrobot
 {
 using steady_clock = std::chrono::steady_clock;
 using lock_guard = std::lock_guard<std::recursive_mutex>;
@@ -125,6 +125,6 @@ private:
     void iostat_tx_add(std::size_t bytes);
     void iostat_rx_add(std::size_t bytes);
 };
-} // namespace wescore
+} // namespace westonrobot
 
 #endif /* ASYNC_CAN_HPP */

include/wrp_sdk/asyncio/async_serial.hpp

@@ -40,10 +40,10 @@
 
 #include "asio.hpp"
 
-#include "wrp_sdk/async_io/device_error.hpp"
+#include "wrp_sdk/asyncio/device_error.hpp"
-#include "wrp_sdk/async_io/msg_buffer.hpp"
+#include "wrp_sdk/asyncio/msg_buffer.hpp"
 
-namespace wescore
+namespace westonrobot
 {
 using steady_clock = std::chrono::steady_clock;
 using lock_guard = std::lock_guard<std::recursive_mutex>;
@@ -140,6 +140,6 @@ private:
     void iostat_tx_add(std::size_t bytes);
     void iostat_rx_add(std::size_t bytes);
 };
-} // namespace wescore
+} // namespace westonrobot
 
 #endif /* ASYNC_SERIAL_HPP */

include/wrp_sdk/asyncio/device_error.hpp

@@ -19,7 +19,7 @@
 #include <cstring>
 #include <stdexcept>
 
-namespace wescore
+namespace westonrobot
 {
 class DeviceError : public std::runtime_error
 {

include/wrp_sdk/asyncio/msg_buffer.hpp

@@ -19,7 +19,7 @@
 #include <cstring>
 #include <cassert>
 
-namespace wescore
+namespace westonrobot
 {
 
 /**
@@ -66,4 +66,4 @@ struct MsgBuffer
         return len - pos;
     }
 };
-} // namespace wescore
+} // namespace westonrobot

include/wrp_sdk/platforms/common/mobile_base.hpp

@@ -0,0 +1,96 @@
+/*
+ * 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 <functional>
+
+#include "wrp_sdk/asyncio/async_can.hpp"
+#include "wrp_sdk/asyncio/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;
+
+  // 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();
+
+  // cmd thread runs at 100Hz (10ms) by default
+  void SetCmdThreadPeriodMs(int32_t period_ms) {
+    cmd_thread_period_ms_ = period_ms;
+  };
+
+  // motion control
+  void SetMotionCommand(double linear_vel, double steering_angle,
+                        HunterMotionCmd::FaultClearFlag fault_clr_flag =
+                            HunterMotionCmd::FaultClearFlag::NO_FAULT);
+
+  // get robot state
+  HunterState GetHunterState();
+
+ protected:
+  // hardware communication interface
+  std::shared_ptr<ASyncCAN> can_if_;
+  std::shared_ptr<ASyncSerial> serial_if_;
+
+  // CAN priority higher than serial if both connected
+  bool can_connected_ = false;
+  bool serial_connected_ = false;
+
+  // serial port related variables
+  uint8_t tx_cmd_len_;
+  uint8_t tx_buffer_[HUNTER_CMD_BUF_LEN];
+
+  // cmd/status update related variables
+  std::thread cmd_thread_;
+  std::mutex hunter_state_mutex_;
+  std::mutex motion_cmd_mutex_;
+
+  HunterState hunter_state_;
+  HunterMotionCmd current_motion_cmd_;
+
+  int32_t cmd_thread_period_ms_ = 10;
+  bool cmd_thread_started_ = 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 ParseCANFrame(can_frame *rx_frame);
+  void ParseUARTBuffer(uint8_t *buf, const size_t bufsize,
+                       size_t bytes_received);
+
+  void NewStatusMsgReceivedCallback(const HunterMessage &msg);
+};
+}  // namespace westonrobot
+
+#endif /* MOBILE_BASE_HPP */

include/wrp_sdk/platforms/hunter/hunter_base.hpp

@@ -16,18 +16,16 @@
 #include <mutex>
 #include <functional>
 
-#include "wrp_sdk/async_io/async_can.hpp"
+#include "wrp_sdk/asyncio/async_can.hpp"
-#include "wrp_sdk/async_io/async_serial.hpp"
+#include "wrp_sdk/asyncio/async_serial.hpp"
 
 #include "wrp_sdk/platforms/hunter/hunter_protocol.h"
 #include "wrp_sdk/platforms/hunter/hunter_can_parser.h"
 
 #include "wrp_sdk/platforms/hunter/hunter_types.hpp"
 
-namespace wescore
+namespace westonrobot {
-{
+class HunterBase {
-class HunterBase
-{
  public:
   HunterBase() = default;
   ~HunterBase();
@@ -44,11 +42,14 @@ public:
   void Disconnect();
 
   // cmd thread runs at 100Hz (10ms) by default
-    void SetCmdThreadPeriodMs(int32_t period_ms) { cmd_thread_period_ms_ = period_ms; };
+  void SetCmdThreadPeriodMs(int32_t period_ms) {
+    cmd_thread_period_ms_ = period_ms;
+  };
 
   // motion control
   void SetMotionCommand(double linear_vel, double steering_angle,
-                          HunterMotionCmd::FaultClearFlag fault_clr_flag = HunterMotionCmd::FaultClearFlag::NO_FAULT);
+                        HunterMotionCmd::FaultClearFlag fault_clr_flag =
+                            HunterMotionCmd::FaultClearFlag::NO_FAULT);
 
   // get robot state
   HunterState GetHunterState();
@@ -79,7 +80,8 @@ private:
 
   // 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 ConfigureSerial(const std::string uart_name = "/dev/ttyUSB0",
+                       int32_t baud_rate = 115200);
 
   void StartCmdThread();
   void ControlLoop(int32_t period_ms);
@@ -87,13 +89,15 @@ private:
   void SendMotionCmd(uint8_t count);
 
   void ParseCANFrame(can_frame *rx_frame);
-    void ParseUARTBuffer(uint8_t *buf, const size_t bufsize, size_t bytes_received);
+  void ParseUARTBuffer(uint8_t *buf, const size_t bufsize,
+                       size_t bytes_received);
 
   void NewStatusMsgReceivedCallback(const HunterMessage &msg);
 
  public:
-    static void UpdateHunterState(const HunterMessage &status_msg, HunterState &state);
+  static void UpdateHunterState(const HunterMessage &status_msg,
+                                HunterState &state);
 };
-} // namespace wescore
+}  // namespace westonrobot
 
 #endif /* HUNTER_BASE_HPP */

include/wrp_sdk/platforms/hunter/hunter_types.hpp

@@ -13,7 +13,7 @@
 #include <cstdint>
 #include <iostream>
 
-namespace wescore
+namespace westonrobot
 {
 struct HunterState
 {
@@ -69,6 +69,6 @@ struct HunterMotionCmd
     static constexpr double max_steering_angle = 0.4622;    // 0.4622 rad ~= 26.5 degree
     static constexpr double min_steering_angle = -0.4622;   // -0.4622 rad
 };
-} // namespace wescore
+} // namespace westonrobot
 
 #endif /* HUNTER_TYPES_HPP */

include/wrp_sdk/platforms/scout/scout_base.hpp

@@ -16,8 +16,8 @@
 #include <mutex>
 #include <functional>
 
-#include "wrp_sdk/async_io/async_can.hpp"
+#include "wrp_sdk/asyncio/async_can.hpp"
-#include "wrp_sdk/async_io/async_serial.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"
@@ -25,7 +25,7 @@
 
 #include "wrp_sdk/platforms/scout/scout_types.hpp"
 
-namespace wescore
+namespace westonrobot
 {
 class ScoutBase
 {
@@ -105,6 +105,6 @@ private:
 public:
     static void UpdateScoutState(const ScoutMessage &status_msg, ScoutState &state);
 };
-} // namespace wescore
+} // namespace westonrobot
 
 #endif /* SCOUT_BASE_HPP */

include/wrp_sdk/platforms/scout/scout_types.hpp

@@ -13,7 +13,7 @@
 #include <cstdint>
 #include <iostream>
 
-namespace wescore
+namespace westonrobot
 {
 struct ScoutState
 {
@@ -106,6 +106,6 @@ struct ScoutLightCmd
     LightMode rear_mode;
     uint8_t rear_custom_value;
 };
-} // namespace wescore
+} // namespace westonrobot
 
 #endif /* SCOUT_STATE_HPP */

include/wrp_sdk/platforms/tracer/tracer_base.hpp

@@ -24,7 +24,7 @@
 
 #include "tracer_base/tracer_types.hpp"
 
-namespace wescore
+namespace westonrobot
 {
 class TracerBase
 {
@@ -104,6 +104,6 @@ private:
 public:
     static void UpdateTracerState(const TracerMessage &status_msg, TracerState &state);
 };
-} // namespace wescore
+} // namespace westonrobot
 
 #endif /* TRACER_BASE_HPP */

include/wrp_sdk/platforms/tracer/tracer_types.hpp

@@ -13,7 +13,7 @@
 #include <cstdint>
 #include <iostream>
 
-namespace wescore
+namespace westonrobot
 {
 struct TracerState
 {
@@ -106,6 +106,6 @@ struct TracerLightCmd
     LightMode rear_mode;
     uint8_t rear_custom_value;
 };
-} // namespace wescore
+} // namespace westonrobot
 
 #endif /* TRACER_TYPES_HPP */

src/async_can.cpp

@@ -13,7 +13,7 @@
 #define _GNU_SOURCE
 #endif
 
-#include "wrp_sdk/async_io/async_can.hpp"
+#include "wrp_sdk/asyncio/async_can.hpp"
 
 #include <net/if.h>
 #include <poll.h>
@@ -26,7 +26,7 @@
 
 #include "asyncio_utils.hpp"
 
-using namespace wescore;
+using namespace westonrobot;
 
 using asio::buffer;
 using asio::io_service;

src/async_serial.cpp

@@ -26,14 +26,14 @@
 #include <cassert>
 #include <iostream>
 
-#include "wrp_sdk/async_io/async_serial.hpp"
+#include "wrp_sdk/asyncio/async_serial.hpp"
 #include "asyncio_utils.hpp"
 
 #if defined(__linux__)
 #include <linux/serial.h>
 #endif
 
-using namespace wescore;
+using namespace westonrobot;
 
 using asio::buffer;
 using asio::io_service;

src/asyncio_utils.cpp

@@ -9,7 +9,7 @@
 
 #include "asyncio_utils.hpp"
 
-namespace wescore
+namespace westonrobot
 {
 void url_parse_host(std::string host,
                     std::string &host_out, int &port_out,
@@ -79,4 +79,4 @@ void url_parse_query(std::string query)
     sys.assign(ids_it, comma_it);
     comp.assign(comma_it + 1, query.end());
 }
-} // namespace wescore
+} // namespace westonrobot

src/asyncio_utils.hpp

@@ -19,7 +19,7 @@
 #include <algorithm>
 #include <iostream>
 
-namespace wescore
+namespace westonrobot
 {
 template <typename... Args>
 std::string format(const std::string &fmt, Args... args)
@@ -58,6 +58,6 @@ void url_parse_host(std::string host,
  * Parse ?ids=sid,cid
  */
 void url_parse_query(std::string query);
-} // namespace wescore
+} // namespace westonrobot
 
 #endif /* ASYNCIO_UTILS_HPP */

src/platforms/hunter_base.cpp

@@ -12,7 +12,7 @@
 
 #include "stopwatch.h"
 
-namespace wescore {
+namespace westonrobot {
 HunterBase::~HunterBase() {
   if (serial_connected_) serial_if_->close();
 
@@ -253,4 +253,4 @@ void HunterBase::UpdateHunterState(const HunterMessage &status_msg,
       break;
   }
 }
-}  // namespace wescore
+}  // namespace westonrobot

src/platforms/mobile_base.cpp

@@ -0,0 +1,350 @@
+/*
+ * mobile_base.cpp
+ *
+ * Created on: Jun 17, 2020 11:26
+ * Description:
+ *
+ * Copyright (c) 2020 Ruixiang Du (rdu)
+ */
+
+#include "wrp_sdk/platforms/mobile_base.hpp"
+
+#include <cstring>
+#include <iostream>
+#include <algorithm>
+
+#include "stopwatch.h"
+
+namespace westonrobot {
+MobileBase::~MobileBase() {
+  if (serial_connected_) serial_if_->close();
+  if (cmd_thread_.joinable()) cmd_thread_.join();
+}
+
+void MobileBase::Connect(std::string dev_name, int32_t baud_rate) {
+  if (baud_rate == 0) {
+    ConfigureCANBus(dev_name);
+  } else {
+    ConfigureSerial(dev_name, baud_rate);
+
+    if (!serial_connected_)
+      std::cerr << "ERROR: Failed to connect to serial port" << std::endl;
+  }
+}
+
+void MobileBase::Disconnect() {
+  if (serial_connected_) {
+    if (serial_if_->is_open()) serial_if_->close();
+  }
+}
+
+void MobileBase::ConfigureCANBus(const std::string &can_if_name) {
+  can_if_ = std::make_shared<ASyncCAN>(can_if_name);
+
+  can_if_->set_receive_callback(
+      std::bind(&MobileBase::ParseCANFrame, this, std::placeholders::_1));
+
+  can_connected_ = true;
+}
+
+void MobileBase::ConfigureSerial(const std::string uart_name,
+                                 int32_t baud_rate) {
+  serial_if_ = std::make_shared<ASyncSerial>(uart_name, baud_rate);
+  serial_if_->open();
+
+  if (serial_if_->is_open()) serial_connected_ = true;
+
+  serial_if_->set_receive_callback(
+      std::bind(&MobileBase::ParseUARTBuffer, this, std::placeholders::_1,
+                std::placeholders::_2, std::placeholders::_3));
+}
+
+void MobileBase::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(&MobileBase::ControlLoop, this, cmd_thread_period_ms_));
+  cmd_thread_started_ = true;
+}
+
+void MobileBase::SendMotionCmd(uint8_t count) {
+  // motion control message
+  ScoutMessage m_msg;
+  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 MobileBase::SendLightCmd(uint8_t count) {
+  ScoutMessage l_msg;
+  l_msg.type = ScoutLightControlMsg;
+
+  light_cmd_mutex_.lock();
+  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 MobileBase::ControlLoop(int32_t period_ms) {
+  StopWatch ctrl_sw;
+  uint8_t cmd_count = 0;
+  uint8_t light_cmd_count = 0;
+  while (true) {
+    ctrl_sw.tic();
+
+    // motion control message
+    SendMotionCmd(cmd_count++);
+
+    // check if there is request for light control
+    if (light_ctrl_requested_) SendLightCmd(light_cmd_count++);
+
+    ctrl_sw.sleep_until_ms(period_ms);
+    // std::cout << "control loop update frequency: " << 1.0 / ctrl_sw.toc() <<
+    // std::endl;
+  }
+}
+
+ScoutState MobileBase::GetScoutState() {
+  std::lock_guard<std::mutex> guard(scout_state_mutex_);
+  return scout_state_;
+}
+
+void MobileBase::SetMotionCommand(
+    double linear_vel, double angular_vel,
+    ScoutMotionCmd::FaultClearFlag fault_clr_flag) {
+  // make sure cmd thread is started before attempting to send commands
+  if (!cmd_thread_started_) StartCmdThread();
+
+  if (linear_vel < ScoutMotionCmd::min_linear_velocity)
+    linear_vel = ScoutMotionCmd::min_linear_velocity;
+  if (linear_vel > ScoutMotionCmd::max_linear_velocity)
+    linear_vel = ScoutMotionCmd::max_linear_velocity;
+  if (angular_vel < ScoutMotionCmd::min_angular_velocity)
+    angular_vel = ScoutMotionCmd::min_angular_velocity;
+  if (angular_vel > ScoutMotionCmd::max_angular_velocity)
+    angular_vel = ScoutMotionCmd::max_angular_velocity;
+
+  std::lock_guard<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 MobileBase::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 MobileBase::DisableLightCmdControl() {
+  std::lock_guard<std::mutex> guard(light_cmd_mutex_);
+  light_ctrl_enabled_ = false;
+  light_ctrl_requested_ = true;
+}
+
+void MobileBase::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;
+  }
+
+  // otherwise, update robot state with new frame
+  ScoutMessage status_msg;
+  DecodeScoutMsgFromCAN(rx_frame, &status_msg);
+  NewStatusMsgReceivedCallback(status_msg);
+}
+
+void MobileBase::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 MobileBase::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 MobileBase::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 < 4; ++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

src/platforms/scout_base.cpp

@@ -12,7 +12,7 @@
 
 #include "stopwatch.h"
 
-namespace wescore
+namespace westonrobot
 {
 ScoutBase::~ScoutBase()
 {

src/platforms/tracer_base.cpp

@@ -12,7 +12,7 @@
 
 #include "stopwatch/stopwatch.h"
 
-namespace wescore
+namespace westonrobot
 {
 TracerBase::~TracerBase()
 {
@@ -335,4 +335,4 @@ void TracerBase::UpdateTracerState(const TracerMessage &status_msg, TracerState
     }
     }
 }
-} // namespace wescore
+} // namespace westonrobot

src/stopwatch.h

@@ -35,7 +35,7 @@ For more information, please refer to <http://unlicense.org>
 
 #include <thread>
 
-namespace wescore {
+namespace westonrobot {
 // only supported on x86 processors
 #if (defined __x86_64__) || (defined __i386)
 // An implementation of the 'TrivialClock' concept using the rdtscp instruction.
@@ -157,6 +157,6 @@ struct StopWatch {
   };
 };
 
-}  // namespace wescore
+}  // namespace westonrobot
 
 #endif  // STOPWATCH_H

tests/test_acan.cpp

@@ -1,7 +1,7 @@
 #include <iostream>
-#include "wrp_sdk/async_io/async_can.hpp"
+#include "wrp_sdk/asyncio/async_can.hpp"
 
-using namespace wescore;
+using namespace westonrobot;
 
 void parse_buffer(uint8_t *buf, const size_t bufsize, size_t bytes_received)
 {

tests/test_aserial.cpp

@@ -7,9 +7,9 @@
 
 #include <iostream>
 
-#include "wrp_sdk/async_io/async_serial.hpp"
+#include "wrp_sdk/asyncio/async_serial.hpp"
 
-using namespace wescore;
+using namespace westonrobot;
 
 void parse_buffer(uint8_t *buf, const size_t bufsize, size_t bytes_received)
 {

tests/test_aserial_comm.cpp

@@ -7,9 +7,9 @@
 
 #include <iostream>
 
-#include "wrp_sdk/async_io/async_serial.hpp"
+#include "wrp_sdk/asyncio/async_serial.hpp"
 
-using namespace wescore;
+using namespace westonrobot;
 
 void parse_buffer(uint8_t *buf, const size_t bufsize, size_t bytes_received)
 {

tests/test_can_msg.cpp

@@ -2,7 +2,7 @@
 
 #include "scout_base/details/scout_can_parser.hpp"
 
-using namespace wescore;
+using namespace westonrobot;
 
 void print_msg(uint8_t data[8])
 {

tests/test_hunter_base.cpp

@@ -12,7 +12,7 @@
 
 #define TEST_WITHOUT_SERIAL_HARDWARE
 
-using namespace wescore;
+using namespace westonrobot;
 
 int main(int argc, char **argv) {
   std::string device_name;

tests/test_scout_base.cpp

@@ -12,7 +12,7 @@
 
 #define TEST_WITHOUT_SERIAL_HARDWARE
 
-using namespace wescore;
+using namespace westonrobot;
 
 int main(int argc, char **argv)
 {

tests/test_scout_serial.cpp

@@ -2,13 +2,13 @@
 
 #include "scout_base/details/scout_serial_parser.hpp"
 
-using namespace wescore;
+using namespace westonrobot;
 
 #include "scout_base/scout_base.hpp"
 
 #define TEST_WITHOUT_SERIAL_HARDWARE
 
-using namespace wescore;
+using namespace westonrobot;
 
 int main(int argc, char **argv)
 {

tests/test_serial_parser.cpp

@@ -2,7 +2,7 @@
 
 #include "scout_base/details/scout_serial_parser.hpp"
 
-using namespace wescore;
+using namespace westonrobot;
 
 void print_msg(uint8_t data[8])
 {

tests/test_tracer_base.cpp

@@ -12,7 +12,7 @@
 
 #define TEST_WITHOUT_SERIAL_HARDWARE
 
-using namespace wescore;
+using namespace westonrobot;
 
 int main(int argc, char **argv)
 {

unit_tests/gtests/scout_can_protocol_test.cpp

@@ -17,7 +17,7 @@
 
 #include "wrp_sdk/platforms/scout/scout_base.hpp"
 
-using namespace wescore;
+using namespace westonrobot;
 
 struct ScoutCANProtocolTest : testing::Test
 {