moved ugv_sdk out from sub-folder

src/agx_msg_parser.c

@@ -0,0 +1,219 @@
+/*
+ * agx_msg_parser.c
+ *
+ * Created on: Aug 31, 2019 04:25
+ * Description:
+ *
+ * Copyright (c) 2019 Ruixiang Du (rdu)
+ */
+
+#include "ugv_sdk/proto/agx_msg_parser.h"
+
+#include "string.h"
+
+bool DecodeCanFrame(const struct can_frame *rx_frame, AgxMessage *msg) {
+  msg->type = AgxMsgUnkonwn;
+
+  switch (rx_frame->can_id) {
+    // command frame
+    case CAN_MSG_MOTION_COMMAND_ID: {
+      msg->type = AgxMsgMotionCommand;
+      memcpy(msg->body.motion_command_msg.raw, rx_frame->data,
+             rx_frame->can_dlc * sizeof(uint8_t));
+      break;
+    }
+    case CAN_MSG_LIGHT_COMMAND_ID: {
+      msg->type = AgxMsgLightCommand;
+      memcpy(msg->body.light_command_msg.raw, rx_frame->data,
+             rx_frame->can_dlc * sizeof(uint8_t));
+      break;
+    }
+    case CAN_MSG_RC_STATE_ID: {
+      msg->type = AgxMsgRcState;
+      memcpy(msg->body.rc_state_msg.raw, rx_frame->data,
+             rx_frame->can_dlc * sizeof(uint8_t));
+      break;
+    }
+    case CAN_MSG_CTRL_MODE_SELECT_ID: {
+      msg->type = AgxMsgCtrlModeSelect;
+      memcpy(msg->body.ctrl_mode_select_msg.raw, rx_frame->data,
+             rx_frame->can_dlc * sizeof(uint8_t));
+      break;
+    }
+    case CAN_MSG_STATE_RESET_ID: {
+      msg->type = AgxMsgFaultByteReset;
+      memcpy(msg->body.state_reset_msg.raw, rx_frame->data,
+             rx_frame->can_dlc * sizeof(uint8_t));
+      break;
+    }
+    // state feedback frame
+    case CAN_MSG_SYSTEM_STATE_ID: {
+      msg->type = AgxMsgSystemState;
+      memcpy(msg->body.system_state_msg.raw, rx_frame->data,
+             rx_frame->can_dlc * sizeof(uint8_t));
+      break;
+    }
+    case CAN_MSG_MOTION_STATE_ID: {
+      msg->type = AgxMsgMotionState;
+      memcpy(msg->body.motion_state_msg.raw, rx_frame->data,
+             rx_frame->can_dlc * sizeof(uint8_t));
+      break;
+    }
+    case CAN_MSG_LIGHT_STATE_ID: {
+      msg->type = AgxMsgLightState;
+      memcpy(msg->body.light_state_msg.raw, rx_frame->data,
+             rx_frame->can_dlc * sizeof(uint8_t));
+      break;
+    }
+    case CAN_MSG_ODOMETRY_ID: {
+      msg->type = AgxMsgOdometry;
+      memcpy(msg->body.odometry_msg.raw, rx_frame->data,
+             rx_frame->can_dlc * sizeof(uint8_t));
+      break;
+    }
+    case CAN_MSG_ACTUATOR1_HS_STATE_ID:
+    case CAN_MSG_ACTUATOR2_HS_STATE_ID:
+    case CAN_MSG_ACTUATOR3_HS_STATE_ID:
+    case CAN_MSG_ACTUATOR4_HS_STATE_ID: {
+      msg->type = AgxMsgActuatorLSState;
+      msg->body.actuator_hs_state_msg.motor_id =
+          (uint8_t)(rx_frame->can_id - CAN_MSG_ACTUATOR1_HS_STATE_ID);
+      memcpy(msg->body.actuator_hs_state_msg.data.raw, rx_frame->data,
+             rx_frame->can_dlc * sizeof(uint8_t));
+      break;
+    }
+    case CAN_MSG_ACTUATOR1_LS_STATE_ID:
+    case CAN_MSG_ACTUATOR2_LS_STATE_ID:
+    case CAN_MSG_ACTUATOR3_LS_STATE_ID:
+    case CAN_MSG_ACTUATOR4_LS_STATE_ID: {
+      msg->type = AgxMsgActuatorLSState;
+      msg->body.actuator_ls_state_msg.motor_id =
+          (uint8_t)(rx_frame->can_id - CAN_MSG_ACTUATOR1_LS_STATE_ID);
+      memcpy(msg->body.actuator_ls_state_msg.data.raw, rx_frame->data,
+             rx_frame->can_dlc * sizeof(uint8_t));
+      break;
+    }
+    default:
+      break;
+  }
+
+  return true;
+}
+
+void EncodeCanFrame(const AgxMessage *msg, struct can_frame *tx_frame) {
+  switch (msg->type) {
+    // command frame
+    case AgxMsgMotionCommand: {
+      tx_frame->can_id = CAN_MSG_MOTION_COMMAND_ID;
+      tx_frame->can_dlc = 8;
+      memcpy(tx_frame->data, msg->body.motion_command_msg.raw,
+             tx_frame->can_dlc);
+      break;
+    }
+    case AgxMsgLightCommand: {
+      tx_frame->can_id = CAN_MSG_LIGHT_COMMAND_ID;
+      tx_frame->can_dlc = 8;
+      memcpy(tx_frame->data, msg->body.light_command_msg.raw,
+             tx_frame->can_dlc);
+      break;
+    }
+    case AgxMsgCtrlModeSelect: {
+      tx_frame->can_id = CAN_MSG_CTRL_MODE_SELECT_ID;
+      tx_frame->can_dlc = 8;
+      memcpy(tx_frame->data, msg->body.ctrl_mode_select_msg.raw,
+             tx_frame->can_dlc);
+      break;
+    }
+    case AgxMsgFaultByteReset: {
+      tx_frame->can_id = CAN_MSG_STATE_RESET_ID;
+      tx_frame->can_dlc = 8;
+      memcpy(tx_frame->data, msg->body.state_reset_msg.raw, tx_frame->can_dlc);
+      break;
+    }
+    // state feedback frame
+    case AgxMsgSystemState: {
+      tx_frame->can_id = CAN_MSG_SYSTEM_STATE_ID;
+      tx_frame->can_dlc = 8;
+      memcpy(tx_frame->data, msg->body.system_state_msg.raw, tx_frame->can_dlc);
+      break;
+    }
+    case AgxMsgMotionState: {
+      tx_frame->can_id = CAN_MSG_MOTION_STATE_ID;
+      tx_frame->can_dlc = 8;
+      memcpy(tx_frame->data, msg->body.motion_state_msg.raw, tx_frame->can_dlc);
+      break;
+    }
+    case AgxMsgLightState: {
+      tx_frame->can_id = CAN_MSG_LIGHT_STATE_ID;
+      tx_frame->can_dlc = 8;
+      memcpy(tx_frame->data, msg->body.light_state_msg.raw, tx_frame->can_dlc);
+      break;
+    }
+    case AgxMsgOdometry: {
+      tx_frame->can_id = CAN_MSG_ODOMETRY_ID;
+      tx_frame->can_dlc = 8;
+      memcpy(tx_frame->data, msg->body.odometry_msg.raw, tx_frame->can_dlc);
+      break;
+    }
+    case AgxMsgActuatorHSState: {
+      switch (msg->body.actuator_hs_state_msg.motor_id) {
+        case ACTUATOR1_ID: {
+          tx_frame->can_id = CAN_MSG_ACTUATOR1_HS_STATE_ID;
+          break;
+        }
+        case ACTUATOR2_ID: {
+          tx_frame->can_id = CAN_MSG_ACTUATOR2_HS_STATE_ID;
+          break;
+        }
+        case ACTUATOR3_ID: {
+          tx_frame->can_id = CAN_MSG_ACTUATOR3_HS_STATE_ID;
+          break;
+        }
+        case ACTUATOR4_ID: {
+          tx_frame->can_id = CAN_MSG_ACTUATOR4_HS_STATE_ID;
+          break;
+        }
+      }
+      tx_frame->can_dlc = 8;
+      memcpy(tx_frame->data, msg->body.actuator_hs_state_msg.data.raw,
+             tx_frame->can_dlc);
+      break;
+    }
+    case AgxMsgActuatorLSState: {
+      switch (msg->body.actuator_ls_state_msg.motor_id) {
+        case ACTUATOR1_ID: {
+          tx_frame->can_id = CAN_MSG_ACTUATOR1_LS_STATE_ID;
+          break;
+        }
+        case ACTUATOR2_ID: {
+          tx_frame->can_id = CAN_MSG_ACTUATOR2_LS_STATE_ID;
+          break;
+        }
+        case ACTUATOR3_ID: {
+          tx_frame->can_id = CAN_MSG_ACTUATOR3_LS_STATE_ID;
+          break;
+        }
+        case ACTUATOR4_ID: {
+          tx_frame->can_id = CAN_MSG_ACTUATOR4_LS_STATE_ID;
+          break;
+        }
+      }
+      tx_frame->can_dlc = 8;
+      memcpy(tx_frame->data, msg->body.actuator_ls_state_msg.data.raw,
+             tx_frame->can_dlc);
+      break;
+    }
+    default:
+      break;
+  }
+  //   tx_frame->data[7] =
+  //       CalcCanFrameChecksum(tx_frame->can_id, tx_frame->data,
+  //       tx_frame->can_dlc);
+}
+
+uint8_t CalcCanFrameChecksum(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;
+}

