Path plan test #62
Path plan test #62
Conversation
Signed-off-by: Nano <nanoticity@gmail.com>
src/test/path_plan_test.cc
Outdated
| target->color = { 255, 0, 0 }; | ||
| openList.push(std::make_pair(start->g, start)); | ||
|
|
||
| std::vector<std::pair<int, int>> dirs = { {0,-1},{0,1},{-1,0},{1,0},{-1,-1},{-1,1},{1,-1},{1,1} }; |
There was a problem hiding this comment.
Fine for testing but should probably be a struct with x and y or row and col to avoid confusion
There was a problem hiding this comment.
ill fix it later I think when i'm cleaning up the code and refactoring it.
…ance. Signed-off-by: Nano <nanoticity@gmail.com>
Signed-off-by: Nano <nanoticity@gmail.com>
Signed-off-by: Ari <66433127+nanoticity@users.noreply.github.com>
Signed-off-by: Nano <nanoticity@gmail.com>
There was a problem hiding this comment.
Pull request overview
This PR adds a new path planning test that implements grid-based pathfinding with B-spline trajectory smoothing and visualization. The test loads a navigation grid from JSON, finds a path using Dijkstra's algorithm (mislabeled as BFS), smooths it with B-splines, and publishes the result to NetworkTables while displaying a visualization.
Key Changes
- Implements Dijkstra's shortest path algorithm with B-spline trajectory smoothing on a navigation grid
- Adds OpenCV-based visualization showing the grid, obstacles, path, and smoothed trajectory
- Integrates with NetworkTables to publish generated trajectories as Pose2d arrays
Reviewed changes
Copilot reviewed 3 out of 3 changed files in this pull request and generated 11 comments.
| File | Description |
|---|---|
| src/test/path_plan_test.cc | New test file implementing pathfinding, B-spline smoothing, and trajectory visualization with NetworkTables publishing |
| src/test/CMakeLists.txt | Adds build configuration for path_plan_test executable with required dependencies |
| constants/navgrid.json | Navigation grid data defining the field layout and obstacle positions |
| auto basisFunction(double i, double p, double t, | ||
| const std::vector<double>& knots) -> double { | ||
| int idx = static_cast<int>(i); | ||
| int deg = static_cast<int>(p); | ||
|
|
||
| if (deg == 0) { | ||
| if (p == 0) { | ||
| if (knots[i] <= t && t < knots[i + 1]) | ||
| return 1.0; | ||
|
|
||
| if (t == 0.0 && knots[i] == 0.0 && knots[i + 1] > 0.0) { | ||
| return 1.0; | ||
| } | ||
|
|
||
| if (t == 1.0 && knots[i + 1] == 1.0 && knots[i] < 1.0) { | ||
| return 1.0; | ||
| } | ||
|
|
||
| return 0.0; | ||
| } | ||
| } | ||
|
|
||
| double weight = 0.0; | ||
|
|
||
| double denom1 = knots[idx + deg] - knots[idx]; | ||
| if (denom1 != 0) { | ||
| weight += ((t - knots[idx]) / denom1) * basisFunction(i, p - 1, t, knots); | ||
| } | ||
| double denom2 = knots[idx + deg + 1] - knots[idx + 1]; | ||
| if (denom2 != 0) { | ||
| weight += ((knots[idx + deg + 1] - t) / denom2) * | ||
| basisFunction(i + 1, p - 1, t, knots); | ||
| } | ||
| return weight; |
There was a problem hiding this comment.
Missing bounds checking when accessing the knots array with indices like "knots[i]", "knots[i+1]", "knots[idx + deg]", and "knots[idx + deg + 1]". If the input parameters are incorrect or the knot vector is malformed, this could lead to out-of-bounds access. Add validation to ensure array accesses are within bounds.
| std::ifstream file("/bos/constants/navgrid.json"); | ||
| nlohmann::json data = nlohmann::json::parse(file); | ||
|
|
||
| const int GRID_W = data["grid"][0].size(); | ||
| const int GRID_H = data["grid"].size(); | ||
| const int CELL_SIZE = 20; | ||
| std::vector<std::vector<Node>> grid(GRID_H, std::vector<Node>(GRID_W)); |
There was a problem hiding this comment.
Global variables should not be initialized at file scope with file operations and complex initialization. The file parsing happens before main() executes, which means errors cannot be properly handled and the program may crash before reaching main. Move these initializations inside main() where errors can be caught and handled appropriately.
| while (!openList.empty()) { | ||
| std::pair<double, Node*> currentPair = openList.top(); | ||
| openList.pop(); | ||
| Node* current = currentPair.second; | ||
| if (current == target) | ||
| return; | ||
|
|
||
| for (const std::pair<int, int>& d : dirs) { | ||
| int nx = current->x + d.first; | ||
| int ny = current->y + d.second; | ||
| if (nx < 0 || nx >= GRID_W || ny < 0 || ny >= GRID_H) | ||
| continue; | ||
| Node* neighbor = &grid[ny][nx]; | ||
| if (!neighbor->walkable) | ||
| continue; | ||
| auto dx = static_cast<double>(d.first); | ||
| auto dy = static_cast<double>(d.second); | ||
| double cost = std::sqrt(dx * dx + dy * dy); | ||
| double tentativeG = current->g + cost; | ||
| if (tentativeG < neighbor->g) { | ||
| neighbor->g = tentativeG; | ||
| neighbor->parent = current; | ||
| openList.push(std::make_pair(neighbor->g, neighbor)); | ||
| } | ||
| } | ||
| } |
There was a problem hiding this comment.
The Dijkstra implementation can visit the same node multiple times because nodes are not marked as "visited" or "closed" after processing. A node can be added to the priority queue multiple times with different costs, leading to redundant processing. Add a check to skip nodes that have already been processed (e.g., track visited nodes in a set or use a flag in the Node class).
| return path; | ||
| } | ||
|
|
||
| void BFS(std::vector<std::vector<Node>>& grid, Node* start, Node* target) { |
There was a problem hiding this comment.
The function is named "BFS" but implements Dijkstra's algorithm (uniform cost search), not Breadth-First Search. BFS uses a simple queue without costs, while this implementation uses a priority queue with path costs. Rename this function to "dijkstra" or "uniformCostSearch" to accurately reflect the algorithm being used.
Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> Signed-off-by: Ari <66433127+nanoticity@users.noreply.github.com>
Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> Signed-off-by: Ari <66433127+nanoticity@users.noreply.github.com>
Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> Signed-off-by: Ari <66433127+nanoticity@users.noreply.github.com>
Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> Signed-off-by: Ari <66433127+nanoticity@users.noreply.github.com>
No description provided.