Merge branch 'master' of https://bitbucket.org/westonrobotsoftware/wrp_sdk

docs/misc/check_endianness.md

@@ -1,36 +0,0 @@
-On a Big Endian-System (Solaris on SPARC)
-
-```
-$ echo -n I | od -to2 | head -n1 | cut -f2 -d" " | cut -c6 
-0
-```
-On a little endian system (Linux on x86)
-```
-$ echo -n I | od -to2 | head -n1 | cut -f2 -d" " | cut -c6 
-1
-```
-The solution above is clever and works great for Linux *86 and Solaris Sparc.
-
-I needed a shell-only (no Perl) solution that also worked on AIX/Power and HPUX/Itanium. Unfortunately the last two don't play nice: AIX reports "6" and HPUX gives an empty line.
-
-Using your solution, I was able to craft something that worked on all these Unix systems:
-```
-$ echo I | tr -d [:space:] | od -to2 | head -n1 | awk '{print $2}' | cut -c6
-```
-Regarding the Python solution someone posted, it does not work in Jython because the JVM treats everything as Big. If anyone can get it to work in Jython, please post!
-
-Also, I found this, which explains the endianness of various platforms. Some hardware can operate in either mode depending on what the O/S selects: http://labs.hoffmanlabs.com/node/544
-
-If you're going to use awk this line can be simplified to:
-```
-echo -n I | od -to2 | awk '{ print substr($2,6,1); exit}'
-```
-For small Linux boxes that don't have 'od' (say OpenWrt) then try 'hexdump':
-```
-echo -n I | hexdump -o | awk '{ print substr($2,6,1); exit}'
-```
-
-Reference: 
-
-* [1] https://serverfault.com/questions/163487/how-to-tell-if-a-linux-system-is-big-endian-or-little-endian
-* [2] https://wiki.rdu.im/_pages/Knowledge-Base/Computing/Computing.html

src/CMakeLists.txt

@@ -1,5 +1,6 @@
 # Add source directories
 add_subdirectory(apps)
 add_subdirectory(common)
-add_subdirectory(hunter_sdk)
 add_subdirectory(scout_sdk)
+add_subdirectory(hunter_sdk)
+add_subdirectory(tracer_sdk)

src/apps/CMakeLists.txt

@@ -11,6 +11,9 @@ target_link_libraries(app_scout_demo scoutbase)
 add_executable(app_hunter_demo hunter_demo/hunter_demo.cpp)
 target_link_libraries(app_hunter_demo hunterbase)
 
+add_executable(app_tracer_demo tracer_demo/tracer_demo.cpp)
+target_link_libraries(app_tracer_demo tracerbase)
+
 if(BUILD_MONITOR)
     add_subdirectory(scout_monitor)
 endif()

src/apps/tracer_demo/tracer_demo.cpp

@@ -0,0 +1,108 @@
+/* 
+ * demo_tracer_can.cpp
+ * 
+ * Created on: Jun 12, 2019 05:03
+ * Description: 
+ * 
+ * Copyright (c) 2019 Ruixiang Du (rdu)
+ */
+
+#include "tracer_base/tracer_base.hpp"
+
+using namespace wescore;
+
+int main(int argc, char **argv)
+{
+    std::string device_name;
+    int32_t baud_rate = 0;
+
+    if (argc == 2)
+    {
+        device_name = {argv[1]};
+        std::cout << "Specified CAN: " << device_name << std::endl;
+    }
+    else
+    {
+        std::cout << "Usage: app_tracer_demo <interface>" << std::endl
+                  << "Example 1: ./app_tracer_demo can0" << std::endl;
+        return -1;
+    }
+
+    TracerBase tracer;
+    tracer.Connect(device_name);
+
+    // light control
+    std::cout << "Light: const off" << std::endl;
+    tracer.SetLightCommand({TracerLightCmd::LightMode::CONST_OFF, 0, TracerLightCmd::LightMode::CONST_OFF, 0});
+    sleep(3);
+    std::cout << "Light: const on" << std::endl;
+    tracer.SetLightCommand({TracerLightCmd::LightMode::CONST_ON, 0, TracerLightCmd::LightMode::CONST_ON, 0});
+    sleep(3);
+    std::cout << "Light: breath" << std::endl;
+    tracer.SetLightCommand({TracerLightCmd::LightMode::BREATH, 0, TracerLightCmd::LightMode::BREATH, 0});
+    sleep(3);
+    std::cout << "Light: custom 90-80" << std::endl;
+    tracer.SetLightCommand({TracerLightCmd::LightMode::CUSTOM, 90, TracerLightCmd::LightMode::CUSTOM, 80});
+    sleep(3);
+    std::cout << "Light: diabled cmd control" << std::endl;
+    tracer.DisableLightCmdControl();
+
+    int count = 0;
+    while (true)
+    {
+        // motion control
+        if (count < 5)
+        {
+            std::cout << "Motor: 0.2, 0.0" << std::endl;
+            tracer.SetMotionCommand(0.2, 0.0);
+        }
+        else if (count < 10)
+        {
+        std::cout << "Motor: 0.8, 0.3" << std::endl;
+        tracer.SetMotionCommand(0.8, 0.3);
+        }
+        else if (count < 15)
+        {
+            std::cout << "Motor: 1.5, 0.5" << std::endl;
+            tracer.SetMotionCommand(1.5, 0.5);
+        }
+        else if (count < 20)
+        {
+            std::cout << "Motor: 1.0, 0.3" << std::endl;
+            tracer.SetMotionCommand(1.0, 0.3);
+        }
+        else if (count < 25)
+        {
+            std::cout << "Motor: 0.0, 0.0" << std::endl;
+            tracer.SetMotionCommand(0.0, 0.0);
+        }
+        else if (count < 30)
+        {
+            std::cout << "Motor: -0.5, -0.3" << std::endl;
+            tracer.SetMotionCommand(-0.5, -0.3);
+        }
+        else if (count < 35)
+        {
+            std::cout << "Motor: -1.0, -0.5" << std::endl;
+            tracer.SetMotionCommand(-1.0, -0.5);
+        }
+        else if (count < 40)
+        {
+            std::cout << "Motor: 0.0, 0.0," << std::endl;
+            tracer.SetMotionCommand(0.0, 0.0);
+        }
+
+        auto state = tracer.GetTracerState();
+        std::cout << "-------------------------------" << std::endl;
+        std::cout << "count: " << count << std::endl;
+        std::cout << "control mode: " << static_cast<int>(state.control_mode) << " , base state: " << static_cast<int>(state.base_state) << std::endl;
+        std::cout << "battery voltage: " << state.battery_voltage << std::endl;
+        std::cout << "velocity (linear, angular): " << state.linear_velocity << ", " << state.angular_velocity << std::endl;
+        std::cout << "-------------------------------" << std::endl;
+
+        sleep(1);
+        ++count;
+    }
+
+    return 0;
+}

