From 366b68d1d4ea4fcd198052fb6a5799f4dde37172 Mon Sep 17 00:00:00 2001 From: Brice Date: Fri, 12 Apr 2024 14:37:50 +0200 Subject: [PATCH] floor float to check start = goal --- nav2_smac_planner/src/smac_planner_2d.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index 27292837022..00cc07b464b 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -254,7 +254,7 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( pose.pose.orientation.w = 1.0; // Corner case of start and goal beeing on the same cell - if (mx_start == mx_goal && my_start == my_goal) { + if (std::floor(mx_start) == std::floor(mx_goal) && std::floor(my_start) == std::floor(my_goal)) { pose.pose = start.pose; // if we have a different start and goal orientation, set the unique path pose to the goal // orientation, unless use_final_approach_orientation=true where we need it to be the start