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

[TauTheLepton]

Description

DO NOT MERGE | WHEN THE RIGHT TIME COMES, TARGET WILL BE CHANGED 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 (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.

Note

A result of aforementioned changes, most of cpp_mock_scenarios had to be adjusted to communicate using regular lanelet pose instead of canonicalized one.
This leads to GitHub reporting many lines changed and the PR seem to be big, but many of these changes are simply adjusting cpp_mock_scenarios like the following.

Before

scenario_simulator_v2/mock/cpp_mock_scenarios/src/collision/crashing_npc.cpp

Lines 54 to 56 in bcc35ae

	auto & ego_entity = api_.spawn(
	"ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 0.0, 0.0),
	getVehicleParameters());

After

scenario_simulator_v2/mock/cpp_mock_scenarios/src/collision/crashing_npc.cpp

Lines 54 to 56 in f1cb1c2

	auto & ego_entity = api_.spawn(
	"ego", traffic_simulator::helper::constructLaneletPose(34741, 0.0, 0.0),
	getVehicleParameters());

Important

Because of the change of canonicalization, the NativeLanePosition type in SimulatorCore has been changed

Before

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

Lines 37 to 39 in bcc35ae

	using NativeLanePosition = traffic_simulator::CanonicalizedLaneletPose;
	using NativeRelativeLanePosition = traffic_simulator::LaneletPose;

After

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

Lines 36 to 38 in f1cb1c2

	using NativeLanePosition = traffic_simulator::LaneletPose;
	using NativeRelativeLanePosition = NativeLanePosition;

SimulatorCore

This PR focuses on improving the interaction between the API and SimulatorCore.
This is why SimulatorCore needed many modifications.
Below they are broken down into the subclasses of SimulatorCore.

General

Changes include conversion of some SimulatorCore 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 302 to 306 in dbf0c03

	template <typename... Ts>
	static auto applyAddEntityAction(Ts &&... xs) -> decltype(auto)
	{
	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 332 to 338 in 6c8d04b

	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) -> void
	{
	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 540 to 544 in dbf0c03

	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 641 to 649 in 6c8d04b

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

Others

Function NonStandardOperation::activateNonUserDefinedControllers has been moved directly to the SimulatorCore class.

CoordinateSystemConversion

• SimulatorCore::CoordinateSystemConversion::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 relative pose related functions from SimulatorCore::CoordinateSystemConversion have been refactored and moved to a new class SimulatorCore::DistanceConditionEvaluation as they were only used for distance calculations anyway.

DistanceConditionEvaluation

This subclass has been added to group all distance functions. Many functions added to this class are functions moved from SimulatorCore::CoordinateSystemConversion.

Note

Keep in mind, that due to the fact, that many underlying API functions can be passed some combination of Entity name and position as arguments, many function of this class have been converted to templates to accept different types as arguments.
These functions perform common checks whether an Entity exists (when the name is passed as an argument). These checks have been separated in a prerequisite function.

• Relative pose functions from CoordinateSystemConversion class have been removed in favor of new distance functions, because the relative pose was only used for distance calculation.
  • 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.
• CoordinateSystemConversion::evaluateLateralRelativeLanes has been changed to lateralRelativeLanes with some slight modifications.
• lateralLaneDistance has been developed to be used instead of CoordinateSystemConversion::makeNativeRelativeWorldPosition
• longitudinalLaneDistance has been developed to be used instead of CoordinateSystemConversion::makeNativeRelativeWorldPosition
• lateralLaneBoundingBoxDistance has been developed to be used instead of CoordinateSystemConversion::makeNativeBoundingBoxRelativeLanePosition
• longitudinalLaneBoundingBoxDistance has been developed to be used instead of CoordinateSystemConversion::makeNativeBoundingBoxRelativeLanePosition

ActionApplication

• Function activatePerformanceAssertion has been moved from NonStandardOperation class.

ConditionEvaluation

• Function evaluateRelativeSpeed has been divided into part in ConditionEvaluation
  

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

  Lines 622 to 630 in f1cb1c2

  	static auto evaluateRelativeSpeed(
  	const std::string & from_entity_name, const std::string & to_entity_name) -> Eigen::Vector3d
  	{
  	if (core->isEntityExist(from_entity_name) && core->isEntityExist(to_entity_name)) {
  	return core->relativeSpeed(from_entity_name, to_entity_name);
  	} else {
  	return Eigen::Vector3d::Constant(std::numeric_limits<double>::quiet_NaN());
  	}
  	}

  
  and part in API
  

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

  Lines 458 to 473 in f1cb1c2

  	auto API::relativeSpeed(const std::string & from_entity_name, const std::string & to_entity_name)
  	-> Eigen::Vector3d
  	{
  	auto velocity = [](const auto & entity) -> Eigen::Vector3d {
  	auto direction = [](const auto & q) -> Eigen::Vector3d {
  	return Eigen::Quaternion(q.w, q.x, q.y, q.z) * Eigen::Vector3d::UnitX();
  	};
  	return direction(entity.getMapPose().orientation) * entity.getCurrentTwist().linear.x;
  	};
  	const auto & observer = getEntity(from_entity_name);
  	const auto & observed = getEntity(to_entity_name);
  	const Eigen::Matrix3d rotation =
  	math::geometry::getRotationMatrix(observer.getMapPose().orientation);
  	return rotation.transpose() * velocity(observed) - rotation.transpose() * velocity(observer);
  	}

• Function evaluateTimeHeadaway has been divided into part in ConditionEvaluation
  

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

  Lines 651 to 660 in f1cb1c2

  	static auto evaluateTimeHeadway(
  	const std::string & from_entity_name, const std::string & to_entity_name) -> double
  	{
  	if (core->isEntityExist(from_entity_name) && core->isEntityExist(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();
  	}

  
  and part in API
  

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

  Lines 374 to 389 in f1cb1c2

  	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 (const 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.0) / to_entity.getCurrentTwist().linear.x;
  	return std::isnan(time_headway) ? std::numeric_limits<double>::infinity() : time_headway;
  	}
  	}
  	return std::nullopt;
  	}

• Some functions have been moved from NonStandardOperation
  • evaluateCurrentState
  • evaluateRelativeHeading

TrafficLightsOperation

• Some functions have been moved from NonStandardOperation
  • setConventionalTrafficLightsState
  • setConventionalTrafficLightConfidence
  • getConventionalTrafficLightsComposedState
  • compareConventionalTrafficLightsState
  • resetConventionalTrafficLightPublishRate
  • setV2ITrafficLightsState
  • resetV2ITrafficLightPublishRate

Syntax

Important

Because of the changes listed in SimulatorCore, many openscenario_interpreter syntax classes had to be adjusted.
For most of the syntax classes the changes were limited only to changing the parent class in inheritance structure, or just using the new functions from SimulatorCore rather than the old ones.

DistanceCondition

Many changes have been applied to DistanceCondition 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 SimulatorCore::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 144 to 176 in dbf0c03

	template <>
	auto DistanceCondition::distance<
	CoordinateSystem::entity, RelativeDistanceType::euclidianDistance, RoutingAlgorithm::undefined,
	false>(const EntityRef & triggering_entity, const Position & position) -> 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);
	},
	[&](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);
	},
	[&](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);
	},
	[&](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);
	}),
	position);
	}