src/hunter_base.cpp

@@ -0,0 +1,239 @@
+#include "ugv_sdk/hunter/hunter_base.hpp"
+
+#include <algorithm>
+#include <array>
+#include <chrono>
+#include <cstdint>
+#include <cstring>
+#include <iostream>
+#include <ratio>
+#include <string>
+#include <thread>
+
+#include "stopwatch.hpp"
+
+namespace westonrobot {
+
+void HunterBase::SendRobotCmd() {
+  static uint8_t cmd_count = 0;
+  SendModeCtl();
+  SetParkMode();
+  SendMotionCmd(cmd_count++);
+}
+
+void HunterBase::SendMotionCmd(uint8_t count) {
+  // motion control message
+  HunterMessage m_msg;
+  m_msg.type = HunterMotionControlMsg;
+  /*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_cmd_msg.data.cmd.control_mode = CTRL_MODE_CMD_UART*/
+  ;
+  motion_cmd_mutex_.lock();
+  m_msg.body.motion_control_msg.data.cmd.linear_velocity_cmd.high_byte =
+      current_motion_cmd_.linear_velocity_height_byte;
+  m_msg.body.motion_control_msg.data.cmd.linear_velocity_cmd.low_byte =
+      current_motion_cmd_.linear_velocity_low_byte;
+  m_msg.body.motion_control_msg.data.cmd.angular_velocity_cmd.high_byte =
+      current_motion_cmd_.angular_velocity_height_byte;
+  m_msg.body.motion_control_msg.data.cmd.angular_velocity_cmd.low_byte =
+      current_motion_cmd_.angular_velocity_low_byte;
+  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.reserved2 = 0;
+  m_msg.body.motion_control_msg.data.cmd.reserved3 = 0;
+
+  // send to can bus
+  if (can_connected_) {
+    can_frame m_frame;
+    EncodeHunterMsgToCAN(&m_msg, &m_frame);
+    can_if_->SendFrame(m_frame);
+  }
+}
+
+HunterState HunterBase::GetHunterState() {
+  std::lock_guard<std::mutex> guard(hunter_state_mutex_);
+  return hunter_state_;
+}
+
+void HunterBase::SetMotionCommand(
+    double linear_vel, double angular_vel, double steering_angle,
+    HunterMotionCmd::FaultClearFlag fault_clr_flag) {
+  // make sure cmd thread is started before attempting to send commands
+  if (!cmd_thread_started_) StartCmdThread();
+
+  if (linear_vel < HunterMotionCmd::min_linear_velocity)
+    linear_vel = HunterMotionCmd::min_linear_velocity;
+  if (linear_vel > HunterMotionCmd::max_linear_velocity)
+    linear_vel = HunterMotionCmd::max_linear_velocity;
+  if (steering_angle < HunterMotionCmd::min_steering_angle)
+    steering_angle = HunterMotionCmd::min_steering_angle;
+  if (steering_angle > HunterMotionCmd::max_steering_angle)
+    steering_angle = HunterMotionCmd::max_steering_angle;
+
+  std::lock_guard<std::mutex> guard(motion_cmd_mutex_);
+  current_motion_cmd_.linear_velocity_height_byte =
+      static_cast<int16_t>(linear_vel * 1000) >> 8;
+  current_motion_cmd_.linear_velocity_low_byte =
+      static_cast<int16_t>(linear_vel * 1000) & 0xff;
+  current_motion_cmd_.angular_velocity_height_byte =
+      static_cast<int16_t>(angular_vel * 1000) >> 8;
+  current_motion_cmd_.angular_velocity_low_byte =
+      static_cast<int16_t>(angular_vel * 1000) & 0xff;
+  current_motion_cmd_.fault_clear_flag = fault_clr_flag;
+
+  FeedCmdTimeoutWatchdog();
+}
+
+void HunterBase::SendModeCtl() {
+  HunterMessage m_msg;
+  m_msg.type = HunterControlModeMsg;
+  mode_cmd_mutex_.lock();
+  m_msg.body.mode_cmd_msg.data.cmd.mode_control = 0x01;
+  mode_cmd_mutex_.unlock();
+  m_msg.body.mode_cmd_msg.data.cmd.reserved0 = 0;
+  m_msg.body.mode_cmd_msg.data.cmd.reserved1 = 0;
+  m_msg.body.mode_cmd_msg.data.cmd.reserved2 = 0;
+  m_msg.body.mode_cmd_msg.data.cmd.reserved3 = 0;
+  m_msg.body.mode_cmd_msg.data.cmd.reserved4 = 0;
+  m_msg.body.mode_cmd_msg.data.cmd.reserved5 = 0;
+  m_msg.body.mode_cmd_msg.data.cmd.reserved6 = 0;
+  if (can_connected_) {
+    // send to can bus
+    can_frame m_frame;
+    EncodeHunterMsgToCAN(&m_msg, &m_frame);
+    can_if_->SendFrame(m_frame);
+  } else {
+  }
+}
+
+void HunterBase::SetParkMode() {
+  HunterMessage m_msg;
+  m_msg.type = HunterParkControlMsg;
+  bool flag = current_motion_cmd_.linear_velocity_height_byte ||
+              current_motion_cmd_.linear_velocity_low_byte;
+  if (flag) {
+    pack_mode_cmd_mutex_.lock();
+    m_msg.body.park_control_msg.data.cmd.packing_mode = 0x00;
+    pack_mode_cmd_mutex_.unlock();
+  } else {
+    pack_mode_cmd_mutex_.lock();
+    m_msg.body.park_control_msg.data.cmd.packing_mode = 0x01;
+    pack_mode_cmd_mutex_.unlock();
+  }
+  m_msg.body.park_control_msg.data.cmd.reserved0 = 0;
+  m_msg.body.park_control_msg.data.cmd.reserved1 = 0;
+  m_msg.body.park_control_msg.data.cmd.reserved2 = 0;
+  m_msg.body.park_control_msg.data.cmd.reserved3 = 0;
+  m_msg.body.park_control_msg.data.cmd.reserved4 = 0;
+  m_msg.body.park_control_msg.data.cmd.reserved5 = 0;
+  m_msg.body.park_control_msg.data.cmd.reserved6 = 0;
+  if (can_connected_) {
+    // send to can bus
+    can_frame m_frame;
+    EncodeHunterMsgToCAN(&m_msg, &m_frame);
+    can_if_->SendFrame(m_frame);
+  } else {
+  }
+}
+
+void HunterBase::ParseCANFrame(can_frame *rx_frame) {
+  // update robot state with new frame
+  HunterMessage status_msg;
+  DecodeHunterMsgFromCAN(rx_frame, &status_msg);
+  NewStatusMsgReceivedCallback(status_msg);
+}
+
+void HunterBase::NewStatusMsgReceivedCallback(const HunterMessage &msg) {
+  // std::cout << "new status msg received" << std::endl;
+  std::lock_guard<std::mutex> guard(hunter_state_mutex_);
+  UpdateHunterState(msg, hunter_state_);
+}
+
+void HunterBase::UpdateHunterState(const HunterMessage &status_msg,
+                                   HunterState &state) {
+  switch (status_msg.type) {
+    case HunterMotionStatusMsg: {
+      // 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.steering_angle =
+          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 HunterSystemStatusMsg: {
+      // 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.park_mode = msg.data.status.park_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 HunterMotorDriverHeightSpeedStatusMsg: {
+      // std::cout << "motor driver height speed feedback received" <<
+      // std::endl;
+      const MotorDriverHeightSpeedStatusMessage &msg =
+          status_msg.body.motor_driver_height_speed_status_msg;
+      for (int i = 0; i < HunterState::motor_num; ++i) {
+        state.motor_hs_state[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_hs_state[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_hs_state[msg.motor_id].motor_pose = static_cast<int32_t>(
+            static_cast<uint32_t>(msg.data.status.moter_pose.lowest) |
+            static_cast<uint32_t>(msg.data.status.moter_pose.sec_lowest) << 8 |
+            static_cast<uint32_t>(msg.data.status.moter_pose.sec_heighest)
+                << 16 |
+            static_cast<uint32_t>(msg.data.status.moter_pose.heighest) << 24);
+      }
+      break;
+    }
+    case HunterMotorDriverLowSpeedStatusMsg: {
+      // std::cout << "motor driver low speed feedback received" << std::endl;
+      const MotorDriverLowSpeedStatusMessage &msg =
+          status_msg.body.motor_driver_low_speed_status_msg;
+      for (int i = 0; i < HunterState::motor_num; ++i) {
+        state.motor_ls_state[msg.motor_id].driver_voltage =
+            (static_cast<uint16_t>(msg.data.status.driver_voltage.low_byte) |
+             static_cast<uint16_t>(msg.data.status.driver_voltage.high_byte)
+                 << 8) /
+            10.0;
+        state.motor_ls_state[msg.motor_id]
+            .driver_temperature = static_cast<int16_t>(
+            static_cast<uint16_t>(msg.data.status.driver_temperature.low_byte) |
+            static_cast<uint16_t>(msg.data.status.driver_temperature.high_byte)
+                << 8);
+        state.motor_ls_state[msg.motor_id].motor_temperature =
+            msg.data.status.motor_temperature;
+        state.motor_ls_state[msg.motor_id].driver_state =
+            msg.data.status.driver_status;
+      }
+    }
+    default:
+      break;
+  }
+}
+}  // namespace westonrobot

src/mobile_base.cpp

@@ -0,0 +1,107 @@
+/*
+ * mobile_base.cpp
+ *
+ * Created on: Jun 17, 2020 11:26
+ * Description:
+ *
+ * Copyright (c) 2020 Ruixiang Du (rdu)
+ */
+
+#include "ugv_sdk/mobile_base.hpp"
+
+#include <cstring>
+#include <iostream>
+#include <algorithm>
+
+#include "stopwatch.hpp"
+
+namespace westonrobot {
+MobileBase::~MobileBase() {
+  // release resource if occupied
+  Disconnect();
+
+  // joint cmd thread
+  if (cmd_thread_.joinable()) cmd_thread_.join();
+}
+
+void MobileBase::Connect(std::string dev_name, int32_t baud_rate) {
+  if (baud_rate == 0) {
+    ConfigureCAN(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 (can_connected_) can_if_->StopService();
+  if (serial_connected_ && serial_if_->IsOpened()) {
+    serial_if_->StopService();
+  }
+}
+
+void MobileBase::Terminate() {
+  keep_running_ = false;
+  std::terminate();
+}
+
+void MobileBase::ConfigureCAN(const std::string &can_if_name) {
+  can_if_ = std::make_shared<AsyncCAN>(can_if_name);
+  can_if_->SetReceiveCallback(
+      std::bind(&MobileBase::ParseCANFrame, this, std::placeholders::_1));
+  can_if_->StartListening();
+  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_->SetReceiveCallback(
+      std::bind(&MobileBase::ParseUARTBuffer, this, std::placeholders::_1,
+                std::placeholders::_2, std::placeholders::_3));
+  serial_if_->StartListening();
+  if (serial_if_->IsOpened()) serial_connected_ = true;
+}
+
+void MobileBase::SetCmdTimeout(bool enable, uint32_t timeout_ms) {
+  enable_timeout_ = true;
+  timeout_ms_ = timeout_ms;
+}
+
+void MobileBase::StartCmdThread() {
+  keep_running_ = true;
+  cmd_thread_ = std::thread(
+      std::bind(&MobileBase::ControlLoop, this, cmd_thread_period_ms_));
+  cmd_thread_started_ = true;
+}
+
+void MobileBase::ControlLoop(int32_t period_ms) {
+  uint32_t timeout_iter_num;
+
+  if (enable_timeout_) {
+    if (timeout_ms_ < period_ms) timeout_ms_ = period_ms;
+    timeout_iter_num = static_cast<uint32_t>(timeout_ms_ / period_ms);
+    // std::cout << "Timeout iteration number: " << timeout_iter_num <<
+    // std::endl;
+  }
+
+  Timer tm;
+  while (keep_running_) {
+    tm.reset();
+    if (enable_timeout_) {
+      if (watchdog_counter_ < timeout_iter_num) {
+        SendRobotCmd();
+        ++watchdog_counter_;
+      }
+      //   else {
+      //     std::cout << "Warning: cmd timeout, no cmd sent to robot" <<
+      //     std::endl;
+      //   }
+    } else {
+      SendRobotCmd();
+    }
+    tm.sleep_until_ms(period_ms);
+  }
+}
+}  // namespace westonrobot

src/scout_base.cpp

@@ -0,0 +1,231 @@
+#include "ugv_sdk/scout/scout_base.hpp"
+
+#include <stdio.h>
+#include <string>
+#include <cstring>
+#include <iostream>
+#include <algorithm>
+#include <array>
+#include <chrono>
+#include <cstdint>
+#include <ratio>
+#include <thread>
+
+#include "stopwatch.hpp"
+
+namespace westonrobot {
+void ScoutBase::SendRobotCmd() {
+  static uint8_t cmd_count = 0;
+  //   EnableCommandedMode();
+  if (can_connected_) {
+    SendMotionCmd(cmd_count++);
+  }
+}
+
+void ScoutBase::EnableCommandedMode() {
+  AgxMessage c_msg;
+  c_msg.type = AgxMsgCtrlModeSelect;
+  memset(c_msg.body.ctrl_mode_select_msg.raw, 0, 8);
+  c_msg.body.ctrl_mode_select_msg.cmd.control_mode = CTRL_MODE_CMD_CAN;
+
+  // send to can bus
+  can_frame c_frame;
+  EncodeCanFrame(&c_msg, &c_frame);
+  can_if_->SendFrame(c_frame);
+}
+
+void ScoutBase::SendMotionCmd(uint8_t count) {
+  // motion control message
+  AgxMessage m_msg;
+  m_msg.type = AgxMsgMotionCommand;
+  memset(m_msg.body.motion_command_msg.raw, 0, 8);
+
+  motion_cmd_mutex_.lock();
+  int16_t linear_cmd =
+      static_cast<int16_t>(current_motion_cmd_.linear_velocity * 1000);
+  int16_t angular_cmd =
+      static_cast<int16_t>(current_motion_cmd_.angular_velocity * 1000);
+  motion_cmd_mutex_.unlock();
+
+  // SendControlCmd();
+  m_msg.body.motion_command_msg.cmd.linear_velocity.high_byte =
+      (static_cast<uint16_t>(linear_cmd) >> 8) & 0x00ff;
+  m_msg.body.motion_command_msg.cmd.linear_velocity.low_byte =
+      (static_cast<uint16_t>(linear_cmd) >> 0) & 0x00ff;
+  m_msg.body.motion_command_msg.cmd.angular_velocity.high_byte =
+      (static_cast<uint16_t>(angular_cmd) >> 8) & 0x00ff;
+  m_msg.body.motion_command_msg.cmd.angular_velocity.low_byte =
+      (static_cast<uint16_t>(angular_cmd) >> 0) & 0x00ff;
+
+  // send to can bus
+  can_frame m_frame;
+  EncodeCanFrame(&m_msg, &m_frame);
+  can_if_->SendFrame(m_frame);
+}
+
+void ScoutBase::SendLightCmd(const ScoutLightCmd &lcmd, uint8_t count) {
+  AgxMessage l_msg;
+  l_msg.type = AgxMsgLightCommand;
+  memset(l_msg.body.light_command_msg.raw, 0, 8);
+
+  if (lcmd.enable_ctrl) {
+    l_msg.body.light_command_msg.cmd.light_ctrl_enabled = LIGHT_CTRL_ENABLE;
+
+    l_msg.body.light_command_msg.cmd.front_light_mode =
+        static_cast<uint8_t>(lcmd.front_mode);
+    l_msg.body.light_command_msg.cmd.front_light_custom =
+        lcmd.front_custom_value;
+    l_msg.body.light_command_msg.cmd.rear_light_mode =
+        static_cast<uint8_t>(lcmd.rear_mode);
+    l_msg.body.light_command_msg.cmd.rear_light_custom = lcmd.rear_custom_value;
+  } else {
+    l_msg.body.light_command_msg.cmd.light_ctrl_enabled = LIGHT_CTRL_DISABLE;
+  }
+
+  l_msg.body.light_command_msg.cmd.count = count;
+
+  // send to can bus
+  can_frame l_frame;
+  EncodeCanFrame(&l_msg, &l_frame);
+  can_if_->SendFrame(l_frame);
+}
+
+ScoutState ScoutBase::GetScoutState() {
+  std::lock_guard<std::mutex> guard(scout_state_mutex_);
+  return scout_state_;
+}
+
+void ScoutBase::SetMotionCommand(double linear_vel, double angular_vel) {
+  // 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 = linear_vel;
+  current_motion_cmd_.angular_velocity = angular_vel;
+
+  FeedCmdTimeoutWatchdog();
+}
+
+void ScoutBase::SetLightCommand(const ScoutLightCmd &cmd) {
+  static uint8_t light_cmd_count = 0;
+  SendLightCmd(cmd, light_cmd_count++);
+}
+
+void ScoutBase::ParseCANFrame(can_frame *rx_frame) {
+  AgxMessage status_msg;
+  DecodeCanFrame(rx_frame, &status_msg);
+  NewStatusMsgReceivedCallback(status_msg);
+}
+
+void ScoutBase::NewStatusMsgReceivedCallback(const AgxMessage &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 AgxMessage &status_msg,
+                                 ScoutState &state) {
+  switch (status_msg.type) {
+    case AgxMsgSystemState: {
+      // std::cout << "system status feedback received" << std::endl;
+      const SystemStateMessage &msg = status_msg.body.system_state_msg;
+      state.control_mode = msg.state.control_mode;
+      state.base_state = msg.state.vehicle_state;
+      state.battery_voltage =
+          (static_cast<uint16_t>(msg.state.battery_voltage.low_byte) |
+           static_cast<uint16_t>(msg.state.battery_voltage.high_byte) << 8) /
+          10.0;
+      state.fault_code = msg.state.fault_code;
+      break;
+    }
+    case AgxMsgMotionState: {
+      // std::cout << "motion control feedback received" << std::endl;
+      const MotionStateMessage &msg = status_msg.body.motion_state_msg;
+      state.linear_velocity =
+          static_cast<int16_t>(
+              static_cast<uint16_t>(msg.state.linear_velocity.low_byte) |
+              static_cast<uint16_t>(msg.state.linear_velocity.high_byte) << 8) /
+          1000.0;
+      state.angular_velocity =
+          static_cast<int16_t>(
+              static_cast<uint16_t>(msg.state.angular_velocity.low_byte) |
+              static_cast<uint16_t>(msg.state.angular_velocity.high_byte)
+                  << 8) /
+          1000.0;
+      break;
+    }
+    case AgxMsgLightState: {
+      // std::cout << "light control feedback received" << std::endl;
+      const LightStateMessage &msg = status_msg.body.light_state_msg;
+      if (msg.state.light_ctrl_enabled == LIGHT_CTRL_DISABLE)
+        state.light_control_enabled = false;
+      else
+        state.light_control_enabled = true;
+      state.front_light_state.mode = msg.state.front_light_mode;
+      state.front_light_state.custom_value = msg.state.front_light_custom;
+      state.rear_light_state.mode = msg.state.rear_light_mode;
+      state.rear_light_state.custom_value = msg.state.rear_light_custom;
+      break;
+    }
+    case AgxMsgActuatorHSState: {
+      // std::cout << "actuator hs feedback received" << std::endl;
+      const ActuatorHSStateMessage &msg = status_msg.body.actuator_hs_state_msg;
+      state.actuator_states[msg.motor_id].motor_current =
+          (static_cast<uint16_t>(msg.data.state.current.low_byte) |
+           static_cast<uint16_t>(msg.data.state.current.high_byte) << 8) /
+          10.0;
+      state.actuator_states[msg.motor_id].motor_rpm = static_cast<int16_t>(
+          static_cast<uint16_t>(msg.data.state.rpm.low_byte) |
+          static_cast<uint16_t>(msg.data.state.rpm.high_byte) << 8);
+      state.actuator_states[msg.motor_id].motor_pulses = static_cast<int16_t>(
+          static_cast<uint16_t>(msg.data.state.pulse_count.low_byte) |
+          static_cast<uint16_t>(msg.data.state.pulse_count.high_byte) << 8);
+      break;
+    }
+    case AgxMsgActuatorLSState: {
+      // std::cout << "actuator ls feedback received" << std::endl;
+      const ActuatorLSStateMessage &msg = status_msg.body.actuator_ls_state_msg;
+      for (int i = 0; i < 2; ++i) {
+        state.actuator_states[msg.motor_id].driver_voltage =
+            (static_cast<uint16_t>(msg.data.state.driver_voltage.low_byte) |
+             static_cast<uint16_t>(msg.data.state.driver_voltage.high_byte)
+                 << 8) /
+            10.0;
+        state.actuator_states[msg.motor_id]
+            .driver_temperature = static_cast<int16_t>(
+            static_cast<uint16_t>(msg.data.state.driver_temperature.low_byte) |
+            static_cast<uint16_t>(msg.data.state.driver_temperature.high_byte)
+                << 8);
+        state.actuator_states[msg.motor_id].motor_temperature =
+            msg.data.state.motor_temperature;
+        state.actuator_states[msg.motor_id].driver_state =
+            msg.data.state.driver_state;
+      }
+      break;
+    }
+    case AgxMsgOdometry: {
+      // std::cout << "Odometer msg feedback received" << std::endl;
+      const OdometryMessage &msg = status_msg.body.odometry_msg;
+      state.right_odometry = static_cast<int32_t>(
+          (static_cast<uint32_t>(msg.state.right_wheel.lsb)) |
+          (static_cast<uint32_t>(msg.state.right_wheel.low_byte) << 8) |
+          (static_cast<uint32_t>(msg.state.right_wheel.high_byte) << 16) |
+          (static_cast<uint32_t>(msg.state.right_wheel.msb) << 24));
+      state.left_odometry = static_cast<int32_t>(
+          (static_cast<uint32_t>(msg.state.left_wheel.lsb)) |
+          (static_cast<uint32_t>(msg.state.left_wheel.low_byte) << 8) |
+          (static_cast<uint32_t>(msg.state.left_wheel.high_byte) << 16) |
+          (static_cast<uint32_t>(msg.state.left_wheel.msb) << 24));
+    }
+  }
+}
+}  // namespace westonrobot

src/stopwatch.hpp

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

src/tracer_base.cpp

@@ -0,0 +1,228 @@
+#include "ugv_sdk/tracer/tracer_base.hpp"
+
+#include <string>
+#include <cstring>
+#include <iostream>
+#include <algorithm>
+#include <array>
+#include <chrono>
+#include <cstdint>
+#include <ratio>
+#include <thread>
+
+#include "stopwatch.hpp"
+
+namespace westonrobot {
+void TracerBase::SendRobotCmd() {
+  static uint8_t cmd_count = 0;
+  //   EnableCommandedMode();
+  if (can_connected_) {
+    SendMotionCmd(cmd_count++);
+  }
+}
+
+void TracerBase::EnableCommandedMode() {
+  AgxMessage c_msg;
+  c_msg.type = AgxMsgCtrlModeSelect;
+  memset(c_msg.body.ctrl_mode_select_msg.raw, 0, 8);
+  c_msg.body.ctrl_mode_select_msg.cmd.control_mode = CTRL_MODE_CMD_CAN;
+
+  // send to can bus
+  can_frame c_frame;
+  EncodeCanFrame(&c_msg, &c_frame);
+  can_if_->SendFrame(c_frame);
+}
+
+void TracerBase::SendMotionCmd(uint8_t count) {
+  // motion control message
+  AgxMessage m_msg;
+  m_msg.type = AgxMsgMotionCommand;
+  memset(m_msg.body.motion_command_msg.raw, 0, 8);
+
+  motion_cmd_mutex_.lock();
+  int16_t linear_cmd =
+      static_cast<int16_t>(current_motion_cmd_.linear_velocity * 1000);
+  int16_t angular_cmd =
+      static_cast<int16_t>(current_motion_cmd_.angular_velocity * 1000);
+  motion_cmd_mutex_.unlock();
+
+  // SendControlCmd();
+  m_msg.body.motion_command_msg.cmd.linear_velocity.high_byte =
+      (static_cast<uint16_t>(linear_cmd) >> 8) & 0x00ff;
+  m_msg.body.motion_command_msg.cmd.linear_velocity.low_byte =
+      (static_cast<uint16_t>(linear_cmd) >> 0) & 0x00ff;
+  m_msg.body.motion_command_msg.cmd.angular_velocity.high_byte =
+      (static_cast<uint16_t>(angular_cmd) >> 8) & 0x00ff;
+  m_msg.body.motion_command_msg.cmd.angular_velocity.low_byte =
+      (static_cast<uint16_t>(angular_cmd) >> 0) & 0x00ff;
+
+  // send to can bus
+  can_frame m_frame;
+  EncodeCanFrame(&m_msg, &m_frame);
+  can_if_->SendFrame(m_frame);
+}
+
+void TracerBase::SendLightCmd(const TracerLightCmd &lcmd, uint8_t count) {
+  AgxMessage l_msg;
+  l_msg.type = AgxMsgLightCommand;
+  memset(l_msg.body.light_command_msg.raw, 0, 8);
+
+  if (lcmd.enable_ctrl) {
+    l_msg.body.light_command_msg.cmd.light_ctrl_enabled = LIGHT_CTRL_ENABLE;
+
+    l_msg.body.light_command_msg.cmd.front_light_mode =
+        static_cast<uint8_t>(lcmd.front_mode);
+    l_msg.body.light_command_msg.cmd.front_light_custom =
+        lcmd.front_custom_value;
+    l_msg.body.light_command_msg.cmd.rear_light_mode =
+        static_cast<uint8_t>(lcmd.rear_mode);
+    l_msg.body.light_command_msg.cmd.rear_light_custom = lcmd.rear_custom_value;
+  } else {
+    l_msg.body.light_command_msg.cmd.light_ctrl_enabled = LIGHT_CTRL_DISABLE;
+  }
+
+  l_msg.body.light_command_msg.cmd.count = count;
+
+  // send to can bus
+  can_frame l_frame;
+  EncodeCanFrame(&l_msg, &l_frame);
+  can_if_->SendFrame(l_frame);
+}
+
+TracerState TracerBase::GetTracerState() {
+  std::lock_guard<std::mutex> guard(tracer_state_mutex_);
+  return tracer_state_;
+}
+
+void TracerBase::SetMotionCommand(double linear_vel, double angular_vel) {
+  // 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 = linear_vel;
+  current_motion_cmd_.angular_velocity = angular_vel;
+
+  FeedCmdTimeoutWatchdog();
+}
+
+void TracerBase::SetLightCommand(const TracerLightCmd &cmd) {
+  static uint8_t light_cmd_count = 0;
+  SendLightCmd(cmd, light_cmd_count++);
+}
+
+void TracerBase::ParseCANFrame(can_frame *rx_frame) {
+  AgxMessage status_msg;
+  DecodeCanFrame(rx_frame, &status_msg);
+  NewStatusMsgReceivedCallback(status_msg);
+}
+
+void TracerBase::NewStatusMsgReceivedCallback(const AgxMessage &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 AgxMessage &status_msg,
+                                   TracerState &state) {
+  switch (status_msg.type) {
+    case AgxMsgSystemState: {
+      // std::cout << "system status feedback received" << std::endl;
+      const SystemStateMessage &msg = status_msg.body.system_state_msg;
+      state.control_mode = msg.state.control_mode;
+      state.base_state = msg.state.vehicle_state;
+      state.battery_voltage =
+          (static_cast<uint16_t>(msg.state.battery_voltage.low_byte) |
+           static_cast<uint16_t>(msg.state.battery_voltage.high_byte) << 8) /
+          10.0;
+      state.fault_code = msg.state.fault_code;
+      break;
+    }
+    case AgxMsgMotionState: {
+      // std::cout << "motion control feedback received" << std::endl;
+      const MotionStateMessage &msg = status_msg.body.motion_state_msg;
+      state.linear_velocity =
+          static_cast<int16_t>(
+              static_cast<uint16_t>(msg.state.linear_velocity.low_byte) |
+              static_cast<uint16_t>(msg.state.linear_velocity.high_byte) << 8) /
+          1000.0;
+      state.angular_velocity =
+          static_cast<int16_t>(
+              static_cast<uint16_t>(msg.state.angular_velocity.low_byte) |
+              static_cast<uint16_t>(msg.state.angular_velocity.high_byte)
+                  << 8) /
+          1000.0;
+      break;
+    }
+    case AgxMsgLightState: {
+      // std::cout << "light control feedback received" << std::endl;
+      const LightStateMessage &msg = status_msg.body.light_state_msg;
+      if (msg.state.light_ctrl_enabled == LIGHT_CTRL_DISABLE)
+        state.light_control_enabled = false;
+      else
+        state.light_control_enabled = true;
+      state.front_light_state.mode = msg.state.front_light_mode;
+      state.front_light_state.custom_value = msg.state.front_light_custom;
+      break;
+    }
+    case AgxMsgActuatorHSState: {
+      // std::cout << "actuator hs feedback received" << std::endl;
+      const ActuatorHSStateMessage &msg = status_msg.body.actuator_hs_state_msg;
+      state.actuator_states[msg.motor_id].motor_current =
+          (static_cast<uint16_t>(msg.data.state.current.low_byte) |
+           static_cast<uint16_t>(msg.data.state.current.high_byte) << 8) /
+          10.0;
+      state.actuator_states[msg.motor_id].motor_rpm = static_cast<int16_t>(
+          static_cast<uint16_t>(msg.data.state.rpm.low_byte) |
+          static_cast<uint16_t>(msg.data.state.rpm.high_byte) << 8);
+      state.actuator_states[msg.motor_id].motor_pulses = static_cast<int16_t>(
+          static_cast<uint16_t>(msg.data.state.pulse_count.low_byte) |
+          static_cast<uint16_t>(msg.data.state.pulse_count.high_byte) << 8);
+      break;
+    }
+    case AgxMsgActuatorLSState: {
+      // std::cout << "actuator ls feedback received" << std::endl;
+      const ActuatorLSStateMessage &msg = status_msg.body.actuator_ls_state_msg;
+      for (int i = 0; i < 2; ++i) {
+        state.actuator_states[msg.motor_id].driver_voltage =
+            (static_cast<uint16_t>(msg.data.state.driver_voltage.low_byte) |
+             static_cast<uint16_t>(msg.data.state.driver_voltage.high_byte)
+                 << 8) /
+            10.0;
+        state.actuator_states[msg.motor_id]
+            .driver_temperature = static_cast<int16_t>(
+            static_cast<uint16_t>(msg.data.state.driver_temperature.low_byte) |
+            static_cast<uint16_t>(msg.data.state.driver_temperature.high_byte)
+                << 8);
+        state.actuator_states[msg.motor_id].motor_temperature =
+            msg.data.state.motor_temperature;
+        state.actuator_states[msg.motor_id].driver_state =
+            msg.data.state.driver_state;
+      }
+      break;
+    }
+    case AgxMsgOdometry: {
+      // std::cout << "Odometer msg feedback received" << std::endl;
+      const OdometryMessage &msg = status_msg.body.odometry_msg;
+      state.right_odometry = static_cast<int32_t>(
+          (static_cast<uint32_t>(msg.state.right_wheel.lsb)) |
+          (static_cast<uint32_t>(msg.state.right_wheel.low_byte) << 8) |
+          (static_cast<uint32_t>(msg.state.right_wheel.high_byte) << 16) |
+          (static_cast<uint32_t>(msg.state.right_wheel.msb) << 24));
+      state.left_odometry = static_cast<int32_t>(
+          (static_cast<uint32_t>(msg.state.left_wheel.lsb)) |
+          (static_cast<uint32_t>(msg.state.left_wheel.low_byte) << 8) |
+          (static_cast<uint32_t>(msg.state.left_wheel.high_byte) << 16) |
+          (static_cast<uint32_t>(msg.state.left_wheel.msb) << 24));
+    }
+  }
+}
+}  // namespace westonrobot