diff --git a/include/ackermann_spline_generator.h b/include/ackermann_spline_generator.h
index 46d1c3e744136e47fd7aafe1de70115a4f1ffaf2..46763e50114c4daad293229475b6157684ef2630 100644
--- a/include/ackermann_spline_generator.h
+++ b/include/ackermann_spline_generator.h
@@ -39,6 +39,7 @@
 #define ACKERMANN_SPLINE_GENERATOR_H_
 
 #include <base_local_planner/trajectory_sample_generator.h>
+#include <geometry_msgs/PoseStamped.h>
 #include <Eigen/Core>
 
 #include <iri_ackermann_local_planner/AckermannLocalPlannerConfig.h>
@@ -69,7 +70,7 @@ class AckermannSplineGenerator: public base_local_planner::TrajectorySampleGener
      */
     bool nextTrajectory(base_local_planner::Trajectory &traj);
 
-    void initialise(const Eigen::Vector3f& pos,const Eigen::Vector3f& ackermann,const Eigen::Vector3f& goal,bool forward);
+    void initialise(const Eigen::Vector3f& pos,const Eigen::Vector3f& ackermann,const Eigen::Vector3f& goal,bool forward,std::vector<geometry_msgs::PoseStamped>& current_plan);
 
     void set_sim_period(double period);
 
@@ -82,9 +83,11 @@ class AckermannSplineGenerator: public base_local_planner::TrajectorySampleGener
     Eigen::Vector3f pos_;
     Eigen::Vector3f goal_pos_;
     Eigen::Vector3f ackermann_;
+    std::vector<geometry_msgs::PoseStamped> current_plan;
     bool forward;
-
     double sim_period_;
+
+    void get_path_closest_pose(double &path_x,double &path_y);
 };
 
 #endif /* SIMPLE_TRAJECTORY_GENERATOR_H_ */
diff --git a/src/ackermann_planner.cpp b/src/ackermann_planner.cpp
index 42cad18af3e37aa2ca0217c816f761cd2d804e69..5562dd92edaba78b38f7aea2075d249d58abe382 100644
--- a/src/ackermann_planner.cpp
+++ b/src/ackermann_planner.cpp
@@ -228,7 +228,7 @@ base_local_planner::Trajectory AckermannPlanner::find_best_path(geometry_msgs::P
 
   // prepare cost functions and generators for this run
   generator_.initialise(pos,ack_state,goal);
-  spline_generator_.initialise(pos,ack_state,goal,plan_forward);
+  spline_generator_.initialise(pos,ack_state,goal,plan_forward,global_plan_);
 
   result_traj_.cost_ = -7;
   // find best trajectory by sampling and scoring the samples
diff --git a/src/ackermann_spline_generator.cpp b/src/ackermann_spline_generator.cpp
index 674467d8a4d4e40a53f9c62cd08c8efa6c8849bc..1e15304cdc285b2bb0dfd06e74a97d4196cf28d5 100644
--- a/src/ackermann_spline_generator.cpp
+++ b/src/ackermann_spline_generator.cpp
@@ -50,11 +50,31 @@ AckermannSplineGenerator::AckermannSplineGenerator()
 {
 }
 
+void 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;
+
+  /* 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));
+    if(dist<min_dist)
+    {
+      min_dist=dist;
+      min_index=i;
+    }
+  }
+  path_x=this->current_plan[min_index].pose.position.x;
+  path_y=this->current_plan[min_index].pose.position.y;
+}
+
 void AckermannSplineGenerator::initialise(
   const Eigen::Vector3f& pos,
   const Eigen::Vector3f& ackermann,//[0] -> trans_vel, [1]-> steer_angle, [2]-> steer_vel
   const Eigen::Vector3f& goal,
-  bool forward)
+  bool forward,
+  std::vector<geometry_msgs::PoseStamped>& current_plan)
 {
   double diff_x,diff_y; 
   double min_n1,max_n1,min_n2,max_n2,min_n3,max_n3,min_n4,max_n4;
@@ -81,6 +101,7 @@ void AckermannSplineGenerator::initialise(
         for(double current_n4=min_n4;current_n4<=max_n4;current_n4+=(max_n4-min_n4)/(current_config_.n4_samples-1))
           samples_.push_back(Eigen::Vector4f(current_n1,current_n2,current_n3,current_n4));
   this->forward=forward;
+  this->current_plan=current_plan;
 }
 
 void AckermannSplineGenerator::reconfigure(iri_ackermann_local_planner::AckermannLocalPlannerConfig &cfg)
@@ -137,80 +158,72 @@ bool AckermannSplineGenerator::nextTrajectory(base_local_planner::Trajectory &co
       k_max=curvature[i];
   max_curvature=tan(std::max(current_config_.max_steer_angle,fabs(current_config_.min_steer_angle)))/fabs(current_config_.axis_distance);
   next_sample_index_++;
-//  if(k_max>max_curvature)
-//  {
-//    std::cout << "Too much curvature: " << k_max << "(" << max_curvature << ")" << std::endl;
-//    return false;
-//  }
-//  else
-//  {
-    /* fill the ouput trajectory object */
-    double k_max_speed,min_max_speed,new_start_vel,new_max_vel;
-    double acceleration,length,steer_angle;
-    CVelTrapezoid vel_profile;
-    TPoint point;
-
-    if(k_max==0)
-      k_max_speed=std::numeric_limits<double>::quiet_NaN();
-    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);
+  /* fill the ouput trajectory object */
+  double k_max_speed,min_max_speed,new_start_vel,new_max_vel;
+  double acceleration,length,steer_angle,path_x,path_y;
+  CVelTrapezoid vel_profile;
+  TPoint point;
+
+  if(k_max==0)
+    k_max_speed=std::numeric_limits<double>::quiet_NaN();
+  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);
+  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{
-      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;
-        comp_traj.yv_=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;
-        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);
-        if(steer_angle>1.5707)
-          steer_angle-=3.14159;
-        if(current_config_.use_steer_angle_cmd)
-        {
-          if(forward)
-            comp_traj.thetav_ = steer_angle;
-          else
-            comp_traj.thetav_ = -steer_angle;
-          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;
-        }
-        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]);
-        return true;
-      }
-      else
-        return false;
+      vel_profile.evaluate_at_time(sim_period_,comp_traj.xv_,acceleration,length);
     }catch(CException &e){
-      //ROS_INFO_STREAM(e.what());
-      return false;
+      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);
+    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;
+    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)
+      steer_angle=0.0;
+    else
+      steer_angle=atan2(current_config_.axis_distance,1.0/point.curvature);
+    if(steer_angle>1.5707)
+      steer_angle-=3.14159;
+    if(current_config_.use_steer_angle_cmd)
+    {
+      if(forward)
+        comp_traj.thetav_ = steer_angle;
+      else
+        comp_traj.thetav_ = -steer_angle;
+      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;
+    }
+    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]);
+    return true;
+  }
+  else
+    return false;
 }
 
 void AckermannSplineGenerator::set_sim_period(double period)