diff --git a/src/ackermann_spline_generator.cpp b/src/ackermann_spline_generator.cpp
index 1e15304cdc285b2bb0dfd06e74a97d4196cf28d5..6261d0bc087fa60c99be2329bf57564d9d3200db 100644
--- a/src/ackermann_spline_generator.cpp
+++ b/src/ackermann_spline_generator.cpp
@@ -58,7 +58,7 @@ void AckermannSplineGenerator::get_path_closest_pose(double &path_x,double &path
   /* find the path point closest to the robot */
   for(unsigned int i=0;i<this->current_plan.size();i++)
   {
-    dist=sqrt(pow(pos_(0)-this->current_plan[i].pose.position.x,2.0)+pow(pos_(1)-this->current_plan[i].pose.position.x,2.0));
+    dist=sqrt(pow(pos_(0)-this->current_plan[i].pose.position.x,2.0)+pow(pos_(1)-this->current_plan[i].pose.position.y,2.0));
     if(dist<min_dist)
     {
       min_dist=dist;
@@ -126,6 +126,7 @@ bool AckermannSplineGenerator::nextTrajectory(base_local_planner::Trajectory &co
   double resolution,k_max,max_curvature;
   std::vector<double> x,y,curvature,heading;
 
+  try{
   start.x=pos_(0);
   start.y=pos_(1);
   if(this->forward)
@@ -172,29 +173,32 @@ bool AckermannSplineGenerator::nextTrajectory(base_local_planner::Trajectory &co
   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))
   {
     try{
-      vel_profile.evaluate_at_time(sim_period_,comp_traj.xv_,acceleration,length);
+      vel_profile.evaluate_at_time(current_config_.temp_horizon,comp_traj.xv_,acceleration,length);
     }catch(CException &e){
+      length=0.0;
       comp_traj.xv_=vel_profile.get_max_vel();
     }
-    std::cout << "x velocity: " << comp_traj.xv_ << std::endl; 
     if(!this->forward)
       comp_traj.xv_*=-1.0;
     comp_traj.yv_=0.0;
-    point=spline_.evaluate(0.0);
+    point=spline_.evaluate(length);
     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;
+    this->get_path_closest_pose(path_x,path_y);
+    double y_offset=(-(path_x-pos_(0))*sin(pos_(2))+(path_y-pos_(1))*cos(pos_(2)));
+    if(forward)
+      comp_traj.thetav_-=atan2(current_config_.lateral_offset_gain*y_offset,comp_traj.xv_);
+    else
+      comp_traj.thetav_+=atan2(current_config_.lateral_offset_gain*y_offset,-comp_traj.xv_);
     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;
-    this->get_path_closest_pose(path_x,path_y);
-    double y_offset=(-(path_x-pos_(0))*sin(pos_(2))+(path_y-pos_(1))*cos(pos_(2)));
-    comp_traj.thetav_-=atan2(0.01*y_offset,comp_traj.xv_);
     /*
     point=spline_.evaluate(length);
     if(fabs(point.curvature)<0.001)
@@ -224,6 +228,9 @@ bool AckermannSplineGenerator::nextTrajectory(base_local_planner::Trajectory &co
   }
   else
     return false;
+  }catch(CException &e){
+    std::cout << e.what() << std::endl;	  
+  }
 }
 
 void AckermannSplineGenerator::set_sim_period(double period)