From a7460f197bf47caabde1720e15b1295d5f009b40 Mon Sep 17 00:00:00 2001 From: Ruixiang Du Date: Thu, 5 Nov 2020 12:47:50 +0800 Subject: [PATCH] updated mobile_base to allow quick termination when SIGINT signal is catched --- .../include/ugv_sdk/bunker/bunker_base.hpp | 2 +- .../include/ugv_sdk/hunter/hunter_base.hpp | 2 +- .../ugv_sdk/{common => }/mobile_base.hpp | 6 +++++ ugv_sdk/include/ugv_sdk/scout/scout_base.hpp | 2 +- .../include/ugv_sdk/tracer/tracer_base.hpp | 2 +- ugv_sdk/src/mobile_base.cpp | 25 +++++++++++++++---- 6 files changed, 30 insertions(+), 9 deletions(-) rename ugv_sdk/include/ugv_sdk/{common => }/mobile_base.hpp (92%) diff --git a/ugv_sdk/include/ugv_sdk/bunker/bunker_base.hpp b/ugv_sdk/include/ugv_sdk/bunker/bunker_base.hpp index fccdf60..ff79963 100644 --- a/ugv_sdk/include/ugv_sdk/bunker/bunker_base.hpp +++ b/ugv_sdk/include/ugv_sdk/bunker/bunker_base.hpp @@ -16,7 +16,7 @@ #include #include -#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" diff --git a/ugv_sdk/include/ugv_sdk/hunter/hunter_base.hpp b/ugv_sdk/include/ugv_sdk/hunter/hunter_base.hpp index d117276..a946a97 100644 --- a/ugv_sdk/include/ugv_sdk/hunter/hunter_base.hpp +++ b/ugv_sdk/include/ugv_sdk/hunter/hunter_base.hpp @@ -15,7 +15,7 @@ #include #include -#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" diff --git a/ugv_sdk/include/ugv_sdk/common/mobile_base.hpp b/ugv_sdk/include/ugv_sdk/mobile_base.hpp similarity index 92% rename from ugv_sdk/include/ugv_sdk/common/mobile_base.hpp rename to ugv_sdk/include/ugv_sdk/mobile_base.hpp index b30d65e..3adc3a1 100644 --- a/ugv_sdk/include/ugv_sdk/common/mobile_base.hpp +++ b/ugv_sdk/include/ugv_sdk/mobile_base.hpp @@ -17,6 +17,7 @@ #include #include #include +#include #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 keep_running_; // internal functions void ConfigureCAN(const std::string &can_if_name = "can0"); diff --git a/ugv_sdk/include/ugv_sdk/scout/scout_base.hpp b/ugv_sdk/include/ugv_sdk/scout/scout_base.hpp index f2798b9..81763a7 100644 --- a/ugv_sdk/include/ugv_sdk/scout/scout_base.hpp +++ b/ugv_sdk/include/ugv_sdk/scout/scout_base.hpp @@ -16,7 +16,7 @@ #include #include -#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" diff --git a/ugv_sdk/include/ugv_sdk/tracer/tracer_base.hpp b/ugv_sdk/include/ugv_sdk/tracer/tracer_base.hpp index 3577f81..7795d3c 100644 --- a/ugv_sdk/include/ugv_sdk/tracer/tracer_base.hpp +++ b/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" diff --git a/ugv_sdk/src/mobile_base.cpp b/ugv_sdk/src/mobile_base.cpp index 43ffe50..cc11e6b 100644 --- a/ugv_sdk/src/mobile_base.cpp +++ b/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 #include @@ -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(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(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(); }