diff --git a/planning/obstacle_avoidance_planner/src/utils/utils.cpp b/planning/obstacle_avoidance_planner/src/utils/utils.cpp index bb74a3e57677c..892c844610283 100644 --- a/planning/obstacle_avoidance_planner/src/utils/utils.cpp +++ b/planning/obstacle_avoidance_planner/src/utils/utils.cpp @@ -661,18 +661,18 @@ bg_polygon createDrivablePolygon( { bg_polygon drivable_area_poly; - // right bound - for (const auto rp : right_bound) { - drivable_area_poly.outer().push_back(bg_point(rp.x, rp.y)); - } - // left bound - auto reversed_left_bound = left_bound; - std::reverse(reversed_left_bound.begin(), reversed_left_bound.end()); - for (const auto lp : reversed_left_bound) { + for (const auto lp : left_bound) { drivable_area_poly.outer().push_back(bg_point(lp.x, lp.y)); } + // right bound + auto reversed_right_bound = right_bound; + std::reverse(reversed_right_bound.begin(), reversed_right_bound.end()); + for (const auto rp : reversed_right_bound) { + drivable_area_poly.outer().push_back(bg_point(rp.x, rp.y)); + } + drivable_area_poly.outer().push_back(drivable_area_poly.outer().front()); return drivable_area_poly; }