Solved a bug when computing the footprint collision: One of the variables was...

Solved a bug when computing the footprint collision: One of the variables was cast to an integer and the Infinity value has been changed by the maximum double value.
Solved a bug when the linear speed was 0.0: The trajectory was computed backwards in this case.

src/safe_cmd_alg.cpp

@@ -43,7 +43,7 @@ void SafeCmdAlgorithm::compute_trajectory_ackermann_steer_angle(double linear_x,
   {
     if(radius!=std::numeric_limits<double>::max())
     {
-      if(linear_x>0.0)
+      if(linear_x>=0.0)
         traj[i].theta=traj[i-1].theta+this->config_.traj_sim_step/radius;
       else
         traj[i].theta=traj[i-1].theta-this->config_.traj_sim_step/radius;

src/safe_cmd_alg_node.cpp

@@ -116,7 +116,7 @@ void SafeCmdAlgNode::mainNodeThread(void)
             if(collision_length>this->config.safety_distance)
               break;
           }
-        } 
+        }
         if(collision_length>=this->config.safety_distance)
         {
           if(this->config.architecture==0) 
@@ -284,7 +284,7 @@ bool SafeCmdAlgNode::on_segment(geometry_msgs::Point &p,geometry_msgs::Point &q,
 
 int SafeCmdAlgNode::orientation(geometry_msgs::Point &p,geometry_msgs::Point &q,geometry_msgs::Point &r)
 {
-  int val = (q.y - p.y) * (r.x - q.x) -
+  double val = (q.y - p.y) * (r.x - q.x) -
             (q.x - p.x) * (r.y - q.y);
 
   if (val == 0) return 0;
@@ -315,7 +315,7 @@ bool SafeCmdAlgNode::inside(std::vector<geometry_msgs::Point> &footprint,pcl::Po
   int count=0,i=0;
   geometry_msgs::Point extreme,p;
 
-  extreme.x=std::numeric_limits<double>::infinity();
+  extreme.x=std::numeric_limits<double>::max();
   extreme.y=point.y;
   p.x=point.x;
   p.y=point.y;