From 6dd23398da3c31c5d489dab39bc26f6a9b585520 Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Mon, 27 May 2024 18:42:42 +0900 Subject: [PATCH] fix(crosswalk): fix calclation distance from the null polygons (#7122) Signed-off-by: Yuki Takagi --- .../src/scene_crosswalk.hpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index e9fdc38304603..faba8730aa6d9 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -182,7 +182,7 @@ class CrosswalkModule : public SceneModuleInterface const rclcpp::Time & now, const geometry_msgs::msg::Point & position, const double vel, const bool is_ego_yielding, const std::optional & collision_point, const PlannerParam & planner_param, const lanelet::BasicPolygon2d & crosswalk_polygon, - const bool is_object_on_ego_path) + const bool is_object_away_from_path) { const bool is_stopped = vel < planner_param.stop_object_velocity; @@ -202,7 +202,7 @@ class CrosswalkModule : public SceneModuleInterface planner_param.timeout_set_for_no_intention_to_walk, distance_to_crosswalk); const bool intent_to_cross = (now - *time_to_start_stopped).seconds() < timeout_no_intention_to_walk; - if (is_ego_yielding && !intent_to_cross && !is_object_on_ego_path) { + if (is_ego_yielding && !intent_to_cross && is_object_away_from_path) { collision_state = CollisionState::IGNORE; return; } @@ -261,15 +261,16 @@ class CrosswalkModule : public SceneModuleInterface // update current uuids current_uuids_.push_back(uuid); - const bool is_object_on_ego_path = - boost::geometry::distance(tier4_autoware_utils::fromMsg(position).to_2d(), attention_area) < - 0.5; + const bool is_object_away_from_path = + !attention_area.outer().empty() && + boost::geometry::distance(tier4_autoware_utils::fromMsg(position).to_2d(), attention_area) > + 0.5; // add new object if (objects.count(uuid) == 0) { if ( has_traffic_light && planner_param.disable_yield_for_new_stopped_object && - !is_object_on_ego_path) { + is_object_away_from_path) { objects.emplace(uuid, ObjectInfo{CollisionState::IGNORE}); } else { objects.emplace(uuid, ObjectInfo{CollisionState::YIELD}); @@ -279,7 +280,7 @@ class CrosswalkModule : public SceneModuleInterface // update object state objects.at(uuid).transitState( now, position, vel, is_ego_yielding, collision_point, planner_param, crosswalk_polygon, - is_object_on_ego_path); + is_object_away_from_path); objects.at(uuid).collision_point = collision_point; objects.at(uuid).position = position; objects.at(uuid).classification = classification;