-
Notifications
You must be signed in to change notification settings - Fork 1.8k
Description
I have a unit test setup for the Smac Hybrid Planner that works without starting ROS nodes.
I noticed that I have different test results when I run only a single test, instead of running all the test cases together.
I investigated the issue, and I found some problems in the nav2_smac_planner::HybridMotionTable initialization.
My test cases only affected each other, because the motion_table is a static variable in the NodeHybrid class.
There is no problem of course if the program (node) is restarted every time, but the HybridMotionTable initialization code looks like it wants to support reinitialization, but doesn't do it correctly.
Issues
node_hybrid.cpp, HybridMotionTable::initDubin and HybridMotionTable::initReedsShepp
// if nothing changed, no need to re-compute primitives
if (num_angle_quantization_in == num_angle_quantization &&
min_turning_radius == search_info.minimum_turning_radius)
{
return;
}
I believe this check is wrong. It skips the primitive setup when switching between DUBIN and REEDS_SHEPP motion models without changing anything else. This same check is present in both motion model init functions.
node_hybrid.cpp, HybridMotionTable::initDubin
// Create the correct OMPL state space
if (!state_space) {
state_space = std::make_unique<ompl::base::DubinsStateSpace>(min_turning_radius);
}
node_hybrid.cpp, HybridMotionTable::initReedsShepp
// Create the correct OMPL state space
if (!state_space) {
state_space = std::make_unique<ompl::base::ReedsSheppStateSpace>(
min_turning_radius);
}
These checks will skip the state_space reinitialization in case the motion_model or the min_turning_radius is changed.
Changes
I fixed the issue on my side by completely removing these checks, because I don't mind the slower reinitialization. Otherwise, for more proper checks the motion table should know its current motion model.