src/hunter_sdk/hunter_base/CMakeLists.txt

@@ -9,7 +9,7 @@ target_include_directories(hunterbase PUBLIC
 	$<INSTALL_INTERFACE:include>	
     PRIVATE src)
 
- ## Add executables
+## Add executables
-# if(BUILD_TESTS)
+if(BUILD_TESTS)
-#     add_subdirectory(tests)
+    add_subdirectory(tests)
-# endif()
+endif()

src/hunter_sdk/hunter_base/tests/CMakeLists.txt

@@ -2,15 +2,5 @@
 #find_package(LIBRARY_NAME REQUIRED)
 
 # tests
-add_executable(test_scout_base test_scout_base.cpp)
+add_executable(test_hunter_base test_hunter_base.cpp)
-target_link_libraries(test_scout_base scoutbase)
+target_link_libraries(test_hunter_base hunterbase)
-
-# add_executable(test_scout_serial test_scout_serial.cpp)
-# target_link_libraries(test_scout_serial scoutbase)
-
-# add_executable(test_can_msg test_can_msg.cpp)
-# target_link_libraries(test_can_msg scoutbase)
-
-# add_executable(test_serial_parser test_serial_parser.cpp)
-# target_link_libraries(test_serial_parser scoutbase)
-

src/hunter_sdk/hunter_base/tests/test_can_msg.cpp

@@ -1,39 +0,0 @@
-#include <iostream>
-
-#include "scout_base/details/scout_can_parser.hpp"
-
-using namespace wescore;
-
-void print_msg(uint8_t data[8])
-{
-    for (int i = 0; i < 8; ++i)
-        std::cout << std::hex << static_cast<int>(data[i]) << " ";
-    std::cout << std::dec << std::endl;
-}
-
-int main()
-{
-    MotionControlMessage msg;
-    msg.msg.cmd.control_mode = CTRL_MODE_CMD_CAN;
-    msg.msg.cmd.fault_clear_flag = FAULT_CLR_NONE;
-    msg.msg.cmd.linear_velocity_cmd = 10;
-    msg.msg.cmd.angular_velocity_cmd = 0;
-    msg.msg.cmd.reserved0 = 0;
-    msg.msg.cmd.reserved1 = 0;
-    msg.msg.cmd.count = 0;
-    msg.msg.cmd.checksum = ScoutCANParser::Agilex_CANMsgChecksum(ScoutCANParser::CAN_MSG_MOTION_CONTROL_CMD_ID, msg.msg.raw, msg.len);
-    print_msg(msg.msg.raw);
-
-    LightControlMessage msg2;
-    msg2.msg.cmd.light_ctrl_enable = LIGHT_DISABLE_CTRL;
-    msg2.msg.cmd.front_light_mode = LIGHT_MODE_CONST_ON;
-    msg2.msg.cmd.front_light_custom = 0;
-    msg2.msg.cmd.rear_light_mode = LIGHT_MODE_CONST_ON;
-    msg2.msg.cmd.rear_light_custom = 0;
-    msg2.msg.cmd.reserved0 = 0;
-    msg2.msg.cmd.count = 0;
-    msg2.msg.cmd.checksum = ScoutCANParser::Agilex_CANMsgChecksum(ScoutCANParser::CAN_MSG_LIGHT_CONTROL_CMD_ID, msg2.msg.raw, msg2.len);
-    print_msg(msg2.msg.raw);
-
-    return 0;
-}

src/hunter_sdk/hunter_base/tests/test_hunter_base.cpp

