fix(intersection): fix infinite loop in tsort

[soblin]

Description

If detection lanelets have a loop, topological sort runs into infinite loops. I fixed this by checking visited nodes.

Related links

Tests performed

with a map that causes loop detection lanelets due to improper RightOfWay setting.

Notes for reviewers

Interface changes

none.

Effects on system behavior

none.

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

• I've confirmed the contribution guidelines.
• The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

• The PR follows the pull request guidelines.
• The PR has been properly tested.
• The PR has been reviewed by the code owners.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

• There are no open discussions or they are tracked via tickets.
• The PR is ready for merge.

After all checkboxes are checked, anyone who has write access can merge the PR.

[codecov]

Codecov Report

Attention: 211 lines in your changes are missing coverage. Please review.

Comparison is base (4383d6b) 14.81% compared to head (8e05ebf) 14.81%.
Report is 22 commits behind head on main.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #5332      +/-   ##
==========================================
- Coverage   14.81%   14.81%   -0.01%     
==========================================
  Files        1665     1665              
  Lines      115176   115237      +61     
  Branches    35519    35553      +34     
==========================================
  Hits        17069    17069              
- Misses      78869    78926      +57     
- Partials    19238    19242       +4     

Flag	Coverage Δ		*Carryforward flag
differential	5.83% <0.00%> (?)
total	14.83% <ø> (+0.01%)	⬆️	Carriedforward from 4383d6b

*This pull request uses carry forward flags. Click here to find out more.

Files	Coverage Δ
...ity_intersection_module/src/scene_intersection.hpp	0.00% <ø> (ø)
...ehavior_velocity_intersection_module/src/debug.cpp	0.00% <0.00%> (ø)
...ior_velocity_intersection_module/src/util_type.hpp	0.00% <0.00%> (ø)
...avior_velocity_intersection_module/src/manager.cpp	14.66% <0.00%> (-0.23%)	⬇️
...ity_intersection_module/src/scene_intersection.cpp	0.00% <0.00%> (ø)
...behavior_velocity_intersection_module/src/util.cpp	0.00% <0.00%> (ø)

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.