Used a partial implementation of the stanley controller to control the steering

src/ackermann_spline_generator.cpp

@@ -129,7 +129,7 @@ bool AckermannSplineGenerator::nextTrajectory(base_local_planner::Trajectory &co
   resolution=current_config_.sim_granularity;
   spline_.generate(resolution,samples_[next_sample_index_](0),samples_[next_sample_index_](1),samples_[next_sample_index_](2),samples_[next_sample_index_](3));
   spline_.evaluate_all(x,y,curvature,heading);
-  std::cout << "generate: " << samples_[next_sample_index_](0) << "," << samples_[next_sample_index_](1) << "," << samples_[next_sample_index_](2) << "," << samples_[next_sample_index_](3) << std::endl;
+//  std::cout << "generate: " << samples_[next_sample_index_](0) << "," << samples_[next_sample_index_](1) << "," << samples_[next_sample_index_](2) << "," << samples_[next_sample_index_](3) << std::endl;
   // discard trajectories with two much curvature
   k_max=-std::numeric_limits<double>::max();
   for(unsigned int i=0;i<curvature.size();i++)
@@ -155,22 +155,34 @@ bool AckermannSplineGenerator::nextTrajectory(base_local_planner::Trajectory &co
     else
       k_max_speed=sqrt(current_config_.comfort_lat_acc/fabs(k_max));
     min_max_speed=std::min(current_config_.max_trans_vel,k_max_speed);
-//    std::cout << ackermann_(0) << "," << min_max_speed << "," << current_config_.max_trans_vel << "," << k_max_speed << std::endl;
     try{
       if(vel_profile.generate(fabs(ackermann_(0)),0.0,min_max_speed,current_config_.max_trans_acc,spline_.get_length(),new_start_vel,new_max_vel))
       {
         vel_profile.evaluate_at_time(sim_period_,comp_traj.xv_,acceleration,length);
         if(!this->forward)
           comp_traj.xv_*=-1.0;
-        std::cout << "speed at " << sim_period_ << ": " << comp_traj.xv_ << std::endl;
         comp_traj.yv_=0.0;
         point=spline_.evaluate(length);
-        std::cout << "curvature at " << sim_period_ << ": " << point.curvature << std::endl;
+        comp_traj.thetav_=-(point.heading-start.heading);
+        if(comp_traj.thetav_>3.14159)
+          comp_traj.thetav_-=2*3.14159;
+        else if(comp_traj.thetav_<-3.14159)
+          comp_traj.thetav_+=2*3.14159;
+        if(forward)
+          comp_traj.thetav_*=-1.0;
+        if(comp_traj.thetav_>current_config_.max_steer_angle)
+          comp_traj.thetav_=current_config_.max_steer_angle;
+        else if(comp_traj.thetav_<current_config_.min_steer_angle)
+          comp_traj.thetav_=current_config_.min_steer_angle;
+
+        //double y_offset=(-(transformed_plan.back().pose.position.x-current_pose_.pose.position.x)*sin(yaw)+(transformed_plan.back().pose.position.y-current_pose_.pose.position.y)*cos(yaw));
+        //comp_traj.thetav_-=atan2(0.01*y_offset,comp_traj.xv_);
+        /*
+        point=spline_.evaluate(length);
         if(fabs(point.curvature)<0.001)
           steer_angle=0.0;
         else
           steer_angle=atan2(current_config_.axis_distance,1.0/point.curvature);
-        std::cout << "steer angle: " << steer_angle << std::endl;
         if(steer_angle>1.5707)
           steer_angle-=3.14159;
         if(current_config_.use_steer_angle_cmd)
@@ -186,12 +198,10 @@ bool AckermannSplineGenerator::nextTrajectory(base_local_planner::Trajectory &co
         }
         else
           comp_traj.thetav_ = comp_traj.xv_*point.curvature;
+        */
         comp_traj.resetPoints();
         for(unsigned int i=0;i<x.size();i++)
-        {
           comp_traj.addPoint(x[i],y[i],heading[i]);
-//          std::cout << x[i] << "," << y[i] << "," << heading[i] << "," << curvature[i] << std::endl;
-        }
         return true;
       }
       else