Added unsafe param. Added minimum distance. Changed scale to collision time, in seconds

cfg/safe_cmd_alg_config.cfg

@@ -39,7 +39,7 @@ from dynamic_reconfigure.parameter_generator import *
 gen = ParameterGenerator()
 
 #       Name                       Type       Reconfiguration level            Description                       Default   Min   Max
-#gen.add("velocity_scale_factor",  double_t,  0,                               "Maximum velocity scale factor",  0.5,      0.0,  1.0)
+gen.add("unsafe", bool_t,  0, "Not use the safe module", False)
 #gen.add("velocity_scale_factor",  double_t,  0,                               "Maximum velocity scale factor",  0.5,      0.0,  1.0)
 
 exit(gen.generate(PACKAGE, "SafeCmdAlgorithm", "SafeCmd"))

docs/SafeCmdConfig-usage.dox

 \subsubsection usage Usage
 \verbatim
 <node name="SafeCmdAlgorithm" pkg="iri_safe_cmd" type="SafeCmdAlgorithm">
+  <param name="unsafe" type="bool" value="False" />
 </node>
 \endverbatim
 

docs/SafeCmdConfig.dox

@@ -2,4 +2,5 @@
 
 Reads and maintains the following parameters on the ROS server
 
+- \b "~unsafe" : \b [bool] Not use the safe module min: False, default: False, max: True
 

docs/SafeCmdConfig.wikidoc

@@ -3,6 +3,10 @@ param {
 group.0 {
 name=Dynamically Reconfigurable Parameters
 desc=See the [[dynamic_reconfigure]] package for details on dynamically reconfigurable parameters.
+0.name= ~unsafe
+0.default= False
+0.type= bool
+0.desc=Not use the safe module 
 }
 }
 # End of autogenerated section. You may edit below.

include/safe_cmd_alg_node.h

@@ -68,7 +68,8 @@ class SafeCmdAlgNode : public algorithm_base::IriBaseAlgorithm<SafeCmdAlgorithm>
     // [action client attributes]
     bool front_obstacle_;
     bool rear_obstacle_;
-    float scale_;
+    float collision_time_;
+    float min_dist_;
     bool is_point_in_safe_area_(const sensor_msgs::LaserScan::ConstPtr& scan);
 
   public:

src/safe_cmd_alg_node.cpp

@@ -4,18 +4,19 @@ SafeCmdAlgNode::SafeCmdAlgNode(void) :
   algorithm_base::IriBaseAlgorithm<SafeCmdAlgorithm>(),
   front_obstacle_(false),
   rear_obstacle_(false),
-  scale_(1)
+  collision_time_(1),
+  min_dist_(0.2)
 {
   //init class attributes if necessary
-  //this->loop_rate_ = 2;//in [Hz]
+  loop_rate_ = 20;//in [Hz]
 
   // [init publishers]
-  this->cmd_vel_safe_publisher_ = this->public_node_handle_.advertise<geometry_msgs::Twist>("cmd_vel_safe", 100);
+  cmd_vel_safe_publisher_ = public_node_handle_.advertise<geometry_msgs::Twist>("cmd_vel_safe", 100);
   
   // [init subscribers]
-  this->cmd_vel_subscriber_ = this->public_node_handle_.subscribe("cmd_vel", 100, &SafeCmdAlgNode::cmd_vel_callback, this);
-  this->rear_laser_subscriber_ = this->public_node_handle_.subscribe("rear_laser", 100, &SafeCmdAlgNode::rear_laser_callback, this);
-  this->front_laser_subscriber_ = this->public_node_handle_.subscribe("front_laser", 100, &SafeCmdAlgNode::front_laser_callback, this);
+  cmd_vel_subscriber_ = public_node_handle_.subscribe("cmd_vel", 100, &SafeCmdAlgNode::cmd_vel_callback,this);
+  rear_laser_subscriber_ = public_node_handle_.subscribe("rear_laser", 100, &SafeCmdAlgNode::rear_laser_callback, this);
+  front_laser_subscriber_ = public_node_handle_.subscribe("front_laser", 100, &SafeCmdAlgNode::front_laser_callback, this);
   
   // [init services]
   
@@ -36,17 +37,23 @@ void SafeCmdAlgNode::mainNodeThread(void)
   // [fill msg structures]
   //this->Twist_msg_.data = my_var;
   Twist_msg_ = last_twist_;
-  if(front_obstacle_ && last_twist_.linear.x > 0.0)
-  {
-    Twist_msg_.linear.x = 0.0;
-    ROS_WARN("heading to front obstacle avoided");
-  }  
 
-  if(rear_obstacle_ && last_twist_.linear.x < 0.0)
+  if(!alg_.config_.unsafe)
   {
-    Twist_msg_.linear.x = 0.0; 
-    ROS_WARN("heading to rear obstacle avoided");
-  }  
+
+    if(front_obstacle_ && last_twist_.linear.x > 0.0)
+    {
+      Twist_msg_.linear.x = 0.0;
+      ROS_WARN("heading to front obstacle avoided");
+    }  
+
+    if(rear_obstacle_ && last_twist_.linear.x < 0.0)
+    {
+      Twist_msg_.linear.x = 0.0; 
+      ROS_WARN("heading to rear obstacle avoided");
+    }
+
+  }
   // [fill srv structure and make request to the server]
   
   // [fill action structure and make request to the action server]
@@ -113,9 +120,9 @@ void SafeCmdAlgNode::front_laser_callback(const sensor_msgs::LaserScan::ConstPtr
 
 void SafeCmdAlgNode::node_config_update(Config &config, uint32_t level)
 {
-  this->alg_.lock();
+  alg_.lock();
 
-  this->alg_.unlock();
+  alg_.unlock();
 }
 
 void SafeCmdAlgNode::addNodeDiagnostics(void)
@@ -134,9 +141,9 @@ bool SafeCmdAlgNode::is_point_in_safe_area_(const sensor_msgs::LaserScan::ConstP
   uint i=0;
   while (i < scan->ranges.size())
   {
-    if(scan->ranges[i] < fabs(last_twist_.linear.x)*scale_ && scan->ranges[i] > scan->range_min)
+    if((scan->ranges[i] < fabs(last_twist_.linear.x)*collision_time_ || scan->ranges[i] < min_dist_)  && scan->ranges[i] > scan->range_min)
       return true;
     i++;
   }
   return false;
-}
\ No newline at end of file
+}