fix(cpp_mock_scenarios, ego_entity_simulation): fix ego issue in random001, fix getCurrentPose()

[dmoszynski]

Description

The same issue as RJD 1200 happens, when scenario_simulator_v2 and Autoware are built in RelWithDebInfo.

Abstract

The issue that occurs is related Ego position update - position is not updated correctly and EgoEntitySimulation::getCurrentPose() returns incorrect values.
The issue is most likely related to the optimization performed by the compiler when the -DCMAKE_BUILD_TYPE=RelWithDebInfo is used - more in the details section.

Background

To get rid of the issue, it was necessary to cast the temporary structure Eigen::Procuct<...> to Eigen::Vector3d - otherwise the optimization resulted in different results.

Details

• I’ve added a rotation matrix as a constant attribute of the EgoEntitySimulation (now the matrix is created at the time of initialization).

  • Previously it was created in many places every time it was needed, this gives a computational profit and also some form of safeguard.

scenario_simulator_v2/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp

Line 56 in dd31892

const Eigen::Matrix3d initial_rotation_matrix_;

• I’ve ensured that the temporary Eigen type that can cause issues is cast to Eigen::Vector3d.

  • It is worth to pay extra attention to the commonly used auto in ss2 code and try to avoid it in parts where Eigen structures are used.

scenario_simulator_v2/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp

Lines 416 to 417 in dd31892

	const auto relative_position =
	Eigen::Vector3d(initial_rotation_matrix_ * world_relative_position_);

• The PR also includes fixes to the random001 scenario that @SzymonParapura developed in RJD-1200 (the PR was not merged but closed - the fixes are required to make the scenario work well)

scenario_simulator_v2/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp

Lines 204 to 216 in dd31892

	}
	if (!api_.reachPosition("ego", trigger_position, 20.0) && api_.entityExists(entity_name)) {
	api_.despawn(entity_name);
	}
	}
	if (api_.reachPosition("ego", ego_goal_position, 1.0)) {
	api_.despawn("ego");
	stop(cpp_mock_scenarios::Result::SUCCESS);
	}
	}
	}

• I’ve conducted tests of the fixed version - everything works fine for -DCMAKE_BUILD_TYPE=RelWithDebInfo - in this jira command I've attached recordings as evidences.
• In this comment I also described more details about the issue itself.

References

RJD-1296 Jira ticket

Destructive Changes

--

Known Limitations

--

[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.