created mobile base class

CMakeLists.txt

@@ -51,6 +51,7 @@ set(WRP_SDK_SRC
     src/async_can.cpp
     src/asyncio_utils.cpp
     # agilex mobile platforms
+    src/platforms/mobile_base.cpp
     src/platforms/hunter_base.cpp
     src/platforms/hunter_can_parser.c
     src/platforms/scout_base.cpp

include/wrp_sdk/platforms/common/mobile_base.hpp

@@ -43,53 +43,31 @@ class MobileBase {
     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
+  // 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
+  std::shared_ptr<ASyncCAN> can_if_;
-  uint8_t tx_cmd_len_;
+  std::shared_ptr<ASyncSerial> serial_if_;
-  uint8_t tx_buffer_[HUNTER_CMD_BUF_LEN];
 
-  // cmd/status update related variables
+  // command thread
   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 ConfigureCAN(const std::string &can_if_name = "can0");
   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);
+  virtual void SendRobotCmd() = 0;
-
+  virtual void ParseCANFrame(can_frame *rx_frame) = 0;
-  void ParseCANFrame(can_frame *rx_frame);
+  virtual void ParseUARTBuffer(uint8_t *buf, const size_t bufsize,
-  void ParseUARTBuffer(uint8_t *buf, const size_t bufsize,
+                               size_t bytes_received) = 0;
-                       size_t bytes_received);
-
-  void NewStatusMsgReceivedCallback(const HunterMessage &msg);
 };
 }  // namespace westonrobot
 

src/platforms/mobile_base.cpp

@@ -7,7 +7,7 @@
  * Copyright (c) 2020 Ruixiang Du (rdu)
  */
 
-#include "wrp_sdk/platforms/mobile_base.hpp"
+#include "wrp_sdk/platforms/common/mobile_base.hpp"
 
 #include <cstring>
 #include <iostream>
@@ -17,33 +17,32 @@
 
 namespace westonrobot {
 MobileBase::~MobileBase() {
-  if (serial_connected_) serial_if_->close();
+  // 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) {
-    ConfigureCANBus(dev_name);
+    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 (serial_connected_) {
+  if (serial_connected_ && serial_if_->is_open()) {
-    if (serial_if_->is_open()) serial_if_->close();
+    serial_if_->close();
   }
 }
 
-void MobileBase::ConfigureCANBus(const std::string &can_if_name) {
+void MobileBase::ConfigureCAN(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;
 }
 
@@ -51,300 +50,28 @@ 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;
+  bool print_loop_freq = false;
-  uint8_t light_cmd_count = 0;
   while (true) {
     ctrl_sw.tic();
-
+    SendRobotCmd();
-    // 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() <<
+    if (print_loop_freq)
-    // std::endl;
+      std::cout << "control loop 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