From 1ff1303537dfaceaa953faff7530febace585f83 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 27 Mar 2024 17:33:42 -0700 Subject: [PATCH] throw exceptions if out of bounds (#4224) --- nav2_smac_planner/src/smac_planner_hybrid.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index c4c3019afc..08ae3b5e8d 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -287,7 +287,9 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( // Set starting point, in A* bin search coordinates unsigned int mx, my; - costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my); + if (!costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my)) { + throw std::runtime_error("Start pose is out of costmap!"); + } double orientation_bin = tf2::getYaw(start.pose.orientation) / _angle_bin_size; while (orientation_bin < 0.0) { orientation_bin += static_cast(_angle_quantizations); @@ -300,7 +302,9 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( _a_star->setStart(mx, my, orientation_bin_id); // Set goal point, in A* bin search coordinates - costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my); + if (!costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my)) { + throw std::runtime_error("Goal pose is out of costmap!"); + } orientation_bin = tf2::getYaw(goal.pose.orientation) / _angle_bin_size; while (orientation_bin < 0.0) { orientation_bin += static_cast(_angle_quantizations);