floor float to check start = goal

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