Skip to content

Smac Hybrid Planner reinitialization issues #2676

@zoltan-lengyel

Description

@zoltan-lengyel

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.

Metadata

Metadata

Assignees

No one assigned

    Labels

    No labels
    No labels

    Type

    No type

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions