modelec/modelec-marcel-ROS
1
0
Fork 0
mirror of https://github.com/modelec/modelec-marcel-ROS.git synced 2026-01-18 16:47:43 +01:00
Code Issues Packages Projects Releases Wiki Activity

log

Browse Source
This commit is contained in:
acki
2025-05-29 17:23:42 -04:00
parent 86b299e3c3
commit 031fa82a0f
1 changed files with 5 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
src/modelec_strat/src/pathfinding.cpp
View File

@@ -291,17 +291,17 @@ namespace Modelec
}
// 3. Convert start and goal (avec inversion Y)
const int start_x = start->x / cell_size_mm_x;
const int start_y = (grid_height_ - 1) - (start->y / cell_size_mm_y);
const int goal_x = goal->x / cell_size_mm_x;
const int goal_y = (grid_height_ - 1) - (goal->y / cell_size_mm_y);
const double start_x = start->x / cell_size_mm_x;
const double start_y = (grid_height_ - 1) - (start->y / cell_size_mm_y);
const double goal_x = goal->x / cell_size_mm_x;
const double goal_y = (grid_height_ - 1) - (goal->y / cell_size_mm_y);
if (start_x < 0 || start_y < 0 || goal_x < 0 || goal_y < 0 ||
start_x >= grid_width_ || start_y >= grid_height_ ||
goal_x >= grid_width_ || goal_y >= grid_height_)
{
RCLCPP_WARN(node_->get_logger(), "Start or Goal out of bounds");
RCLCPP_WARN(node_->get_logger(), "Start: (%d, %d), Goal: (%d, %d)", start_x, start_y, goal_x, goal_y);
RCLCPP_WARN(node_->get_logger(), "Start: (%f, %f), Goal: (%f, %f)", start_x, start_y, goal_x, goal_y);
return {-2, waypoints};
}