Bug report

Required Info:

• Operating System:
  • Debian 10
• ROS2 Version:
  • rolling
• Version or commit hash:
  • a136239 (current nav2 main)
• DDS implementation:
  • Fast-RTPS

Steps to reproduce issue

Start tb3 simulation with ros2 launch nav2_bringup tb3_simulation_launch.py. Replace planner_server parameters in nav2_params with the ones suggested in
https://github.com/ros-planning/navigation2/blob/d3fc28af28df764a153c85434f8b30270e963622/smac_planner/README.md#parameters
Set multiple waypoints and look at the memory usage of your system. If your used memory suddenly goes to 100% in a matter of about 15 seconds when reaching a waypoint look for the error message described below.

Expected behavior

Planner reaches one waypoint after the other and then plans to the next one.

Actual behavior

Sometimes when switching from one waypoint to the next, the planner gets stuck in an infinite loop during the backtrace function.
https://github.com/ros-planning/navigation2/blob/a13623922bc93ec8f12cdd40d2a817ea54998aaf/smac_planner/src/a_star.cpp#L290-L291.
https://github.com/ros-planning/navigation2/blob/a13623922bc93ec8f12cdd40d2a817ea54998aaf/smac_planner/src/a_star.cpp#L448-L463
It keeps backtracing until it runs out of memory and then crashes with a std::bad_alloc.

Additional information

Error output

[controller_server-8] [INFO] [1602599767.854334277] [controller_server]: Passing new path to controller.
[controller_server-8] [INFO] [1602599769.362043128] [controller_server]: Reached the goal!
[bt_navigator-11] [INFO] [1602599769.387884280] [bt_navigator]: Navigation succeeded
[waypoint_follower-12] [INFO] [1602599770.074093025] [waypoint_follower]: Succeeded processing waypoint 0, processing waypoint task execution
[waypoint_follower-12] [INFO] [1602599770.074309872] [waypoint_follower]: Task execution at waypoint 0 succeeded
[waypoint_follower-12] [INFO] [1602599770.074368300] [waypoint_follower]: Handled task execution on waypoint 0, moving to next.
[bt_navigator-11] [INFO] [1602599770.125854408] [bt_navigator]: Begin navigating from current location to (-1.74, -0.06)
[planner_server-9] [WARN] [1602599786.702648352] [planner_server]: GridBased plugin failed to plan calculation to (1.91, -0.09): "std::bad_alloc"
[planner_server-9] [WARN] [1602599786.705805908] [planner_server_rclcpp_node]: [compute_path_to_pose] [ActionServer] Client requested to cancel the goal. Cancelling.
[planner_server-9] It took 3.68827 milliseconds with 396 iterations.
[controller_server-8] [INFO] [1602599786.750957954] [controller_server]: Received a goal, begin computing control effort.

asan Backtrace

Debug Output

I printed the current node index and path size in each while loop of the backtrace. It seems to get stuck in a loop of only 3 nodes.

   while (current_node->parent) {
     path.push_back(current_node->pose);
     current_node = current_node->parent;
     std::cout << "Current Node = "  << current_node->getIndex() << " Path size =  " << path.size() << std::endl;
   }

It took 118.231 milliseconds with 1 iterations.
Current Node = 20651835 Path size = 1
Current Node = 20597980 Path size = 2
Current Node = 20597907 Path size = 3
Current Node = 20544050 Path size = 4
Current Node = 20597834 Path size = 5
It took 113.746 milliseconds with 1 iterations.
Current Node = 20651835 Path size = 1
Current Node = 20651764 Path size = 2
Current Node = 20597907 Path size = 3
Current Node = 20597835 Path size = 4
It took 137.276 milliseconds with 1 iterations.
Current Node = 20651834 Path size = 1
Current Node = 20651761 Path size = 2
Current Node = 20651690 Path size = 3
Current Node = 20651619 Path size = 4
It took 127.842 milliseconds with 1 iterations.
Current Node = 20651834 Path size = 1
Current Node = 20651691 Path size = 2
It took 134.659 milliseconds with 1 iterations.
(Here it reaches one waypoint and starts planning to the next one)
Current Node = 20651835 Path size = 1
Current Node = 20651763 Path size = 2
Current Node = 20597906 Path size = 3
Current Node = 20597834 Path size = 4
Current Node = 20651763 Path size = 5
Current Node = 20597906 Path size = 6
Current Node = 20597834 Path size = 7
Current Node = 20651763 Path size = 8
Current Node = 20597906 Path size = 9
Current Node = 20597834 Path size = 10
Current Node = 20651763 Path size = 11
Current Node = 20597906 Path size = 12
Current Node = 20597834 Path size = 13
Current Node = 20651763 Path size = 14
Current Node = 20597906 Path size = 15
Current Node = 20597834 Path size = 16
Current Node = 20651763 Path size = 17
Current Node = 20597906 Path size = 18
Current Node = 20597834 Path size = 19
Current Node = 20651763 Path size = 20
Current Node = 20597906 Path size = 21
Current Node = 20597834 Path size = 22
Current Node = 20651763 Path size = 23
Current Node = 20597906 Path size = 24
...