@@ -0,0 +1,54 @@
+#include <unistd.h>
+
+#include <chrono>
+#include <cmath>
+#include <functional>
+#include <iostream>
+#include <mutex>
+#include <string>
+#include <thread>
+
+#include "scout_base/scout_base.hpp"
+
+#define TEST_WITHOUT_SERIAL_HARDWARE
+
+using namespace wescore;
+
+int main(int argc, char **argv) {
+  std::string device_name;
+  int32_t baud_rate = 0;
+
+  if (argc == 2) {
+    device_name = {argv[1]};
+    std::cout << "Specified CAN: " << device_name << std::endl;
+  } else {
+    std::cout << "Usage: app_scout_demo <interface>" << std::endl
+              << "Example 1: ./app_scout_demo can0" << std::endl;
+    return -1;
+  }
+
+  HunterBase scout;
+  scout.Connect(device_name);
+
+  int count = 0;
+  while (true) {
+    std::cout << "Motor: 0.0, 0.0," << std::endl;
+    scout.SetMotionCommand(0.0, 0.0);
+
+    auto state = scout.GetHunterState();
+    std::cout << "-------------------------------" << std::endl;
+    std::cout << "count: " << count << std::endl;
+    std::cout << "control mode: " << static_cast<int>(state.control_mode)
+              << " , base state: " << static_cast<int>(state.base_state)
+              << std::endl;
+    std::cout << "battery voltage: " << state.battery_voltage << std::endl;
+    std::cout << "velocity (linear, angular): " << state.linear_velocity << ", "
+              << state.angular_velocity << std::endl;
+    std::cout << "-------------------------------" << std::endl;
+
+    sleep(1);
+    ++count;
+  }
+
+  return 0;
+}

src/hunter_sdk/hunter_base/tests/test_scout_serial.cpp

@@ -1,45 +0,0 @@
-#include <iostream>
-
-#include "scout_base/details/scout_serial_parser.hpp"
-
-using namespace wescore;
-
-#include "scout_base/scout_base.hpp"
-
-#define TEST_WITHOUT_SERIAL_HARDWARE
-
-using namespace wescore;
-
-int main(int argc, char **argv)
-{
-    ScoutBase scout;
-    // scout.ConfigureCANBus("can1");
-    scout.Connect("/dev/ttyUSB0", 115200);
-    // scout.StartCmdThread(10);
-
-    // scout.SetLightCommand({ScoutLightCmd::LightMode::CONST_ON, 0, ScoutLightCmd::LightMode::CONST_ON, 0});
-
-    int count = 0;
-    while (true)
-    {
-        // scout.SetMotionCommand(0.5, 0.2);
-
-        // if(count == 10)
-        // {
-        //     // scout.SetLightCommand({ScoutLightCmd::LightMode::LIGHT_MODE_CONST_OFF, 0, ScoutLightCmd::LightMode::LIGHT_MODE_CONST_OFF, 0});
-        //     scout.DisableLightCmdControl();
-        // }
-
-        auto state = scout.GetScoutState();
-        std::cout << "-------------------------------" << std::endl;
-        std::cout << "control mode: " << static_cast<int>(state.control_mode) << " , base state: " << static_cast<int>(state.base_state) << std::endl;
-        std::cout << "battery voltage: " << state.battery_voltage << std::endl;
-        std::cout << "velocity (linear, angular): " << state.linear_velocity << ", " << state.angular_velocity << std::endl;
-        std::cout << "-------------------------------" << std::endl;
-
-        sleep(1);
-        ++count;
-    }
-
-    return 0;
-}

src/hunter_sdk/hunter_base/tests/test_serial_parser.cpp

@@ -1,51 +0,0 @@
-#include <iostream>
-
-#include "scout_base/details/scout_serial_parser.hpp"
-
-using namespace wescore;
-
-void print_msg(uint8_t data[8])
-{
-    for (int i = 0; i < 8; ++i)
-        std::cout << std::hex << static_cast<int>(data[i]) << " ";
-    std::cout << std::dec << std::endl;
-}
-
-uint8_t calc_checksum(uint8_t *buf, uint8_t len)
-{
-    uint8_t checksum = 0;
-    for (int i = 0; i < len; ++i)
-        checksum ^= buf[i];
-    return checksum;
-}
-
-int main()
-{
-    uint8_t frame_data[16];
-
-    // SOF
-    frame_data[0] = 0x5a;
-    frame_data[1] = 0xa5;
-
-    // Frame len, type, ID
-    frame_data[2] = 0x0a;
-    frame_data[3] = 0xaa;
-    frame_data[4] = 0x01;
-
-    // Frame payload
-    frame_data[5] = 0;
-    frame_data[6] = 1;
-    frame_data[7] = 2;
-    frame_data[8] = 3;
-    frame_data[9] = 4;
-    frame_data[10] = 5;
-
-    // Frame count, checksum
-    frame_data[11] = 1;
-    frame_data[12] = calc_checksum(frame_data, 12);
-
-    ScoutSerialParser parser;
-    parser.ParseBuffer(frame_data, 7 + 6);
-
-    return 0;
-}

src/tracer_sdk/CMakeLists.txt

@@ -0,0 +1,3 @@
+# Add source directories
+add_subdirectory(tracer_base)
+add_subdirectory(tracer_protocol)

src/tracer_sdk/tracer_base/CMakeLists.txt

@@ -0,0 +1,16 @@
+## Add libraries
+set(TRACER_BASE_SRC 
+    src/tracer_base.cpp
+)
+add_library(tracerbase STATIC ${TRACER_BASE_SRC})
+target_link_libraries(tracerbase asyncio tracer_protocol stopwatch)
+target_include_directories(tracerbase PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<BUILD_INTERFACE:${Boost_INCLUDE_DIRS}>
+	$<INSTALL_INTERFACE:include>	
+    PRIVATE src)
+
+ ## Add executables
+if(BUILD_TESTS)
+    add_subdirectory(tests)
+endif()

