Solved a bug in the heading cost function: If the trajectory had less points...

Solved a bug in the heading cost function: If the trajectory had less points than the ones reuired to compute the cost, the node crashed.

src/ackermann_trajectory_search.cpp

@@ -125,7 +125,10 @@ bool AckermannTrajectorySearch::findBestTrajectory(base_local_planner::Trajector
       }
       count++;
       if (max_samples_ > 0 && count >= max_samples_) 
+      {
+        std::cout << "Max sample limit" << std::endl;
         break;
+      }
     }
     if (best_traj_cost >= 0) 
     {

src/heading_cost_function.cpp

@@ -53,10 +53,20 @@ bool HeadingCostFunction::prepare()
 double HeadingCostFunction::scoreTrajectory(base_local_planner::Trajectory &traj) 
 {
   double dist,x,y,theta,near_dist=DBL_MAX,heading_diff=0.0,diff=0.0;
-  int interval=traj.getPointsSize()/this->num_points;
+  int interval,samples;
   unsigned int near_index=0,i,j;
 
-  for(j=0;j<this->num_points;j++)
+  if(this->num_points>traj.getPointsSize())
+  {
+    interval=1;
+    samples=traj.getPointsSize();
+  }
+  else
+  {
+    interval=traj.getPointsSize()/this->num_points;
+    samples=this->num_points;
+  }
+  for(j=0;j<samples;j++)
   {
     traj.getPoint((j+1)*interval-1,x,y,theta);
     // find the nearrest point on the path
@@ -66,8 +76,8 @@ double HeadingCostFunction::scoreTrajectory(base_local_planner::Trajectory &traj
                 (this->global_plan[i].pose.position.y-y)*(this->global_plan[i].pose.position.y-y));
       if(dist<near_dist)
       {
-	near_dist=dist;
-	near_index=i;
+        near_dist=dist;
+        near_index=i;
       }
     }
     double v1_x,v1_y;