to

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

Lines 143 to 149 in 6c8d04b

	template <>
	auto DistanceCondition::distance<
	CoordinateSystem::entity, RelativeDistanceType::euclidianDistance, RoutingAlgorithm::undefined,
	false>(const EntityRef & triggering_entity, const Position & position) -> double
	{
	return euclideanDistance(triggering_entity, static_cast<NativeWorldPosition>(position));
	}

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

ReachPositionCondition

Similarly to DistanceCondition the structure has been reworked to not use overloaded lambda functions and visitors, but rather directly call distance function.

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 245 to 298 in 6c8d04b

	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 relativeSpeed(const std::string & from_entity_name, const std::string & to_entity_name)
	-> Eigen::Vector3d;
	auto countLaneChanges(
	const std::string & from_entity_name, const std::string & to_entity_name,
	const RoutingConfiguration & routing_configuration) const -> std::optional<std::pair<int, int>>;
	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 RoutingConfiguration & routing_configuration) -> LaneletDistance;
	auto laneletDistance(
	const std::string & from_entity_name, const LaneletPose & to_lanelet_pose,
	const RoutingConfiguration & routing_configuration) -> LaneletDistance;
	auto laneletDistance(
	const LaneletPose & from_lanelet_pose, const std::string & to_entity_name,
	const RoutingConfiguration & routing_configuration) -> LaneletDistance;
	auto boundingBoxLaneletDistance(
	const std::string & from_entity_name, const std::string & to_entity_name,
	const RoutingConfiguration & routing_configuration) -> LaneletDistance;
	auto boundingBoxLaneletDistance(
	const std::string & from_entity_name, const LaneletPose & to_lanelet_pose,
	const RoutingConfiguration & routing_configuration) -> LaneletDistance;

EntityBase (traffic_simulator)

requestSynchronize

In this PR additionally to all other changes, the EntityBase::requestSynchronize function has been refactored in order to make it easier to read and understand.
Most of the logic code was kept unchanged, but some reformating was applied to use the init-statement to declare variables inside the if statements which in the same time checks whether the value is correct.
What is more, a check whether the entity this synchronize request is applied on is an Ego entity, has been moved to overloaded member function of EgoEntity class here

scenario_simulator_v2/simulation/traffic_simulator/src/entity/ego_entity.cpp

Lines 279 to 285 in e5f3ce9

	auto EgoEntity::requestSynchronize(
	const std::string & /*target_name*/, const LaneletPose & /*target_sync_pose*/,
	const LaneletPose & /*entity_target*/, const double /*target_speed*/, const double /*tolerance*/)
	-> bool
	{
	THROW_SYNTAX_ERROR("Request synchronize is only for non-ego entities.");
	}

References

INTERNAL LINK 1
INTERNAL LINK 2

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

[TauTheLepton]

No regressions confirmed here

[sonarqubecloud]

Quality Gate passed

Issues
10 New issues
0 Accepted issues

Measures
0 Security Hotspots
11.8% Coverage on New Code
1.6% Duplication on New Code

See analysis details on SonarQube Cloud