src/tracer_sdk/tracer_base/include/tracer_base/tracer_base.hpp

@@ -0,0 +1,109 @@
+/* 
+ * 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 <functional>
+
+#include "async_io/async_can.hpp"
+#include "async_io/async_serial.hpp"
+
+#include "tracer_protocol/tracer_protocol.h"
+#include "tracer_protocol/tracer_can_parser.h"
+
+#include "tracer_base/tracer_types.hpp"
+
+namespace wescore
+{
+class TracerBase
+{
+public:
+    TracerBase() = default;
+    ~TracerBase();
+
+    // do not allow copy
+    TracerBase(const TracerBase &tracer) = delete;
+    TracerBase &operator=(const TracerBase &tracer) = delete;
+
+public:
+    // connect to roboot from CAN
+    void Connect(std::string dev_name);
+
+    // 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 angular_vel,
+                          TracerMotionCmd::FaultClearFlag fault_clr_flag = TracerMotionCmd::FaultClearFlag::NO_FAULT);
+
+    // light control
+    void SetLightCommand(TracerLightCmd cmd);
+    void DisableLightCmdControl();
+
+    // get robot state
+    TracerState GetTracerState();
+
+private:
+    // 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_[TRACER_CMD_BUF_LEN];
+
+    // cmd/status update related variables
+    std::thread cmd_thread_;
+    std::mutex tracer_state_mutex_;
+    std::mutex motion_cmd_mutex_;
+    std::mutex light_cmd_mutex_;
+
+    TracerState tracer_state_;
+    TracerMotionCmd current_motion_cmd_;
+    TracerLightCmd 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 TracerMessage &msg);
+
+public:
+    static void UpdateTracerState(const TracerMessage &status_msg, TracerState &state);
+};
+} // namespace wescore
+
+#endif /* TRACER_BASE_HPP */

src/tracer_sdk/tracer_base/include/tracer_base/tracer_types.hpp

@@ -0,0 +1,111 @@
+/* 
+ * 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 wescore
+{
+struct TracerState
+{
+    enum MotorID
+    {
+        FRONT_RIGHT = 0,
+        FRONT_LEFT = 1,
+        REAR_LEFT = 2,
+        REAR_RIGHT = 3
+    };
+
+    struct MotorState
+    {
+        double current = 0; // in A
+        double rpm = 0;
+        double temperature = 0;
+    };
+
+    struct LightState
+    {
+        uint8_t mode = 0;
+        uint8_t custom_value = 0;
+    };
+
+    // base state
+    uint8_t base_state = 0;
+    uint8_t control_mode = 0;
+    uint16_t fault_code = 0;
+    double battery_voltage = 0.0;
+
+    // motor state
+    MotorState motor_states[4];
+
+    // 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;
+};
+
+struct TracerMotionCmd
+{
+    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
+    };
+
+    TracerMotionCmd(int8_t linear = 0, int8_t angular = 0,
+                   FaultClearFlag fault_clr_flag = FaultClearFlag::NO_FAULT)
+        : linear_velocity(linear), angular_velocity(angular),
+          fault_clear_flag(fault_clr_flag) {}
+
+    int8_t linear_velocity;
+    int8_t angular_velocity;
+    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_angular_velocity = 0.5235;  // 0.5235 rad/s
+    static constexpr double min_angular_velocity = -0.5235; // -0.5235 rad/s
+};
+
+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, LightMode r_mode, uint8_t r_value) : front_mode(f_mode), front_custom_value(f_value),
+                                                                                          rear_mode(r_mode), rear_custom_value(r_value) {}
+
+    LightMode front_mode;
+    uint8_t front_custom_value;
+    LightMode rear_mode;
+    uint8_t rear_custom_value;
+};
+} // namespace wescore
+
+#endif /* TRACER_TYPES_HPP */

src/tracer_sdk/tracer_base/src/tracer_base.cpp

