[tmp] RJD-1057 (5/5): Remove non-API member functions: Improve responsibility simulator_core<->api

[TauTheLepton]

Description

DO NOT MERGE | THIS IS A TEMPORARY PR | WHEN THE RIGHT TIME COMES, A CLONE WILL BE CAST TO MASTER

Abstract

This pull request aims to improve the responsibility division between traffic_simulator API and openscenario_interpreter SimulatorCore.
The changes include refactoring, renaming functions and reordering them in the source files. So even though the number of changes reported by github is rather high, many of the changes consist of moving functions around with slight if none modifications.

Background

This pull request is one of many that aim to modularize the scenario_simulator_v2.

Details

Canonicalization

The main changes are that canonicalization process (e.g. of lanelet pose) is a responsibility of traffic_simulator. This way the communication with traffic_simulator API is done on NON canonicalized data structures and traffic_simulator has to canonicalize the data.
As a result, cpp mock scenarios had to be adjusted to communicate using regular lanelet pose instead of canonicalized one.
Another result of this change is that the NativeLanePosition in SimulatorCore has been converted to NON canonicalized lanelet pose.

SimulatorCore modifications and organizing

As for further changes:

• SimulatorCore::CoordinateSystemConverion::convert<> functions have been renamed to convertToNativeLanePosition and convertToNativeWorldPosition and slightly improved
• 2 out of 3 CoordinateSystemConversion::makeNativeRelativeWorldPosition functions have been removed, as they were no longer needed
• Many of the distance related functions from SimulatorCore::CoordinateSystemConversion have been refactored and moved to a new class SimulatorCore::DistanceConditionEvaluation. Also, relative pose functions have been removed in favor of new distance functions, because the relative pose was only used for distance calculation anyway.
  • CoordinateSystemConversion::makeNativeRelativeLanePosition has been removed in favor of DistanceConditionEvaluation::lateralEntityDistance and DistanceConditionEvaluation::longitudinalEntityDistance
  • CoordinateSystemConversion::makeNativeBoundingBoxRelativeLanePosition has been removed in favor of DistanceConditionEvaluation::lateralEntityBoundingBoxDistance and DistanceConditionEvaluation::longitudinalEntityBoundingBoxDistance
  • CoordinateSystemConversion::makeNativeBoundingBoxRelativeWorldPosition has been removed in favor of DistanceConditionEvaluation::euclideanBoundingBoxDistance
  • DistanceConditionEvaluation::euclideanDistance has been added for distance calculation

Some other differences include conversion of some simulator core classes member functions to explicitly list arguments instead of using the variadic templates.
For example applyAddEntityAction before change

scenario_simulator_v2/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp

Lines 271 to 275 in ff759fe

	template <typename... Ts>
	static auto applyAddEntityAction(Ts &&... xs)
	{
	return core->spawn(std::forward<decltype(xs)>(xs)...);
	}

and after the change

scenario_simulator_v2/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp

Lines 310 to 316 in 98c1866

	template <typename PoseType, typename ParamsType>
	static auto applyAddEntityAction(
	const std::string & entity_name, const PoseType & pose, const ParamsType & parameters,
	const std::string & behavior, const std::string & model3d) -> bool
	{
	return static_cast<bool>(core->spawn(entity_name, pose, parameters, behavior, model3d));
	}

Many member functions of SimulatorCore::ConditionEvaluation have been changed to include a check whether the Entity exists and if not return NaN or other appropriate default value.
For example SimulatorCore::ConditionEvaluation::evaluateCollisionCondition before change

scenario_simulator_v2/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp

Lines 489 to 493 in ff759fe

	template <typename... Ts>
	static auto evaluateCollisionCondition(Ts &&... xs) -> bool
	{
	return core->checkCollision(std::forward<decltype(xs)>(xs)...);
	}

and after the change

scenario_simulator_v2/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp

Lines 563 to 571 in 98c1866

	static auto evaluateCollisionCondition(
	const std::string & first_entity_name, const std::string & second_entity_name) -> bool
	{
	if (core->isEntitySpawned(first_entity_name) && core->isEntitySpawned(second_entity_name)) {
	return core->checkCollision(first_entity_name, second_entity_name);
	} else {
	return false;
	}
	}

