HdMapUtils refactor (PR 3/6) - use lanelet_wrapper wide in behavior_tree_plugin, remove unused methods

[dmoszynski]

Description

Abstract

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

Background

The changes focus on using lanelet_wrapper namespace in the behavior_tree_plugin.
Thus, the HdMapUtils is not used in behavior_tree_plugin after the changes.

As a result of changes mentioned above, some behavior_tree_plugin functions have been moved to other parts of traffic_simulator, sometimes with slight modifications of the functionality.

In other cases, functions inside behavior_tree_plugin have been refactored to better suit their purpose.

Additionally, the member functions of HdMapUtils that after these changes became unused were removed.

Details

The main goal of this PR was to extend the usage of lanelet_wrapper namespace and remove usage of HdMapUtils class in behavior_tree_plugin.
This was also a good opportunity to slightly tidy up the behavior_tree_plugin.
Minor changes were introduced - for example in the ActionNode class, where generic member functions were extracted as free functions in utils namespaces.

Examples of changes made to the behavior_tree_plugin

Before

scenario_simulator_v2/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp

Line 80 in fc56adc

if (getRightOfWayEntities(route_lanelets).size() != 0) {

After

scenario_simulator_v2/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp

Line 80 in e6a8a68

if (isNeedToRightOfWay(route_lanelets)) {

---

Before

scenario_simulator_v2/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp

Line 100 in fc56adc

distance_to_front_entity_ = getDistanceToTargetEntity(*trajectory, front_entity_status);

After

scenario_simulator_v2/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp

Lines 104 to 108 in e6a8a68

	distance_to_front_entity_ = traffic_simulator::distance::splineDistanceToBoundingBox(
	*trajectory, canonicalized_entity_status->getCanonicalizedLaneletPose().value(),
	canonicalized_entity_status->getBoundingBox(),
	front_entity_status.getCanonicalizedLaneletPose().value(),
	front_entity_status.getBoundingBox());

---

Before

scenario_simulator_v2/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_crossing_entity_action.cpp

Line 114 in fc56adc

distance_to_stop_target_ = getDistanceToConflictingEntity(route_lanelets, *trajectory);

After

scenario_simulator_v2/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_crossing_entity_action.cpp

Lines 114 to 116 in e6a8a68

	distance_to_stop_target_ = traffic_simulator::distance::distanceToNearestConflictingPose(
	route_lanelets, *trajectory, *canonicalized_entity_status,
	getOtherEntitiesCanonicalizedEntityStatuses());

Examples of changes made to the functions

Function ActionNode::getDistanceToTrafficLightStopLine

scenario_simulator_v2/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp

Lines 61 to 63 in fc56adc

	auto getDistanceToTrafficLightStopLine(
	const lanelet::Ids & route_lanelets,
	const math::geometry::CatmullRomSplineInterface & spline) const -> std::optional<double>;

Has been converted to TrafficLightsBase::getDistanceToActiveTrafficLightStopLine

scenario_simulator_v2/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights_base.hpp

Lines 74 to 76 in e6a8a68

	auto getDistanceToActiveTrafficLightStopLine(
	const lanelet::Ids & route_lanelets, const math::geometry::CatmullRomSplineInterface & spline)
	-> std::optional<double>;

---

Function ActionNode::getRightOfWayEntities

scenario_simulator_v2/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp

Lines 65 to 66 in fc56adc

	auto getRightOfWayEntities(const lanelet::Ids & following_lanelets) const
	-> std::vector<traffic_simulator::CanonicalizedEntityStatus>;

Has been converted to ActionNode::isNeedToRightOfWay which does not return statuses, but only information whether there are any right of way entities.
This change could have been made, because the original implementation was only ever used to check whether the returned vector was empty.

scenario_simulator_v2/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp

Line 53 in e6a8a68

auto isNeedToRightOfWay(const lanelet::Ids & following_lanelets) const -> bool;

List of all functions changed

Apart from changes in the behavior_tree_plugin implementation, which are not covered in great detail here, some changes to free functions and member functions in scenario_simulator_v2 were introduced.
These changes have a potential to break some code that is developed in parallel and should be stated explicitly - that is why the table below has been prepared. It lists all changes made to function signatures, whether the functions have been renamed, removed, added or their functionality was altered.

Most functions marked as removed were removed because they were no longer used anywhere - they were fully replaced by other functions from utils namespaces.

Function	Changes made
CatmullRomSplineInterface::getAltitudeRange	➕ added
CatmullRomSubspline::getAltitudeRange	➕ added
CatmullRomSpline::getAltitudeRange	➕ added
ActionNode::foundConflictingEntity	➖ removed
ActionNode::getDistanceToTrafficLightStopLine	➡️ TrafficLightsBase::getDistanceToActiveTrafficLightStopLine
ActionNode::getRightOfWayEntities	➡️ ActionNode::isNeedToRightOfWay with changes so that no entities are returned
ActionNode::getDistanceToConflictingEntity	➡️ distance::distanceToNearestConflictingPose with changes so that all statuses have to be passed
ActionNode::getOtherEntitiesCanonicalizedEntityStatuses	➕ added for the reason of distance::distanceToNearestConflictingPose
ActionNode::getDistanceToTargetEntity	➡️ distance::splineDistanceToBoundingBox
ActionNode::getDistanceToTargetEntityOnCrosswalk	➖ removed
ActionNode::getConflictingEntityStatus	➖ removed
ActionNode::getConflictingEntityStatusOnCrossWalk	➖ removed
ActionNode::getConflictingEntityStatusOnLane	➖ removed
ActionNode::isOtherEntityAtConsideredAltitude	➖ removed
HdMapUtils::clipTrajectoryFromLaneletIds	➖ removed
HdMapUtils::filterLaneletIds	➡️ lanelet_wrapper::lanelet_map::filterLaneletIds
HdMapUtils::getClosestLaneletId	➖ removed
HdMapUtils::getCollisionPointInLaneCoordinate	➖ removed
HdMapUtils::getConflictingCrosswalkIds	➡️ lanelet_wrapper::lanelet_map::conflictingCrosswalkIds
HdMapUtils::getConflictingLaneIds	➡️ lanelet_wrapper::lanelet_map::conflictingLaneIds
HdMapUtils::getAltitude	➖ removed
HdMapUtils::getLaneletPolygon	➖ removed
HdMapUtils::getRightOfWayLaneletIds	➖ removed
HdMapUtils::getRightOfWayLaneletIds	➖ removed
HdMapUtils::isInIntersection	➡️ lanelet_wrapper::lanelet_map::isInIntersection
HdMapUtils::isInLanelet	➖ removed
HdMapUtils::toMapBin	➡️ lanelet_wrapper::LaneletLoader::convertMapToBin
HdMapUtils::toMapPoints	➖ removed
HdMapUtils::absoluteHull	➖ removed
HdMapUtils::calcEuclidDist	➖ removed
HdMapUtils::filterLanelets	➖ removed
HdMapUtils::mapCallback	➖ removed
HdMapUtils::toPoint2d	➖ removed
HdMapUtils::toPolygon	➖ removed
lanelet_wrapper::pose::isAltitudeMatching	➡️ lanelet_wrapper::pose::isAltitudeWithinThreshold
lanelet_wrapper::pose::isAltitudeWithinRange	➕ added
ScenarioSimulator::getOrigin	➖ removed
HdMapUtils::getNearbyLaneletIds	➖ removed

References

INTERNAL LINK

Destructive Changes

For code developped in parallel please see List of all functions changed.

Known Limitations

[github-actions]

Failure optional scenarios

Note

This is an experimental check and does not block merging the pull-request.
But, please be aware that this may indicate a regression.

scenario failed: execution_time_test

      <failure type="SimulationFailure" message="CustomCommandAction typed &quot;exitFailure&quot; was triggered by the named Conditions {&quot;update time checker&quot;, &quot;avoid startup&quot;}: {&quot;update time checker&quot;: Is the /simulation/interpreter/execution_time/update (= 0.006347000000000000211053396981) is greaterThan 0.005?}, {&quot;avoid startup&quot;: Is the simulation time (= 9.900000000000005684341886080801) is greaterThan 1.000000000000000000000000000000?}" />

[sonarqubecloud]

Quality Gate passed

Issues
29 New issues
0 Accepted issues

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

See analysis details on SonarQube Cloud