Open robomic opened 1 month ago
Explaination
HdMapUtils::getLongitudinalDistance
needs to handle longitudinal distance calculation even with a lane change on a given route. This is the part of the function that handles this case:
https://github.com/tier4/scenario_simulator_v2/blob/a3e59a7051400776473288eceb2e4198082b4215/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp#L1506-L1525
In summary, when on a given route a lane change occurs, a horizontal bar (perpendicular to the lanelet's centerline) at the starting position is created. Then, if a collision with the neighboring lanelet's centerline is found, the distance is calculated. Otherwise, the bug occurs and std::nullopt is returned (this is done for both lanelets participating in the lane change).
Because starting poses have been chosen for this process, an uncovered edge case can occur, where centerlines are skewed in such a way, that no collision exists.
Full bug trace
To start off: traffic_simulator::distance::longitudinalDistance
uses HdMapUtils::getLongitudinalDistance
:
https://github.com/tier4/scenario_simulator_v2/blob/a3e59a7051400776473288eceb2e4198082b4215/simulation/traffic_simulator/src/utils/distance.cpp#L72-L78
HdMapUtils::getLongitudinalDistance
calculates longitudinal distance based on a path (a vector of lanelet::Id) given by HdMapUtils::getRoute
function. This vector implicitly contains the information if and where a lane change occurs: when the next lanelet::Id in the vector is not the following lanelet, it is assumed that the next lanelet::Id is to the right or left and a lane change has to occur. To calculate the longitudinal distance during the lane change, traffic_simulator_msgs::msg::LaneletPose
of the start of the next lanelet is being matched to the current lanelet::Id and vice versa:
https://github.com/tier4/scenario_simulator_v2/blob/a3e59a7051400776473288eceb2e4198082b4215/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp#L1506-L1525
This is the origin of the bug; HdMapUtils::toLaneletPose
fails twice and a std::nullopt is returned.
HdMapUtils::toLaneletPose
relies on lanelet centerlines to find the exact position of the lane change. However, CatmullRomSpline::getSValue
always returns a std::nullopt in these buggy examples.
https://github.com/tier4/scenario_simulator_v2/blob/a3e59a7051400776473288eceb2e4198082b4215/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp#L593-L602
CatmullRomSpline::getSValue
checks at which part of the curve the collision occurs, but there is no collision, the loop attached below finishes, because HermiteCurve::getSValue
never returns a value and a std::nullopt is returned.
https://github.com/tier4/scenario_simulator_v2/blob/a3e59a7051400776473288eceb2e4198082b4215/common/math/geometry/src/spline/catmull_rom_spline.cpp#L429-L439
Here is the last step, HermiteCurve::getSValue
checks if a horizontal bar of the original traffic_simulator_msgs::msg::LaneletPose
that is being matched, collides with the Hermite curve. However, no collision exists and std::nullopt is returned.
https://github.com/tier4/scenario_simulator_v2/blob/a3e59a7051400776473288eceb2e4198082b4215/common/math/geometry/src/spline/hermite_curve.cpp#L204-L219
@hakuturu583 Please let me know if this issue needs to be solved in any particular way. Possible solutions:
I understood... I think this one is best.
Check for collision with an extension of neighboring lanelet's centerline.
Description Consider white crosses as starting points and red crosses and finish points. Please note that provided examples necessitate a lane change.
In the examples provided, functions:
traffic_simulator::distance::longitudinalDistance
HdMapUtils::getLongitudinalDistance
fail to calculate the distance from start to finish but the route is correctly designated. The fact that the route does exist crosses out many issues with the map.
Expected behavior If the route is calculated, there should not be a problem with its length calculation.
To Reproduce
Remark I believe the problem arises exclusively while the distance of lane-switching is being calculated. https://github.com/tier4/scenario_simulator_v2/blob/a3e59a7051400776473288eceb2e4198082b4215/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp#L1511-L1525 Here, both
toLaneletPose
calls fail to find a candidate, and astd::nullopt
is returned. I suspect this might be caused by a bug in geometry package, namely Hermite curve or Catmull-Rom spline implementation.