void SimpleTrajectoryGenerator::initialise(
    const Eigen::Vector3f& pos,
    const Eigen::Vector3f& vel,
    const Eigen::Vector3f& goal,
    base_local_planner::LocalPlannerLimits* limits,
    const Eigen::Vector3f& vsamples,
    bool discretize_by_time) {
  /*
   * We actually generate all velocity sample vectors here, from which to generate trajectories later on
   */
  double max_vel_th = limits->max_rot_vel;
  double min_vel_th = -1.0 * max_vel_th;
  discretize_by_time_ = discretize_by_time;
  Eigen::Vector3f acc_lim = limits->getAccLimits();
  pos_ = pos;
  vel_ = vel;
  limits_ = limits;
  next_sample_index_ = 0;
  sample_params_.clear();

  double min_vel_x = limits->min_vel_x;
  double max_vel_x = limits->max_vel_x;
  double min_vel_y = limits->min_vel_y;
  double max_vel_y = limits->max_vel_y;

  // if sampling number is zero in any dimension, we don't generate samples generically
  if (vsamples[0] * vsamples[1] * vsamples[2] > 0) {
    //compute the feasible velocity space based on the rate at which we run
    Eigen::Vector3f max_vel = Eigen::Vector3f::Zero();
    Eigen::Vector3f min_vel = Eigen::Vector3f::Zero();

    if ( ! use_dwa_) {
      // there is no point in overshooting the goal, and it also may break the
      // robot behavior, so we limit the velocities to those that do not overshoot in sim_time
      double dist = ::hypot(goal[0] - pos[0], goal[1] - pos[1]);
      max_vel_x = std::max(std::min(max_vel_x, dist / sim_time_), min_vel_x);
      max_vel_y = std::max(std::min(max_vel_y, dist / sim_time_), min_vel_y);

      // if we use continous acceleration, we can sample the max velocity we can reach in sim_time_
      max_vel[0] = std::min(max_vel_x, vel[0] + acc_lim[0] * sim_time_);
      max_vel[1] = std::min(max_vel_y, vel[1] + acc_lim[1] * sim_time_);
      max_vel[2] = std::min(max_vel_th, vel[2] + acc_lim[2] * sim_time_);

      min_vel[0] = std::max(min_vel_x, vel[0] - acc_lim[0] * sim_time_);
      min_vel[1] = std::max(min_vel_y, vel[1] - acc_lim[1] * sim_time_);
      min_vel[2] = std::max(min_vel_th, vel[2] - acc_lim[2] * sim_time_);
    } else {
      // with dwa do not accelerate beyond the first step, we only sample within velocities we reach in sim_period
      max_vel[0] = std::min(max_vel_x, vel[0] + acc_lim[0] * sim_period_);
      max_vel[1] = std::min(max_vel_y, vel[1] + acc_lim[1] * sim_period_);
      max_vel[2] = std::min(max_vel_th, vel[2] + acc_lim[2] * sim_period_);

      min_vel[0] = std::max(min_vel_x, vel[0] - acc_lim[0] * sim_period_);
      min_vel[1] = std::max(min_vel_y, vel[1] - acc_lim[1] * sim_period_);
      min_vel[2] = std::max(min_vel_th, vel[2] - acc_lim[2] * sim_period_);
    }

    Eigen::Vector3f vel_samp = Eigen::Vector3f::Zero();
    VelocityIterator x_it(min_vel[0], max_vel[0], vsamples[0]);
    VelocityIterator y_it(min_vel[1], max_vel[1], vsamples[1]);
    VelocityIterator th_it(min_vel[2], max_vel[2], vsamples[2]);
    for(; !x_it.isFinished(); x_it++) {
      vel_samp[0] = x_it.getVelocity();
      for(; !y_it.isFinished(); y_it++) {
        vel_samp[1] = y_it.getVelocity();
        for(; !th_it.isFinished(); th_it++) {
          vel_samp[2] = th_it.getVelocity();
          //ROS_DEBUG("Sample %f, %f, %f", vel_samp[0], vel_samp[1], vel_samp[2]);
          sample_params_.push_back(vel_samp);
        }
        th_it.reset();
      }
      y_it.reset();
    }
  }
}
  base_local_planner::Trajectory DWAPlanner::computeTrajectories(const Eigen::Vector3f& pos, const Eigen::Vector3f& vel){
    tf::Stamped<tf::Pose> robot_pose_tf;
    geometry_msgs::PoseStamped robot_pose;

    //compute the distance between the robot and the last point on the global_plan
    costmap_ros_->getRobotPose(robot_pose_tf);
    tf::poseStampedTFToMsg(robot_pose_tf, robot_pose);

    double sq_dist = squareDist(robot_pose, global_plan_.back());

    bool two_point_scoring = true;
    if(sq_dist < forward_point_distance_ * forward_point_distance_)
      ROS_INFO("single points scoring");
      two_point_scoring = false;

    //compute the feasible velocity space based on the rate at which we run
    Eigen::Vector3f max_vel = Eigen::Vector3f::Zero();
    max_vel[0] = std::min(max_vel_x_, vel[0] + acc_lim_[0] * sim_period_);
    max_vel[1] = std::min(max_vel_y_, vel[1] + acc_lim_[1] * sim_period_);
    max_vel[2] = std::min(max_vel_th_, vel[2] + acc_lim_[2] * sim_period_);

    Eigen::Vector3f min_vel = Eigen::Vector3f::Zero();
    min_vel[0] = std::max(min_vel_x_, vel[0] - dec_lim_[0] * sim_period_);
    min_vel[1] = std::max(min_vel_y_, vel[1] - dec_lim_[1] * sim_period_);
    min_vel[2] = std::max(min_vel_th_, vel[2] - dec_lim_[2] * sim_period_);

    Eigen::Vector3f dv = Eigen::Vector3f::Zero();
    //we want to sample the velocity space regularly
    for(unsigned int i = 0; i < 3; ++i){
      dv[i] = (max_vel[i] - min_vel[i]) / (std::max(1.0, double(vsamples_[i]) - 1));
    }

    //keep track of the best trajectory seen so far... we'll re-use two memeber vars for efficiency
    base_local_planner::Trajectory* best_traj = &traj_one_;
    best_traj->cost_ = -1.0;

    base_local_planner::Trajectory* comp_traj = &traj_two_;
    comp_traj->cost_ = -1.0;

    Eigen::Vector3f vel_samp = Eigen::Vector3f::Zero();

    //ROS_ERROR("x(%.2f, %.2f), y(%.2f, %.2f), th(%.2f, %.2f)", min_vel[0], max_vel[0], min_vel[1], max_vel[1], min_vel[2], max_vel[2]);
    //ROS_ERROR("x(%.2f, %.2f), y(%.2f, %.2f), th(%.2f, %.2f)", min_vel_x_, max_vel_x_, min_vel_y_, max_vel_y_, min_vel_th_, max_vel_th_);
    //ROS_ERROR("dv %.2f %.2f %.2f", dv[0], dv[1], dv[2]);

    for(VelocityIterator x_it(min_vel[0], max_vel[0], dv[0]); !x_it.isFinished(); x_it++){
      vel_samp[0] = x_it.getVelocity();
      for(VelocityIterator y_it(min_vel[1], max_vel[1], dv[1]); !y_it.isFinished(); y_it++){
        vel_samp[1] = y_it.getVelocity();
        for(VelocityIterator th_it(min_vel[2], max_vel[2], dv[2]); !th_it.isFinished(); th_it++){
          vel_samp[2] = th_it.getVelocity();
          generateTrajectory(pos, vel_samp, *comp_traj, two_point_scoring, sq_dist);
          selectBestTrajectory(best_traj, comp_traj);
        }
      }
    }

    ROS_DEBUG_NAMED("oscillation_flags", "forward_pos_only: %d, forward_neg_only: %d, strafe_pos_only: %d, strafe_neg_only: %d, rot_pos_only: %d, rot_neg_only: %d",
        forward_pos_only_, forward_neg_only_, strafe_pos_only_, strafe_neg_only_, rot_pos_only_, rot_neg_only_);

    //ok... now we have our best trajectory
    if(best_traj->cost_ >= 0){
      //we want to check if we need to set any oscillation flags
      if(setOscillationFlags(best_traj)){
        prev_stationary_pos_ = pos;
      }

      //if we've got restrictions... check if we can reset any oscillation flags
      if(forward_pos_only_ || forward_neg_only_ 
          || strafe_pos_only_ || strafe_neg_only_
          || rot_pos_only_ || rot_neg_only_){
        resetOscillationFlagsIfPossible(pos, prev_stationary_pos_);
      }
    }

    //TODO: Think about whether we want to try to do things like back up when a valid trajectory is not found

    return *best_traj;

  }