add watchdog

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)

launch/rplidar.launch

@@ -1,5 +1,5 @@
 <launch>
-  <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
+  <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen" respawn="true" respawn_delay="2">
   <param name="serial_port"         type="string" value="/dev/ttyUSB0"/>
   <param name="serial_baudrate"     type="int"    value="115200"/><!--A1/A2 -->
   <!--param name="serial_baudrate"     type="int"    value="256000"--><!--A3 -->

launch/rplidar_a3.launch

@@ -1,5 +1,5 @@
 <launch>
-  <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
+  <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen" respawn="true" respawn_delay="2">
   <param name="serial_port"         type="string" value="/dev/ttyUSB0"/>  
   <param name="serial_baudrate"     type="int"    value="256000"/><!--A3 -->
   <param name="frame_id"            type="string" value="laser"/>

launch/rplidar_s1.launch

@@ -1,5 +1,5 @@
 <launch>
-  <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
+  <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen" respawn="true" respawn_delay="2">
   <param name="serial_port"         type="string" value="/dev/ttyUSB0"/>  
   <param name="serial_baudrate"     type="int"    value="256000"/>
   <param name="frame_id"            type="string" value="laser"/>

launch/rplidar_s1_tcp.launch

@@ -1,5 +1,5 @@
 <launch>
-  <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
+  <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen" respawn="true" respawn_delay="2">
   <param name="channel_type"        type="string" value="tcp"/>  
   <param name="tcp_ip"              type="string" value="192.168.0.7"/>  
   <param name="tcp_port"            type="int"    value="20108"/>

sdk/include/watchdog.h

@@ -0,0 +1,37 @@
+#ifndef _watchdog_h
+#define _watchdog_h
+
+#include <thread>
+#include <atomic>
+#include <chrono>
+#include <condition_variable>
+#include <mutex>
+#include <iostream>
+#include <functional>
+
+namespace rp {
+
+class Watchdog {
+  public:
+    Watchdog();
+    Watchdog(std::function<void()> callback);
+    ~Watchdog();
+    void start(unsigned int _interval);
+    void stop();
+    void refresh();
+
+  private:
+    unsigned int mInterval;
+    std::atomic<bool> mIsRunning;
+    std::thread mThread;
+    std::function<void()> mCallback;
+    std::mutex mMutex;
+    std::chrono::steady_clock::time_point mLastRefreshTime;
+    std::condition_variable mStopCondition;
+    void loop();
+};
+
+}
+
+#endif /* _watchdog_h */
+

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<bool>("angle_compensate", angle_compensate, false);
     nh_private.param<std::string>("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;

src/watchdog.cpp

@@ -0,0 +1,69 @@
+#include <thread>
+#include <atomic>
+#include <chrono>
+#include <condition_variable>
+#include <mutex>
+#include <iostream>
+#include <unistd.h>
+#include <csignal>
+#include <sys/types.h>
+#include <watchdog.h>
+
+namespace rp {
+
+Watchdog::Watchdog() {
+  mCallback = [] {
+      std::cerr << "watchdog triggered\n";
+      kill(getpid(), SIGTERM);
+  };
+  mIsRunning = false;
+}
+
+Watchdog::Watchdog(std::function<void()> callback) {
+  mCallback = callback;
+  mIsRunning = false;
+}
+
+Watchdog::~Watchdog() {
+  stop();
+}
+
+void Watchdog::start(unsigned int interval) {
+    std::unique_lock<std::mutex> 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<std::mutex> lock(mMutex);
+    if(!mIsRunning) return;
+
+    mIsRunning = false;
+    mStopCondition.notify_all();
+    lock.unlock();
+    mThread.join();
+}
+
+void Watchdog::refresh() {
+    std::unique_lock<std::mutex> lock(mMutex);
+    mLastRefreshTime = std::chrono::steady_clock::now();
+    mStopCondition.notify_all();
+}
+
+void Watchdog::loop() {
+    std::unique_lock<std::mutex> lock(mMutex);
+    while(mIsRunning) {
+      if(mStopCondition.wait_for(lock, std::chrono::milliseconds(mInterval)) == std::cv_status::timeout) {
+        if(mCallback != nullptr) {
+          mIsRunning = false;
+          mCallback();
+        }
+      }
+    }
+}  
+
+}