diff --git a/src/ackermann_planner.cpp b/src/ackermann_planner.cpp index f4c335a3775e027ec6c4ecada0dc983644cd4806..f1b6846fbdf57f15e6884210eb7eaff7497f7a17 100644 --- a/src/ackermann_planner.cpp +++ b/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;