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

dmoszynski · 2024-12-05T12:51:03Z

Description

Important

Right before merging this PR, please change the target branch of #1479 to `master`.

Otherwise that PR (which is next in line to be merged) will be closed automatically.

Abstract

This is the second PR of 6 related to the HdMapUtils refactor (for the first one, see #1472).
For a full overview of changes that all 6 PRs will introduce, please see the Abstract of the first PR.

Background

The lanelet_wrapper namespace contains:

LaneletLoader
LaneletWrapper
RouteCache,CenterPointsCache, LaneletLengthCache
TrafficRulesWithRoutingGraph
6 category-specific sub-namespaces

These static methods are used by the sub-namespaces of lanelet_wrapper.
lanelet_wrapper namespace consist of 6 sub-namespaces:

As a result of the merge of this PR 2/6, the namespaces lanelet_wrapper::lanelet_map, lanelet_wrapper::distance, lanelet_wrapper::route and lanelet_wrapper::traffic_lights were added to the project. But most importantly, the ::distance from utils is adapted for its use.
In addition to this, PR is making several other improvements to the code, see below for details.

Details

The distance namespace in lanelet_wrapper and only the strictly necessary parts of route and traffic_lights are developed.

Important

By moving from HdMapUtils, we mean moving the entire member function to lanelet_wrapper - making it a free function.
This is accessible to traffic_simulator through free functions in utils namespace.

Before these changes the structure looked like this:

<HdMapUtils member function name>

After the changes structure looks like this:

<utils namespace and function name> → <lanelet_wrapper namespace and function name>

Below is the list of moved function (member functions transformed to free functions)

Tip

Most functions that have been removed from HdMapUtils have been renamed in such a way that the get prefix has been removed and thus the first letter of the new function name has been made lower case.

Example of such name change can be the following

HdMapUtils::getLateralDistance → lanelet_wrapper::distance::lateralDistance

Old name	New name
`HdMapUtils::getLateralDistance`	`distance::lateralDistance` → `lanelet_wrapper::distance::lateralDistance`
`HdMapUtils::getLongitudinalDistance`	`distance::longitudinalDistance` → `lanelet_wrapper::distance::longitudinalDistance`
`HdMapUtils::getDistanceToStopLine`	`distance::distanceToStopLine` → `lanelet_wrapper::distance::distanceToStopLine`
`HdMapUtils::getDistanceToTrafficLightStopLine`	`distance::distanceToTrafficLightStopLine` → `lanelet_wrapper::distance::distanceToTrafficLightStopLine`
`HdMapUtils::getDistanceToCrosswalk`	`distance::distanceToCrosswalk` → `lanelet_wrapper::distance::distanceToCrosswalk`
`HdMapUtils::getLeftBound`	`lanelet_wrapper::lanelet_map::leftBound`
`HdMapUtils::getRightBound`	`lanelet_wrapper::lanelet_map::rightBound`
`HdMapUtils::getLaneletPolygon`	`lanelet_wrapper::lanelet_map::laneletPolygon`
`HdMapUtils::getStopLinePolygon`	`lanelet_wrapper::lanelet_map::stopLinePolygon`
`HdMapUtils::toPolygon`	`lanelet_wrapper::lanelet_map::toPolygon`
`HdMapUtils::getRightOfWayLaneletIds`	`lanelet_wrapper::lanelet_map::rightOfWayLaneletIds`
`HdMapUtils::getTrafficSignRegulatoryElementsOnPath`	`lanelet_wrapper::lanelet_map::trafficSignsOnPath`
`HdMapUtils::getStopLinesOnPath`	`lanelet_wrapper::lanelet_map::stopLinesOnPath`
`HdMapUtils::getStopLineIdsOnPath`	`lanelet_wrapper::lanelet_map::stopLineIdsOnPath`
`HdMapUtils::getRoute`	`route::route` → `lanelet_wrapper::route::route`
`HdMapUtils::getTrafficLightStopLinesPoints`	`lanelet_wrapper::traffic_lights::trafficLightStopLinesPoints`
`HdMapUtils::getTrafficLightIdsOnPath`	`lanelet_wrapper::traffic_lights::trafficLightStopLinesPoints`
`HdMapUtils::getTrafficLights`	`lanelet_wrapper::traffic_lights::toAutowareTrafficLights`
`HdMapUtils::getTrafficLightRegulatoryElementsOnPath`	`lanelet_wrapper::traffic_lights::autowareTrafficLightsOnPath`

Additionally, distance::distanceToYieldStop function has been separated from ActionNode::getYieldStopDistance in order to improve readability and to maintain the division of functionality.

Note

Some of the functions copied from HdMapUtils class have been slightly modified. The modifications are small improvements of things like removing storing of unnecessary values.
One such change is in function distanceToStopLine where instead of adding all values to std::set and in the end using the first one, only the smallest value is stored.
Before:

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

Lines 1504 to 1511 in 5140e05

    
             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 149 to 155 in 1d2453b

    
             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;

Copyright

It has been notices that in some files the copyright date is wrong (other than 2015), thus the date has been fixed both in files added or modified by this PR and any other files where the incorrect copyright date has been noticed.

References

INTERNAL LINK

Destructive Changes

Known Limitations

…ecessary parts of ::route, ::lanelet_map, ::traffic_lights

…): use ::distance from lanelet_wrapper instead of hdmap_utils

…arated parts, adapt hdmap_utils tests