diff --git a/cfg/SafeCmd.cfg b/cfg/SafeCmd.cfg
index 7bc2faedea841f6d4d6c426b970434fcca653f97..2e543d2d84140c1dc8cfc805706c60f313c17c63 100755
--- a/cfg/SafeCmd.cfg
+++ b/cfg/SafeCmd.cfg
@@ -37,12 +37,13 @@ from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
-# Name Type Reconfiguration level Description Default Min Max
-gen.add("unsafe", bool_t, 0, "Not use the safe module", False)
-gen.add("min_dist", double_t, 0, "distance in meters where to stop", 0.4, 0.0, 2.0)
+# Name Type Reconfiguration level Description Default Min Max
+gen.add("unsafe", bool_t, 0, "Not use the safe module", False)
+gen.add("min_dist",double_t, 0, "distance in meters where to stop",0.4, 0.0, 2.0)
gen.add("collision_time", double_t, 0, "time in seconds to compute collision", 1.0, 0.1, 5.0)
gen.add("limit_vel_front", double_t, 0, "max speed in meters per second forward", 1.0, 0.0, 2.0)
gen.add("limit_vel_rear", double_t, 0, "max speed in meters per second backward", 1.0, 0.0, 2.0)
-#gen.add("velocity_scale_factor", double_t, 0, "Maximum velocity scale factor", 0.5, 0.0, 1.0)
+gen.add("front_laser_watchdog_time", double_t, 0, "Maximum time (seconds) between front_laser msgs",1.0, 0.1, 2.0)
+gen.add("rear_laser_watchdog_time", double_t, 0, "Maximum time (seconds) between rear_laser msgs",1.0, 0.1, 2.0)
exit(gen.generate(PACKAGE, "SafeCmdAlgorithm", "SafeCmd"))
diff --git a/include/safe_cmd_alg_node.h b/include/safe_cmd_alg_node.h
index 0043cc417959ae8aefb6cb1b6f36b4ae5ac4e7e9..5c88505a3b624cac663cef9695df3cbecd4338a1 100644
--- a/include/safe_cmd_alg_node.h
+++ b/include/safe_cmd_alg_node.h
@@ -79,6 +79,56 @@ class SafeCmdAlgNode : public algorithm_base::IriBaseAlgorithm<SafeCmdAlgorithm>
float limit_vel_rear_;
float compute_max_velocity_(const sensor_msgs::LaserScan::ConstPtr& scan) const;
+ /**
+ * \brief config variable
+ *
+ * This variable has all the parameters defined in the cfg config file.
+ * Is updated everytime function node_config_update() is called.
+ */
+ Config config;
+
+ /**
+ * \brief Resets front_laser_watchdog time
+ */
+ void reset_front_laser_watchdog(void);
+ /**
+ * \brief Returns true if front_laser_watchdog timeouts
+ */
+ bool front_laser_watchdog_active(void);
+ /**
+ * \brief Updates front_laser_watchdog time
+ */
+ void update_front_laser_watchdog(void);
+ /**
+ * \brief Watchdog timeout duration
+ */
+ ros::Duration front_laser_watchdog_duration;
+ /**
+ * \brief Watchdog access mutex
+ */
+ CMutex front_laser_watchdog_access;
+
+ /**
+ * \brief Resets rear_laser_watchdog time
+ */
+ void reset_rear_laser_watchdog(void);
+ /**
+ * \brief Returns true if rear_laser_watchdog timeouts
+ */
+ bool rear_laser_watchdog_active(void);
+ /**
+ * \brief Updates rear_laser_watchdog time
+ */
+ void update_rear_laser_watchdog(void);
+ /**
+ * \brief Watchdog timeout duration
+ */
+ ros::Duration rear_laser_watchdog_duration;
+ /**
+ * \brief Watchdog access mutex
+ */
+ CMutex rear_laser_watchdog_access;
+
public:
/**
* \brief Constructor
diff --git a/src/safe_cmd_alg_node.cpp b/src/safe_cmd_alg_node.cpp
index 0557c2ec70d9921e737b0882099b3e02ae1ab1fe..fbf704fe97b1cbb8b4aee35d3fa5b69a53078f33 100644
--- a/src/safe_cmd_alg_node.cpp
+++ b/src/safe_cmd_alg_node.cpp
@@ -4,10 +4,10 @@ SafeCmdAlgNode::SafeCmdAlgNode(void) :
algorithm_base::IriBaseAlgorithm<SafeCmdAlgorithm>(),
collision_time_(1),
min_dist_(0.4),
- max_vel_front_(7),
- max_vel_rear_(7),
- limit_vel_front_(7),
- limit_vel_rear_(7),
+ max_vel_front_(1.0),
+ max_vel_rear_(1.0),
+ limit_vel_front_(1.0),
+ limit_vel_rear_(1.0),
front_laser_received_(false),
rear_laser_received_(false)
{
@@ -38,33 +38,49 @@ SafeCmdAlgNode::~SafeCmdAlgNode(void)
void SafeCmdAlgNode::mainNodeThread(void)
{
+ this->alg_.lock();
// [fill msg structures]
//Twist_msg_.data = my_var;
if(!alg_.config_.unsafe)
{
- if(!front_laser_received_)
- ROS_FATAL_THROTTLE(1,"SafeCmdAlgNode::mainNodeThread: Front laser not received");
- if(!rear_laser_received_)
- ROS_FATAL_THROTTLE(1,"SafeCmdAlgNode::mainNodeThread: Rear laser not received");
-
- front_laser_received_ = false;
- rear_laser_received_ = false;
-
- if(Twist_msg_.linear.x > fabs(max_vel_front_))
+ this->update_front_laser_watchdog();
+ this->update_rear_laser_watchdog();
+ if(this->front_laser_watchdog_active())
{
- ROS_WARN_STREAM("heading to Front obstacle, reducing velocity"<<fabs(max_vel_front_));
- Twist_msg_.linear.x = fabs(max_vel_front_);
- if(max_vel_front_==0)
- Twist_msg_.angular.z = 0;
+ ROS_FATAL("SafeCmdAlgNode::mainNodeThread: Front laser watchdog timeout!");
+ Twist_msg_.linear.x = 0.0;
}
-
- if(Twist_msg_.linear.x < -fabs(max_vel_rear_))
+ else if(this->rear_laser_watchdog_active())
{
- ROS_WARN_STREAM("heading to Rear obstacle, reducing velocity"<<fabs(max_vel_rear_));
- Twist_msg_.linear.x = -fabs(max_vel_rear_);
- if(max_vel_rear_==0)
- Twist_msg_.angular.z = 0;
+ ROS_FATAL("SafeCmdAlgNode::mainNodeThread: Rear laser watchdog timeout!");
+ Twist_msg_.linear.x = 0.0;
+ }
+ else
+ {
+ // if(!front_laser_received_)
+ // ROS_FATAL_THROTTLE(1,"SafeCmdAlgNode::mainNodeThread: Front laser not received");
+ // if(!rear_laser_received_)
+ // ROS_FATAL_THROTTLE(1,"SafeCmdAlgNode::mainNodeThread: Rear laser not received");
+ //
+ // front_laser_received_ = false;
+ // rear_laser_received_ = false;
+
+ if(Twist_msg_.linear.x > fabs(max_vel_front_))
+ {
+ ROS_WARN_STREAM("heading to Front obstacle, reducing velocity"<<fabs(max_vel_front_));
+ Twist_msg_.linear.x = fabs(max_vel_front_);
+ if(max_vel_front_==0)
+ Twist_msg_.angular.z = 0;
+ }
+
+ if(Twist_msg_.linear.x < -fabs(max_vel_rear_))
+ {
+ ROS_WARN_STREAM("heading to Rear obstacle, reducing velocity"<<fabs(max_vel_rear_));
+ Twist_msg_.linear.x = -fabs(max_vel_rear_);
+ if(max_vel_rear_==0)
+ Twist_msg_.angular.z = 0;
+ }
}
}
// [fill srv structure and make request to the server]
@@ -74,6 +90,8 @@ void SafeCmdAlgNode::mainNodeThread(void)
// [publish messages]
cmd_vel_safe_publisher_.publish(Twist_msg_);
+ this->alg_.unlock();
+
}
/* [subscriber callbacks] */
@@ -82,7 +100,7 @@ void SafeCmdAlgNode::cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg)
//ROS_INFO("SafeCmdAlgNode::cmd_vel_callback: New Message Received");
//use appropiate mutex to shared variables if necessary
- //alg_.lock();
+ this->alg_.lock();
//cmd_vel_mutex_.enter();
if(!alg_.config_.unsafe)
@@ -103,7 +121,7 @@ void SafeCmdAlgNode::cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg)
Twist_msg_ = *msg;
//unlock previously blocked shared variables
- //alg_.unlock();
+ this->alg_.unlock();
//cmd_vel_mutex_.exit();
}
void SafeCmdAlgNode::rear_laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg)
@@ -111,15 +129,17 @@ void SafeCmdAlgNode::rear_laser_callback(const sensor_msgs::LaserScan::ConstPtr&
//ROS_INFO("SafeCmdAlgNode::rear_laser_callback: New Message Received");
//use appropiate mutex to shared variables if necessary
- //alg_.lock();
+ this->alg_.lock();
//rear_laser_mutex_.enter();
+
+ this->reset_rear_laser_watchdog();
max_vel_rear_ = std::min(compute_max_velocity_(msg),limit_vel_rear_);
rear_laser_received_ = true;
//ROS_INFO("Max vel r: %f",max_vel_rear_);
//unlock previously blocked shared variables
- //alg_.unlock();
+ this->alg_.unlock();
//rear_laser_mutex_.exit();
}
void SafeCmdAlgNode::front_laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg)
@@ -127,15 +147,17 @@ void SafeCmdAlgNode::front_laser_callback(const sensor_msgs::LaserScan::ConstPtr
//ROS_INFO("SafeCmdAlgNode::front_laser_callback: New Message Received");
//use appropiate mutex to shared variables if necessary
- //alg_.lock();
- //front_laser_mutex_.enter();
+ this->alg_.lock();
+ //front_laser_mutex_.enter();
+
+ this->reset_front_laser_watchdog();
max_vel_front_ = std::min(compute_max_velocity_(msg),limit_vel_front_);
front_laser_received_ = true;
//ROS_INFO("Max vel f: %f",max_vel_front_);
//unlock previously blocked shared variables
- //alg_.unlock();
+ this->alg_.unlock();
//front_laser_mutex_.exit();
}
@@ -147,12 +169,13 @@ void SafeCmdAlgNode::front_laser_callback(const sensor_msgs::LaserScan::ConstPtr
void SafeCmdAlgNode::node_config_update(Config &config, uint32_t level)
{
- alg_.lock();
- collision_time_ = config.collision_time;
- min_dist_ = config.min_dist;
- limit_vel_front_ = config.limit_vel_front;
- limit_vel_rear_ = config.limit_vel_rear;
- alg_.unlock();
+ this->alg_.lock();
+ collision_time_ = config.collision_time;
+ min_dist_ = config.min_dist;
+ limit_vel_front_ = config.limit_vel_front;
+ limit_vel_rear_ = config.limit_vel_rear;
+ this->config=config;
+ this->alg_.unlock();
}
void SafeCmdAlgNode::addNodeDiagnostics(void)
@@ -188,4 +211,69 @@ float SafeCmdAlgNode::compute_max_velocity_(const sensor_msgs::LaserScan::ConstP
return max_velocity;
}
+void SafeCmdAlgNode::reset_front_laser_watchdog(void)
+{
+ this->front_laser_watchdog_access.enter();
+ this->front_laser_watchdog_duration=ros::Duration(this->config.front_laser_watchdog_time);
+ this->front_laser_watchdog_access.exit();
+}
+
+bool SafeCmdAlgNode::front_laser_watchdog_active(void)
+{
+ this->front_laser_watchdog_access.enter();
+ if(this->front_laser_watchdog_duration.toSec()<=0.0)
+ {
+ this->front_laser_watchdog_access.exit();
+ return true;
+ }
+ else
+ {
+ this->front_laser_watchdog_access.exit();
+ return false;
+ }
+}
+
+void SafeCmdAlgNode::update_front_laser_watchdog(void)
+{
+ static ros::Time start_time=ros::Time::now();
+ ros::Time current_time=ros::Time::now();
+
+ this->front_laser_watchdog_access.enter();
+ this->front_laser_watchdog_duration-=(current_time-start_time);
+ start_time=current_time;
+ this->front_laser_watchdog_access.exit();
+}
+
+void SafeCmdAlgNode::reset_rear_laser_watchdog(void)
+{
+ this->rear_laser_watchdog_access.enter();
+ this->rear_laser_watchdog_duration=ros::Duration(this->config.rear_laser_watchdog_time);
+ this->rear_laser_watchdog_access.exit();
+}
+
+bool SafeCmdAlgNode::rear_laser_watchdog_active(void)
+{
+ this->rear_laser_watchdog_access.enter();
+ if(this->rear_laser_watchdog_duration.toSec()<=0.0)
+ {
+ this->rear_laser_watchdog_access.exit();
+ return true;
+ }
+ else
+ {
+ this->rear_laser_watchdog_access.exit();
+ return false;
+ }
+}
+
+void SafeCmdAlgNode::update_rear_laser_watchdog(void)
+{
+ static ros::Time start_time=ros::Time::now();
+ ros::Time current_time=ros::Time::now();
+
+ this->rear_laser_watchdog_access.enter();
+ this->rear_laser_watchdog_duration-=(current_time-start_time);
+ start_time=current_time;
+ this->rear_laser_watchdog_access.exit();
+}