updated mobile_base to allow quick termination when SIGINT signal is catched

ugv_sdk/include/ugv_sdk/bunker/bunker_base.hpp

@@ -16,7 +16,7 @@
 #include <mutex>
 #include <functional>
 
-#include "ugv_sdk/common/mobile_base.hpp"
+#include "ugv_sdk/mobile_base.hpp"
 
 #include "ugv_sdk/bunker/bunker_protocol.h"
 #include "ugv_sdk/bunker/bunker_can_parser.h"

ugv_sdk/include/ugv_sdk/hunter/hunter_base.hpp

@@ -15,7 +15,7 @@
 #include <thread>
 #include <mutex>
 
-#include "ugv_sdk/common/mobile_base.hpp"
+#include "ugv_sdk/mobile_base.hpp"
 
 #include "ugv_sdk/hunter/hunter_protocol.h"
 #include "ugv_sdk/hunter/hunter_can_parser.h"

ugv_sdk/include/ugv_sdk/mobile_base.hpp

@@ -17,6 +17,7 @@
 #include <cstdint>
 #include <thread>
 #include <mutex>
+#include <atomic>
 
 #include "wrp_io/async_can.hpp"
 #include "wrp_io/async_serial.hpp"
@@ -32,6 +33,7 @@ class MobileBase {
   MobileBase &operator=(const MobileBase &hunter) = delete;
 
   void SetCmdTimeout(bool enable, uint32_t timeout_ms = 100);
+  void DisableTimeout() { enable_timeout_ = false; }
 
   // connect to roboot from CAN or serial
   void Connect(std::string dev_name, int32_t baud_rate = 0);
@@ -39,6 +41,9 @@ class MobileBase {
   // disconnect from roboot, only valid for serial port
   void Disconnect();
 
+  // ask background thread to shutdown properly
+  void Terminate();
+
   // cmd thread runs at 100Hz (10ms) by default
   void SetCmdThreadPeriodMs(int32_t period_ms) {
     cmd_thread_period_ms_ = period_ms;
@@ -62,6 +67,7 @@ class MobileBase {
   std::thread cmd_thread_;
   int32_t cmd_thread_period_ms_ = 10;
   bool cmd_thread_started_ = false;
+  std::atomic<bool> keep_running_;
 
   // internal functions
   void ConfigureCAN(const std::string &can_if_name = "can0");

ugv_sdk/include/ugv_sdk/scout/scout_base.hpp

@@ -16,7 +16,7 @@
 #include <mutex>
 #include <functional>
 
-#include "ugv_sdk/common/mobile_base.hpp"
+#include "ugv_sdk/mobile_base.hpp"
 
 #include "ugv_sdk/scout/scout_protocol.h"
 #include "ugv_sdk/scout/scout_can_parser.h"

ugv_sdk/include/ugv_sdk/tracer/tracer_base.hpp

@@ -17,7 +17,7 @@
 
 //#include "wrp_sdk/asyncio/async_can.hpp"
 //#include "wrp_sdk/asyncio/async_serial.hpp"
-#include "ugv_sdk/common/mobile_base.hpp"
+#include "ugv_sdk/mobile_base.hpp"
 
 #include "ugv_sdk/tracer/tracer_protocol.h"
 #include "ugv_sdk/tracer/tracer_can_parser.h"

ugv_sdk/src/mobile_base.cpp

@@ -7,7 +7,7 @@
  * Copyright (c) 2020 Ruixiang Du (rdu)
  */
 
-#include "ugv_sdk/common/mobile_base.hpp"
+#include "ugv_sdk/mobile_base.hpp"
 
 #include <cstring>
 #include <iostream>
@@ -19,6 +19,7 @@ namespace westonrobot {
 MobileBase::~MobileBase() {
   // release resource if occupied
   Disconnect();
+
   // joint cmd thread
   if (cmd_thread_.joinable()) cmd_thread_.join();
 }
@@ -40,6 +41,11 @@ void MobileBase::Disconnect() {
   }
 }
 
+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(
@@ -58,7 +64,13 @@ void MobileBase::ConfigureSerial(const std::string uart_name,
   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;
@@ -72,18 +84,21 @@ void MobileBase::ControlLoop(int32_t period_ms) {
   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;
+    // std::cout << "Timeout iteration number: " << timeout_iter_num <<
+    // std::endl;
   }
 
-  while (true) {
+  while (keep_running_) {
     ctrl_sw.tic();
     if (enable_timeout_) {
       if (watchdog_counter_ < timeout_iter_num) {
         SendRobotCmd();
         ++watchdog_counter_;
-      } else {
-        std::cout << "Warning: cmd timeout, old cmd not sent to robot" << std::endl;
       }
+      //   else {
+      //     std::cout << "Warning: cmd timeout, no cmd sent to robot" <<
+      //     std::endl;
+      //   }
     } else {
       SendRobotCmd();
     }