From 670eaf1efae1e85c710d57fbe3d3ca2df7318d0a Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 29 May 2025 17:07:16 -0400 Subject: [PATCH] obs --- src/modelec_strat/src/missions/test.go_home_mission.cpp | 4 ++-- src/modelec_strat/src/pathfinding.cpp | 1 + 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modelec_strat/src/missions/test.go_home_mission.cpp b/src/modelec_strat/src/missions/test.go_home_mission.cpp index def9a7b..c81c683 100644 --- a/src/modelec_strat/src/missions/test.go_home_mission.cpp +++ b/src/modelec_strat/src/missions/test.go_home_mission.cpp @@ -20,7 +20,7 @@ namespace Modelec auto curr = nav_->GetCurrentPos(); RCLCPP_INFO(node_->get_logger(), "GoHomeMission: Starting at position (%f, %f)", curr->x, curr->y); - nav_->GoTo(375, 1600, -M_PI_2, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE); + nav_->GoTo(375, 1900, -M_PI_2, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE); go_home_time_ = node_->now(); @@ -43,7 +43,7 @@ namespace Modelec case AWAIT_10S: { - nav_->GoTo(370, 1400, -M_PI_2, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE); + nav_->GoTo(370, 1600, -M_PI_2, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE); step_ = GO_HOME; } diff --git a/src/modelec_strat/src/pathfinding.cpp b/src/modelec_strat/src/pathfinding.cpp index 49b4214..06f9628 100644 --- a/src/modelec_strat/src/pathfinding.cpp +++ b/src/modelec_strat/src/pathfinding.cpp @@ -301,6 +301,7 @@ namespace Modelec 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); return {-2, waypoints}; }