double PreferStraightCostFunction::scoreTrajectory(base_local_planner::Trajectory & traj)
{

  double score = 0.0;
  double prev_th;

  for (int i = 0; i < traj.getPointsSize(); ++i)
  {

    double x, y, th;
    traj.getPoint(i, x, y, th);

    if (i == 0)
    {
      prev_th = th;
      continue;
    }

    score += fabs(th - prev_th);

    prev_th = th;

  }

}
 double DirectionTrajectoryScorer::scoreTrajectory(base_local_planner::Trajectory& traj) {
     obstacle_scorer_->prepare();
     //ROS_WARN("Scorer After Prepare ROS Inscribed radius=%.4f", costmap_ros_->getInscribedRadius()); //sd1074
     double obstacle_cost = obstacle_scorer_->scoreTrajectory(traj);
     ROS_WARN("Obstacle cost: %f", obstacle_cost);
     if (obstacle_cost < 0)
         return obstacle_cost;
     
     double px, py, pth;
     traj.getPoint(traj.getPointsSize()-1, px, py, pth);
     //ROS_WARN("tx=%f, dx=%f", px, desired_[0]);
     double d2 = (px-desired_[0])*(px-desired_[0]) + (py-desired_[1])*(py-desired_[1]);
     return obstacle_scale_*obstacle_cost + d2;
 }
double HeadingCostFunction::scoreTrajectory(base_local_planner::Trajectory &traj) 
{
  double dist,x,y,theta,near_dist=DBL_MAX,heading_diff=0.0,diff=0.0;
  int interval=traj.getPointsSize()/this->num_points;
  unsigned int near_index=0,i,j;

  for(j=0;j<this->num_points;j++)
  {
    traj.getPoint((j+1)*interval-1,x,y,theta);
    // find the nearrest point on the path
    for(i=1;i<this->global_plan.size();i++)
    {
      dist=sqrt((this->global_plan[i].pose.position.x-x)*(this->global_plan[i].pose.position.x-x)+
                (this->global_plan[i].pose.position.y-y)*(this->global_plan[i].pose.position.y-y));
      if(dist<near_dist)
      {
	near_dist=dist;
	near_index=i;
      }
    }
    double v1_x,v1_y;
    v1_x = this->global_plan[near_index].pose.position.x - this->global_plan[near_index-1].pose.position.x;
    v1_y = this->global_plan[near_index].pose.position.y - this->global_plan[near_index-1].pose.position.y;
    double v2_x = cos(theta);
    double v2_y = sin(theta);

    double perp_dot = v1_x * v2_y - v1_y * v2_x;
    double dot = v1_x * v2_x + v1_y * v2_y;

    //get the signed angle
    diff = fabs(atan2(perp_dot, dot));
    if(diff>(3.14159/2.0))
      diff=fabs(diff-3.14159);
    heading_diff+=diff;
  }

  return heading_diff;
}
/**
 * @param pos current position of robot
 * @param vel desired velocity for sampling
 */
