using velocity to define the safe area

include/safe_cmd_alg_node.h

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

src/safe_cmd_alg_node.cpp

@@ -4,7 +4,7 @@ SafeCmdAlgNode::SafeCmdAlgNode(void) :
   algorithm_base::IriBaseAlgorithm<SafeCmdAlgorithm>(),
   front_obstacle_(false),
   rear_obstacle_(false),
-  radius_(0.5)
+  scale_(1)
 {
   //init class attributes if necessary
   //this->loop_rate_ = 2;//in [Hz]
@@ -134,7 +134,7 @@ bool SafeCmdAlgNode::is_point_in_safe_area_(const sensor_msgs::LaserScan::ConstP
   uint i=0;
   while (i < scan->ranges.size())
   {
-    if(scan->ranges[i] < radius_ && scan->ranges[i] > scan->range_min)
+    if(scan->ranges[i] < fabs(last_twist_.linear.x)*scale_ && scan->ranges[i] > scan->range_min)
       return true;
     i++;
   }