diff --git a/cfg/AckermannLocalPlanner.cfg b/cfg/AckermannLocalPlanner.cfg
index 0748ebc70179f2978069afcc7cb16f95a354b565..a22f7c557e360847c22b3af2c1d1ee5835b0728e 100755
--- a/cfg/AckermannLocalPlanner.cfg
+++ b/cfg/AckermannLocalPlanner.cfg
@@ -40,55 +40,61 @@ from dynamic_reconfigure.parameter_generator_catkin import *
 
 gen = ParameterGenerator()
 
-#       Name                       Type       Reconfiguration level            Description                       Default   Min   Max
-gen.add("max_sim_time",            double_t,  0, "The amount of time to roll trajectories out for in seconds", 10, 0, 20)
-gen.add("min_sim_time",            double_t,  0, "The amount of time to roll trajectories out for in seconds", 1.7, 0, 10)
-gen.add("sim_granularity",         double_t,  0, "The granularity with which to check for collisions along each trajectory in meters", 0.025, 0)
-gen.add("angular_sim_granularity", double_t,  0, "The granularity with which to check for collisions for rotations in radians", 0.1, 0)
-
-gen.add("path_distance_bias",      double_t,  0, "The weight for the path distance part of the cost function", 32.0, 0.0)
-gen.add("goal_distance_bias",      double_t,  0, "The weight for the goal distance part of the cost function", 24.0, 0.0)
-gen.add("occdist_scale",           double_t,  0, "The weight for the obstacle distance part of the cost function", 0.01, 0.0)
-
-gen.add("stop_time_buffer",        double_t,  0, "The amount of time that the robot must stop before a collision in order for a trajectory to be considered valid in seconds", 0.2, 0)
-gen.add("oscillation_reset_dist",  double_t,  0, "The distance the robot must travel before oscillation flags are reset, in meters", 0.05, 0)
-gen.add("oscillation_reset_angle", double_t,  0, "The angle the robot must turn before oscillation flags are reset, in radians", 0.2, 0)
-
-gen.add("forward_point_distance",  double_t,  0, "The distance from the center point of the robot to place an additional scoring point, in meters", 0.325)
-
-gen.add("scaling_speed",           double_t,  0, "The absolute value of the velocity at which to start scaling the robot's footprint, in m/s", 0.25, 0)
-gen.add("max_scaling_factor",      double_t,  0, "The maximum factor to scale the robot's footprint by", 0.2, 0)
-
-gen.add("trans_vel_samples",       int_t,     0, "The number of samples to use when exploring the x velocity space", 3, 1)
-gen.add("steer_angle_samples",     int_t,     0, "The number of samples to use when exploring the y velocity space", 10, 1)
-
-gen.add("prune_plan",              bool_t,    0, "Start following closest point of global plan, not first point (if different).", False)
-gen.add("xy_goal_tolerance",       double_t,  0, "Within what maximum distance we consider the robot to be in goal", 0.1)
-gen.add("yaw_goal_tolerance",      double_t,  0, "Within what maximum angle difference we consider the robot to face goal direction", 0.1)
-gen.add("trans_stopped_vel",       double_t,  0, "Below what maximum velocity we consider the robot to be stopped in translation", 0.1)
-gen.add("rot_stopped_vel",         double_t,  0, "Below what maximum rotation velocity we consider the robot to be stopped in rotation", 0.1)
-
-gen.add("restore_defaults",        bool_t,    0, "Restore to the original configuration.", False)
-
-gen.add("max_trans_vel",           double_t,  0,                               "maximum translational speed",    0.3,      0,    20)
-gen.add("min_trans_vel",           double_t,  0,                               "minimum translational speed",    -0.3,     -20,  0)
-gen.add("max_trans_acc",           double_t,  0,                               "maximum translational acceleration", 1.0,  0,    20)
-gen.add("max_steer_angle",         double_t,  0,                               "maximum steer angle",            0.45,     0,    0.54)
-gen.add("min_steer_angle",         double_t,  0,                               "minimum steer angle",            -0.45,    -0.54, 0)
-gen.add("max_steer_vel",           double_t,  0,                               "maximum steer speed",            1.0,      0,    2)
-gen.add("min_steer_vel",           double_t,  0,                               "minimum steer speed",            -1.0,     -2,   0)
-gen.add("max_steer_acc",           double_t,  0,                               "maximum steer acceleration",     0.36,     0,    5)
-gen.add("axis_distance",           double_t,  0,                               "distance between axes",          1.65,     -2.0,    2)
-gen.add("wheel_distance",          double_t,  0,                               "distance between wheels",        1.2,      0,    2)
-gen.add("wheel_radius",            double_t,  0,                               "Wheel diameter",                 0.436,    0,    2)
-gen.add("split_ignore_length",     double_t,  0,                               "Paths segments under this length will be ignored",1.0,0.0,2.0)
-gen.add("cmd_vel_avg",             int_t,     0,                               "Number of cmd_vel to average",   1,        1,    20)
-gen.add("odom_avg",                int_t,     0,                               "Number of odom to average",      1,        1,    20)
-
-gen.add("planner_patience",        int_t,     0,                               "number of impossible paths before replanning",2,     0,    10)
-
-gen.add("hdiff_scale",             double_t,  0,                               "The weight for the heading distance part of the cost function", 1.0 ,0, 500)
-gen.add("heading_points",          int_t,     0,                               "The number of points to check the heading", 8, 1, 64)
+#       Name                       Type       Reconfiguration level            Description                                                                                     Default   Min   Max
+gen.add("max_sim_time",            double_t,  0, "The amount of time to roll trajectories out for in seconds",                                                                 10, 0,    20)
+gen.add("min_sim_time",            double_t,  0, "The amount of time to roll trajectories out for in seconds",                                                                 1.7, 0,   10)
+gen.add("sim_granularity",         double_t,  0, "The granularity with which to check for collisions along each trajectory in meters",                                         0.025,    0)
+gen.add("angular_sim_granularity", double_t,  0, "The granularity with which to check for collisions for rotations in radians",                                                0.1,      0)
+
+gen.add("path_distance_bias",      double_t,  0, "The weight for the path distance part of the cost function",                                                                 32.0,     0.0)
+gen.add("goal_distance_bias",      double_t,  0, "The weight for the goal distance part of the cost function",                                                                 24.0,     0.0)
+gen.add("occdist_scale",           double_t,  0, "The weight for the obstacle distance part of the cost function",                                                             0.01,     0.0)
+
+gen.add("stop_time_buffer",        double_t,  0, "The amount of time that the robot must stop before a collision in order for a trajectory to be considered valid in seconds", 0.2,      0)
+gen.add("oscillation_reset_dist",  double_t,  0, "The distance the robot must travel before oscillation flags are reset, in meters",                                           0.05,     0)
+gen.add("oscillation_reset_angle", double_t,  0, "The angle the robot must turn before oscillation flags are reset, in radians",                                               0.2,      0)
+
+gen.add("forward_point_distance",  double_t,  0, "The distance from the center point of the robot to place an additional scoring point, in meters",                            0.325)
+
+gen.add("scaling_speed",           double_t,  0, "The absolute value of the velocity at which to start scaling the robot's footprint, in m/s",                                 0.25,     0)
+gen.add("max_scaling_factor",      double_t,  0, "The maximum factor to scale the robot's footprint by",                                                                       0.2,      0)
+
+gen.add("trans_vel_samples",       int_t,     0, "The number of samples to use when exploring the x velocity space",                                                           3, 1)
+gen.add("steer_angle_samples",     int_t,     0, "The number of samples to use when exploring the steer angle space",                                                          10, 1)
+gen.add("angular_vel_samples",     int_t,     0, "The number of samples to use when exploring the angular velocity space",                                                     10, 1)
+gen.add("use_steer_angle_cmd",     bool_t,    0, "Whether to use the steer angle or the angular velocity.",                                                                    False)
+
+gen.add("prune_plan",              bool_t,    0, "Start following closest point of global plan, not first point (if different).",                                              False)
+gen.add("xy_goal_tolerance",       double_t,  0, "Within what maximum distance we consider the robot to be in goal",                                                           0.1)
+gen.add("yaw_goal_tolerance",      double_t,  0, "Within what maximum angle difference we consider the robot to face goal direction",                                          0.1)
+gen.add("trans_stopped_vel",       double_t,  0, "Below what maximum velocity we consider the robot to be stopped in translation",                                             0.1)
+gen.add("rot_stopped_vel",         double_t,  0, "Below what maximum rotation velocity we consider the robot to be stopped in rotation",                                       0.1)
+
+gen.add("restore_defaults",        bool_t,    0, "Restore to the original configuration.",                                                                                     False)
+
+gen.add("max_trans_vel",           double_t,  0, "maximum translational speed",                                                                                                0.3,      0,     20)
+gen.add("min_trans_vel",           double_t,  0, "minimum translational speed",                                                                                                -0.3,     -20,   0)
+gen.add("max_trans_acc",           double_t,  0, "maximum translational acceleration",                                                                                         1.0,      0,     20)
+gen.add("max_steer_angle",         double_t,  0, "maximum steer angle",                                                                                                        0.45,     0,     0.54)
+gen.add("min_steer_angle",         double_t,  0, "minimum steer angle",                                                                                                        -0.45,    -0.54, 0)
+gen.add("max_steer_vel",           double_t,  0, "maximum steer speed",                                                                                                        1.0,      0,     2)
+gen.add("min_steer_vel",           double_t,  0, "minimum steer speed",                                                                                                        -1.0,     -2,    0)
+gen.add("max_steer_acc",           double_t,  0, "maximum steer acceleration",                                                                                                 0.36,     0,     5)
+gen.add("max_angular_vel",         double_t,  0, "maximum angular speed",                                                                                                      0.3,      0,     20)
+gen.add("min_angular_vel",         double_t,  0, "minimum angular speed",                                                                                                      -0.3,     -20,   0)
+gen.add("axis_distance",           double_t,  0, "distance between axes",                                                                                                      1.65,     -2.0,  2)
+gen.add("wheel_distance",          double_t,  0, "distance between wheels",                                                                                                    1.2,      0,     2)
+gen.add("wheel_radius",            double_t,  0, "Wheel diameter",                                                                                                             0.436,    0,     2)
+gen.add("use_trans_vel_deadzone",  bool_t,    0, "Whether to use a deadzone in the translational velocity or not",                                                             False)
+gen.add("trans_vel_deadzone",      double_t,  0, "Translatinal velocity minimum value",                                                                                        0.1,      0.01,  1.0)
+gen.add("split_ignore_length",     double_t,  0, "Paths segments under this length will be ignored",                                                                           1.0,      0.0,   2.0)
+gen.add("cmd_vel_avg",             int_t,     0, "Number of cmd_vel to average",                                                                                               1,        1,     20)
+gen.add("odom_avg",                int_t,     0, "Number of odom to average",                                                                                                  1,        1,     20)
+
+gen.add("planner_patience",        int_t,     0, "number of impossible paths before replanning",                                                                               2,        0,     10)
+
+gen.add("hdiff_scale",             double_t,  0, "The weight for the heading distance part of the cost function",                                                              1.0,      0,     500)
+gen.add("heading_points",          int_t,     0, "The number of points to check the heading",                                                                                  8,        1,     64)
 
 
 exit(gen.generate(PACKAGE, "AckermannLocalPlannerAlgorithm", "AckermannLocalPlanner"))