@@ -0,0 +1,338 @@
+#include "tracer_base/tracer_base.hpp"
+
+#include <string>
+#include <cstring>
+#include <iostream>
+#include <algorithm>
+#include <array>
+#include <chrono>
+#include <cstdint>
+#include <ratio>
+#include <thread>
+
+#include "stopwatch/stopwatch.h"
+
+namespace wescore
+{
+TracerBase::~TracerBase()
+{
+    if (serial_connected_)
+        serial_if_->close();
+
+    if (cmd_thread_.joinable())
+        cmd_thread_.join();
+}
+
+void TracerBase::Connect(std::string dev_name)
+{
+  //   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 TracerBase::Disconnect()
+{
+    if (serial_connected_)
+    {
+        if (serial_if_->is_open())
+            serial_if_->close();
+    }
+}
+
+void TracerBase::ConfigureCANBus(const std::string &can_if_name)
+{
+    can_if_ = std::make_shared<ASyncCAN>(can_if_name);
+
+    can_if_->set_receive_callback(std::bind(&TracerBase::ParseCANFrame, this, std::placeholders::_1));
+
+    can_connected_ = true;
+}
+
+void TracerBase::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(&TracerBase::ParseUARTBuffer, this,
+                                               std::placeholders::_1,
+                                               std::placeholders::_2,
+                                               std::placeholders::_3));
+}
+
+void TracerBase::StartCmdThread()
+{
+    current_motion_cmd_.linear_velocity = 0;
+    current_motion_cmd_.angular_velocity = 0;
+    current_motion_cmd_.fault_clear_flag = TracerMotionCmd::FaultClearFlag::NO_FAULT;
+
+    cmd_thread_ = std::thread(std::bind(&TracerBase::ControlLoop, this, cmd_thread_period_ms_));
+    cmd_thread_started_ = true;
+}
+
+void TracerBase::SendMotionCmd(uint8_t count)
+{
+    // motion control message
+    TracerMessage m_msg;
+    m_msg.type = TracerMotionCmdMsg;
+
+    if (can_connected_)
+        m_msg.body.motion_cmd_msg.data.cmd.control_mode = CTRL_MODE_CMD_CAN;
+    else if (serial_connected_)
+        m_msg.body.motion_cmd_msg.data.cmd.control_mode = CTRL_MODE_CMD_UART;
+
+    motion_cmd_mutex_.lock();
+    m_msg.body.motion_cmd_msg.data.cmd.fault_clear_flag = static_cast<uint8_t>(current_motion_cmd_.fault_clear_flag);
+    m_msg.body.motion_cmd_msg.data.cmd.linear_velocity_cmd = current_motion_cmd_.linear_velocity;
+    m_msg.body.motion_cmd_msg.data.cmd.angular_velocity_cmd = current_motion_cmd_.angular_velocity;
+    motion_cmd_mutex_.unlock();
+
+    m_msg.body.motion_cmd_msg.data.cmd.reserved0 = 0;
+    m_msg.body.motion_cmd_msg.data.cmd.reserved1 = 0;
+    m_msg.body.motion_cmd_msg.data.cmd.count = count;
+
+    if (can_connected_)
+        m_msg.body.motion_cmd_msg.data.cmd.checksum = CalcTracerCANChecksum(CAN_MSG_MOTION_CMD_ID, m_msg.body.motion_cmd_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;
+        EncodeTracerMsgToCAN(&m_msg, &m_frame);
+        can_if_->send_frame(m_frame);
+    }
+    else
+    {
+        // TODO
+        // send to serial port
+        // EncodeTracerMsgToUART(&m_msg, tx_buffer_, &tx_cmd_len_);
+        // serial_if_->send_bytes(tx_buffer_, tx_cmd_len_);
+    }
+}
+
+void TracerBase::SendLightCmd(uint8_t count)
+{
+    TracerMessage l_msg;
+    l_msg.type = TracerLightControlMsg;
+
+    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 = CalcTracerCANChecksum(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;
+        EncodeTracerMsgToCAN(&l_msg, &l_frame);
+
+        can_if_->send_frame(l_frame);
+    }
+    // else
+    // {
+    //     // send to serial port
+    //     EncodeTracerMsgToUART(&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 TracerBase::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;
+    }
+}
+
+TracerState TracerBase::GetTracerState()
+{
+    std::lock_guard<std::mutex> guard(tracer_state_mutex_);
+    return tracer_state_;
+}
+
+void TracerBase::SetMotionCommand(double linear_vel, double angular_vel, TracerMotionCmd::FaultClearFlag fault_clr_flag)
+{
+    // make sure cmd thread is started before attempting to send commands
+    if (!cmd_thread_started_)
+        StartCmdThread();
+
+    if (linear_vel < TracerMotionCmd::min_linear_velocity)
+        linear_vel = TracerMotionCmd::min_linear_velocity;
+    if (linear_vel > TracerMotionCmd::max_linear_velocity)
+        linear_vel = TracerMotionCmd::max_linear_velocity;
+    if (angular_vel < TracerMotionCmd::min_angular_velocity)
+        angular_vel = TracerMotionCmd::min_angular_velocity;
+    if (angular_vel > TracerMotionCmd::max_angular_velocity)
+        angular_vel = TracerMotionCmd::max_angular_velocity;
+
+    std::lock_guard<std::mutex> guard(motion_cmd_mutex_);
+    current_motion_cmd_.linear_velocity = static_cast<int8_t>(linear_vel / TracerMotionCmd::max_linear_velocity * 100.0);
+    current_motion_cmd_.angular_velocity = static_cast<int8_t>(angular_vel / TracerMotionCmd::max_angular_velocity * 100.0);
+    current_motion_cmd_.fault_clear_flag = fault_clr_flag;
+}
+
+void TracerBase::SetLightCommand(TracerLightCmd 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 TracerBase::DisableLightCmdControl()
+{
+    std::lock_guard<std::mutex> guard(light_cmd_mutex_);
+    light_ctrl_enabled_ = false;
+    light_ctrl_requested_ = true;
+}
+
+void TracerBase::ParseCANFrame(can_frame *rx_frame)
+{
+    // validate checksum, discard frame if fails
+    if (!rx_frame->data[7] == CalcTracerCANChecksum(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
+    TracerMessage status_msg;
+    DecodeTracerMsgFromCAN(rx_frame, &status_msg);
+    NewStatusMsgReceivedCallback(status_msg);
+}
+
+void TracerBase::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);
+
+    // TODO
+    // TracerMessage status_msg;
+    // for (int i = 0; i < bytes_received; ++i)
+    // {
+    //     if (DecodeTracerMsgFromUART(buf[i], &status_msg))
+    //         NewStatusMsgReceivedCallback(status_msg);
+    // }
+}
+
+void TracerBase::NewStatusMsgReceivedCallback(const TracerMessage &msg)
+{
+    // std::cout << "new status msg received" << std::endl;
+    std::lock_guard<std::mutex> guard(tracer_state_mutex_);
+    UpdateTracerState(msg, tracer_state_);
+}
+
+void TracerBase::UpdateTracerState(const TracerMessage &status_msg, TracerState &state)
+{
+    switch (status_msg.type)
+    {
+    case TracerMotionStatusMsg:
+    {
+        // 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 TracerLightStatusMsg:
+    {
+        // 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 TracerSystemStatusMsg:
+    {
+        // 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 TracerMotorDriverStatusMsg:
+    {
+        // 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 wescore

src/tracer_sdk/tracer_base/tests/CMakeLists.txt

@@ -0,0 +1,16 @@
+# Dependency libraries
+#find_package(LIBRARY_NAME REQUIRED)
+
+# tests
+add_executable(test_scout_base test_scout_base.cpp)
+target_link_libraries(test_scout_base scoutbase)
+
+# add_executable(test_scout_serial test_scout_serial.cpp)
+# target_link_libraries(test_scout_serial scoutbase)
+
+# add_executable(test_can_msg test_can_msg.cpp)
+# target_link_libraries(test_can_msg scoutbase)
+
+# add_executable(test_serial_parser test_serial_parser.cpp)
+# target_link_libraries(test_serial_parser scoutbase)
+

src/tracer_sdk/tracer_protocol/CMakeLists.txt

@@ -0,0 +1,9 @@
+## Add libraries
+set(TRACER_PROTOCOL_SRC 
+    src/tracer_can_parser.c
+)
+add_library(tracer_protocol STATIC ${TRACER_PROTOCOL_SRC})
+target_include_directories(tracer_protocol PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+	$<INSTALL_INTERFACE:include>	
+    PRIVATE src)

src/tracer_sdk/tracer_protocol/include/tracer_protocol/tracer_can_parser.h

@@ -0,0 +1,42 @@
+/* 
+ * tracer_can_parser.h
+ * 
+ * Created on: Apr 14, 2020 10:52
+ * Description: 
+ * 
+ * Copyright (c) 2020 Ruixiang Du (rdu)
+ */ 
+
+#ifndef TRACER_CAN_PARSER_H
+#define TRACER_CAN_PARSER_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "tracer_protocol/tracer_protocol.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
+
+bool DecodeTracerMsgFromCAN(const struct can_frame *rx_frame, TracerMessage *msg);
+void EncodeTracerMsgToCAN(const TracerMessage *msg, struct can_frame *tx_frame);
+
+uint8_t CalcTracerCANChecksum(uint16_t id, uint8_t *data, uint8_t dlc);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* TRACER_CAN_PARSER_H */

src/tracer_sdk/tracer_protocol/include/tracer_protocol/tracer_protocol.h

@@ -0,0 +1,259 @@
+/* 
+ * tracer_protocol.h
+ * 
+ * Created on: Apr 14, 2020 10:34
+ * Description: 
+ * 
+ * Copyright (c) 2020 Ruixiang Du (rdu)
+ */ 
+
+#ifndef TRACER_PROTOCOL_H
+#define TRACER_PROTOCOL_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+
+#define TRACER_CMD_BUF_LEN               32
+#define TRACER_STATUS_BUF_LEN            32
+#define TRACER_FRAME_SIZE                13
+
+#define TRACER_MOTOR1_ID                 ((uint8_t)0x00)
+#define TRACER_MOTOR2_ID                 ((uint8_t)0x01)
+
+// CAN Definitions
+#define CAN_MSG_MOTION_CMD_ID            ((uint32_t)0x130)
+#define CAN_MSG_MOTION_STATUS_ID         ((uint32_t)0x131)
+#define CAN_MSG_LIGHT_CONTROL_CMD_ID     ((uint32_t)0x140)
+#define CAN_MSG_LIGHT_CONTROL_STATUS_ID  ((uint32_t)0x141)
+#define CAN_MSG_SYSTEM_STATUS_STATUS_ID  ((uint32_t)0x151)
+#define CAN_MSG_MOTOR1_DRIVER_STATUS_ID  ((uint32_t)0x201)
+#define CAN_MSG_MOTOR2_DRIVER_STATUS_ID  ((uint32_t)0x202)
+
+/*--------------------- Control/State Constants ------------------------*/
+
+// Motion Control
+#define CTRL_MODE_REMOTE                ((uint8_t)0x00)
+#define CTRL_MODE_CMD_CAN               ((uint8_t)0x01)
+#define CTRL_MODE_CMD_UART              ((uint8_t)0x02)
+#define CTRL_MODE_COMMANDED             ((uint8_t)0x03)
+
+#define FAULT_CLR_NONE                  ((uint8_t)0x00)
+#define FAULT_CLR_BAT_UNDER_VOL         ((uint8_t)0x01)
+#define FAULT_CLR_BAT_OVER_VOL          ((uint8_t)0x02)
+#define FAULT_CLR_MOTOR1_COMM           ((uint8_t)0x03)
+#define FAULT_CLR_MOTOR2_COMM           ((uint8_t)0x04)
+#define FAULT_CLR_MOTOR3_COMM           ((uint8_t)0x05)
+#define FAULT_CLR_MOTOR4_COMM           ((uint8_t)0x06)
+#define FAULT_CLR_MOTOR_DRV_OVERHEAT    ((uint8_t)0x07)
+#define FAULT_CLR_MOTOR_OVERCURRENT     ((uint8_t)0x08)
+
+// Light Control
+#define LIGHT_DISABLE_CTRL              ((uint8_t)0x00)
+#define LIGHT_ENABLE_CTRL               ((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)
+
+// System Status Feedback
+#define BASE_STATE_NORMAL               ((uint8_t)0x00)
+#define BASE_STATE_ESTOP                ((uint8_t)0x01)
+#define BASE_STATE_EXCEPTION            ((uint8_t)0x02)
+
+#define FAULT_CAN_CHECKSUM_ERROR        ((uint16_t)0x0100)
+#define FAULT_FRONT_STEER_ENCODER_F     ((uint16_t)0x0200)
+#define FAULT_RC_SIGNAL_LOSS            ((uint16_t)0x0400)
+#define FAULT_HIGH_BYTE_RESERVED1       ((uint16_t)0x0800)
+#define FAULT_HIGH_BYTE_RESERVED2       ((uint16_t)0x1000)
+#define FAULT_HIGH_BYTE_RESERVED3       ((uint16_t)0x2000)
+#define FAULT_HIGH_BYTE_RESERVED4       ((uint16_t)0x4000)
+#define FAULT_HIGH_BYTE_RESERVED5       ((uint16_t)0x8000)
+
+#define FAULT_BAT_UNDER_VOL_F           ((uint16_t)0x0001)
+#define FAULT_BAT_OVER_VOL_F            ((uint16_t)0x0002)
+#define FAULT_MOTOR1_COMM_F             ((uint16_t)0x0004)
+#define FAULT_MOTOR2_COMM_F             ((uint16_t)0x0008)
+#define FAULT_RESERVED1                 ((uint16_t)0x0010)
+#define FAULT_RESERVED2                 ((uint16_t)0x0020)
+#define FAULT_MOTOR_DRV_OVERHEAT_F      ((uint16_t)0x0040)
+#define FAULT_MOTOR_OVERCURRENT_F       ((uint16_t)0x0080)
+
+/*-------------------- Control/Feedback Messages -----------------------*/
+
+/* No padding in the struct */
+// reference: https://stackoverflow.com/questions/3318410/pragma-pack-effect
+#pragma pack(push, 1)
+
+// Note: id could be different for UART and CAN protocol
+
+// Motion Control
+typedef struct {
+    union
+    {
+        struct
+        {
+            uint8_t control_mode;
+            uint8_t fault_clear_flag;
+            int8_t linear_velocity_cmd;
+            int8_t angular_velocity_cmd;
+            uint8_t reserved0;
+            uint8_t reserved1;
+            uint8_t count;
+            uint8_t checksum;
+        } cmd;
+        uint8_t raw[8];
+    } data;
+} MotionCmdMessage;
+
+typedef struct {
+    union
+    {
+        struct
+        {
+            struct
+            {
+                uint8_t high_byte;
+                uint8_t low_byte;
+            } linear_velocity;
+            struct
+            {
+                uint8_t high_byte;
+                uint8_t low_byte;
+            } angular_velocity;
+            uint8_t reserved0;
+            uint8_t reserved1;
+            uint8_t count;
+            uint8_t checksum;
+        } status;
+        uint8_t raw[8];
+    } data;
+} MotionStatusMessage;
+
+// System Status Feedback
+typedef struct {    
+    union
+    {
+        struct
+        {
+            uint8_t base_state;
+            uint8_t control_mode;
+            struct
+            {
+                uint8_t high_byte;
+                uint8_t low_byte;
+            } battery_voltage;
+            struct
+            {
+                uint8_t high_byte;
+                uint8_t low_byte;
+            } fault_code;
+            uint8_t count;
+            uint8_t checksum;
+        } status;
+        uint8_t raw[8];
+    } data;
+} SystemStatusMessage;
+
+// Light Control
+typedef struct {
+    union
+    {
+        struct
+        {
+            uint8_t light_ctrl_enable;
+            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 count;
+            uint8_t checksum;
+        } cmd;
+        uint8_t raw[8];
+    } data;
+} LightControlMessage;
+
+typedef struct {
+    union
+    {
+        struct
+        {
+            uint8_t light_ctrl_enable;
+            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 count;
+            uint8_t checksum;
+        } status;
+        uint8_t raw[8];
+    } data;
+} LightStatusMessage;
+
+// Motor Driver Feedback
+typedef struct
+{
+    uint8_t motor_id;
+    union {
+        struct
+        {
+            struct
+            {
+                uint8_t high_byte;
+                uint8_t low_byte;
+            } current;
+            struct
+            {
+                uint8_t high_byte;
+                uint8_t low_byte;
+            } rpm;
+            int8_t temperature;
+            uint8_t reserved0;
+            uint8_t count;
+            uint8_t checksum;
+        } status;
+        uint8_t raw[8];
+    } data;
+} MotorDriverStatusMessage;
+
+// For convenience to access status/control message
+typedef enum
+{
+    TracerMsgNone = 0x00,
+    // status messages
+    TracerMotionStatusMsg = 0x01,
+    TracerLightStatusMsg = 0x02,
+    TracerSystemStatusMsg = 0x03,
+    TracerMotorDriverStatusMsg = 0x04,
+    // control messages
+    TracerMotionCmdMsg = 0x21,
+    TracerLightControlMsg = 0x22
+} TracerMsgType;
+
+typedef struct 
+{
+    TracerMsgType type;
+    union {
+        // status messages
+        MotionStatusMessage motion_status_msg;
+        LightStatusMessage light_status_msg;
+        SystemStatusMessage system_status_msg;
+        MotorDriverStatusMessage motor_driver_status_msg;
+        // control messages
+        MotionCmdMessage motion_cmd_msg;
+        LightControlMessage light_control_msg;
+    } body;
+} TracerMessage;
+
+#pragma pack(pop)
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* TRACER_PROTOCOL_H */

src/tracer_sdk/tracer_protocol/src/tracer_can_parser.c

@@ -0,0 +1,118 @@
+/* 
+ * tracer_can_parser.c
+ * 
+ * Created on: Apr 14, 2020 10:35
+ * Description: 
+ * 
+ * Copyright (c) 2020 Ruixiang Du (rdu)
+ */ 
+
+#include "tracer_protocol/tracer_can_parser.h"
+
+#include "string.h"
+
+static void EncodeTracerMotionControlMsgToCAN(const MotionCmdMessage *msg, struct can_frame *tx_frame);
+
+bool DecodeTracerMsgFromCAN(const struct can_frame *rx_frame, TracerMessage *msg)
+{
+    msg->type = TracerMsgNone;
+
+    switch (rx_frame->can_id)
+    {
+    // in the current implementation, both MsgType and can_frame include 8 * uint8_t
+    case CAN_MSG_MOTION_STATUS_ID:
+    {
+        msg->type = TracerMotionStatusMsg;
+        memcpy(msg->body.motion_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
+        break;
+    }
+    case CAN_MSG_SYSTEM_STATUS_STATUS_ID:
+    {
+        msg->type = TracerSystemStatusMsg;
+        memcpy(msg->body.system_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
+        break;
+    }
+    case CAN_MSG_MOTOR1_DRIVER_STATUS_ID:
+    {
+        msg->type = TracerMotorDriverStatusMsg;
+        msg->body.motor_driver_status_msg.motor_id = TRACER_MOTOR1_ID;
+        memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
+        break;
+    }
+    case CAN_MSG_MOTOR2_DRIVER_STATUS_ID:
+    {
+        msg->type = TracerMotorDriverStatusMsg;
+        msg->body.motor_driver_status_msg.motor_id = TRACER_MOTOR2_ID;
+        memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
+        break;
+    }
+    // in the current implementation, both MsgType and can_frame include 8 * uint8_t
+    case CAN_MSG_MOTION_CMD_ID:
+    {
+        msg->type = TracerMotionCmdMsg;
+        memcpy(msg->body.motion_cmd_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
+        break;
+    }
+    default:
+        break;
+    }
+
+    return true;
+}
+
+void EncodeTracerMsgToCAN(const TracerMessage *msg, struct can_frame *tx_frame)
+{
+    switch (msg->type)
+    {
+    // in the current implementation, both MsgType and can_frame include 8 * uint8_t
+    case TracerMotionStatusMsg:
+    {
+        tx_frame->can_id = CAN_MSG_MOTION_STATUS_ID;
+        tx_frame->can_dlc = 8;
+        memcpy(tx_frame->data, msg->body.motion_status_msg.data.raw, tx_frame->can_dlc);
+        break;
+    }
+    case TracerSystemStatusMsg:
+    {
+        tx_frame->can_id = CAN_MSG_SYSTEM_STATUS_STATUS_ID;
+        tx_frame->can_dlc = 8;
+        memcpy(tx_frame->data, msg->body.system_status_msg.data.raw, tx_frame->can_dlc);
+        break;
+    }
+    case TracerMotorDriverStatusMsg:
+    {
+        if (msg->body.motor_driver_status_msg.motor_id == TRACER_MOTOR1_ID)
+            tx_frame->can_id = CAN_MSG_MOTOR1_DRIVER_STATUS_ID;
+        else if (msg->body.motor_driver_status_msg.motor_id == TRACER_MOTOR2_ID)
+            tx_frame->can_id = CAN_MSG_MOTOR2_DRIVER_STATUS_ID;
+        tx_frame->can_dlc = 8;
+        memcpy(tx_frame->data, msg->body.motor_driver_status_msg.data.raw, tx_frame->can_dlc);
+        break;
+    }
+    case TracerMotionCmdMsg:
+    {
+        EncodeTracerMotionControlMsgToCAN(&(msg->body.motion_cmd_msg), tx_frame);
+        break;
+    }
+    default:
+        break;
+    }
+    tx_frame->data[7] = CalcTracerCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
+}
+
+void EncodeTracerMotionControlMsgToCAN(const MotionCmdMessage *msg, struct can_frame *tx_frame)
+{
+    tx_frame->can_id = CAN_MSG_MOTION_CMD_ID;
+    tx_frame->can_dlc = 8;
+    memcpy(tx_frame->data, msg->data.raw, tx_frame->can_dlc);
+    tx_frame->data[7] = CalcTracerCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
+}
+
+uint8_t CalcTracerCANChecksum(uint16_t id, uint8_t *data, uint8_t dlc)
+{
+    uint8_t checksum = 0x00;
+    checksum = (uint8_t)(id & 0x00ff) + (uint8_t)(id >> 8) + dlc;
+    for (int i = 0; i < (dlc - 1); ++i)
+        checksum += data[i];
+    return checksum;
+}