Set start and goal as float (ros-navigation#4255)

* Set start and goal as float Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix worldToMapContinuous type Signed-off-by: Brice <brice.renaudeau@gmail.com> * add static_cast<float> Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix linting Signed-off-by: Brice <brice.renaudeau@gmail.com> * floor float to check start = goal Signed-off-by: Brice <brice.renaudeau@gmail.com> --------- Signed-off-by: Brice <brice.renaudeau@gmail.com>
quasi-robotics · Oct 22, 2024 · 2e11245 · 2e11245
1 parent b2d0b80
commit 2e11245
Show file tree

Hide file tree

Showing 7 changed files with 79 additions and 35 deletions.
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp
@@ -183,6 +183,16 @@ class Costmap2D
    */
   bool worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my) const;
 
+  /**
+   * @brief  Convert from world coordinates to map coordinates
+   * @param  wx The x world coordinate
+   * @param  wy The y world coordinate
+   * @param  mx Will be set to the associated map x coordinate
+   * @param  my Will be set to the associated map y coordinate
+   * @return True if the conversion was successful (legal bounds) false otherwise
+   */
+  bool worldToMapContinuous(double wx, double wy, float & mx, float & my) const;
+
   /**
    * @brief  Convert from world coordinates to map coordinates without checking for legal bounds
    * @param  wx The x world coordinate

diff --git a/nav2_costmap_2d/src/costmap_2d.cpp b/nav2_costmap_2d/src/costmap_2d.cpp
@@ -296,6 +296,21 @@ bool Costmap2D::worldToMap(double wx, double wy, unsigned int & mx, unsigned int
   return false;
 }
 
+bool Costmap2D::worldToMapContinuous(double wx, double wy, float & mx, float & my) const
+{
+  if (wx < origin_x_ || wy < origin_y_) {
+    return false;
+  }
+
+  mx = static_cast<float>((wx - origin_x_) / resolution_) + 0.5f;
+  my = static_cast<float>((wy - origin_y_) / resolution_) + 0.5f;
+
+  if (mx < size_x_ && my < size_y_) {
+    return true;
+  }
+  return false;
+}
+
 void Costmap2D::worldToMapNoBounds(double wx, double wy, int & mx, int & my) const
 {
   mx = static_cast<int>((wx - origin_x_) / resolution_);

diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp
@@ -129,8 +129,8 @@ class AStarAlgorithm
    * @param dim_3 The node dim_3 index of the goal
    */
   void setGoal(
-    const unsigned int & mx,
-    const unsigned int & my,
+    const float & mx,
+    const float & my,
     const unsigned int & dim_3);
 
   /**
@@ -140,8 +140,8 @@ class AStarAlgorithm
    * @param dim_3 The node dim_3 index of the goal
    */
   void setStart(
-    const unsigned int & mx,
-    const unsigned int & my,
+    const float & mx,
+    const float & my,
     const unsigned int & dim_3);
 
   /**

diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp
@@ -136,28 +136,32 @@ typename AStarAlgorithm<NodeT>::NodePtr AStarAlgorithm<NodeT>::addToGraph(
 
 template<>
 void AStarAlgorithm<Node2D>::setStart(
-  const unsigned int & mx,
-  const unsigned int & my,
+  const float & mx,
+  const float & my,
   const unsigned int & dim_3)
 {
   if (dim_3 != 0) {
     throw std::runtime_error("Node type Node2D cannot be given non-zero starting dim 3.");
   }
-  _start = addToGraph(Node2D::getIndex(mx, my, getSizeX()));
+  _start = addToGraph(
+    Node2D::getIndex(
+      static_cast<unsigned int>(mx),
+      static_cast<unsigned int>(my),
+      getSizeX()));
 }
 
 template<typename NodeT>
 void AStarAlgorithm<NodeT>::setStart(
-  const unsigned int & mx,
-  const unsigned int & my,
+  const float & mx,
+  const float & my,
   const unsigned int & dim_3)
 {
-  _start = addToGraph(NodeT::getIndex(mx, my, dim_3));
-  _start->setPose(
-    Coordinates(
-      static_cast<float>(mx),
-      static_cast<float>(my),
-      static_cast<float>(dim_3)));
+  _start = addToGraph(
+    NodeT::getIndex(
+      static_cast<unsigned int>(mx),
+      static_cast<unsigned int>(my),
+      dim_3));
+  _start->setPose(Coordinates(mx, my, dim_3));
 }
 
 template<>
@@ -186,30 +190,35 @@ void AStarAlgorithm<NodeT>::populateExpansionsLog(
 
 template<>
 void AStarAlgorithm<Node2D>::setGoal(
-  const unsigned int & mx,
-  const unsigned int & my,
+  const float & mx,
+  const float & my,
   const unsigned int & dim_3)
 {
   if (dim_3 != 0) {
     throw std::runtime_error("Node type Node2D cannot be given non-zero goal dim 3.");
   }
 
-  _goal = addToGraph(Node2D::getIndex(mx, my, getSizeX()));
+  _goal = addToGraph(
+    Node2D::getIndex(
+      static_cast<unsigned int>(mx),
+      static_cast<unsigned int>(my),
+      getSizeX()));
   _goal_coordinates = Node2D::Coordinates(mx, my);
 }
 
 template<typename NodeT>
 void AStarAlgorithm<NodeT>::setGoal(
-  const unsigned int & mx,
-  const unsigned int & my,
+  const float & mx,
+  const float & my,
   const unsigned int & dim_3)
 {
-  _goal = addToGraph(NodeT::getIndex(mx, my, dim_3));
+  _goal = addToGraph(
+    NodeT::getIndex(
+      static_cast<unsigned int>(mx),
+      static_cast<unsigned int>(my),
+      dim_3));
 
-  typename NodeT::Coordinates goal_coords(
-    static_cast<float>(mx),
-    static_cast<float>(my),
-    static_cast<float>(dim_3));
+  typename NodeT::Coordinates goal_coords(mx, my, dim_3);
 
   if (!_search_info.cache_obstacle_heuristic || goal_coords != _goal_coordinates) {
     if (!_start) {

diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp
@@ -215,16 +215,26 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan(
   _a_star->setCollisionChecker(&_collision_checker);
 
   // Set starting point
-  unsigned int mx_start, my_start, mx_goal, my_goal;
-  if (!costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx_start, my_start)) {
+  float mx_start, my_start, mx_goal, my_goal;
+  if (!costmap->worldToMapContinuous(
+      start.pose.position.x,
+      start.pose.position.y,
+      mx_start,
+      my_start))
+  {
     throw nav2_core::StartOutsideMapBounds(
             "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " +
             std::to_string(start.pose.position.y) + ") was outside bounds");
   }
   _a_star->setStart(mx_start, my_start, 0);
 
   // Set goal point
-  if (!costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal)) {
+  if (!costmap->worldToMapContinuous(
+      goal.pose.position.x,
+      goal.pose.position.y,
+      mx_goal,
+      my_goal))
+  {
     throw nav2_core::GoalOutsideMapBounds(
             "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " +
             std::to_string(goal.pose.position.y) + ") was outside bounds");
@@ -244,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

diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp
@@ -346,8 +346,8 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
   _a_star->setCollisionChecker(&_collision_checker);
 
   // Set starting point, in A* bin search coordinates
-  unsigned int mx, my;
-  if (!costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my)) {
+  float mx, my;
+  if (!costmap->worldToMapContinuous(start.pose.position.x, start.pose.position.y, mx, my)) {
     throw nav2_core::StartOutsideMapBounds(
             "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " +
             std::to_string(start.pose.position.y) + ") was outside bounds");
@@ -365,7 +365,7 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
   _a_star->setStart(mx, my, orientation_bin_id);
 
   // Set goal point, in A* bin search coordinates
-  if (!costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my)) {
+  if (!costmap->worldToMapContinuous(goal.pose.position.x, goal.pose.position.y, mx, my)) {
     throw nav2_core::GoalOutsideMapBounds(
             "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " +
             std::to_string(goal.pose.position.y) + ") was outside bounds");

diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp
@@ -281,8 +281,8 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan(
   _a_star->setCollisionChecker(&_collision_checker);
 
   // Set starting point, in A* bin search coordinates
-  unsigned int mx, my;
-  if (!_costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my)) {
+  float mx, my;
+  if (!_costmap->worldToMapContinuous(start.pose.position.x, start.pose.position.y, mx, my)) {
     throw nav2_core::StartOutsideMapBounds(
             "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " +
             std::to_string(start.pose.position.y) + ") was outside bounds");
@@ -292,7 +292,7 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan(
     NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(start.pose.orientation)));
 
   // Set goal point, in A* bin search coordinates
-  if (!_costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my)) {
+  if (!_costmap->worldToMapContinuous(goal.pose.position.x, goal.pose.position.y, mx, my)) {
     throw nav2_core::GoalOutsideMapBounds(
             "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " +
             std::to_string(goal.pose.position.y) + ") was outside bounds");