diff --git a/include/ackermann_planner_limits.h b/include/ackermann_planner_limits.h
index 1c3f9ff0518df893856bca5cb670fe7e4f784343..2907939a4ae5f3fe4728b7722964a64e0f7fe094 100644
--- a/include/ackermann_planner_limits.h
+++ b/include/ackermann_planner_limits.h
@@ -51,11 +51,16 @@ class AckermannPlannerLimits
     double max_steer_vel;
     double min_steer_vel;
     double max_steer_acc;
+    // angular velocity limits
+    double max_angular_vel;
+    double min_angular_vel;
     // kinematic attributes
     double axis_distance;
     double wheel_distance;
     double wheel_radius; 
     // general planner limits
+    bool use_trans_vel_deadzone;
+    double trans_vel_deadzone;
     double split_ignore_length;
     bool prune_plan;
     double xy_goal_tolerance;
@@ -77,11 +82,16 @@ class AckermannPlannerLimits
       double max_steer_vel,
       double min_steer_vel,
       double max_steer_acc,
+      // angular velocity limits
+      double max_angular_vel,
+      double min_angular_vel,
       // kinematic attributes
       double axis_distance,
       double wheel_distance,
       double wheel_radius, 
       // general planner limits
+      bool use_trans_vel_deadzone,
+      double trans_vel_deadzone,
       double split_ignore_length,
       double xy_goal_tolerance,
       double yaw_goal_tolerance,
