Taken into account the circumscrived radius of the footprint to check for the...

Taken into account the circumscrived radius of the footprint to check for the local goal on the path.

src/ackermann_planner.cpp

@@ -241,8 +241,10 @@ 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;
+  std::vector<geometry_msgs::Point> circular_footprint;
   geometry_msgs::PoseStamped goal_pose;
+  double min_radius,max_radius;
+  unsigned int path_index=global_plan_.size()-1;
 
   obstacle_costs_.setFootprint(footprint_spec);
 
@@ -251,20 +253,16 @@ 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);
+  costmap_2d::calculateMinAndMaxDistances(footprint_spec,min_radius,max_radius);
+  for(double angle=0;angle<2*3.14159;angle+=0.1)
+  {
+    geometry_msgs::Point new_point;
+    new_point.x=max_radius*cos(angle);
+    new_point.y=max_radius*sin(angle);
+    circular_footprint.push_back(new_point);
+  }
   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(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),circular_footprint)<0)
     {
       if(path_index>0)
         path_index--;