From d4cb58c8fc377281df7239785f239974a1188aef Mon Sep 17 00:00:00 2001
From: LotfiZ <zeghmilot@gmail.com>
Date: Mon, 19 Sep 2022 13:29:50 -0400
Subject: [PATCH] add icmp routine in a separate thread

---
 .../SickSafetyscannersRos.h                   |  2 ++
 src/SickSafetyscannersRos.cpp                 | 29 +++++++++++++++++++
 2 files changed, 31 insertions(+)

diff --git a/include/sick_safetyscanners/SickSafetyscannersRos.h b/include/sick_safetyscanners/SickSafetyscannersRos.h
index 7a999bd..586453b 100644
--- a/include/sick_safetyscanners/SickSafetyscannersRos.h
+++ b/include/sick_safetyscanners/SickSafetyscannersRos.h
@@ -220,6 +220,8 @@ class SickSafetyscannersRos
 
   bool getFieldData(sick_safetyscanners::FieldData::Request& req,
                     sick_safetyscanners::FieldData::Response& res);
+  std::function<void(void)> watchdog();
+  void run(const int duration);
 };
 
 } // namespace sick
diff --git a/src/SickSafetyscannersRos.cpp b/src/SickSafetyscannersRos.cpp
index cb8ea23..0efd032 100644
--- a/src/SickSafetyscannersRos.cpp
+++ b/src/SickSafetyscannersRos.cpp
@@ -56,6 +56,7 @@ SickSafetyscannersRos::SickSafetyscannersRos()
     ROS_ERROR("Could not read parameters.");
     ros::requestShutdown();
   }
+  run(1000);
   // tcp port can not be changed in the sensor configuration, therefore it is hardcoded
   m_communication_settings.setSensorTcpPort(2122);
   m_laser_scan_publisher = m_nh.advertise<sensor_msgs::LaserScan>("scan", 100);
@@ -828,6 +829,34 @@ bool SickSafetyscannersRos::getFieldData(sick_safetyscanners::FieldData::Request
 
   return true;
 }
+std::function<void(void)> SickSafetyscannersRos::watchdog()
+{
+        
+        int sick_1 = system("ping -c1 -s1 192.168.1.11  > /dev/null 2>&1");
+
+        if (sick_1 == 0)
+        {
+            ROS_INFO("ICMP Connection Alive !");
+        }
+        else
+        {   
+            ROS_ERROR("ICMP Connection Failed, Shutdown the node...");
+            ros::requestShutdown();
+        }
+    
+}
+
+void SickSafetyscannersRos::run(const int duration)
+{
+    std::thread([this, duration]() {
+        while (true)
+        {
+            watchdog();
+            auto ms = std::chrono::steady_clock::now() + std::chrono::milliseconds(duration);
+            std::this_thread::sleep_until(ms);
+        }
+    }).detach();
+}
 
 
 } // namespace sick