HdMapUtils refactor (PR 1/6) - create lanelet_wrapper: use ::lanelet_map and ::pose

[dmoszynski]

Description

The non-regression is evidenced here.

Abstract

This is the first PR of 6 related to the HdMapUtils refactor - a class that allows you to get access to lanelet data - from an *.osm file.

The refactor is implemented in accordance with the proposal presented in the report WP11_Q2_2023: shared here.

As a result of merging all 6 PRs, HdMapUtils will no longer exist in the source code - it will be replaced by the LaneletWrapper and its namespace lanelet_wrapper - see a diagram below.

Background

The lanelet_wrapper namespace contains:

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

Main class LaneletWrapper is developed in the singleton pattern, and contains the activate(<path_to_lanelet_file>) method. LaneletWrapper uses static methods from the LaneletLoader class to load data from a *.osm file. This namespace also contains RouteCache, CenterPointsCache and LaneletLengthCache classes which are analogous to those in HdMapUtils. LaneletWrapper allows access to map data, graphs, rules and caches via static methods:

scenario_simulator_v2/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_wrapper.hpp

Lines 271 to 281 in 2645a65

	static auto activate(const std::string & lanelet_map_path) -> void;
	[[nodiscard]] static auto map() -> lanelet::LaneletMapPtr;
	[[nodiscard]] static auto routingGraph(const RoutingGraphType type)
	-> lanelet::routing::RoutingGraphConstPtr;
	[[nodiscard]] static auto trafficRules(const RoutingGraphType type)
	-> lanelet::traffic_rules::TrafficRulesPtr;
	[[nodiscard]] static auto routeCache(const RoutingGraphType type) -> RouteCache &;
	[[nodiscard]] static auto centerPointsCache() -> CenterPointsCache &;
	[[nodiscard]] static auto laneletLengthCache() -> LaneletLengthCache &;

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

• lanelet_wrapper::lanelet_map,
• lanelet_wrapper::pose,
• lanelet_wrapper::distance,
• lanelet_wrapper::route,
• lanelet_wrapper::lane_change,
• lanelet_wrapper::traffic_lights.

As a result of the merge of this PR 1/6, the necessary source code for LaneletWrapper as a singleton and its activation in the required places is added to the project. As well as the first and second namespaces: lanelet_wrapper::lanelet_map, lanelet_wrapper::pose are added, But most importantly, the ::pose 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

LaneletWrapper is added and the lanelet_map namespace in utils is developed.
Now, in order to access the lanelet data, there is no need to create HdMapUtils, just call lanelet_map::activate() in a single instance of the program - passing the path to the *.osm map.

Currently, LaneletWrapper is activated in:

• Configuration constructor - i.e. in openscenario_intepreter and cpp_scenario_node,
• simple_sensor_simulator,
• classes used in the unittests, such as TrafficLightsTest.

Note: By moving from HdMapUtils, we mean moving the entire method to lanelet_wrapper (as a free function), which traffic_simulator has access to via free functions from utils. Sample of the relationship between utils and lanelet_wrapper after changes: <HdMapUtils method name> to <utils namespace and function name> -> <lanelet_wrapper namespace and function name>.

Moved from HdMapUtils to lanelet_wrapper namespaces - used by lanelet_map and pose from utils:

• getLaneletLength to lanelet_map::laneletLength -> lanelet_wrapper::lanelet_map::laneletLength

• getLaneletAltitude to lanelet_map::laneletAltitude -> lanelet_wrapper::lanelet_map::laneletAltitude

• getCenterPoints to lanelet_wrapper::lanelet_map::centerPoints - this has been copied, the original will be deleted soon

• canonicalizeLaneletPose to pose::canonicalize() -> lanelet_wrapper::pose::canonicalizeLaneletPose

• getAllCanonicalizedLaneletPoses to pose::alternativeLaneletPoses -> lanelet_wrapper::pose::alternativeLaneletPoses

• getAlongLaneletPose to pose::alongLaneletPose -> lanelet_wrapper::pose::alongLaneletPose

• getLeftLaneletIds to pose::leftLaneletIds -> lanelet_wrapper::pose::leftLaneletIds

• getRightLaneletIds to pose::rightLaneletIds -> lanelet_wrapper::pose::rightLaneletIds

• getNextLaneletIds to pose::nextLaneletIds -> lanelet_wrapper::pose::nextLaneletIds

• getPreviousLaneletIds to pose::previousLaneletIds -> lanelet_wrapper::pose::previousLaneletIds

• toMapPose to pose::toMapPose -> lanelet_wrapper::pose::toMapPose

• matchToLane to pose::matchToLane -> lanelet_wrapper::pose::matchToLane

• toLaneletPose to pose::toLaneletPose -> lanelet_wrapper::pose::toLaneletPose

• toLaneletPoses to pose::toLaneletPoses -> lanelet_wrapper::pose::toLaneletPoses

• isAltitudeMatching to pose::isAltitudeMatching -> lanelet_wrapper::pose::isAltitudeMatching

Thanks to this, the passing of HdMapUtils ptr has been removed from functions:

• CanonicalizedEntityStatus::set
• pose::canonicalize
• pose::toCanonicalizedLaneletPose
• pose::toMapPose
• pose::pedestrian::transformToCanonicalizedLaneletPose
• CanonicalizedLaneletPose contructor
• helper::constructCanonicalizedLaneletPose
• helper::constructLaneletPose

In addition:

• Adapted:
  • test_hdmap_utils - to use functions from lanelet_wrapper - in the next PRs this file will be broken down so that tests for each sub-namespace of lanelet_wrapper will be separated file.
  • helper_functions and other files from the test/* folder to lanelet_wrapper.
  • Returned type and the definition of pose::canonicalize() - now it returns LaneletPose and throws an exception - which is in harmony with the name and expectations. Whenever CanonicalizedLaneletPose is needed, the pose::toCanonicalizedLaneletPose() function is used. In addition, there is no longer a CanonicalizedLaneletPose::canonicalize() method - common pose::canonicalize is used there.
  • Configuration constructor and its use in cpp_scenario_node and openscenario_interpreter.
• Fixed
  • Distance along lanelet calculation in follow_trajectory_action - previously the orientation was not taken into account leading to failures in some scenarios,
• Simplified:
  • ActionNode::getDistanceToTargetEntityPolygon - by developing a more coherent version of pose::isAltitudeMatching
  • ActionNode::calculateUpdatedEntityStatus - thanks to the separation of the function lanelet_wrapper::pose::alongLaneletPose (in subsequent PRs an intermediate route:: sub-namespace will be created, then it will be route::moveAlongLaneletPose)
  • TrafficController::appendAutoSinks - by separating the function lanelet_map::boarderlinePoses
• Added:
  • math::geometry::convertDirectionToQuaternion - i.e. an assistant function to convert a direction vector into an orientation in a quaternion
  • CanonicalizedLaneletPose::getAltitude()
  • CanonicalizedLaneletPose::getLaneletId()
  • CanonicalziedLaneletPose::alignOrientationtoLanelet()
  • pose::isInLanelet -> lanelet_wrapper::pose::isInLanelet - with argument type of geometry_msgs::msg::Point

References

Internal Jira Link

Destructive Changes

None

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
28 New issues
0 Accepted issues

Measures
0 Security Hotspots
59.7% Coverage on New Code
0.6% Duplication on New Code

See analysis details on SonarQube Cloud