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

common/math/geometry/include/geometry/quaternion/get_rotation_matrix.hpp

@@ -30,7 +30,7 @@ namespace geometry
 {
 template <
   typename T, std::enable_if_t<std::conjunction_v<IsLikeQuaternion<T>>, std::nullptr_t> = nullptr>
-auto getRotationMatrix(T quat)
+auto getRotationMatrix(T quat) -> Eigen::Matrix3d
 {
   auto x = quat.x;
   auto y = quat.y;

mock/cpp_mock_scenarios/src/random_scenario/random001.cpp

@@ -186,7 +186,10 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode
     {
       const auto trigger_position = traffic_simulator::helper::constructCanonicalizedLaneletPose(
         34621, 10, 0.0, api_.getHdmapUtils());
+      const auto ego_goal_position = traffic_simulator::helper::constructCanonicalizedLaneletPose(
+        34606, 0.0, 0.0, api_.getHdmapUtils());
       const auto entity_name = "spawn_nearby_ego";
+
       if (const auto ego = api_.getEntity("ego")) {
         if (api_.reachPosition("ego", trigger_position, 20.0) && !api_.entityExists(entity_name)) {
           api_.spawn(
@@ -198,12 +201,16 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode
                 .orientation(geometry_msgs::msg::Quaternion())),
             getVehicleParameters(),
             traffic_simulator::entity::VehicleEntity::BuiltinBehavior::doNothing());
-        } else {
-          stop(cpp_mock_scenarios::Result::FAILURE);
+        }
+
+        if (!api_.reachPosition("ego", trigger_position, 20.0) && api_.entityExists(entity_name)) {
+          api_.despawn(entity_name);
         }
       }
-      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);
       }
     }
   }
@@ -232,6 +239,7 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode
         getVehicleParameters(),
         traffic_simulator::entity::VehicleEntity::BuiltinBehavior::doNothing());
     } else {
+      api_.despawn("ego");
       stop(cpp_mock_scenarios::Result::FAILURE);
     }
   }

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

@@ -51,7 +51,9 @@ class EgoEntitySimulation
 
   traffic_simulator::CanonicalizedEntityStatus status_;
 
-  geometry_msgs::msg::Pose initial_pose_;
+  const geometry_msgs::msg::Pose initial_pose_;
+
+  const Eigen::Matrix3d initial_rotation_matrix_;
 
   static auto getVehicleModelType() -> VehicleModelType;
 

simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp

@@ -45,12 +45,13 @@ EgoEntitySimulation::EgoEntitySimulation(
   vehicle_model_type_(getVehicleModelType()),
   vehicle_model_ptr_(makeSimulationModel(vehicle_model_type_, step_time, parameters)),
   status_(initial_status, std::nullopt),
+  initial_pose_(status_.getMapPose()),
+  initial_rotation_matrix_(math::geometry::getRotationMatrix(initial_pose_.orientation)),
   consider_acceleration_by_road_slope_(consider_acceleration_by_road_slope),
   hdmap_utils_ptr_(hdmap_utils),
   vehicle_parameters(parameters)
 {
   setStatus(initial_status);
-  initial_pose_ = status_.getMapPose();
   autoware->set_parameter(use_sim_time);
 }
 
@@ -232,17 +233,16 @@ auto EgoEntitySimulation::overwrite(
      considered unchangeable and stored in an additional variable
      world_relative_position_ that is used in calculations.
   */
-  world_relative_position_ = getRotationMatrix(initial_pose_.orientation).transpose() *
-                             Eigen::Vector3d(
-                               status.pose.position.x - initial_pose_.position.x,
-                               status.pose.position.y - initial_pose_.position.y,
-                               status.pose.position.z - initial_pose_.position.z);
+  world_relative_position_ =
+    initial_rotation_matrix_.transpose() * Eigen::Vector3d(
+                                             status.pose.position.x - initial_pose_.position.x,
+                                             status.pose.position.y - initial_pose_.position.y,
+                                             status.pose.position.z - initial_pose_.position.z);
 
   if (is_npc_logic_started) {
     const auto yaw = [&]() {
       const auto q = Eigen::Quaterniond(
-        getRotationMatrix(initial_pose_.orientation).transpose() *
-        getRotationMatrix(status.pose.orientation));
+        initial_rotation_matrix_.transpose() * getRotationMatrix(status.pose.orientation));
       geometry_msgs::msg::Quaternion relative_orientation;
       relative_orientation.x = q.x();
       relative_orientation.y = q.y();
@@ -296,7 +296,7 @@ void EgoEntitySimulation::update(
      considered unchangeable and stored in an additional variable
      world_relative_position_ that is used in calculations.
   */
-  world_relative_position_ = getRotationMatrix(initial_pose_.orientation).transpose() *
+  world_relative_position_ = initial_rotation_matrix_.transpose() *
                              Eigen::Vector3d(
                                status_.getMapPose().position.x - initial_pose_.position.x,
                                status_.getMapPose().position.y - initial_pose_.position.y,
@@ -413,8 +413,8 @@ auto EgoEntitySimulation::getCurrentPose(const double pitch_angle = 0.) const
   -> geometry_msgs::msg::Pose
 {
   using math::geometry::operator*;
-  const Eigen::Vector3d relative_position =
-    math::geometry::getRotationMatrix(initial_pose_.orientation) * world_relative_position_;
+  const auto relative_position =
+    Eigen::Vector3d(initial_rotation_matrix_ * world_relative_position_);
   const auto relative_orientation = math::geometry::convertEulerAngleToQuaternion(
     geometry_msgs::build<geometry_msgs::msg::Vector3>()
       .x(0)