From 10f899071c28965495b49d16c58e369dc6c31cd9 Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Fri, 24 May 2024 15:29:21 +0900 Subject: [PATCH 1/2] fix Signed-off-by: Yuki Takagi --- .../behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index e9fdc38304603..4c8fef099b43c 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -262,8 +262,9 @@ class CrosswalkModule : public SceneModuleInterface current_uuids_.push_back(uuid); const bool is_object_on_ego_path = + !attention_area.outer().empty() && boost::geometry::distance(tier4_autoware_utils::fromMsg(position).to_2d(), attention_area) < - 0.5; + 0.5; // add new object if (objects.count(uuid) == 0) { From ce9ad849b64ac0371f97ec45505a68b7b4f7b63e Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Mon, 27 May 2024 11:35:05 +0900 Subject: [PATCH 2/2] boolean inversion Signed-off-by: Yuki Takagi --- .../src/scene_crosswalk.hpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 4c8fef099b43c..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,16 +261,16 @@ class CrosswalkModule : public SceneModuleInterface // update current uuids current_uuids_.push_back(uuid); - const bool is_object_on_ego_path = + const bool is_object_away_from_path = !attention_area.outer().empty() && - boost::geometry::distance(tier4_autoware_utils::fromMsg(position).to_2d(), attention_area) < + 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}); @@ -280,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;