Used the farthest point of the path not outside of the costmap.

src/ackermann_planner.cpp

@@ -185,7 +185,6 @@ void AckermannPlanner::update_plan_and_local_costs(geometry_msgs::PoseStamped &g
 
   // costs for going away from path
   path_costs_.setTargetPoses(global_plan_);
-
   // costs for not going towards the local goal as much as possible
   goal_costs_.setTargetPoses(global_plan_);
 
@@ -197,6 +196,9 @@ void AckermannPlanner::update_plan_and_local_costs(geometry_msgs::PoseStamped &g
  */
 base_local_planner::Trajectory AckermannPlanner::find_best_path(geometry_msgs::PoseStamped &global_pose,ackermann_msgs::AckermannDrive &ackermann,geometry_msgs::PoseStamped& drive_velocities,std::vector<geometry_msgs::Point> footprint_spec) 
 {
+  unsigned int cell_x,cell_y,path_index=global_plan_.size()-1;
+  geometry_msgs::PoseStamped goal_pose;
+
   obstacle_costs_.setFootprint(footprint_spec);
 
   //make sure that our configuration doesn't change mid-run
@@ -204,7 +206,24 @@ base_local_planner::Trajectory AckermannPlanner::find_best_path(geometry_msgs::P
 
   Eigen::Vector3f pos(global_pose.pose.position.x, global_pose.pose.position.y, tf::getYaw(global_pose.pose.orientation));
   Eigen::Vector3f ack_state(ackermann.speed,ackermann.steering_angle,ackermann.steering_angle_velocity);
-  geometry_msgs::PoseStamped goal_pose = global_plan_.back();
+  while(!planner_util_->get_costmap()->worldToMap(global_plan_[path_index].pose.position.x,global_plan_[path_index].pose.position.y, cell_x, cell_y) && path_index>0)
+  {
+    if(path_index>0)
+      path_index--;
+  }
+  if(path_index==0)
+  {
+    drive_velocities.pose.position.x = 0;
+    drive_velocities.pose.position.y = 0;
+    drive_velocities.pose.position.z = 0;
+    drive_velocities.pose.orientation.w = 1;
+    drive_velocities.pose.orientation.x = 0;
+    drive_velocities.pose.orientation.y = 0;
+    drive_velocities.pose.orientation.z = 0;
+    return base_local_planner::Trajectory();
+  }
+  else
+    goal_pose = global_plan_[path_index];
   Eigen::Vector3f goal(goal_pose.pose.position.x, goal_pose.pose.position.y, tf::getYaw(goal_pose.pose.orientation));
 
   // prepare cost functions and generators for this run