Some functionality has been moved from openscenario interpreter simulator core to traffic simulator API, like the function SimulatorCore::ConditionEvaluation::evaluateTimeHeadway was converted from

scenario_simulator_v2/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp

Lines 532 to 549 in ff759fe

	static auto evaluateTimeHeadway(
	const std::string & from_entity_name, const std::string & to_entity_name)
	{
	if (auto from_entity = core->getEntityOrNullptr(from_entity_name); from_entity) {
	if (auto to_entity = core->getEntityOrNullptr(to_entity_name); to_entity) {
	if (auto relative_pose = traffic_simulator::pose::relativePose(
	from_entity->getMapPose(), to_entity->getMapPose());
	relative_pose && relative_pose->position.x <= 0) {
	const double time_headway =
	(relative_pose->position.x * -1) / to_entity->getCurrentTwist().linear.x;
	return std::isnan(time_headway) ? std::numeric_limits<double>::infinity()
	: time_headway;
	}
	}
	}
	return std::numeric_limits<double>::quiet_NaN();
	}
	};

to

scenario_simulator_v2/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp

Lines 573 to 582 in 98c1866

	static auto evaluateTimeHeadway(
	const std::string & from_entity_name, const std::string & to_entity_name) -> double
	{
	if (core->isEntitySpawned(from_entity_name) && core->isEntitySpawned(to_entity_name)) {
	if (const auto time_headway = core->timeHeadway(from_entity_name, to_entity_name)) {
	return time_headway.value();
	}
	}
	return std::numeric_limits<double>::quiet_NaN();
	}

with an addition of traffic_siulator::API::timeHeadway

scenario_simulator_v2/simulation/traffic_simulator/src/api/api.cpp

Lines 367 to 381 in 98c1866

	auto API::timeHeadway(const std::string & from_entity_name, const std::string & to_entity_name)
	-> std::optional<double>
	{
	if (from_entity_name != to_entity_name) {
	const auto from_entity = getEntity(from_entity_name);
	const auto to_entity = getEntity(to_entity_name);
	if (auto relative_pose = pose::relativePose(from_entity->getMapPose(), to_entity->getMapPose());
	relative_pose && relative_pose->position.x <= 0) {
	const double time_headway =
	(relative_pose->position.x * -1) / to_entity->getCurrentTwist().linear.x;
	return std::isnan(time_headway) ? std::numeric_limits<double>::infinity() : time_headway;
	}
	}
	return std::nullopt;
	}

Function activatePerformanceAssertion has been moved from NonStandardOperation to ActionApplication.

Function activateNonUserDefinedControllers has been moved from NonStandardOperation directly to SimulatorCore.

Functions

• evaluateCurrentState
• evaluateRelativeHeading

have been moved from NonStandardOperation to ConditionEvaluation.

Functions

• setConventionalTrafficLightsState
• setConventionalTrafficLightConfidence
• getConventionalTrafficLightsComposedState
• compareConventionalTrafficLightsState
• resetConventionalTrafficLightPublishRate
• setV2ITrafficLightsState
• resetV2ITrafficLightPublishRate

have been moved from NonStandardOperation to newly created TrafficLightsOperation class.

Because of the changes listed above, many openscenario interpreter syntax classes had to have their parent classes adjusted.

DistanceCondition

Many changes have been applied to distance condition of openscenario interpreter. Most of these changes are simplifications that don't use the overloaded lambda functions and visitors, but rather just use newly developed DistanceConditionEvaluation member functions.
For example the function DistanceCondition::distance<CoordinateSystem::entity, RelativeDistanceType::euclidianDistance, RoutingAlgorithm::undefined, false> has been changed from

scenario_simulator_v2/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp

Lines 143 to 179 in ff759fe

	template <>
	auto DistanceCondition::distance<
	CoordinateSystem::entity, RelativeDistanceType::euclidianDistance, RoutingAlgorithm::undefined,
	false>(const EntityRef & triggering_entity) const -> double
	{
	return apply<double>(
	overload(
	[&](const WorldPosition & position) {
	const auto relative_world = makeNativeRelativeWorldPosition(
	triggering_entity, static_cast<NativeWorldPosition>(position));
	return hypot(
	relative_world.position.x, relative_world.position.y, relative_world.position.z,
	consider_z);
	},
	[&](const RelativeWorldPosition & position) {
	const auto relative_world = makeNativeRelativeWorldPosition(
	triggering_entity, static_cast<NativeWorldPosition>(position));
	return hypot(
	relative_world.position.x, relative_world.position.y, relative_world.position.z,
	consider_z);
	},
	[&](const RelativeObjectPosition & position) {
	const auto relative_world = makeNativeRelativeWorldPosition(
	triggering_entity, static_cast<NativeWorldPosition>(position));
	return hypot(
	relative_world.position.x, relative_world.position.y, relative_world.position.z,
	consider_z);
	},
	[&](const LanePosition & position) {
	const auto relative_world = makeNativeRelativeWorldPosition(
	triggering_entity, static_cast<NativeWorldPosition>(position));
	return hypot(
	relative_world.position.x, relative_world.position.y, relative_world.position.z,
	consider_z);
	}),
	position);
	}

to

scenario_simulator_v2/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp

Lines 143 to 154 in 98c1866

	template <>
	auto DistanceCondition::distance<
	CoordinateSystem::entity, RelativeDistanceType::euclidianDistance, RoutingAlgorithm::undefined,
	false>(const EntityRef & triggering_entity) const -> double
	{
	if (global().entities->ref(triggering_entity).as<ScenarioObject>().is_added) {
	return euclideanDistance(
	triggering_entity, static_cast<NativeWorldPosition>(position), consider_z);
	} else {
	return std::numeric_limits<double>::quiet_NaN();
	}
	}

The simplification comes from reducing the overloaded lambdas that have identical implementations to just one function call.

Distance calculation in traffic simulator

For simplicity some distance calculation functions have been added to traffic simulator API so that openscenario interpreter did not implement distance calculations but could use these functions here

scenario_simulator_v2/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp

Lines 235 to 281 in 98c1866

	auto laneletRelativeYaw(const std::string & entity_name, const LaneletPose & lanelet_pose) const
	-> std::optional<double>;
	auto timeHeadway(const std::string & from_entity_name, const std::string & to_entity_name)
	-> std::optional<double>;
	auto boundingBoxDistance(const std::string & from_entity_name, const std::string & to_entity_name)
	-> std::optional<double>;
	auto relativePose(const std::string & from_entity_name, const std::string & to_entity_name)
	-> std::optional<geometry_msgs::msg::Pose>;
	auto relativePose(
	const std::string & from_entity_name, const geometry_msgs::msg::Pose & to_map_pose)
	-> std::optional<geometry_msgs::msg::Pose>;
	auto relativePose(
	const geometry_msgs::msg::Pose & from_map_pose, const std::string & to_entity_name)
	-> std::optional<geometry_msgs::msg::Pose>;
	auto boundingBoxRelativePose(
	const std::string & from_entity_name, const geometry_msgs::msg::Pose & to_map_pose)
	-> std::optional<geometry_msgs::msg::Pose>;
	auto boundingBoxRelativePose(
	const std::string & from_entity_name, const std::string & to_entity_name)
	-> std::optional<geometry_msgs::msg::Pose>;
	auto laneletDistance(
	const std::string & from_entity_name, const std::string & to_entity_name,
	const bool allow_lane_change) -> LaneletDistance;
	auto laneletDistance(
	const std::string & from_entity_name, const LaneletPose & to_lanelet_pose,
	const bool allow_lane_change) -> LaneletDistance;
	auto laneletDistance(
	const LaneletPose & from_lanelet_pose, const std::string & to_entity_name,
	const bool allow_lane_change) -> LaneletDistance;
	auto boundingBoxLaneletDistance(
	const std::string & from_entity_name, const std::string & to_entity_name,
	const bool allow_lane_change) -> LaneletDistance;
	auto boundingBoxLaneletDistance(
	const std::string & from_entity_name, const LaneletPose & to_lanelet_pose,
	const bool allow_lane_change) -> LaneletDistance;

Other changes come from the usage of modified functions that have been described above or are just simple style changes that should make the code more readable.

References

INTERNAL LINK

Destructive Changes

Known Limitations

[sonarqubecloud]

Quality Gate passed

Issues
22 New issues
0 Accepted issues

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

See analysis details on SonarCloud

[sonarqubecloud]

Quality Gate passed

Issues
23 New issues
0 Accepted issues

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

See analysis details on SonarQube Cloud