Escape obstacle fix (#2318)

* Inital version fix * Fixing issue * Fix Code Style On escape-obstacle-fix (#2319) automated style fixes Co-authored-by: rishiso <[email protected]> --------- Co-authored-by: Obi Nwosu <[email protected]> Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> Co-authored-by: rishiso <[email protected]>
RoboJackets · Jan 20, 2025 · cf4a280 · cf4a280
1 parent 576ad00
commit cf4a280
Showing 1 changed file with 8 additions and 3 deletions.
diff --git a/soccer/src/soccer/planning/planner/escape_obstacles_path_planner.cpp b/soccer/src/soccer/planning/planner/escape_obstacles_path_planner.cpp
@@ -37,8 +37,14 @@ Trajectory EscapeObstaclesPathPlanner::plan(const PlanRequest& plan_request) {
     std::optional<Point> opt_prev_pt;
 
     LinearMotionInstant goal{unblocked, Point()};
-    auto result = CreatePath::simple(start_instant.linear_motion(), goal, motion_constraints,
-                                     start_instant.stamp);
+
+    rj_geometry::ShapeSet path_obstacles;
+    rj_geometry::Circle ball{plan_request.world_state->ball.position, kBallRadius};
+    path_obstacles.add(std::make_shared<rj_geometry::Circle>(ball));
+
+    auto result = CreatePath::intermediate(start_instant.linear_motion(), goal, motion_constraints,
+                                           start_instant.stamp, path_obstacles, {},
+                                           plan_request.field_dimensions, plan_request.shell_id);
     plan_angles(&result, start_instant, AngleFns::tangent, plan_request.constraints.rot);
     result.set_debug_text("[ESCAPE " + std::to_string(plan_request.shell_id) + "]");
 
@@ -85,7 +91,6 @@ Point EscapeObstaclesPathPlanner::find_non_blocked_goal(Point goal, std::optiona
             return *prev_goal;
         }
     }
-
     return goal;
 }