From dba25d430b61a7fd2deecc57057e3ad83b3c19bc Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Damian=20Bara=C5=84ski?= <damian.baranski@pwr.wroc.pl>
Date: Fri, 20 Feb 2015 13:30:43 +0100
Subject: [PATCH] =?UTF-8?q?Doda=C5=82em=20WatchDog?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 RosAria.cpp | 22 ++++++++++++++++++++++
 1 file changed, 22 insertions(+)

diff --git a/RosAria.cpp b/RosAria.cpp
index 8c9bbf7..d35cfad 100644
--- a/RosAria.cpp
+++ b/RosAria.cpp
@@ -1,3 +1,4 @@
+//#define WATCHDOG 1
 #include <stdio.h>
 #include <math.h>
 #ifdef ADEPT_PKG
@@ -52,6 +53,7 @@ class RosAriaNode
     void readParameters();
 
   protected:
+    int WATCHDOG;
     ros::NodeHandle n;
     ros::Publisher pose_pub;
     ros::Publisher bumpers_pub;
@@ -124,6 +126,8 @@ class RosAriaNode
     //Damian zmienna mówiąca czy robot jest zatrzymany awaryjnie
     bool stop_robot;
     std_msgs::Bool stop_motors_msg;
+   
+    ros::Time watchdog;
 };
 
 void RosAriaNode::readParameters()
@@ -169,6 +173,16 @@ void RosAriaNode::readParameters()
     n_.setParam( "RevCount", RevCount);
     ROS_INFO("Setting RevCount from robot controller stored configuration: %d", RevCount);
   }
+  if (n_.hasParam("WatchDog"))
+  {
+    n_.getParam( "WatchDog", WATCHDOG);
+    ROS_INFO("Setting WatchDog from ROS Parameter: %d", WATCHDOG);
+  }
+  else
+  {
+    n_.setParam( "WatchDog", WATCHDOG);
+    ROS_INFO("Setting default WatchDog : %d", WATCHDOG);
+  }
   robot->unlock();
 }
 
@@ -353,6 +367,7 @@ RosAriaNode::~RosAriaNode()
 
 int RosAriaNode::Setup()
 {
+  WATCHDOG=1;
   // Note, various objects are allocated here which are never deleted (freed), since Setup() is only supposed to be
   // called once per instance, and these objects need to persist until the process terminates.
 
@@ -666,6 +681,12 @@ void RosAriaNode::publish()
       sonar_pub.publish(cloud);
     }
   } // end if sonar_enabled
+
+  if(watchdog.toSec()+WATCHDOG<ros::Time::now().toSec())
+  {
+    robot->comInt(ArCommands::ESTOP,0);
+    // ROS_INFO("WATCHDOG %d", WATCHDOG);
+  }
 }
 
 bool RosAriaNode::enable_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
@@ -724,6 +745,7 @@ void
 RosAriaNode::cmdvel_cb( const geometry_msgs::TwistConstPtr &msg)
 {
   veltime = ros::Time::now();
+  watchdog= ros::Time::now();
   if(stop_motors_msg.data)
   {
       ROS_INFO( "new speed: [%0.2f,%0.2f](%0.3f) but robot is stop", msg->linear.x*1e3, msg->angular.z, veltime.toSec() );