From 638077ee7454adb76c6ba416c1398caa1aac12b8 Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 29 May 2025 17:26:20 -0400 Subject: [PATCH] log --- src/modelec_strat/src/pathfinding.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modelec_strat/src/pathfinding.cpp b/src/modelec_strat/src/pathfinding.cpp index 559564b..d4c518d 100644 --- a/src/modelec_strat/src/pathfinding.cpp +++ b/src/modelec_strat/src/pathfinding.cpp @@ -296,12 +296,13 @@ namespace Modelec const double goal_x = goal->x / cell_size_mm_x; const double goal_y = (grid_height_ - 1) - (goal->y / cell_size_mm_y); + RCLCPP_INFO(node_->get_logger(), "Start: (%f, %f), Goal: (%f, %f)", start_x, start_y, goal_x, goal_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: (%f, %f), Goal: (%f, %f)", start_x, start_y, goal_x, goal_y); return {-2, waypoints}; }