@@ -96,9 +106,13 @@ class AckermannPlannerLimits
       max_steer_vel(max_steer_vel),
       min_steer_vel(min_steer_vel),
       max_steer_acc(max_steer_acc),
+      max_angular_vel(max_angular_vel),
+      min_angular_vel(min_angular_vel),
       axis_distance(axis_distance),
       wheel_distance(wheel_distance),
       wheel_radius(wheel_radius),
+      use_trans_vel_deadzone(use_trans_vel_deadzone),
+      trans_vel_deadzone(trans_vel_deadzone),
       split_ignore_length(split_ignore_length),
       prune_plan(prune_plan),
       xy_goal_tolerance(xy_goal_tolerance),
diff --git a/include/ackermann_planner_ros.h b/include/ackermann_planner_ros.h
index 33f48c061fb25ef8387c81cd53d00cb06b8b4f41..5b45993a7f58b41d6fe62a4b52fc97c86f2060cb 100644
--- a/include/ackermann_planner_ros.h
+++ b/include/ackermann_planner_ros.h
@@ -147,5 +147,6 @@ class AckermannPlannerROS : public nav_core::BaseLocalPlanner
 
     bool stucked;
     bool first;
+    bool use_steer_angle_cmd;
 };
 #endif
diff --git a/include/ackermann_trajectory_generator.h b/include/ackermann_trajectory_generator.h
index b960cf55b3e079eaf5041ec14902d26bba8224fd..26e6a3f6a883c7f9a8ec7ae1e54922c857abf60e 100644
--- a/include/ackermann_trajectory_generator.h
+++ b/include/ackermann_trajectory_generator.h
@@ -91,6 +91,7 @@ class AckermannTrajectoryGenerator: public base_local_planner::TrajectorySampleG
         double min_sim_time,
 	double sim_granularity,
 	double angular_sim_granularity,
