Fix State-Lattice planner crashes due to FP precision loss

Author: AlexeyMerzlyakov

nav2_smac_planner/src/node_lattice.cpp

@@ -223,7 +223,6 @@ bool NodeLattice::isNodeValid(
 
  // If valid motion primitives are set, check intermediary poses > 1 cell apart
  if (motion_primitive) {
- const float pi_2 = 2.0 * M_PI;
  const float & grid_resolution = motion_table.lattice_metadata.grid_resolution;
  const float & resolution_diag_sq = 2.0 * grid_resolution * grid_resolution;
  MotionPose last_pose(1e9, 1e9, 1e9), pose_dist(0.0, 0.0, 0.0);
@@ -246,7 +245,7 @@ bool NodeLattice::isNodeValid(
  // If reversing, invert the angle because the robot is backing into the primitive
  // not driving forward with it
  if (is_backwards) {
- prim_pose._theta = std::fmod(it->_theta + M_PI, pi_2);
+ prim_pose._theta = std::fmod(it->_theta + M_PI, 2.0 * M_PI);
  } else {
  prim_pose._theta = it->_theta;
  }
@@ -561,7 +560,7 @@ void NodeLattice::addNodeToPath(
  Coordinates initial_pose, prim_pose;
  MotionPrimitive * prim = nullptr;
  const float & grid_resolution = NodeLattice::motion_table.lattice_metadata.grid_resolution;
- const float pi_2 = 2.0 * M_PI;
+ const float epsilon = std::numeric_limits<float>::epsilon();
  prim = current_node->getMotionPrimitive();
  // if motion primitive is valid, then was searched (rather than analytically expanded),
  // include dense path of subpoints making up the primitive at grid resolution
@@ -577,7 +576,7 @@ void NodeLattice::addNodeToPath(
  // If reversing, invert the angle because the robot is backing into the primitive
  // not driving forward with it
  if (current_node->isBackward()) {
- prim_pose.theta = std::fmod(it->_theta + M_PI, pi_2);
+ prim_pose.theta = std::fmod(it->_theta + M_PI, 2.0 * M_PI);
  } else {
  prim_pose.theta = it->_theta;
  }

nav2_smac_planner/test/test_nodelattice.cpp

@@ -288,3 +288,70 @@ TEST(NodeLatticeTest, test_get_neighbors)
 
  delete costmapA;
 }
+
+TEST(NodeLatticeTest, test_node_lattice_custom_footprint)
+{
+ std::string pkg_share_dir = ament_index_cpp::get_package_share_directory("nav2_smac_planner");
+ std::string filePath =
+ pkg_share_dir +
+ "/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann" +
+ "/output.json";
+
+ nav2_smac_planner::SearchInfo info;
+ info.minimum_turning_radius = 0.5;
+ info.non_straight_penalty = 1;
+ info.change_penalty = 1;
+ info.reverse_penalty = 1;
+ info.cost_penalty = 1;
+ info.retrospective_penalty = 0.1;
+ info.analytic_expansion_ratio = 1;
+ info.lattice_filepath = filePath;
+ info.cache_obstacle_heuristic = true;
+ info.allow_reverse_expansion = true;
+
+ unsigned int x = 100;
+ unsigned int y = 100;
+ unsigned int angle_quantization = 16;
+
+ nav2_smac_planner::NodeLattice::initMotionModel(
+ nav2_smac_planner::MotionModel::STATE_LATTICE, x, y, angle_quantization, info);
+
+ nav2_smac_planner::NodeLattice node(49);
+
+ nav2_costmap_2d::Costmap2D * costmap = new nav2_costmap_2d::Costmap2D(
+ 40, 40, 0.05, 0.0, 0.0, 0);
+ std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
+ std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmap, 72);
+
+ // Make some custom asymmetrical footprint
+ nav2_costmap_2d::Footprint footprint;
+ geometry_msgs::msg::Point p;
+ p.x = -0.1;
+ p.y = -0.15;
+ footprint.push_back(p);
+ p.x = 0.35;
+ p.y = -0.15;
+ footprint.push_back(p);
+ p.x = 0.35;
+ p.y = 0.22;
+ footprint.push_back(p);
+ p.x = -0.1;
+ p.y = 0.22;
+ footprint.push_back(p);
+ checker->setFootprint(footprint, false, 0.0);
+
+ // Test that the node is valid though all motion primitives poses for custom footprint
+ // Setting initial robot pose to (1.0, 1.0, 0.0)
+ node.pose.x = 20;
+ node.pose.y = 20;
+ node.pose.theta = 0;
+ nav2_smac_planner::MotionPrimitivePtrs motion_primitives =
+ nav2_smac_planner::NodeLattice::motion_table.getMotionPrimitives(&node);
+ EXPECT_GT(motion_primitives.size(), 0);
+ for (unsigned int i = 0; i < motion_primitives.size(); i++) {
+ EXPECT_EQ(node.isNodeValid(true, checker.get(), motion_primitives[i], false), true);
+ EXPECT_EQ(node.isNodeValid(true, checker.get(), motion_primitives[i], true), true);
+ }
+
+ delete costmap;
+}