Fix incorrect world <-> map coordinates conversions (#5049) (#5074)

* Fix incorrect world <-> map coordinates conversions

The conversion between world and map continuous (!) coordinates do not
require a +/-0.5. This offset is only required when converting discrete
map cell indexes to the coordinates of its center.

Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be>

* nav2_smac_planner: fix smoother test when path is in collision

The added pose was indeed invalid, but the it was ignored by the
smoother, since considered as a cusp.

Instead, let's make the end of the plan invalid as it won't be ignored.
Also, let's duplicate the last pose to make the orientation estimation
fail, rather than adding a new arbitrary pose.

Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be>

---------

Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be>
(cherry picked from commit 3ec7c33)

Co-authored-by: DylanDeCoeyer-Quimesis <102609575+DylanDeCoeyer-Quimesis@users.noreply.github.com>

Author: mergify[bot]

nav2_costmap_2d/src/costmap_2d.cpp

@@ -303,8 +303,8 @@ bool Costmap2D::worldToMapContinuous(double wx, double wy, float & mx, float & m
     return false;
   }
 
-  mx = static_cast<float>((wx - origin_x_) / resolution_) + 0.5f;
-  my = static_cast<float>((wy - origin_y_) / resolution_) + 0.5f;
+  mx = static_cast<float>((wx - origin_x_) / resolution_);
+  my = static_cast<float>((wy - origin_y_) / resolution_);
 
   if (mx < size_x_ && my < size_y_) {
     return true;

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 * costmap->getResolution();
   msg.position.y =
-    static_cast<float>(costmap->getOriginY()) + (my - 0.5) * costmap->getResolution();
+    static_cast<float>(costmap->getOriginY()) + my * costmap->getResolution();
   return msg;
 }
 

nav2_smac_planner/test/test_smoother.cpp

@@ -176,12 +176,19 @@ TEST(SmootherTest, test_full_smoother)
   double max_no_time = 0.0;
   EXPECT_FALSE(smoother->smooth(plan, costmap, max_no_time));
 
-  // Failure mode: Path is in collision, do 2x to exercise overlapping point
-  // attempts to update orientation should also fail
-  pose.pose.position.x = 1.25;
-  pose.pose.position.y = 1.25;
-  plan.poses.push_back(pose);
-  plan.poses.push_back(pose);
+  // Failure mode: invalid path and invalid orientation
+  // make the end of the path invalid
+  geometry_msgs::msg::PoseStamped lastPose = plan.poses.back();
+  unsigned int mx, my;
+  costmap->worldToMap(lastPose.pose.position.x, lastPose.pose.position.y, mx, my);
+  for (unsigned int i = mx - 5; i <= mx + 5; ++i) {
+    for (unsigned int j = my - 5; j <= my + 5; ++j) {
+      costmap->setCost(i, j, 254);
+    }
+  }
+
+  // duplicate last pose to make the orientation update fail
+  plan.poses.push_back(lastPose);
   EXPECT_FALSE(smoother->smooth(plan, costmap, maxtime));
   EXPECT_NEAR(plan.poses.end()[-2].pose.orientation.z, 1.0, 1e-3);
   EXPECT_NEAR(plan.poses.end()[-2].pose.orientation.x, 0.0, 1e-3);