Fixed vector::reserve exception in smac planner due to precision error (

#2563) - Related issue: #2547
ros-navigation · Sep 15, 2021 · 2eb6e0f · 2eb6e0f
1 parent 0e59580
commit 2eb6e0f
Show file tree

Hide file tree

Showing 3 changed files with 15 additions and 7 deletions.
diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp
@@ -575,12 +575,12 @@ typename AStarAlgorithm<NodeT>::AnalyticExpansionNodes AStarAlgorithm<NodeT>::ge
     node->motion_table.state_space->interpolate(from(), to(), i / num_intervals, s());
     reals = s.reals();
     angle = reals[2] / node->motion_table.bin_size;
-    while (angle >= node->motion_table.num_angle_quantization_float) {
-      angle -= node->motion_table.num_angle_quantization_float;
-    }
     while (angle < 0.0) {
       angle += node->motion_table.num_angle_quantization_float;
     }
+    while (angle >= node->motion_table.num_angle_quantization_float) {
+      angle -= node->motion_table.num_angle_quantization_float;
+    }
     // Turn the pose into a node, and check if it is valid
     index = NodeT::getIndex(
       static_cast<unsigned int>(reals[0]),

diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp
@@ -230,14 +230,14 @@ MotionPoses HybridMotionTable::getProjections(const NodeHybrid * node)
     const float & node_heading = node->pose.theta;
     float new_heading = node_heading + motion_model._theta;
 
-    if (new_heading >= num_angle_quantization_float) {
-      new_heading -= num_angle_quantization_float;
-    }
-
     if (new_heading < 0.0) {
       new_heading += num_angle_quantization_float;
     }
 
+    if (new_heading >= num_angle_quantization_float) {
+      new_heading -= num_angle_quantization_float;
+    }
+
     projection_list.emplace_back(
       delta_xs[i][node_heading] + node->pose.x,
       delta_ys[i][node_heading] + node->pose.y,

diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp
@@ -263,6 +263,10 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
   while (orientation_bin < 0.0) {
     orientation_bin += static_cast<float>(_angle_quantizations);
   }
+  // This is needed to handle precision issues
+  if (orientation_bin >= static_cast<float>(_angle_quantizations)) {
+    orientation_bin -= static_cast<float>(_angle_quantizations);
+  }
   unsigned int orientation_bin_id = static_cast<unsigned int>(floor(orientation_bin));
   _a_star->setStart(mx, my, orientation_bin_id);
 
@@ -272,6 +276,10 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
   while (orientation_bin < 0.0) {
     orientation_bin += static_cast<float>(_angle_quantizations);
   }
+  // This is needed to handle precision issues
+  if (orientation_bin >= static_cast<float>(_angle_quantizations)) {
+    orientation_bin -= static_cast<float>(_angle_quantizations);
+  }
   orientation_bin_id = static_cast<unsigned int>(floor(orientation_bin));
   _a_star->setGoal(mx, my, orientation_bin_id);