diff --git a/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp b/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp index 4083b616e55..3d09f35dcca 100644 --- a/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp +++ b/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp @@ -473,6 +473,7 @@ auto EgoEntitySimulation::fillLaneletDataAndSnapZToLanelet( traffic_simulator::helper::getUniqueValues(autoware->getRouteLanelets()); std::optional lanelet_pose; + // the lanelet matching algorithm should be equivalent to the one used in EgoEntity::setMapPose const auto get_matching_length = [&] { return std::max( vehicle_parameters.axles.front_axle.track_width, diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index a9fb5ac3fc1..aea0e89b002 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -275,6 +275,8 @@ auto EgoEntity::setMapPose(const geometry_msgs::msg::Pose & map_pose) -> void { const auto unique_route_lanelets = traffic_simulator::helper::getUniqueValues(getRouteLanelets()); std::optional lanelet_pose; + + // the lanelet matching algorithm should be equivalent to the one used in EgoEntitySimulation::fillLaneletDataAndSnapZToLanelet const auto get_matching_length = [&] { return std::max( vehicle_parameters.axles.front_axle.track_width,