+        bool use_steer_angle_cmd,
 	double sim_period = 0.0);
 
     /**
@@ -126,6 +127,7 @@ class AckermannTrajectoryGenerator: public base_local_planner::TrajectorySampleG
     double max_sim_time_,min_sim_time_,sim_time_;
     double sim_granularity_, angular_sim_granularity_;
     double sim_period_; // only for dwa
+    bool use_steer_angle_cmd_;
 };
 
 #endif /* SIMPLE_TRAJECTORY_GENERATOR_H_ */
diff --git a/params/ackermann_planner_params.yaml b/params/ackermann_planner_params.yaml
index f8771137cd6b3131d109d6c80cf9c9e148144efb..ab581039c0fc6c37279ea2ec352f548efc61324f 100644
--- a/params/ackermann_planner_params.yaml
+++ b/params/ackermann_planner_params.yaml
@@ -1,50 +1,54 @@
+base_local_planner: "AckermannPlannerROS"
+
 AckermannPlannerROS:
-  max_sim_time: 15 #8.0
+  max_sim_time: 3.0
   min_sim_time: 1.0
   sim_granularity: 0.2
   angular_sim_granularity: 0.02
 
-  path_distance_bias: 3.0
-  goal_distance_bias: 0.5
-  occdist_scale: 0.01
+  path_distance_bias: 50.0
+  goal_distance_bias: 10.0
+  occdist_scale: 0.05
 
-#  stop_time_buffer: 
   oscillation_reset_dist: 0.002
   oscillation_reset_angle: 0.001
 
