| 46 | } |
| 47 | |
| 48 | std::vector<Point> AStarPathPlanner::Plan(const Point & start, const Point & goal) |
| 49 | { |
| 50 | if (IsPointInCollision(goal)) { |
| 51 | RCLCPP_ERROR(logger_, "Provided goal position would cause a collision"); |
| 52 | return {}; |
| 53 | } |
| 54 | |
| 55 | if (IsPointInCollision(start)) { |
| 56 | RCLCPP_ERROR( |
| 57 | logger_, "Starting position (%f, %f) is currently in a collision", |
| 58 | start.x(), start.y()); |
| 59 | return {}; |
| 60 | } |
| 61 | |
| 62 | expanded_.clear(); |
| 63 | frontier_ = FrontierQueue{}; |
| 64 | |
| 65 | goal_ = goal; |
| 66 | |
| 67 | // BEGIN STUDENT CODE |
| 68 | |
| 69 | return {}; |
| 70 | // END STUDENT CODE |
| 71 | } |
| 72 | |
| 73 | void AStarPathPlanner::ExtendPathAndAddToFrontier( |
| 74 | const std::vector<Point> & path, const double & path_cost, |