Skip to content

Fix State-Lattice planner crashes due to FP precision loss #3531

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 2 commits into from
Apr 12, 2023
Merged

Fix State-Lattice planner crashes due to FP precision loss #3531

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
c7b62bc
Fix State-Lattice planner crashes due to FP precision loss
AlexeyMerzlyakov Apr 12, 2023
0b4e732
Move testcase comment
AlexeyMerzlyakov Apr 12, 2023
File filter

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 2 additions & 4 deletions nav2_smac_planner/src/node_lattice.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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;
}
Expand Down Expand Up @@ -561,7 +560,6 @@ 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;
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
Expand All @@ -577,7 +575,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;
}
Expand Down
67 changes: 67 additions & 0 deletions nav2_smac_planner/test/test_nodelattice.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);

// Setting initial robot pose to (1.0, 1.0, 0.0)
node.pose.x = 20;
node.pose.y = 20;
node.pose.theta = 0;
// Test that the node is valid though all motion primitives poses for custom footprint
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;
}