Taken into account the worst case rotations of the footprint at the farthest...

Taken into account the worst case rotations of the footprint at the farthest point of the trajectory to check if it is fully contained in the local costmap.

src/ackermann_planner.cpp

@@ -251,11 +251,28 @@ 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);
-  while(planner_util_->get_world_model()->footprintCost(global_plan_[path_index].pose.position.x,global_plan_[path_index].pose.position.y,tf::getYaw(global_plan_[path_index].pose.orientation),footprint_spec)<0 && path_index>0)
-  {
-    if(path_index>0)
-      path_index--;
-  }
+  do{
+    if(planner_util_->get_world_model()->footprintCost(global_plan_[path_index].pose.position.x,global_plan_[path_index].pose.position.y,tf::getYaw(global_plan_[path_index].pose.orientation),footprint_spec)<0)
+    {
+      if(path_index>0)
+        path_index--;
+      continue;
+    }
+    else if(planner_util_->get_world_model()->footprintCost(global_plan_[path_index].pose.position.x,global_plan_[path_index].pose.position.y,tf::getYaw(global_plan_[path_index].pose.orientation)+1.5707,footprint_spec)<0)
+    {
+      if(path_index>0)
+        path_index--;
+      continue;
+    }
+    else if(planner_util_->get_world_model()->footprintCost(global_plan_[path_index].pose.position.x,global_plan_[path_index].pose.position.y,tf::getYaw(global_plan_[path_index].pose.orientation)-1.5707,footprint_spec)<0)
+    {
+      if(path_index>0)
+        path_index--;
+      continue;
+    }
+    else
+      break;
+  }while(path_index>0);
   if(path_index==0)
   {
     std::cout << "empty path" << std::endl;