diff --git a/include/ackermann_planner.h b/include/ackermann_planner.h
index 496f78492286f4f7847f74b02851d35bd1c0eef7..13cf62c303414b0ef1b181686341e23a765a885a 100644
--- a/include/ackermann_planner.h
+++ b/include/ackermann_planner.h
@@ -138,6 +138,8 @@ class AckermannPlanner
     bool publish_cost_grid_pc_; ///< @brief Whether or not to build and publish a PointCloud
     bool publish_traj_pc_;
 
+    ros::NodeHandle private_nh;
+
     base_local_planner::MapGridVisualizer map_viz_; ///< @brief The map grid visualizer for outputting the potential field generated by the cost function
 
     // see constructor body for explanations
diff --git a/src/ackermann_planner.cpp b/src/ackermann_planner.cpp
index 5562dd92edaba78b38f7aea2075d249d58abe382..106dd1e6b14195ac6795ec5006295f91fefbc35a 100644
--- a/src/ackermann_planner.cpp
+++ b/src/ackermann_planner.cpp
@@ -53,9 +53,6 @@ void AckermannPlanner::reconfigure(iri_ackermann_local_planner::AckermannLocalPl
   boost::mutex::scoped_lock l(configuration_mutex_);
   double hdiff_scale;
 
-  generator_.reconfigure(config);
-  spline_generator_.reconfigure(config);
-
   double resolution = planner_util_->get_costmap()->getResolution();
   pdist_scale_ = config.path_distance_bias;
   // pdistscale used for both path and alignment, set  forward_point_distance to zero to discard alignment
@@ -77,6 +74,55 @@ 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);
 
+  std::string controller_frequency_param_name;
+  double sim_period;
+  if(!this->private_nh.searchParam("controller_frequency", controller_frequency_param_name))
+    sim_period = 0.05;
+  else
+  {
+    double controller_frequency = 0;
+    this->private_nh.param(controller_frequency_param_name, controller_frequency, 20.0);
+    if(controller_frequency > 0)
+      sim_period = 1.0 / controller_frequency;
+    else
+    {
+      ROS_WARN("A controller_frequency less than 0 has been set. Ignoring the parameter, assuming a rate of 20Hz");
+      sim_period = 0.05;
+    }
+  }
+
+  // set up all the cost functions that will be applied in order
+  // (any function returning negative values will abort scoring, so the order can improve performance)
+  std::vector<base_local_planner::TrajectoryCostFunction*> critics;
+  std::vector<base_local_planner::TrajectorySampleGenerator*> generator_list;
+  if(config.use_splines)
+  {
+    critics.push_back(&obstacle_costs_); // discards trajectories that move into obstacles
+    critics.push_back(&path_costs_); // prefers trajectories on global path
+    critics.push_back(&goal_costs_); // prefers trajectories that go towards (local) goal, based on wave propagation
+    critics.push_back(&heading_costs_); // prefers trajectories that go towards (local) goal, based on wave propagation
+
+    // trajectory generators
+    generator_list.push_back(&spline_generator_);
+    spline_generator_.set_sim_period(sim_period);
+  }
+  else
+  {
+    critics.push_back(&oscillation_costs_); // discards oscillating motions (assisgns cost -1)
+    critics.push_back(&obstacle_costs_); // discards trajectories that move into obstacles
+    critics.push_back(&path_costs_); // prefers trajectories on global path
+    critics.push_back(&goal_costs_); // prefers trajectories that go towards (local) goal, based on wave propagation
+    critics.push_back(&heading_costs_); // prefers trajectories that go towards (local) goal, based on wave propagation
+
+    // trajectory generators
+    generator_list.push_back(&generator_);
+  }
+
+  scored_sampling_planner_ = AckermannTrajectorySearch(generator_list, critics);
+
+  generator_.reconfigure(config);
+  spline_generator_.reconfigure(config);
+
   current_config_=config;
 }
 
@@ -84,18 +130,17 @@ AckermannPlanner::AckermannPlanner(std::string name,AckermannPlannerUtil *planne
   planner_util_(planner_util),
   obstacle_costs_(planner_util->get_costmap()),
   path_costs_(planner_util->get_costmap(), 0.0, 0.0, false,base_local_planner::Sum),
-  goal_costs_(planner_util->get_costmap(), 0.0, 0.0, true)
+  goal_costs_(planner_util->get_costmap(), 0.0, 0.0, true),
+  private_nh("~/" + name)
 {
-  ros::NodeHandle private_nh("~/" + name);
-
   std::string controller_frequency_param_name;
   double sim_period;
-  if(!private_nh.searchParam("controller_frequency", controller_frequency_param_name))
+  if(!this->private_nh.searchParam("controller_frequency", controller_frequency_param_name))
     sim_period = 0.05;
   else
   {
     double controller_frequency = 0;
-    private_nh.param(controller_frequency_param_name, controller_frequency, 20.0);
+    this->private_nh.param(controller_frequency_param_name, controller_frequency, 20.0);
     if(controller_frequency > 0)
       sim_period = 1.0 / controller_frequency;
     else
@@ -213,6 +258,7 @@ base_local_planner::Trajectory AckermannPlanner::find_best_path(geometry_msgs::P
   }
   if(path_index==0)
   {
+    std::cout << "empty path" << std::endl;
     drive_velocities.pose.position.x = 0;
     drive_velocities.pose.position.y = 0;
     drive_velocities.pose.position.z = 0;
@@ -291,6 +337,7 @@ base_local_planner::Trajectory AckermannPlanner::find_best_path(geometry_msgs::P
   //if we don't have a legal trajectory, we'll just command zero
   if (result_traj_.cost_ < 0) 
   {
+    std::cout << "no legal trajectory" << std::endl;
     drive_velocities.pose.position.x = 0;
     drive_velocities.pose.position.y = 0;
     drive_velocities.pose.position.z = 0;
@@ -308,6 +355,7 @@ base_local_planner::Trajectory AckermannPlanner::find_best_path(geometry_msgs::P
     tf2::Quaternion q;
     q.setRPY(0, 0, result_traj_.thetav_);
     tf2::convert(q, drive_velocities.pose.orientation);
+
 //    drive_velocities.pose.position.x = 0.0;
 //    drive_velocities.pose.position.y = 0.0;
 //    drive_velocities.pose.position.z = 0.0;