fix: fit WorldPosition to lanelet in spawn function

Signed-off-by: Kotaro Yoshimoto <[email protected]>

simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp

@@ -518,8 +518,6 @@ class EntityManager
         entity_status.lanelet_pose = static_cast<LaneletPose>(pose);
         entity_status.lanelet_pose_valid = true;
       } else {
-        entity_status.pose = pose;
-
         /// @note If the entity is pedestrian or misc object, we have to consider matching to crosswalk lanelet.
         if (const auto lanelet_pose = toLaneletPose(
               pose, parameters.bounding_box,
@@ -541,8 +539,11 @@ class EntityManager
             lanelet_pose) {
           entity_status.lanelet_pose = *lanelet_pose;
           entity_status.lanelet_pose_valid = true;
+          // @note fix z, roll and pitch to fitting to the lanelet
+          entity_status.pose = toMapPose(*lanelet_pose);
         } else {
           entity_status.lanelet_pose_valid = false;
+          entity_status.pose = pose;
         }
       }