From 44f6d3d421f19a6eff3e3968b6e5340a1937eab3 Mon Sep 17 00:00:00 2001
From: Fernando Herrero Cotarelo <fherrero@iri.upc.edu>
Date: Tue, 24 Nov 2015 09:03:18 +0000
Subject: [PATCH] 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 | 21 +++++++++++++--------
1 file changed, 13 insertions(+), 8 deletions(-)
diff --git a/src/safe_cmd_alg_node.cpp b/src/safe_cmd_alg_node.cpp
index 407e125..babd0f6 100644
--- a/src/safe_cmd_alg_node.cpp
+++ b/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
{
--
GitLab