From 2e82078889365a12b0db7142086c68458ce931a7 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Wed, 21 Feb 2024 14:55:28 +0900 Subject: [PATCH] fix mathing algorithum Signed-off-by: Masaya Kataoka --- .../traffic_simulator/entity/entity_manager.hpp | 15 +++++++++++++-- 1 file changed, 13 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 796be55f374..d675e34c902 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -289,7 +289,6 @@ class EntityManager FORWARD_TO_ENTITY(getCurrentAccel, const); FORWARD_TO_ENTITY(getCurrentAction, const); FORWARD_TO_ENTITY(getCurrentTwist, const); - FORWARD_TO_ENTITY(getDefaultMatchingDistanceForLaneletPoseCalculation, const); FORWARD_TO_ENTITY(getDistanceToLaneBound, ); FORWARD_TO_ENTITY(getDistanceToLaneBound, const); FORWARD_TO_ENTITY(getDistanceToLeftLaneBound, ); @@ -526,7 +525,19 @@ class EntityManager pose, parameters.bounding_box, entity_status.type.type == traffic_simulator_msgs::msg::EntityType::PEDESTRIAN || entity_status.type.type == traffic_simulator_msgs::msg::EntityType::MISC_OBJECT, - getDefaultMatchingDistanceForLaneletPoseCalculation(name)); + [](const auto & parameters) { + if constexpr (std::is_same_v< + std::decay_t, + traffic_simulator_msgs::msg::VehicleParameters>) { + return std::max( + parameters.axles.front_axle.track_width, + parameters.axles.rear_axle.track_width) * + 0.5 + + 1.0; + } else { + return parameters.bounding_box.dimensions.y * 0.5 + 1.0; + } + }(parameters)); lanelet_pose) { entity_status.lanelet_pose = *lanelet_pose; entity_status.lanelet_pose_valid = true;