HdMapUtils refactor lanelet_wrapper::distance::distanceToStopLine

[abco20]

Description

Abstract

This is the partial PR of the HdMapUtils refactor (PR 2/6) ( #1478 )

Details

Replace usage of HdMapUtils::getDistanceToStopLine with non-member function lanelet_wrapper::distance::distanceToStopLine.

In the distanceToStopLine function, instead of adding all values to std::set and then using the first element, only the smallest value is stored.

before:

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

Lines 1492 to 1499 in 3042d19

	if (collision_point) {
	collision_points.insert(collision_point.value());
	}
	}
	if (collision_points.empty()) {
	return std::nullopt;
	}
	return *collision_points.begin();

after:

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

Lines 38 to 44 in 751f68b

	if (const auto & collision_point = route_spline.getCollisionPointIn2D(stop_line_points)) {
	if (not min_distance.has_value() or collision_point.value() < min_distance.value()) {
	min_distance = collision_point;
	}
	}
	}
	return min_distance;

References

• HdMapUtils refactor (PR 2/6) - extend lanelet_wrapper: add ::distance and necessary parts of ::route, ::lanelet_map, ::traffic_lights #1478

Destructive Changes

The arguments of traffic_simulator::distanceToStopLine have been changed.

before

auto distanceToStopLine(
  const traffic_simulator_msgs::msg::WaypointsArray & waypoints_array,
  const lanelet::Id target_stop_line_id,
  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> std::optional<double>;

after

template <typename... Ts>
auto distanceToStopLine(Ts &&... xs)
{
  return lanelet_wrapper::distance::distanceToStopLine(std::forward<decltype(xs)>(xs)...);
}

The definition of lanelet_wrapper::distance::distanceToStopLine, to which this function forwards, is as follows:

auto distanceToStopLine(const lanelet::Ids & route_lanelets, const SplineInterface & route_spline)
  -> std::optional<double>;

auto distanceToStopLine(
  const lanelet::Ids & route_lanelets, const std::vector<Point> & route_waypoints)
  -> std::optional<double>;

auto distanceToStopLine(const std::vector<Point> & route_waypoints, const lanelet::Id stop_line_id)
  -> std::optional<double>;

Migration Guide

The most appropriate overload corresponding to the original distanceToStopLine function call is:

auto distanceToStopLine(const std::vector<Point> & route_waypoints, const lanelet::Id stop_line_id)
  -> std::optional<double>;

Since the third argument hdmap_utils_ptr has been removed, eliminate it from your function calls.

Instead of passing const traffic_simulator_msgs::msg::WaypointsArray & waypoints_array, you should pass const std::vector<Point> & route_waypoints by extracting the waypoints from WaypointsArray msg.

Known Limitations

None.

[github-actions]

Checklist for reviewers ☑️

All references to "You" in the following text refer to the code reviewer.

• Is this pull request written in a way that is easy to read from a third-party perspective?
• Is there sufficient information (background, purpose, specification, algorithm description, list of disruptive changes, and migration guide) in the description of this pull request?
• If this pull request contains a destructive change, does this pull request contain the migration guide?
• Labels of this pull request are valid?
• All unit tests/integration tests are included in this pull request? If you think adding test cases is unnecessary, please describe why and cross out this line.
• The documentation for this pull request is enough? If you think adding documents for this pull request is unnecessary, please describe why and cross out this line.

[sonarqubecloud]

Quality Gate passed

Issues
6 New issues
0 Accepted issues

Measures
0 Security Hotspots
67.1% Coverage on New Code
0.0% Duplication on New Code

See analysis details on SonarQube Cloud