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.

scenario_simulator_v2/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp

Lines 1511 to 1525 in a3e59a7

	if (
	auto next_lanelet_origin_from_current_lanelet =
	toLaneletPose(toMapPose(next_lanelet_pose).pose, route[i], 10.0)) {
	lateral_distance_by_lane_change += next_lanelet_origin_from_current_lanelet->offset;
	} else {
	traffic_simulator_msgs::msg::LaneletPose current_lanelet_pose = next_lanelet_pose;
	current_lanelet_pose.lanelet_id = route[i];
	if (
	auto current_lanelet_origin_from_next_lanelet =
	toLaneletPose(toMapPose(current_lanelet_pose).pose, route[i + 1], 10.0)) {
	lateral_distance_by_lane_change -= current_lanelet_origin_from_next_lanelet->offset;
	} else {
	return std::nullopt;
	}
	}

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:

scenario_simulator_v2/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp

Lines 1506 to 1525 in a3e59a7

	traffic_simulator_msgs::msg::LaneletPose next_lanelet_pose;
	next_lanelet_pose.lanelet_id = route[i + 1];
	next_lanelet_pose.s = 0.0;
	next_lanelet_pose.offset = 0.0;
	if (
	auto next_lanelet_origin_from_current_lanelet =
	toLaneletPose(toMapPose(next_lanelet_pose).pose, route[i], 10.0)) {
	lateral_distance_by_lane_change += next_lanelet_origin_from_current_lanelet->offset;
	} else {
	traffic_simulator_msgs::msg::LaneletPose current_lanelet_pose = next_lanelet_pose;
	current_lanelet_pose.lanelet_id = route[i];
	if (
	auto current_lanelet_origin_from_next_lanelet =
	toLaneletPose(toMapPose(current_lanelet_pose).pose, route[i + 1], 10.0)) {
	lateral_distance_by_lane_change -= current_lanelet_origin_from_next_lanelet->offset;
	} else {
	return std::nullopt;
	}
	}

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:

scenario_simulator_v2/simulation/traffic_simulator/src/utils/distance.cpp

Lines 72 to 78 in a3e59a7

	const auto forward_distance = hdmap_utils_ptr->getLongitudinalDistance(
	static_cast<LaneletPose>(from), to_canonicalized, allow_lane_change);
	const auto backward_distance = hdmap_utils_ptr->getLongitudinalDistance(
	to_canonicalized, static_cast<LaneletPose>(from), allow_lane_change);

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:

scenario_simulator_v2/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp

Lines 1506 to 1525 in a3e59a7

	traffic_simulator_msgs::msg::LaneletPose next_lanelet_pose;
	next_lanelet_pose.lanelet_id = route[i + 1];
	next_lanelet_pose.s = 0.0;
	next_lanelet_pose.offset = 0.0;
	if (
	auto next_lanelet_origin_from_current_lanelet =
	toLaneletPose(toMapPose(next_lanelet_pose).pose, route[i], 10.0)) {
	lateral_distance_by_lane_change += next_lanelet_origin_from_current_lanelet->offset;
	} else {
	traffic_simulator_msgs::msg::LaneletPose current_lanelet_pose = next_lanelet_pose;
	current_lanelet_pose.lanelet_id = route[i];
	if (
	auto current_lanelet_origin_from_next_lanelet =
	toLaneletPose(toMapPose(current_lanelet_pose).pose, route[i + 1], 10.0)) {
	lateral_distance_by_lane_change -= current_lanelet_origin_from_next_lanelet->offset;
	} else {
	return std::nullopt;
	}
	}

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.

scenario_simulator_v2/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp

Lines 593 to 602 in a3e59a7

	auto HdMapUtils::toLaneletPose(
	const geometry_msgs::msg::Pose & pose, const lanelet::Id lanelet_id,
	const double matching_distance) const -> std::optional<traffic_simulator_msgs::msg::LaneletPose>
	{
	const auto spline = getCenterPointsSpline(lanelet_id);
	const auto s = spline->getSValue(pose, matching_distance);
	if (!s) {
	return std::nullopt;
	}
	auto pose_on_centerline = spline->getPose(s.value());

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.

scenario_simulator_v2/common/math/geometry/src/spline/catmull_rom_spline.cpp

Lines 429 to 439 in a3e59a7

	default:
	double s = 0;
	for (size_t i = 0; i < curves_.size(); i++) {
	auto s_value = curves_[i].getSValue(pose, threshold_distance, true);
	if (s_value) {
	s = s + s_value.value();
	return s;
	}
	s = s + curves_[i].getLength();
	}
	return std::nullopt;

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.

scenario_simulator_v2/common/math/geometry/src/spline/hermite_curve.cpp

Lines 204 to 219 in a3e59a7

	std::optional<double> HermiteCurve::getSValue(
	const geometry_msgs::msg::Pose & pose, double threshold_distance, bool denormalize_s) const
	{
	geometry_msgs::msg::Point p0, p1;
	p0.y = threshold_distance;
	p1.y = -threshold_distance;
	const auto line = math::geometry::transformPoints(pose, {p0, p1});
	const auto s = getCollisionPointIn2D(line[0], line[1], false);
	if (!s) {
	return std::nullopt;
	}
	if (denormalize_s) {
	return s.value() * getLength();
	}
	return s.value();
	}

[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.