-#  forward_point_distance:
-
-#  scaling_speed:
-#  max_scaling_factor:
-
-  trans_vel_samples: 21
-  steer_angle_samples: 11
+  trans_vel_samples: 31
+  steer_angle_samples: 21
+  angular_vel_samples: 21
+  use_steer_angle_cmd: false
 
   prune_plan: true
-  xy_goal_tolerance: 0.25
-  yaw_goal_tolerance: 0.5
+  xy_goal_tolerance: 0.1
+  yaw_goal_tolerance: 0.2
   trans_stopped_vel: 0.1
   rot_stopped_vel: 0.1
 
   restore_defaults: false
 
-  max_trans_vel: 2.0 #1.0
-  min_trans_vel: 0.0 #-1.0
-  max_trans_acc: 0.3
-  max_steer_angle: 0.35
-  min_steer_angle: -0.35
-  max_steer_vel: 0.5
-  min_steer_vel: -0.5
-  max_steer_acc: 1.0
-  axis_distance: 4.3  
-  wheel_distance: 4.5
-  wheel_radius: 0.65465 
-
-  cmd_vel_avg: 10
+  max_trans_vel: 1.0
+  min_trans_vel: -1.0
+  max_trans_acc: 0.5
+  max_steer_angle: 0.5
+  min_steer_angle: -0.5
+  max_steer_vel: 1.0
+  min_steer_vel: -1.0
+  max_steer_acc: 5.0
+  max_angular_vel: 0.5
+  min_angular_vel: -0.5
+  axis_distance: 0.36
+  wheel_distance: 0.265
+  wheel_radius: 0.05
+
+  use_trans_vel_deadzone: true
+  trans_vel_deadzone: 0.1
+
+  split_ignore_length: 0.1
+
+  cmd_vel_avg: 1
   odom_avg: 1
 
   planner_patience: 5
 
   hdiff_scale: 0.5
   heading_points: 8
-
diff --git a/src/ackermann_planner.cpp b/src/ackermann_planner.cpp
index 969dcd2b6323032e21f145e6e96457dc687dba4b..c62eaa373eb09caa2857419a604632d7285f9fd8 100644
--- a/src/ackermann_planner.cpp
+++ b/src/ackermann_planner.cpp
@@ -57,6 +57,7 @@ void AckermannPlanner::reconfigure(iri_ackermann_local_planner::AckermannLocalPl
       config.min_sim_time,
       config.sim_granularity,
       config.angular_sim_granularity,
+      config.use_steer_angle_cmd,
       sim_period_);
 
   double resolution = planner_util_->get_costmap()->getResolution();
