diff --git a/docs/developer_guide/lane_pose_calculation/LanePoseCalculation.md b/docs/developer_guide/lane_pose_calculation/LanePoseCalculation.md index 6cdd0ab16f8..b00317b8735 100644 --- a/docs/developer_guide/lane_pose_calculation/LanePoseCalculation.md +++ b/docs/developer_guide/lane_pose_calculation/LanePoseCalculation.md @@ -19,12 +19,12 @@ These are the [timing immediately after the spawn of the Entity](Spawn.md), the The unit of the table is meter. -| EntityType \ Timing of calculation | [Spawn](Spawn.md) | [UpdateFrame](UpdateFrame.md) | [Getting longitudinal distance](GetLongitudinalDistance.md) | -| ---------------------------------- | ----------------- | -------------------------------------------------------------------------------------- | ----------------------------------------------------------- | -| EgoEntity | 2.0 | [(tread of the entity) + 2.0](UpdateFrame.md#calculate-pose-in-lane-coordinate-system) | 10 | -| VehicleEntity | 2.0 | N/A | 10 | -| PedestrianEntity | 2.0 | [2.0 or 4.0](UpdateFrame.md#pedestrian-entity-with-behavior-tree) | 10 | -| MiscObject | 2.0 | N/A | 10 | +| EntityType \ Timing of calculation | [Spawn](Spawn.md) | [UpdateFrame](UpdateFrame.md) | [Getting longitudinal distance](GetLongitudinalDistance.md) | +| ---------------------------------- | --------------------------------------------------------------------------------------------- | -------------------------------------------------------------------------------------- | ----------------------------------------------------------- | +| EgoEntity | [(tread of the entity) + 2.0](Spawn.md#calculate-pose-in-lane-coordinate-system) | [(tread of the entity) + 2.0](UpdateFrame.md#calculate-pose-in-lane-coordinate-system) | 10 | +| VehicleEntity | [(tread of the entity) + 2.0](Spawn.md#calculate-pose-in-lane-coordinate-system) | N/A | 10 | +| PedestrianEntity | [(width of the entity bounding box) + 2.0](Spawn.md#calculate-pose-in-lane-coordinate-system) | [2.0 or 4.0](UpdateFrame.md#pedestrian-entity-with-behavior-tree) | 10 | +| MiscObject | [(width of the entity bounding box) + 2.0](Spawn.md#calculate-pose-in-lane-coordinate-system) | N/A | 10 | ### Detail of the lane coordinate system calculation algorithm for a specific lane diff --git a/docs/developer_guide/lane_pose_calculation/Spawn.md b/docs/developer_guide/lane_pose_calculation/Spawn.md index 71a2e6a4a51..a78890e3fad 100644 --- a/docs/developer_guide/lane_pose_calculation/Spawn.md +++ b/docs/developer_guide/lane_pose_calculation/Spawn.md @@ -30,4 +30,14 @@ After narrowing down the candidate lanes by the above procedure, the pose of the ![Lane pose calculation](../../image/lane_pose_calculation.png "Lane pose calculation.") -In this case, the length of the horizontal bar is `2.0`m. +### Vehicle / Ego Entity + +Let $L_m$ be the length of the horizontal bar used in the lane coordinate system calculation and the tread of the front wheels be $t_f$ and the tread of the rear wheels be $t_r$. + +$$L_m = max(t_r, t_f) + 2.0$$ + +### Pedestrian / MiscObject Entity + +Let $L_m$ be the length of the horizontal bar used in the lane coordinate system calculation and width of the bounding box be $L_w$ + +$$L_m = L_w + 2.0$$ diff --git a/docs/developer_guide/lane_pose_calculation/UpdateFrame.md b/docs/developer_guide/lane_pose_calculation/UpdateFrame.md index 9a49a0dd474..bc201cc8d2f 100644 --- a/docs/developer_guide/lane_pose_calculation/UpdateFrame.md +++ b/docs/developer_guide/lane_pose_calculation/UpdateFrame.md @@ -18,7 +18,7 @@ If 3 is executed and fails, fallbacks to 4. ### Calculate pose in lane coordinate system -Let $L_m$ be the length of the horizontal bar used in the lane coordinate system calculation and the tread of the front wheels be $t_f$ and the tread of the rear wheels be $t_r$. +Let $L_m$ be the length of the horizontal bar used in the lane coordinate system calculation and the tread of the front wheels be $t_f$ and the tread of the rear wheels be $t_r$. See also [here.](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/traffic_simulator/src/entity/ego_entity.cpp#L278-L284) $$L_m = max(t_r, t_f) + 2.0$$ @@ -47,11 +47,11 @@ The procedure for calculating the pose in the lane coordinate system at this tim If lane matching was successful in the previous frame, do 1, otherwise do 2 -1.[Set the length of the horizontal bar to 2.0 and calculate the pose in the lane coordinate system](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c 27914c12/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp#L72-L77) -2.[Calculate the pose in the lane coordinate system considering the size of the BoundingBox of the Entity](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0 c27914c12/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp#L79-L83) +1.[Set the length of the horizontal bar to 2.0 + the width of the bounding box and calculate the pose in the lane coordinate system](https://github.com/tier4/scenario_simulator_v2/blob/a2c04ee2446f80aeacfe59fc87a6737ae18692cc/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp#L72-L77) +2.[Calculate the pose in the lane coordinate system considering the size of the BoundingBox of the Entity](https://github.com/tier4/scenario_simulator_v2/blob/a2c04ee2446f80aeacfe59fc87a6737ae18692cc/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp#L79-L85) If calculation 1 or 2 fails, -3.[Set the length of the horizontal bar to 4.0 and calculate the pose in the lane coordinate system](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c 27914c12/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp#L86-L91) +3.[Set the length of the horizontal bar to 4.0 and calculate the pose in the lane coordinate system](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp#L86-L91) If 1 or 2 are successful, then 3 is skipped. If the pose could not be calculated in the lane coordinate system by considering up to the result of 3, [the pose calculation in the lane coordinate system is a failure](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp#L125). diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp index cd1c395e997..107f053c2c7 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp +++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp @@ -78,15 +78,16 @@ class ActionNode : public BT::ActionNodeBase return { // clang-format off BT::InputPort("current_time"), + BT::InputPort("matching_distance_for_lanelet_pose_calculation"), BT::InputPort("step_time"), BT::InputPort("other_entity_status"), + BT::InputPort("route_lanelets"), BT::InputPort>("target_speed"), BT::InputPort>("hdmap_utils"), BT::InputPort>("entity_status"), + BT::InputPort>("traffic_light_manager"), BT::InputPort>("entity_type_list"), - BT::InputPort("route_lanelets"), BT::InputPort("request"), - BT::InputPort>("traffic_light_manager"), BT::OutputPort>("obstacle"), BT::OutputPort>("updated_status"), BT::OutputPort("waypoints"), @@ -116,6 +117,7 @@ class ActionNode : public BT::ActionNodeBase std::shared_ptr entity_status; double current_time; double step_time; + double default_matching_distance_for_lanelet_pose_calculation; std::optional target_speed; std::shared_ptr updated_status; EntityStatusDict other_entity_status; diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/behavior_tree.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/behavior_tree.hpp index ec5fe9fa38f..96edfb83cb0 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/behavior_tree.hpp +++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/behavior_tree.hpp @@ -49,27 +49,28 @@ class PedestrianBehaviorTree : public BehaviorPluginBase } // clang-format off - DEFINE_GETTER_SETTER(BehaviorParameter, traffic_simulator_msgs::msg::BehaviorParameter) - DEFINE_GETTER_SETTER(CurrentTime, double) - DEFINE_GETTER_SETTER(DebugMarker, std::vector) - DEFINE_GETTER_SETTER(EntityTypeList, EntityTypeDict) - DEFINE_GETTER_SETTER(GoalPoses, std::vector) - DEFINE_GETTER_SETTER(EntityStatus, std::shared_ptr) - DEFINE_GETTER_SETTER(PolylineTrajectory, std::shared_ptr) - DEFINE_GETTER_SETTER(HdMapUtils, std::shared_ptr) - DEFINE_GETTER_SETTER(LaneChangeParameters, traffic_simulator::lane_change::Parameter) - DEFINE_GETTER_SETTER(Obstacle, std::optional) - DEFINE_GETTER_SETTER(OtherEntityStatus, EntityStatusDict) - DEFINE_GETTER_SETTER(PedestrianParameters, traffic_simulator_msgs::msg::PedestrianParameters) - DEFINE_GETTER_SETTER(ReferenceTrajectory, std::shared_ptr) - DEFINE_GETTER_SETTER(Request, traffic_simulator::behavior::Request) - DEFINE_GETTER_SETTER(RouteLanelets, lanelet::Ids) - DEFINE_GETTER_SETTER(StepTime, double) - DEFINE_GETTER_SETTER(TargetSpeed, std::optional) - DEFINE_GETTER_SETTER(TrafficLightManager, std::shared_ptr) - DEFINE_GETTER_SETTER(UpdatedStatus, std::shared_ptr) - DEFINE_GETTER_SETTER(VehicleParameters, traffic_simulator_msgs::msg::VehicleParameters) - DEFINE_GETTER_SETTER(Waypoints, traffic_simulator_msgs::msg::WaypointsArray) + DEFINE_GETTER_SETTER(BehaviorParameter, traffic_simulator_msgs::msg::BehaviorParameter) + DEFINE_GETTER_SETTER(CurrentTime, double) + DEFINE_GETTER_SETTER(DebugMarker, std::vector) + DEFINE_GETTER_SETTER(DefaultMatchingDistanceForLaneletPoseCalculation, double) + DEFINE_GETTER_SETTER(EntityTypeList, EntityTypeDict) + DEFINE_GETTER_SETTER(GoalPoses, std::vector) + DEFINE_GETTER_SETTER(EntityStatus, std::shared_ptr) + DEFINE_GETTER_SETTER(PolylineTrajectory, std::shared_ptr) + DEFINE_GETTER_SETTER(HdMapUtils, std::shared_ptr) + DEFINE_GETTER_SETTER(LaneChangeParameters, traffic_simulator::lane_change::Parameter) + DEFINE_GETTER_SETTER(Obstacle, std::optional) + DEFINE_GETTER_SETTER(OtherEntityStatus, EntityStatusDict) + DEFINE_GETTER_SETTER(PedestrianParameters, traffic_simulator_msgs::msg::PedestrianParameters) + DEFINE_GETTER_SETTER(ReferenceTrajectory, std::shared_ptr) + DEFINE_GETTER_SETTER(Request, traffic_simulator::behavior::Request) + DEFINE_GETTER_SETTER(RouteLanelets, lanelet::Ids) + DEFINE_GETTER_SETTER(StepTime, double) + DEFINE_GETTER_SETTER(TargetSpeed, std::optional) + DEFINE_GETTER_SETTER(TrafficLightManager, std::shared_ptr) + DEFINE_GETTER_SETTER(UpdatedStatus, std::shared_ptr) + DEFINE_GETTER_SETTER(VehicleParameters, traffic_simulator_msgs::msg::VehicleParameters) + DEFINE_GETTER_SETTER(Waypoints, traffic_simulator_msgs::msg::WaypointsArray) // clang-format on #undef DEFINE_GETTER_SETTER diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/behavior_tree.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/behavior_tree.hpp index 5fa521af418..aa28673cf17 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/behavior_tree.hpp +++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/behavior_tree.hpp @@ -55,26 +55,27 @@ class VehicleBehaviorTree : public BehaviorPluginBase } // clang-format off - DEFINE_GETTER_SETTER(CurrentTime, double) - DEFINE_GETTER_SETTER(DebugMarker, std::vector) - DEFINE_GETTER_SETTER(EntityTypeList, EntityTypeDict) - DEFINE_GETTER_SETTER(GoalPoses, std::vector) - DEFINE_GETTER_SETTER(EntityStatus, std::shared_ptr) - DEFINE_GETTER_SETTER(PolylineTrajectory, std::shared_ptr) - DEFINE_GETTER_SETTER(HdMapUtils, std::shared_ptr) - DEFINE_GETTER_SETTER(LaneChangeParameters, traffic_simulator::lane_change::Parameter) - DEFINE_GETTER_SETTER(Obstacle, std::optional) - DEFINE_GETTER_SETTER(OtherEntityStatus, EntityStatusDict) - DEFINE_GETTER_SETTER(PedestrianParameters, traffic_simulator_msgs::msg::PedestrianParameters) - DEFINE_GETTER_SETTER(ReferenceTrajectory, std::shared_ptr) - DEFINE_GETTER_SETTER(Request, traffic_simulator::behavior::Request) - DEFINE_GETTER_SETTER(RouteLanelets, lanelet::Ids) - DEFINE_GETTER_SETTER(StepTime, double) - DEFINE_GETTER_SETTER(TargetSpeed, std::optional) - DEFINE_GETTER_SETTER(TrafficLightManager, std::shared_ptr) - DEFINE_GETTER_SETTER(UpdatedStatus, std::shared_ptr) - DEFINE_GETTER_SETTER(VehicleParameters, traffic_simulator_msgs::msg::VehicleParameters) - DEFINE_GETTER_SETTER(Waypoints, traffic_simulator_msgs::msg::WaypointsArray) + DEFINE_GETTER_SETTER(CurrentTime, double) + DEFINE_GETTER_SETTER(DebugMarker, std::vector) + DEFINE_GETTER_SETTER(DefaultMatchingDistanceForLaneletPoseCalculation, double) + DEFINE_GETTER_SETTER(EntityStatus, std::shared_ptr) + DEFINE_GETTER_SETTER(EntityTypeList, EntityTypeDict) + DEFINE_GETTER_SETTER(GoalPoses, std::vector) + DEFINE_GETTER_SETTER(HdMapUtils, std::shared_ptr) + DEFINE_GETTER_SETTER(LaneChangeParameters, traffic_simulator::lane_change::Parameter) + DEFINE_GETTER_SETTER(Obstacle, std::optional) + DEFINE_GETTER_SETTER(OtherEntityStatus, EntityStatusDict) + DEFINE_GETTER_SETTER(PedestrianParameters, traffic_simulator_msgs::msg::PedestrianParameters) + DEFINE_GETTER_SETTER(PolylineTrajectory, std::shared_ptr) + DEFINE_GETTER_SETTER(ReferenceTrajectory, std::shared_ptr) + DEFINE_GETTER_SETTER(Request, traffic_simulator::behavior::Request) + DEFINE_GETTER_SETTER(RouteLanelets, lanelet::Ids) + DEFINE_GETTER_SETTER(StepTime, double) + DEFINE_GETTER_SETTER(TargetSpeed, std::optional) + DEFINE_GETTER_SETTER(TrafficLightManager, std::shared_ptr) + DEFINE_GETTER_SETTER(UpdatedStatus, std::shared_ptr) + DEFINE_GETTER_SETTER(VehicleParameters, traffic_simulator_msgs::msg::VehicleParameters) + DEFINE_GETTER_SETTER(Waypoints, traffic_simulator_msgs::msg::WaypointsArray) // clang-format on #undef DEFINE_GETTER_SETTER diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 427bb9ba930..8ec69be9490 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -65,6 +65,13 @@ auto ActionNode::getBlackBoardValues() -> void target_speed = std::nullopt; } + if (!getInput( + "matching_distance_for_lanelet_pose_calculation", + default_matching_distance_for_lanelet_pose_calculation)) { + THROW_SIMULATION_ERROR( + "failed to get input matching_distance_for_lanelet_pose_calculation in ActionNode"); + } + if (!getInput("other_entity_status", other_entity_status)) { THROW_SIMULATION_ERROR("failed to get input other_entity_status in ActionNode"); } diff --git a/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp b/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp index 6a332be8cdf..3ea12d82dc7 100644 --- a/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp +++ b/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp @@ -70,17 +70,19 @@ auto PedestrianActionNode::estimateLaneletPose(const geometry_msgs::msg::Pose & std::optional lanelet_pose; if (entity_status->laneMatchingSucceed()) { /** - * @note Hard coded parameter. 1.0 is a matching threshold for lanelet. - * In this branch, try to matching pedestrian entity to specified lanelet_id. + * @note In this branch, try to matching pedestrian entity to specified lanelet_id. */ - lanelet_pose = - hdmap_utils->toLaneletPose(pose, entity_status->getLaneletPose().lanelet_id, 1.0); + lanelet_pose = hdmap_utils->toLaneletPose( + pose, entity_status->getLaneletPose().lanelet_id, + default_matching_distance_for_lanelet_pose_calculation); } else { /** * @note In this branch, try to matching pedestrian entity considering bounding box. * true means considering crosswalk. */ - lanelet_pose = hdmap_utils->toLaneletPose(pose, entity_status->getBoundingBox(), true); + lanelet_pose = hdmap_utils->toLaneletPose( + pose, entity_status->getBoundingBox(), true, + default_matching_distance_for_lanelet_pose_calculation); } if (!lanelet_pose) { /** diff --git a/simulation/do_nothing_plugin/include/do_nothing_plugin/plugin.hpp b/simulation/do_nothing_plugin/include/do_nothing_plugin/plugin.hpp index 8ae3617d028..4c6b2a95baa 100644 --- a/simulation/do_nothing_plugin/include/do_nothing_plugin/plugin.hpp +++ b/simulation/do_nothing_plugin/include/do_nothing_plugin/plugin.hpp @@ -45,22 +45,23 @@ public: \ TYPE get##NAME() override { return TYPE(); }; \ void set##NAME(const TYPE &) override{}; // clang-format off - DEFINE_GETTER_SETTER(DebugMarker, std::vector) - DEFINE_GETTER_SETTER(EntityTypeList, EntityTypeDict) - DEFINE_GETTER_SETTER(PolylineTrajectory, std::shared_ptr) - DEFINE_GETTER_SETTER(GoalPoses, std::vector) - DEFINE_GETTER_SETTER(HdMapUtils, std::shared_ptr) - DEFINE_GETTER_SETTER(LaneChangeParameters, traffic_simulator::lane_change::Parameter) - DEFINE_GETTER_SETTER(Obstacle, std::optional) - DEFINE_GETTER_SETTER(OtherEntityStatus, EntityStatusDict) - DEFINE_GETTER_SETTER(PedestrianParameters, traffic_simulator_msgs::msg::PedestrianParameters) - DEFINE_GETTER_SETTER(ReferenceTrajectory, std::shared_ptr) - DEFINE_GETTER_SETTER(Request, traffic_simulator::behavior::Request) - DEFINE_GETTER_SETTER(RouteLanelets, lanelet::Ids) - DEFINE_GETTER_SETTER(TargetSpeed, std::optional) - DEFINE_GETTER_SETTER(TrafficLightManager, std::shared_ptr) - DEFINE_GETTER_SETTER(VehicleParameters, traffic_simulator_msgs::msg::VehicleParameters) - DEFINE_GETTER_SETTER(Waypoints, traffic_simulator_msgs::msg::WaypointsArray) + DEFINE_GETTER_SETTER(DebugMarker, std::vector) + DEFINE_GETTER_SETTER(DefaultMatchingDistanceForLaneletPoseCalculation, double) + DEFINE_GETTER_SETTER(EntityTypeList, EntityTypeDict) + DEFINE_GETTER_SETTER(PolylineTrajectory, std::shared_ptr) + DEFINE_GETTER_SETTER(GoalPoses, std::vector) + DEFINE_GETTER_SETTER(HdMapUtils, std::shared_ptr) + DEFINE_GETTER_SETTER(LaneChangeParameters, traffic_simulator::lane_change::Parameter) + DEFINE_GETTER_SETTER(Obstacle, std::optional) + DEFINE_GETTER_SETTER(OtherEntityStatus, EntityStatusDict) + DEFINE_GETTER_SETTER(PedestrianParameters, traffic_simulator_msgs::msg::PedestrianParameters) + DEFINE_GETTER_SETTER(ReferenceTrajectory, std::shared_ptr) + DEFINE_GETTER_SETTER(Request, traffic_simulator::behavior::Request) + DEFINE_GETTER_SETTER(RouteLanelets, lanelet::Ids) + DEFINE_GETTER_SETTER(TargetSpeed, std::optional) + DEFINE_GETTER_SETTER(TrafficLightManager, std::shared_ptr) + DEFINE_GETTER_SETTER(VehicleParameters, traffic_simulator_msgs::msg::VehicleParameters) + DEFINE_GETTER_SETTER(Waypoints, traffic_simulator_msgs::msg::WaypointsArray) // clang-format on #undef DEFINE_GETTER_SETTER diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp index 19d8c62bd16..8fed80d249f 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp @@ -55,27 +55,28 @@ class BehaviorPluginBase } // clang-format off - DEFINE_GETTER_SETTER(BehaviorParameter, "behavior_parameter", traffic_simulator_msgs::msg::BehaviorParameter) - DEFINE_GETTER_SETTER(CurrentTime, "current_time", double) - DEFINE_GETTER_SETTER(DebugMarker, "debug_marker", std::vector) - DEFINE_GETTER_SETTER(EntityStatus, "entity_status", std::shared_ptr) - DEFINE_GETTER_SETTER(EntityTypeList, "entity_type_list", EntityTypeDict) - DEFINE_GETTER_SETTER(GoalPoses, "goal_poses", std::vector) - DEFINE_GETTER_SETTER(HdMapUtils, "hdmap_utils", std::shared_ptr) - DEFINE_GETTER_SETTER(LaneChangeParameters, "lane_change_parameters", traffic_simulator::lane_change::Parameter) - DEFINE_GETTER_SETTER(Obstacle, "obstacle", std::optional) - DEFINE_GETTER_SETTER(OtherEntityStatus, "other_entity_status", EntityStatusDict) - DEFINE_GETTER_SETTER(PedestrianParameters, "pedestrian_parameters", traffic_simulator_msgs::msg::PedestrianParameters) - DEFINE_GETTER_SETTER(PolylineTrajectory, "polyline_trajectory", std::shared_ptr) - DEFINE_GETTER_SETTER(ReferenceTrajectory, "reference_trajectory", std::shared_ptr) - DEFINE_GETTER_SETTER(Request, "request", traffic_simulator::behavior::Request) - DEFINE_GETTER_SETTER(RouteLanelets, "route_lanelets", lanelet::Ids) - DEFINE_GETTER_SETTER(StepTime, "step_time", double) - DEFINE_GETTER_SETTER(TargetSpeed, "target_speed", std::optional) - DEFINE_GETTER_SETTER(TrafficLightManager, "traffic_light_manager", std::shared_ptr) - DEFINE_GETTER_SETTER(UpdatedStatus, "updated_status", std::shared_ptr) - DEFINE_GETTER_SETTER(VehicleParameters, "vehicle_parameters", traffic_simulator_msgs::msg::VehicleParameters) - DEFINE_GETTER_SETTER(Waypoints, "waypoints", traffic_simulator_msgs::msg::WaypointsArray) + DEFINE_GETTER_SETTER(BehaviorParameter, "behavior_parameter", traffic_simulator_msgs::msg::BehaviorParameter) + DEFINE_GETTER_SETTER(CurrentTime, "current_time", double) + DEFINE_GETTER_SETTER(DebugMarker, "debug_marker", std::vector) + DEFINE_GETTER_SETTER(DefaultMatchingDistanceForLaneletPoseCalculation, "matching_distance_for_lanelet_pose_calculation", double) + DEFINE_GETTER_SETTER(EntityStatus, "entity_status", std::shared_ptr) + DEFINE_GETTER_SETTER(EntityTypeList, "entity_type_list", EntityTypeDict) + DEFINE_GETTER_SETTER(GoalPoses, "goal_poses", std::vector) + DEFINE_GETTER_SETTER(HdMapUtils, "hdmap_utils", std::shared_ptr) + DEFINE_GETTER_SETTER(LaneChangeParameters, "lane_change_parameters", traffic_simulator::lane_change::Parameter) + DEFINE_GETTER_SETTER(Obstacle, "obstacle", std::optional) + DEFINE_GETTER_SETTER(OtherEntityStatus, "other_entity_status", EntityStatusDict) + DEFINE_GETTER_SETTER(PedestrianParameters, "pedestrian_parameters", traffic_simulator_msgs::msg::PedestrianParameters) + DEFINE_GETTER_SETTER(PolylineTrajectory, "polyline_trajectory", std::shared_ptr) + DEFINE_GETTER_SETTER(ReferenceTrajectory, "reference_trajectory", std::shared_ptr) + DEFINE_GETTER_SETTER(Request, "request", traffic_simulator::behavior::Request) + DEFINE_GETTER_SETTER(RouteLanelets, "route_lanelets", lanelet::Ids) + DEFINE_GETTER_SETTER(StepTime, "step_time", double) + DEFINE_GETTER_SETTER(TargetSpeed, "target_speed", std::optional) + DEFINE_GETTER_SETTER(TrafficLightManager, "traffic_light_manager", std::shared_ptr) + DEFINE_GETTER_SETTER(UpdatedStatus, "updated_status", std::shared_ptr) + DEFINE_GETTER_SETTER(VehicleParameters, "vehicle_parameters", traffic_simulator_msgs::msg::VehicleParameters) + DEFINE_GETTER_SETTER(Waypoints, "waypoints", traffic_simulator_msgs::msg::WaypointsArray) // clang-format on #undef DEFINE_GETTER_SETTER }; diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp index 81399e55e91..ad9547569dd 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -137,6 +137,8 @@ class EntityBase /* */ auto getMapPoseFromRelativePose(const geometry_msgs::msg::Pose &) const -> geometry_msgs::msg::Pose; + virtual auto getDefaultMatchingDistanceForLaneletPoseCalculation() const -> double; + virtual auto getObstacle() -> std::optional = 0; virtual auto getRouteLanelets(double horizon = 100) -> lanelet::Ids = 0; 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 0359bb27b28..44fb81c32ee 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -520,7 +520,24 @@ class EntityManager } else { entity_status.pose = pose; - if (const auto lanelet_pose = toLaneletPose(pose, parameters.bounding_box, false); + /// @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, + entity_status.type.type == traffic_simulator_msgs::msg::EntityType::PEDESTRIAN || + entity_status.type.type == traffic_simulator_msgs::msg::EntityType::MISC_OBJECT, + [](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; diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp index b7b8507e767..5b75362db12 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp @@ -76,6 +76,8 @@ class VehicleEntity : public EntityBase auto getDefaultDynamicConstraints() const -> const traffic_simulator_msgs::msg::DynamicConstraints & override; + auto getDefaultMatchingDistanceForLaneletPoseCalculation() const -> double override; + auto getBehaviorParameter() const -> traffic_simulator_msgs::msg::BehaviorParameter override; auto getEntityType() const -> const traffic_simulator_msgs::msg::EntityType & override; diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index cf59bb9dfa4..30cbaa0a083 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -275,24 +275,15 @@ 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; - - // @note The lanelet matching algorithm should be equivalent to the one used in EgoEntitySimulation::getMatchedLaneletPoseFromEntityStatus - const auto get_matching_length = [&] { - return std::max( - vehicle_parameters.axles.front_axle.track_width, - vehicle_parameters.axles.rear_axle.track_width) * - 0.5 + - 1.0; - }; if (unique_route_lanelets.empty()) { - lanelet_pose = - hdmap_utils_ptr_->toLaneletPose(map_pose, getBoundingBox(), false, get_matching_length()); + lanelet_pose = hdmap_utils_ptr_->toLaneletPose( + map_pose, getBoundingBox(), false, getDefaultMatchingDistanceForLaneletPoseCalculation()); } else { - lanelet_pose = - hdmap_utils_ptr_->toLaneletPose(map_pose, unique_route_lanelets, get_matching_length()); + lanelet_pose = hdmap_utils_ptr_->toLaneletPose( + map_pose, unique_route_lanelets, getDefaultMatchingDistanceForLaneletPoseCalculation()); if (!lanelet_pose) { - lanelet_pose = - hdmap_utils_ptr_->toLaneletPose(map_pose, getBoundingBox(), false, get_matching_length()); + lanelet_pose = hdmap_utils_ptr_->toLaneletPose( + map_pose, getBoundingBox(), false, getDefaultMatchingDistanceForLaneletPoseCalculation()); } } geometry_msgs::msg::Pose map_pose_z_fixed = map_pose; diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index f29591fb621..016f1aec130 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -254,6 +254,11 @@ auto EntityBase::getMapPoseFromRelativePose(const geometry_msgs::msg::Pose & rel return ret; } +auto EntityBase::getDefaultMatchingDistanceForLaneletPoseCalculation() const -> double +{ + return getBoundingBox().dimensions.y * 0.5 + 1.0; +} + auto EntityBase::isTargetSpeedReached(double target_speed) const -> bool { return speed_planner_->isTargetSpeedReached(target_speed, getCurrentTwist()); diff --git a/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp b/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp index 743726912fb..c4f02ff3694 100644 --- a/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp +++ b/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp @@ -42,6 +42,8 @@ PedestrianEntity::PedestrianEntity( behavior_plugin_ptr_->setDebugMarker({}); behavior_plugin_ptr_->setBehaviorParameter(traffic_simulator_msgs::msg::BehaviorParameter()); behavior_plugin_ptr_->setHdMapUtils(hdmap_utils_ptr_); + behavior_plugin_ptr_->setDefaultMatchingDistanceForLaneletPoseCalculation( + getDefaultMatchingDistanceForLaneletPoseCalculation()); } void PedestrianEntity::appendDebugMarker(visualization_msgs::msg::MarkerArray & marker_array) diff --git a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp index f07e8355d23..2bd9a67537e 100644 --- a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp +++ b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp @@ -42,6 +42,8 @@ VehicleEntity::VehicleEntity( behavior_plugin_ptr_->setDebugMarker({}); behavior_plugin_ptr_->setBehaviorParameter(traffic_simulator_msgs::msg::BehaviorParameter()); behavior_plugin_ptr_->setHdMapUtils(hdmap_utils_ptr_); + behavior_plugin_ptr_->setDefaultMatchingDistanceForLaneletPoseCalculation( + getDefaultMatchingDistanceForLaneletPoseCalculation()); } void VehicleEntity::appendDebugMarker(visualization_msgs::msg::MarkerArray & marker_array) @@ -72,6 +74,15 @@ auto VehicleEntity::getDefaultDynamicConstraints() const return default_dynamic_constraints; } +auto VehicleEntity::getDefaultMatchingDistanceForLaneletPoseCalculation() const -> double +{ + return std::max( + vehicle_parameters.axles.front_axle.track_width, + vehicle_parameters.axles.rear_axle.track_width) * + 0.5 + + 1.0; +} + auto VehicleEntity::getBehaviorParameter() const -> traffic_simulator_msgs::msg::BehaviorParameter { return behavior_plugin_ptr_->getBehaviorParameter();