diff --git a/src/ackermann_trajectory_search.cpp b/src/ackermann_trajectory_search.cpp
index 498989342ad31aad3ffb71a348b7025e8f6fc431..ceb10bfcc010babf594736516ebe0d08d0a6bed0 100644
--- a/src/ackermann_trajectory_search.cpp
+++ b/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) 
     {
diff --git a/src/heading_cost_function.cpp b/src/heading_cost_function.cpp
index ad19be0c03c8f0e24d81ae61dfecd5ae5809b2a1..d7b1846cb51f9670db0e85d2ecdc7092dbb25b01 100644
--- a/src/heading_cost_function.cpp
+++ b/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;