@@ -80,9 +81,12 @@ void AckermannPlanner::reconfigure(iri_ackermann_local_planner::AckermannLocalPl
   // obstacle costs can vary due to scaling footprint feature
   obstacle_costs_.setParams(config.max_trans_vel, config.max_scaling_factor, config.scaling_speed);
 
-  int trans_vel_samp, steer_angle_samp;
+  int trans_vel_samp, steer_angle_angular_vel_samp;
   trans_vel_samp = config.trans_vel_samples;
-  steer_angle_samp = config.steer_angle_samples;
+  if(config.use_steer_angle_cmd)
+    steer_angle_angular_vel_samp = config.steer_angle_samples;
+  else
+    steer_angle_angular_vel_samp = config.angular_vel_samples;
 
   if (trans_vel_samp <= 0) 
   {
@@ -91,15 +95,18 @@ void AckermannPlanner::reconfigure(iri_ackermann_local_planner::AckermannLocalPl
     config.trans_vel_samples = trans_vel_samp;
   }
 
-  if (steer_angle_samp <= 0) 
+  if (steer_angle_angular_vel_samp <= 0) 
   {
-    ROS_WARN("You've specified that you don't want any samples in the steering dimension. We'll at least assume that you want to sample one value... so we're going to set vy_samples to 1 instead");
-    steer_angle_samp = 1;
-    config.steer_angle_samples = steer_angle_samp;
+    ROS_WARN("You've specified that you don't want any samples in the steering/angular velocity dimension. We'll at least assume that you want to sample one value... so we're going to set vy_samples to 1 instead");
+    steer_angle_angular_vel_samp = 1;
+    if(config.use_steer_angle_cmd)
+      config.steer_angle_samples = steer_angle_angular_vel_samp;
+    else
+      config.angular_vel_samples = steer_angle_angular_vel_samp;
   }
 
   vsamples_[0] = trans_vel_samp;
-  vsamples_[1] = steer_angle_samp;
+  vsamples_[1] = steer_angle_angular_vel_samp;
 }
 
 AckermannPlanner::AckermannPlanner(std::string name,AckermannPlannerUtil *planner_util) :
diff --git a/src/ackermann_planner_ros.cpp b/src/ackermann_planner_ros.cpp
index ca3bc68cdfb09fc5f6956a4026702d92e46e97ae..7d08e2f61c3ca02efffc82b9611667de89b91464 100644
--- a/src/ackermann_planner_ros.cpp
+++ b/src/ackermann_planner_ros.cpp
@@ -82,11 +82,15 @@ void AckermannPlannerROS::reconfigure_callback(iri_ackermann_local_planner::Acke
   limits.max_steer_vel = config.max_steer_vel;
   limits.min_steer_vel = config.min_steer_vel;
   limits.max_steer_acc = config.max_steer_acc;
+  limits.max_angular_vel = config.max_angular_vel;
+  limits.min_angular_vel = config.min_angular_vel;
   // kinematic attributes
   limits.axis_distance = config.axis_distance;
   limits.wheel_distance = config.wheel_distance;
   limits.wheel_radius = config.wheel_radius;
   // general planner limits
+  limits.use_trans_vel_deadzone = config.use_trans_vel_deadzone;
+  limits.trans_vel_deadzone = config.trans_vel_deadzone;
   limits.split_ignore_length = config.split_ignore_length;
   limits.xy_goal_tolerance = config.xy_goal_tolerance;
   limits.yaw_goal_tolerance = config.yaw_goal_tolerance;
@@ -95,6 +99,7 @@ void AckermannPlannerROS::reconfigure_callback(iri_ackermann_local_planner::Acke
   limits.rot_stopped_vel = config.rot_stopped_vel;
   planner_util_.reconfigure_callback(limits, config.restore_defaults);
   this->last_cmds.resize(config.cmd_vel_avg);
+  this->use_steer_angle_cmd=config.use_steer_angle_cmd;
   odom_helper_.set_average_samples(config.odom_avg);
 
   patience_=config.planner_patience;
@@ -437,34 +442,39 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
       ackermann_msgs::AckermannDrive ackermann;
       odom_helper_.get_ackermann_state(ackermann);
       // steer the car without moving if the steering angle is big
-      
-      /*
-      if(fabs(ackermann.steer_angle-cmd_vel.angular.z)>0.05)
+      if(this->use_steer_angle_cmd)
       {
-        // IRI
-        ROS_WARN("Setting forward speed to 0 (%f,%f)",ackermann.steer_angle,cmd_vel.angular.z);
-        cmd_vel.linear.x=0.0;
-
-        // EURECAT
-        //ROS_WARN("Setting forward speed to 0.1 (%f,%f)",ackermann.steer_angle,cmd_vel.angular.z);        
-        //cmd_vel.linear.x=0.1;
+        if(fabs(ackermann.steering_angle-cmd_vel.angular.z)>0.05)
+        {
+          ROS_WARN("Setting forward speed to 0 (%f,%f)",ackermann.steering_angle,cmd_vel.angular.z);
+          cmd_vel.linear.x=0.0;
+        }
+        else
+        {
+          if(this->first) 
+            this->first=false;
+          if(new_segment) 
+            new_segment=false;
+        }
       }
       else
       {
-      */
-        if(this->first) this->first=false;
-        if(new_segment) new_segment=false;
-      
+        if(this->first) 
+          this->first=false;
+        if(new_segment) 
+          new_segment=false;
+      }
     }
-    else if(fabs(cmd_vel.linear.x)<0.015)
+    else if(fabs(cmd_vel.linear.x)<limits.trans_vel_deadzone)
     {
+/*
        // get the position of the goal
        base_local_planner::getGoalPose(*tf_,
                                        transformed_plan,
                                        costmap_ros_->getGlobalFrameID(),
                                        goal_pose);
        // get the distance to the goal
-       double dist=base_local_planner::getGoalPositionDistance(current_pose_,goal_pose.pose.position.x,goal_pose.pose.position.x);
+       double dist=base_local_planner::getGoalPositionDistance(current_pose_,goal_pose.pose.position.x,goal_pose.pose.position.y);
        if(dist>limits.xy_goal_tolerance && dist<2*limits.xy_goal_tolerance)
        {
          stucked_count++;
@@ -490,6 +500,7 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
            return false;
          }
        }
+*/
     }
     else
     {
@@ -497,7 +508,16 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
       replan_count=0;
     }
     if (isOk) 
+    {
       publish_global_plan(transformed_plan);
+      if(limits.use_trans_vel_deadzone && fabs(cmd_vel.linear.x)<limits.trans_vel_deadzone)
+      { 
+        if(cmd_vel.linear.x>0.0)
+          cmd_vel.linear.x=limits.trans_vel_deadzone;
+        else
+          cmd_vel.linear.x=-limits.trans_vel_deadzone;
+      }
+    }
     else 
     {
       ROS_WARN_NAMED("ackermann_local_planner", "Ackermann planner failed to produce path.");
diff --git a/src/ackermann_trajectory_generator.cpp b/src/ackermann_trajectory_generator.cpp
index 498dd7d5fba5d69d8315db6e8b8e5fc75079fbd9..789c1bec6b67391fa0c1da31ac61dc6e83f0d332 100644
--- a/src/ackermann_trajectory_generator.cpp
+++ b/src/ackermann_trajectory_generator.cpp
@@ -248,6 +248,7 @@ void AckermannTrajectoryGenerator::set_parameters(
     double min_sim_time,
     double sim_granularity,
     double angular_sim_granularity,
+    bool use_steer_angle_cmd,
     double sim_period) 
 {
   max_sim_time_ = max_sim_time;
@@ -255,6 +256,7 @@ void AckermannTrajectoryGenerator::set_parameters(
   sim_granularity_ = sim_granularity;
   angular_sim_granularity_ = angular_sim_granularity;
   sim_period_ = sim_period;
+  use_steer_angle_cmd_ = use_steer_angle_cmd;
 }
 
 /**
@@ -607,13 +609,24 @@ bool AckermannTrajectoryGenerator::generate_trajectory(
     time+=dt;
   } // end for simulation steps
 
-  if(fabs(sample_target_vel[1])>0.0)
+  if(this->use_steer_angle_cmd_)
   {
-    double radius=limits_->axis_distance/tan(sample_target_vel[1]);
-    traj.thetav_ = sample_target_vel[0]/radius;
+    traj.thetav_ = sample_target_vel[1];
+    if(traj.thetav_>limits_->max_angular_vel)
+      traj.thetav_=limits_->max_angular_vel;
+    else if(traj.thetav_<limits_->min_angular_vel)
+      traj.thetav_=limits_->min_angular_vel;
   }
   else
-    traj.thetav_ = 0.0;
+  {
+    if(fabs(sample_target_vel[1])>0.0)
+    {
+      double radius=limits_->axis_distance/tan(sample_target_vel[1]);
+      traj.thetav_ = sample_target_vel[0]/radius;
+    }
+    else
+      traj.thetav_ = 0.0;
+  }
 
   return num_steps > 0; // true if trajectory has at least one point
 }
diff --git a/src/heading_cost_function.cpp b/src/heading_cost_function.cpp
index 41bc7c7babb8b000d5abe90499481f7e76f6f311..ad19be0c03c8f0e24d81ae61dfecd5ae5809b2a1 100644
--- a/src/heading_cost_function.cpp
+++ b/src/heading_cost_function.cpp
@@ -85,6 +85,7 @@ double HeadingCostFunction::scoreTrajectory(base_local_planner::Trajectory &traj
       diff=fabs(diff-3.14159);
     heading_diff+=diff;
   }
+  heading_diff/=this->num_points;
 
   return heading_diff;
 }