From 66310655fffd303743e8b80f5fd86265ca4ac412 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Tue, 27 Feb 2024 20:02:35 +0900 Subject: [PATCH] fix: fit WorldPosition to lanelet in spawn function Signed-off-by: Kotaro Yoshimoto --- .../include/traffic_simulator/entity/entity_manager.hpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp index 44fb81c32ee..822c0248d0a 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -518,8 +518,6 @@ class EntityManager entity_status.lanelet_pose = static_cast(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; } }