Use std::min. Changed params

include/safe_cmd_alg_node.h

@@ -32,6 +32,8 @@
 #include <geometry_msgs/Twist.h>
 #include <sensor_msgs/LaserScan.h>
 
+#include <algorithm>
+
 // [service client headers]
 
 // [action server client headers]
@@ -70,6 +72,8 @@ class SafeCmdAlgNode : public algorithm_base::IriBaseAlgorithm<SafeCmdAlgorithm>
     float min_dist_;
     float max_vel_front_;
     float max_vel_rear_;
+    float limit_vel_front_;
+    float limit_vel_rear_;
     float compute_max_velocity_(const sensor_msgs::LaserScan::ConstPtr& scan) const;
 
   public:

src/safe_cmd_alg_node.cpp

@@ -3,9 +3,11 @@
 SafeCmdAlgNode::SafeCmdAlgNode(void) :
   algorithm_base::IriBaseAlgorithm<SafeCmdAlgorithm>(),
   collision_time_(1),
-  min_dist_(0.3),
-  max_vel_front_(5),
-  max_vel_rear_(-5)
+  min_dist_(0.4),
+  max_vel_front_(7),
+  max_vel_rear_(7),
+  limit_vel_front_(7),
+  limit_vel_rear_(7)
 {
   //init class attributes if necessary
   loop_rate_ = 20;//in [Hz]
@@ -39,15 +41,15 @@ void SafeCmdAlgNode::mainNodeThread(void)
   
   if(!alg_.config_.unsafe)
   {
-    if(Twist_msg_.linear.x > max_vel_front_)
+    if(Twist_msg_.linear.x > fabs(max_vel_front_))
     {
-      Twist_msg_.linear.x = max_vel_front_;
+      Twist_msg_.linear.x = fabs(max_vel_front_);
       ROS_WARN("heading to front obstacle, reducing velocity");
     }  
 
-    if(Twist_msg_.linear.x < max_vel_rear_)
+    if(Twist_msg_.linear.x < -fabs(max_vel_rear_))
     {
-      Twist_msg_.linear.x = max_vel_rear_; 
+      Twist_msg_.linear.x = -fabs(max_vel_rear_); 
       ROS_WARN("heading to rear obstacle, reducing velocity");
     }
   }
@@ -98,7 +100,7 @@ void SafeCmdAlgNode::rear_laser_callback(const sensor_msgs::LaserScan::ConstPtr&
   //this->alg_.lock(); 
   //this->rear_laser_mutex_.enter(); 
 
-  max_vel_rear_ = - compute_max_velocity_(msg);
+  max_vel_rear_ = std::min(compute_max_velocity_(msg),limit_vel_rear_);
   //ROS_INFO("Max vel r: %f",max_vel_rear_);
 
   //unlock previously blocked shared variables 
@@ -113,7 +115,7 @@ void SafeCmdAlgNode::front_laser_callback(const sensor_msgs::LaserScan::ConstPtr
   //this->alg_.lock(); 
   //this->front_laser_mutex_.enter(); 
 
-  max_vel_front_ = compute_max_velocity_(msg);
+  max_vel_front_ = std::min(compute_max_velocity_(msg),limit_vel_front_);
   //ROS_INFO("Max vel f: %f",max_vel_front_);
 
   //unlock previously blocked shared variables 
@@ -146,16 +148,14 @@ int main(int argc,char *argv[])
 
 float SafeCmdAlgNode::compute_max_velocity_(const sensor_msgs::LaserScan::ConstPtr& scan) const
 {
-  float max_velocity = 5;
+  float max_velocity;
 
-  for (uint i = 0; i < scan->ranges.size(); i++)
-  {
-    if (scan->ranges[i] < min_dist_)
-      max_velocity = 0;
-    if (scan->ranges[i] / collision_time_ < max_velocity)
-      max_velocity = scan->ranges[i] / collision_time_;
-  }
+  float min_range = *std::min_element( scan->ranges.begin(), scan->ranges.end() );
 
+  if (min_range < min_dist_)
+    max_velocity = 0;
+  else
+    max_velocity = min_range / collision_time_;
 
   return max_velocity;
 }