From c6d930840eed29f64abfde987da993c9b7efc585 Mon Sep 17 00:00:00 2001 From: dheera Date: Tue, 7 May 2019 18:37:23 +0000 Subject: [PATCH 1/2] add watchdog --- CMakeLists.txt | 4 ++- sdk/include/watchdog.h | 37 ++++++++++++++++++++++ src/node.cpp | 8 ++++- src/watchdog.cpp | 69 ++++++++++++++++++++++++++++++++++++++++++ 4 files changed, 116 insertions(+), 2 deletions(-) create mode 100644 sdk/include/watchdog.h create mode 100644 src/watchdog.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index a5f22489..53ec48ab 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,8 @@ cmake_minimum_required(VERSION 2.8.3) project(rplidar_ros) +add_compile_options(-std=c++11) + set(RPLIDAR_SDK_PATH "./sdk/") FILE(GLOB RPLIDAR_SDK_SRC @@ -23,7 +25,7 @@ include_directories( catkin_package() -add_executable(rplidarNode src/node.cpp ${RPLIDAR_SDK_SRC}) +add_executable(rplidarNode src/node.cpp src/watchdog.cpp ${RPLIDAR_SDK_SRC}) target_link_libraries(rplidarNode ${catkin_LIBRARIES}) add_executable(rplidarNodeClient src/client.cpp) diff --git a/sdk/include/watchdog.h b/sdk/include/watchdog.h new file mode 100644 index 00000000..dc503e9e --- /dev/null +++ b/sdk/include/watchdog.h @@ -0,0 +1,37 @@ +#ifndef _watchdog_h +#define _watchdog_h + +#include +#include +#include +#include +#include +#include +#include + +namespace rp { + +class Watchdog { + public: + Watchdog(); + Watchdog(std::function callback); + ~Watchdog(); + void start(unsigned int _interval); + void stop(); + void refresh(); + + private: + unsigned int mInterval; + std::atomic mIsRunning; + std::thread mThread; + std::function mCallback; + std::mutex mMutex; + std::chrono::steady_clock::time_point mLastRefreshTime; + std::condition_variable mStopCondition; + void loop(); +}; + +} + +#endif /* _watchdog_h */ + diff --git a/src/node.cpp b/src/node.cpp index 4b12ea6b..4a636f89 100644 --- a/src/node.cpp +++ b/src/node.cpp @@ -36,6 +36,7 @@ #include "sensor_msgs/LaserScan.h" #include "std_srvs/Empty.h" #include "rplidar.h" +#include "watchdog.h" #ifndef _countof #define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0])) @@ -182,6 +183,9 @@ static float getAngle(const rplidar_response_measurement_node_hq_t& node) int main(int argc, char * argv[]) { ros::init(argc, argv, "rplidar_node"); + rp::Watchdog watchdog; + watchdog.start(10000); + std::string channel_type; std::string tcp_ip; std::string serial_port; @@ -206,7 +210,7 @@ int main(int argc, char * argv[]) { nh_private.param("angle_compensate", angle_compensate, false); nh_private.param("scan_mode", scan_mode, std::string()); - ROS_INFO("RPLIDAR running on ROS package rplidar_ros. SDK Version:"RPLIDAR_SDK_VERSION""); + ROS_INFO_STREAM("RPLIDAR running on ROS package rplidar_ros. SDK Version: " << RPLIDAR_SDK_VERSION); u_result op_result; @@ -320,6 +324,8 @@ int main(int argc, char * argv[]) { float angle_min = DEG2RAD(0.0f); float angle_max = DEG2RAD(359.0f); if (op_result == RESULT_OK) { + watchdog.refresh(); + if (angle_compensate) { //const int angle_compensate_multiple = 1; const int angle_compensate_nodes_count = 360*angle_compensate_multiple; diff --git a/src/watchdog.cpp b/src/watchdog.cpp new file mode 100644 index 00000000..81025adb --- /dev/null +++ b/src/watchdog.cpp @@ -0,0 +1,69 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rp { + +Watchdog::Watchdog() { + mCallback = [] { + std::cerr << "watchdog triggered\n"; + kill(getpid(), SIGTERM); + }; + mIsRunning = false; +} + +Watchdog::Watchdog(std::function callback) { + mCallback = callback; + mIsRunning = false; +} + +Watchdog::~Watchdog() { + stop(); +} + +void Watchdog::start(unsigned int interval) { + std::unique_lock lock(mMutex); + if(mIsRunning) return; + + mLastRefreshTime = std::chrono::steady_clock::now(); + mInterval = interval; + mIsRunning = true; + mThread = std::thread(&Watchdog::loop, this); +} + +void Watchdog::stop() { + std::unique_lock lock(mMutex); + if(!mIsRunning) return; + + mIsRunning = false; + mStopCondition.notify_all(); + lock.unlock(); + mThread.join(); +} + +void Watchdog::refresh() { + std::unique_lock lock(mMutex); + mLastRefreshTime = std::chrono::steady_clock::now(); + mStopCondition.notify_all(); +} + +void Watchdog::loop() { + std::unique_lock lock(mMutex); + while(mIsRunning) { + if(mStopCondition.wait_for(lock, std::chrono::milliseconds(mInterval)) == std::cv_status::timeout) { + if(mCallback != nullptr) { + mIsRunning = false; + mCallback(); + } + } + } +} + +} From 5955bab6e0753b8077bbfa437d3ed9090a85f0e1 Mon Sep 17 00:00:00 2001 From: dheera Date: Tue, 7 May 2019 19:13:01 +0000 Subject: [PATCH 2/2] respawn --- launch/rplidar.launch | 2 +- launch/rplidar_a3.launch | 2 +- launch/rplidar_s1.launch | 2 +- launch/rplidar_s1_tcp.launch | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/launch/rplidar.launch b/launch/rplidar.launch index aa355347..b85b6ccf 100644 --- a/launch/rplidar.launch +++ b/launch/rplidar.launch @@ -1,5 +1,5 @@ - + diff --git a/launch/rplidar_a3.launch b/launch/rplidar_a3.launch index f8ac8450..9abb4623 100644 --- a/launch/rplidar_a3.launch +++ b/launch/rplidar_a3.launch @@ -1,5 +1,5 @@ - + diff --git a/launch/rplidar_s1.launch b/launch/rplidar_s1.launch index 26668545..82c71a66 100644 --- a/launch/rplidar_s1.launch +++ b/launch/rplidar_s1.launch @@ -1,5 +1,5 @@ - + diff --git a/launch/rplidar_s1_tcp.launch b/launch/rplidar_s1_tcp.launch index a1e6b4bd..0afe6a97 100644 --- a/launch/rplidar_s1_tcp.launch +++ b/launch/rplidar_s1_tcp.launch @@ -1,5 +1,5 @@ - +