iri_safe_cmd:

 - start watchdog on constructor, so ERROR msgs show if lasers don't work from the start
 - using ROS_ERROR_DELAYED_THROTTLED, with period of 5 seconds, which waits first 5 seconds before showing first error message.

src/safe_cmd_alg_node.cpp

@@ -30,6 +30,8 @@ SafeCmdAlgNode::SafeCmdAlgNode(void) :
   // [init action servers]
   
   // [init action clients]
+  this->reset_front_laser_watchdog();
+  this->reset_rear_laser_watchdog();
 }
 
 SafeCmdAlgNode::~SafeCmdAlgNode(void)
@@ -45,15 +47,18 @@ void SafeCmdAlgNode::mainNodeThread(void)
   {
     this->update_front_laser_watchdog();
     this->update_rear_laser_watchdog();
-    if(this->front_laser_watchdog_active())
-    {
-      ROS_ERROR_THROTTLE(1,"SafeCmdAlgNode::mainNodeThread: Front laser watchdog timeout!");
-      Twist_msg_.linear.x = 0.0;
-    }
-    else if(this->rear_laser_watchdog_active())
+    if(this->front_laser_watchdog_active() || this->rear_laser_watchdog_active())
     {
-      ROS_ERROR_THROTTLE(1,"SafeCmdAlgNode::mainNodeThread: Rear laser watchdog timeout!");
-      Twist_msg_.linear.x = 0.0;
+      if(this->front_laser_watchdog_active())
+      {
+        ROS_ERROR_DELAYED_THROTTLE(5,"SafeCmdAlgNode::mainNodeThread: Front laser watchdog timeout!");
+        Twist_msg_.linear.x = 0.0;
+      }
+      if(this->rear_laser_watchdog_active())
+      {
+        ROS_ERROR_DELAYED_THROTTLE(5,"SafeCmdAlgNode::mainNodeThread: Rear laser watchdog timeout!");
+        Twist_msg_.linear.x = 0.0;
+      }
     }
     else
     {