bool SimpleTrajectoryGenerator::generateTrajectory(
      Eigen::Vector3f pos,
      Eigen::Vector3f vel,
      Eigen::Vector3f sample_target_vel,
      base_local_planner::Trajectory& traj) {
  double vmag = ::hypot(sample_target_vel[0], sample_target_vel[1]);
  double eps = 1e-4;
  traj.cost_   = -1.0; // placed here in case we return early
  //trajectory might be reused so we'll make sure to reset it
  traj.resetPoints();

  // make sure that the robot would at least be moving with one of
  // the required minimum velocities for translation and rotation (if set)
  if ((limits_->min_trans_vel >= 0 && vmag + eps < limits_->min_trans_vel) &&
      (limits_->min_rot_vel >= 0 && fabs(sample_target_vel[2]) + eps < limits_->min_rot_vel)) {
    return false;
  }
  // make sure we do not exceed max diagonal (x+y) translational velocity (if set)
  if (limits_->max_trans_vel >=0 && vmag - eps > limits_->max_trans_vel) {
    return false;
  }

  int num_steps;
  if (discretize_by_time_) {
    num_steps = ceil(sim_time_ / sim_granularity_);
  } else {
    //compute the number of steps we must take along this trajectory to be "safe"
    double sim_time_distance = vmag * sim_time_; // the distance the robot would travel in sim_time if it did not change velocity
    double sim_time_angle = fabs(sample_target_vel[2]) * sim_time_; // the angle the robot would rotate in sim_time
    num_steps =
        ceil(std::max(sim_time_distance / sim_granularity_,
            sim_time_angle    / angular_sim_granularity_));
  }

  //compute a timestep
  double dt = sim_time_ / num_steps;
  traj.time_delta_ = dt;

  Eigen::Vector3f loop_vel;
  if (continued_acceleration_) {
    // assuming the velocity of the first cycle is the one we want to store in the trajectory object
    loop_vel = computeNewVelocities(sample_target_vel, vel, limits_->getAccLimits(), dt);
    traj.xv_     = loop_vel[0];
    traj.yv_     = loop_vel[1];
    traj.thetav_ = loop_vel[2];
  } else {
    // assuming sample_vel is our target velocity within acc limits for one timestep
    loop_vel = sample_target_vel;
    traj.xv_     = sample_target_vel[0];
    traj.yv_     = sample_target_vel[1];
    traj.thetav_ = sample_target_vel[2];
  }

  //simulate the trajectory and check for collisions, updating costs along the way
  for (int i = 0; i < num_steps; ++i) {

    //add the point to the trajectory so we can draw it later if we want
    traj.addPoint(pos[0], pos[1], pos[2]);

    if (continued_acceleration_) {
      //calculate velocities
      loop_vel = computeNewVelocities(sample_target_vel, loop_vel, limits_->getAccLimits(), dt);
      //ROS_WARN_NAMED("Generator", "Flag: %d, Loop_Vel %f, %f, %f", continued_acceleration_, loop_vel[0], loop_vel[1], loop_vel[2]);
    }

    //update the position of the robot using the velocities passed in
    pos = computeNewPositions(pos, loop_vel, dt);

  } // end for simulation steps

  return num_steps > 0; // true if trajectory has at least one point
}
  void DWAPlanner::generateTrajectory(Eigen::Vector3f pos, const Eigen::Vector3f& vel, base_local_planner::Trajectory& traj, bool two_point_scoring, double sq_dist){
    //ROS_ERROR("%.2f, %.2f, %.2f - %.2f %.2f", vel[0], vel[1], vel[2], sim_time_, sim_granularity_);
    double impossible_cost = map_.map_.size();

    double vmag = sqrt(vel[0] * vel[0] + vel[1] * vel[1]);
    double eps = 1e-4;

    //make sure that the robot is at least moving with one of the required velocities
    if((vmag + eps < min_vel_trans_ && fabs(vel[2]) + eps < min_rot_vel_) ||
        vmag - eps > max_vel_trans_ ||
        oscillationCheck(vel)){
      traj.cost_ = -1.0;
      return;
    }

    //compute the number of steps we must take along this trajectory to be "safe"
    int num_steps = ceil(std::max((vmag * sim_time_) / sim_granularity_, fabs(vel[2]) / sim_granularity_));

    //compute a timestep
    double dt = sim_time_ / num_steps;
    double time = 0.0;

    //initialize the costs for the trajectory
    double path_dist = 0.0;
    double goal_dist = 0.0;
    double occ_cost = 0.0;

    //we'll also be scoring a point infront of the robot
    double front_path_dist = 0.0;
    double front_goal_dist = 0.0;

    //create a potential trajectory... it might be reused so we'll make sure to reset it
    traj.resetPoints();
    traj.xv_ = vel[0];
    traj.yv_ = vel[1];
    traj.thetav_ = vel[2];
    traj.cost_ = -1.0;

    //if we're not actualy going to simulate... we may as well just return now
    if(num_steps == 0){
      traj.cost_ = -1.0;
      return;
    }

    //we want to check against the absolute value of the velocities for collisions later
    //Eigen::Vector3f abs_vel = vel.array().abs();

    //simulate the trajectory and check for collisions, updating costs along the way
    for(int i = 0; i < num_steps; ++i){
      //get the mapp coordinates of a point
      unsigned int cell_x, cell_y;

      //we won't allow trajectories that go off the map... shouldn't happen that often anyways
      if(!costmap_.worldToMap(pos[0], pos[1], cell_x, cell_y)){
        //we're off the map
        traj.cost_ = -1.0;
        return;
      }

      double front_x = pos[0] + forward_point_distance_ * cos(pos[2]);
      double front_y = pos[1] + forward_point_distance_ * sin(pos[2]);

      unsigned int front_cell_x, front_cell_y;
      //we won't allow trajectories that go off the map... shouldn't happen that often anyways
      if(!costmap_.worldToMap(front_x, front_y, front_cell_x, front_cell_y)){
        //we're off the map
        traj.cost_ = -1.0;
        return;
      }


      //if we're over a certain speed threshold, we'll scale the robot's
      //footprint to make it either slow down or stay further from walls
      double scale = 1.0;
      if(vmag > scaling_speed_){
        //scale up to the max scaling factor linearly... this could be changed later
        double ratio = (vmag - scaling_speed_) / (max_vel_trans_ - scaling_speed_);
        scale = max_scaling_factor_ * ratio + 1.0;
      }

      //we want to find the cost of the footprint
      double footprint_cost = footprintCost(pos, scale);

      //if the footprint hits an obstacle... we'll check if we can stop before we hit it... given the time to get there
      if(footprint_cost < 0){
        traj.cost_ = -1.0;
        return;

        /* TODO: I'm not convinced this code is working properly
        //we want to compute the max allowable speeds to be able to stop
        //to be safe... we'll make sure we can stop some time before we actually hit
        Eigen::Vector3f max_vel = getMaxSpeedToStopInTime(time - stop_time_buffer_);

        //check if we can stop in time
        if(abs_vel[0] < max_vel[0] && abs_vel[1] < max_vel[1] && abs_vel[2] < max_vel[2]){
          //if we can, then we'll just break out of the loop here.. no point in checking future points
          break;
        }
        else{
          //if we can't... then this trajectory is invalid
          traj.cost_ = -1.0;
          return;
        }
        */
      }

      //compute the costs for this point on the trajectory
      occ_cost = std::max(std::max(occ_cost, footprint_cost), double(costmap_.getCost(cell_x, cell_y)));
      path_dist = map_(cell_x, cell_y).path_dist;
      goal_dist = map_(cell_x, cell_y).goal_dist;

      front_path_dist = front_map_(front_cell_x, front_cell_y).path_dist;
      front_goal_dist = front_map_(front_cell_x, front_cell_y).goal_dist;

      //if a point on this trajectory has no clear path to the goal... it is invalid
      if(impossible_cost <= goal_dist || impossible_cost <= path_dist){
        traj.cost_ = -2.0; //-2.0 means that we were blocked because propagation failed
        return;
      }

      //add the point to the trajectory so we can draw it later if we want
      traj.addPoint(pos[0], pos[1], pos[2]);

      //update the position of the robot using the velocities passed in
      pos = computeNewPositions(pos, vel, dt);
      time += dt;
    }

    double goal_heading = tf::getYaw(global_plan_.back().pose.orientation);
    
    double heading_diff = fabs(angles::shortest_angular_distance(pos[2],goal_heading));

    //ROS_ERROR("%.2f, %.2f", goal_heading, heading_diff);
    
    double resolution = costmap_.getResolution();

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

    
    double heading_scale_ = 0;
    if (sq_dist < squared_dist_for_rotating_) {
      heading_scale_ = heading_bias_;
    }
    //if we're not at the last point in the plan, then we can just score 
    if(two_point_scoring)
      traj.cost_ = pdist_scale_ * resolution * ((front_path_dist + path_dist) / 2.0) + gdist_scale_ * resolution * ((front_goal_dist + goal_dist) / 2.0) + occdist_scale_ * occ_cost + heading_scale_ * heading_diff;
    else
      traj.cost_ = pdist_scale_ * resolution * path_dist + gdist_scale_ * resolution * goal_dist + occdist_scale_ * occ_cost + heading_scale_ * heading_diff;
    //ROS_ERROR("%.2f, %.2f, %.2f, %.2f", vel[0], vel[1], vel[2], traj.cost_);
  }