Fix world to map coordinate conversion (ros-navigation#4506)

Signed-off-by: HovorunBh <[email protected]>
quasi-robotics · Oct 23, 2024 · bcd1324 · bcd1324
1 parent 554b012
commit bcd1324
Showing 1 changed file with 5 additions and 0 deletions.
diff --git a/nav2_smac_planner/test/test_utils.cpp b/nav2_smac_planner/test/test_utils.cpp
@@ -155,4 +155,9 @@ TEST(convert_map_to_world_to_map, test_convert_map_to_world_to_map)
   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);
 }