Solved a bug: The current position of the robot was not taken into account...

Solved a bug: The current position of the robot was not taken into account when generatng the trajectory candidates.

cfg/AckermannLocalPlanner.cfg

@@ -104,6 +104,7 @@ splines.add("n2_samples",                int_t,     0, "Number of samples for th
 splines.add("n3_samples",                int_t,     0, "Number of samples for the N3 parameter of the spline",                                           1,1,100)
 splines.add("n4_samples",                int_t,     0, "Number of samples for the N4 parameter of the spline",                                           1,1,100)
 splines.add("dist_samples",              int_t,     0, "Number of distance samples for the current segment",                                             1,1,10)
+splines.add("min_segment_points",        int_t,     0, "Minimum number of points of each segment",                                                       10,1,100)
 splines.add("lateral_offset_gain",       double_t,  0, "Gain for the lateral offset steering correction",                                                0.1,0.001,10)
 splines.add("temp_horizon",              double_t,  0, "Future time to compute the control command",                                                     0.2,0.01,10)
 

include/ackermann_spline_generator.h

@@ -93,7 +93,7 @@ class AckermannSplineGenerator: public base_local_planner::TrajectorySampleGener
     bool forward;
     double sim_period_;
 
-    void get_path_closest_pose(double &path_x,double &path_y);
+    unsigned int get_path_closest_pose(double &path_x,double &path_y);
 };
 
 #endif /* SIMPLE_TRAJECTORY_GENERATOR_H_ */

src/ackermann_spline_generator.cpp

@@ -51,7 +51,7 @@ AckermannSplineGenerator::AckermannSplineGenerator()
 {
 }
 
-void AckermannSplineGenerator::get_path_closest_pose(double &path_x,double &path_y)
+unsigned int AckermannSplineGenerator::get_path_closest_pose(double &path_x,double &path_y)
 {
   double dist,min_dist=std::numeric_limits<double>::max();
   unsigned int min_index=0;
@@ -68,6 +68,8 @@ void AckermannSplineGenerator::get_path_closest_pose(double &path_x,double &path
   }
   path_x=this->current_plan[min_index].pose.position.x;
   path_y=this->current_plan[min_index].pose.position.y;
+  
+  return min_index;
 }
 
 void AckermannSplineGenerator::initialise(
@@ -77,11 +79,13 @@ void AckermannSplineGenerator::initialise(
   bool forward,
   std::vector<geometry_msgs::PoseStamped>& current_plan)
 {
-  double diff_x,diff_y; 
+  double diff_x,diff_y,current_x,current_y,step; 
   double min_n1,max_n1,min_n2,max_n2,min_n3,max_n3,min_n4,max_n4;
-  unsigned int step=current_plan.size()/current_config_.dist_samples;
+  unsigned int int_step,current_index,num_samples;
   TSplineSample new_sample;
   
+  this->forward=forward;
+  this->current_plan=current_plan;
   pos_=pos;
   ackermann_=ackermann;
   goal_pos_=goal;
@@ -93,9 +97,25 @@ void AckermannSplineGenerator::initialise(
   min_n4=0.0;
   max_n4=0.0;
   samples_.clear();
-  for(unsigned int i=0;i<current_config_.dist_samples;i++)
+  current_index=this->get_path_closest_pose(current_x,current_y);
+  step=(current_plan.size()-current_index)/current_config_.dist_samples;
+  if(step<current_config_.min_segment_points)
   {
-    Eigen::Vector3f tmp_goal=Eigen::Vector3f(current_plan[step*(i+1)-1].pose.position.x,current_plan[step*(i+1)-1].pose.position.y,tf::getYaw(current_plan[step*(i+1)-1].pose.orientation));
+    num_samples=(current_plan.size()-current_index)/current_config_.min_segment_points;
+    if(num_samples==0)
+    {
+      num_samples=1;
+      step=(current_plan.size()-current_index);
+    }
+    else
+      step=(current_plan.size()-current_index)/num_samples;
+  }
+  else
+    num_samples=current_config_.dist_samples;
+  for(unsigned int i=0;i<num_samples;i++)
+  {
+    int_step=current_index+(unsigned int)(step*(i+1));
+    Eigen::Vector3f tmp_goal=Eigen::Vector3f(current_plan[int_step-1].pose.position.x,current_plan[int_step-1].pose.position.y,tf::getYaw(current_plan[int_step-1].pose.orientation));
     diff_x=sqrt(pow(tmp_goal(0)-pos(0),2));
     diff_y=sqrt(pow(tmp_goal(1)-pos(1),2));
     max_n1=2.0*diff_x;
@@ -106,14 +126,12 @@ void AckermannSplineGenerator::initialise(
           for(double current_n4=min_n4;current_n4<=max_n4;current_n4+=(max_n4-min_n4)/(current_config_.n4_samples-1))
 	  {
             new_sample.params=Eigen::Vector4f(current_n1,current_n2,current_n3,current_n4);
-            new_sample.end_point.x=current_plan[step*(i+1)-1].pose.position.x;
-	    new_sample.end_point.y=current_plan[step*(i+1)-1].pose.position.y;
-	    new_sample.end_point.heading=tf::getYaw(current_plan[step*(i+1)-1].pose.orientation);
+            new_sample.end_point.x=current_plan[int_step-1].pose.position.x;
+            new_sample.end_point.y=current_plan[int_step-1].pose.position.y;
+	    new_sample.end_point.heading=tf::getYaw(current_plan[int_step-1].pose.orientation);
             samples_.push_back(new_sample);
 	  }
   }
-  this->forward=forward;
-  this->current_plan=current_plan;
 }
 
 void AckermannSplineGenerator::reconfigure(iri_ackermann_local_planner::AckermannLocalPlannerConfig &cfg)
@@ -150,7 +168,7 @@ bool AckermannSplineGenerator::nextTrajectory(base_local_planner::Trajectory &co
   else
     start.curvature=tan(ackermann_(1))/current_config_.axis_distance;
   spline_.set_start_point(start);
-//  std::cout << "start position: " << pos_(0) << "," << pos_(1) << "," << pos_(2) << "," << start.curvature << std::endl;
+  //std::cout << "start position: " << pos_(0) << "," << pos_(1) << "," << pos_(2) << "," << start.curvature << std::endl;
   end.x=samples_[next_sample_index_].end_point.x;
   end.y=samples_[next_sample_index_].end_point.y;
   if(this->forward)