saved work on cmd timeout, not tested yet

src/asyncio/async_can.cpp

@@ -107,7 +107,7 @@ void AsyncCAN::SendFrame(const can_frame &frame) {
       asio::buffer(&frame, sizeof(frame)),
       [](asio::error_code error, size_t bytes_transferred) {
         if (error) {
-          std::cerr << "Failed to send CAN frame";
+          std::cerr << "Failed to send CAN frame" << std::endl;
         }
         // std::cout << "frame sent" << std::endl;
       });

src/platforms/hunter_base.cpp

@@ -17,6 +17,7 @@ namespace westonrobot {
 void HunterBase::SendRobotCmd() {
   static uint8_t cmd_count = 0;
   SendMotionCmd(cmd_count++);
+  FeedCmdTimeoutWatchdog();
 }
 
 void HunterBase::SendMotionCmd(uint8_t count) {

src/platforms/mobile_base.cpp

@@ -67,9 +67,12 @@ void MobileBase::StartCmdThread() {
 void MobileBase::ControlLoop(int32_t period_ms) {
   StopWatch ctrl_sw;
   bool print_loop_freq = false;
+  if (timeout_ms_ < period_ms) timeout_ms_ = period_ms;
+  uint32_t timeout_iter_num = static_cast<uint32_t>(timeout_ms_ / period_ms);
   while (true) {
     ctrl_sw.tic();
-    SendRobotCmd();
+    if (watchdog_counter_ < timeout_iter_num) SendRobotCmd();
+    ++watchdog_counter_;
     ctrl_sw.sleep_until_ms(period_ms);
     if (print_loop_freq)
       std::cout << "control loop frequency: " << 1.0 / ctrl_sw.toc()

src/platforms/scout_base.cpp

@@ -17,6 +17,7 @@ void ScoutBase::SendRobotCmd() {
   static uint8_t cmd_count = 0;
   static uint8_t light_cmd_count = 0;
   SendMotionCmd(cmd_count++);
+  FeedCmdTimeoutWatchdog();
   if (light_ctrl_requested_) SendLightCmd(light_cmd_count++);
 }