Failure of route length calculation

[robomic]

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

1. Switch to RJD-1197/distance. This branch contains the map fragment.
2. Add this testcase to the test_distance.cpp file.
3. Run the test suite.

TEST(distance, longitudinalDistance_noAdjacent_noOpposite_change_case1)
{
  std::shared_ptr<hdmap_utils::HdMapUtils> hdmap_utils_ptr =
    std::make_shared<hdmap_utils::HdMapUtils>(
      ament_index_cpp::get_package_share_directory("traffic_simulator") +
        "/map/intersection/lanelet2_map.osm",
      geographic_msgs::build<geographic_msgs::msg::GeoPoint>()
        .latitude(35.64200728302)
        .longitude(139.74821144562)
        .altitude(0.0));
  {
    const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose(
      makePose(86627.71, 44972.06, 340.0), false, hdmap_utils_ptr);
    ASSERT_TRUE(pose_from.has_value());
    const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose(
      makePose(86647.23, 44882.51, 240.0), false, hdmap_utils_ptr);
    ASSERT_TRUE(pose_from.has_value());

    const auto result = traffic_simulator::distance::longitudinalDistance(
      pose_from.value(), pose_to.value(), false, false, true, hdmap_utils_ptr);
    EXPECT_TRUE(result.has_value());
  }
  {
    const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose(
      makePose(86555.38, 45000.88, 340.0), false, hdmap_utils_ptr);
    ASSERT_TRUE(pose_from.has_value());
    const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose(
      makePose(86647.23, 44882.51, 240.0), false, hdmap_utils_ptr);
    ASSERT_TRUE(pose_from.has_value());

    const auto result = traffic_simulator::distance::longitudinalDistance(
      pose_from.value(), pose_to.value(), false, false, true, hdmap_utils_ptr);
    EXPECT_TRUE(result.has_value());
  }
  {
    const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose(
      makePose(86788.82, 44993.77, 210.0), false, hdmap_utils_ptr);
    ASSERT_TRUE(pose_from.has_value());
    const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose(
      makePose(86553.48, 44990.56, 150.0), false, hdmap_utils_ptr);
    ASSERT_TRUE(pose_from.has_value());

    const auto result = traffic_simulator::distance::longitudinalDistance(
      pose_from.value(), pose_to.value(), false, false, true, hdmap_utils_ptr);
    EXPECT_TRUE(result.has_value());
  }
  {
    const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose(
      makePose(86788.82, 44993.77, 210.0), false, hdmap_utils_ptr);
    ASSERT_TRUE(pose_from.has_value());
    const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose(
      makePose(86579.91, 44979.00, 150.0), false, hdmap_utils_ptr);
    ASSERT_TRUE(pose_from.has_value());

    const auto result = traffic_simulator::distance::longitudinalDistance(
      pose_from.value(), pose_to.value(), false, false, true, hdmap_utils_ptr);
    EXPECT_TRUE(result.has_value());
  }
}

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 a std::nullopt is returned. I suspect this might be caused by a bug in geometry package, namely Hermite curve or Catmull-Rom spline implementation.

[robomic]

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

[robomic]

@hakuturu583 Please let me know if this issue needs to be solved in any particular way. Possible solutions:

• Consider checking the middles (not starts) of neighboring lanelets for matching (take into account very different starting and ending positions of those lanelets).
• Check for collision with an extension of neighboring lanelet's centerline.
• Check for collision with a semicircle at the lanelet's ends.

[hakuturu583]

I understood．．． I think this one is best.

Check for collision with an extension of neighboring lanelet's centerline.