Fix world to map coordinate conversion (#4506)

Signed-off-by: HovorunBh <[email protected]>

Author: HovorunBh

nav2_smac_planner/include/nav2_smac_planner/utils.hpp

@@ -46,9 +46,9 @@ inline geometry_msgs::msg::Pose getWorldCoords(
 {
   geometry_msgs::msg::Pose msg;
   msg.position.x =
-    static_cast<float>(costmap->getOriginX()) + (mx + 0.5) * costmap->getResolution();
+    static_cast<float>(costmap->getOriginX()) + (mx - 0.5) * costmap->getResolution();
   msg.position.y =
-    static_cast<float>(costmap->getOriginY()) + (my + 0.5) * costmap->getResolution();
+    static_cast<float>(costmap->getOriginY()) + (my - 0.5) * costmap->getResolution();
   return msg;
 }
 

nav2_smac_planner/test/test_utils.cpp

@@ -147,3 +147,17 @@ TEST(create_marker, test_createMarker)
   EXPECT_EQ(marker2.id, 8u);
   EXPECT_EQ(marker2.points.size(), 0u);
 }
+
+TEST(convert_map_to_world_to_map, test_convert_map_to_world_to_map)
+{
+  auto costmap = nav2_costmap_2d::Costmap2D(10.0, 10.0, 0.05, 0.0, 0.0, 0);
+
+  float mx = 200.0;
+  float my = 100.0;
+  geometry_msgs::msg::Pose pose = getWorldCoords(mx, my, &costmap);
+
+  float mx1, my1;
+  costmap.worldToMapContinuous(pose.position.x, pose.position.y, mx1, my1);
+  EXPECT_NEAR(mx, mx1, 1e-3);
+  EXPECT_NEAR(my, my1, 1e-3);
+}