From e86f92ba55065fd76f9df40142bea4296dbfe31b Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Tue, 21 Jan 2025 13:27:22 +0900 Subject: [PATCH] chore(crosswalk): port the same direction ignore block (#9983) Signed-off-by: yuki-takagi-66 --- .../src/scene_crosswalk.cpp | 22 +++------------- .../src/scene_crosswalk.hpp | 26 ++++++++++++++++--- 2 files changed, 26 insertions(+), 22 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 52a877f3d9590..43f2f6617e5d0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -337,13 +337,8 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( // Check pedestrian for stop // NOTE: first stop point and its minimum distance from ego to stop - auto isVehicleType = [](const uint8_t label) { - return label == ObjectClassification::MOTORCYCLE || label == ObjectClassification::BICYCLE; - }; std::optional dist_nearest_cp; std::vector stop_factor_points; - const std::optional ego_crosswalk_passage_direction = - findEgoPassageDirectionAlongPath(sparse_resample_path); for (const auto & object : object_info_manager_.getObject()) { const auto & collision_point_opt = object.collision_point; if (collision_point_opt) { @@ -353,19 +348,6 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( continue; } - if ( - isVehicleType(object.classification) && ego_crosswalk_passage_direction && - collision_point.crosswalk_passage_direction) { - double direction_diff = std::abs(std::fmod( - collision_point.crosswalk_passage_direction.value() - - ego_crosswalk_passage_direction.value(), - M_PI_2)); - direction_diff = std::min(direction_diff, M_PI_2 - direction_diff); - if (direction_diff < planner_param_.vehicle_object_cross_angle_threshold) { - continue; - } - } - stop_factor_points.push_back(object.position); const auto dist_ego2cp = @@ -1167,10 +1149,12 @@ void CrosswalkModule::updateObjectState( const auto collision_point = getCollisionPoint(sparse_resample_path, object, crosswalk_attention_range, attention_area); + const std::optional ego_crosswalk_passage_direction = + findEgoPassageDirectionAlongPath(sparse_resample_path); object_info_manager_.update( obj_uuid, obj_pos, std::hypot(obj_vel.x, obj_vel.y), clock_->now(), is_ego_yielding, has_traffic_light, collision_point, object.classification.front().label, planner_param_, - crosswalk_.polygon2d().basicPolygon(), attention_area); + crosswalk_.polygon2d().basicPolygon(), attention_area, ego_crosswalk_passage_direction); const auto collision_state = object_info_manager_.getCollisionState(obj_uuid); if (collision_point) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index b9803207a1a31..cc9b8733eb6b6 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -194,7 +194,8 @@ class CrosswalkModule : public SceneModuleInterfaceWithRTC 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_away_from_path) + const bool is_object_away_from_path, + const std::optional & ego_crosswalk_passage_direction) { const bool is_stopped = vel < planner_param.stop_object_velocity; @@ -224,6 +225,24 @@ class CrosswalkModule : public SceneModuleInterfaceWithRTC // Compare time to collision and vehicle if (collision_point) { + auto isVehicleType = [](const uint8_t label) { + return label == ObjectClassification::MOTORCYCLE || + label == ObjectClassification::BICYCLE; + }; + if ( + isVehicleType(classification) && ego_crosswalk_passage_direction && + collision_point->crosswalk_passage_direction) { + double direction_diff = std::abs(std::fmod( + collision_point->crosswalk_passage_direction.value() - + ego_crosswalk_passage_direction.value(), + M_PI_2)); + direction_diff = std::min(direction_diff, M_PI_2 - direction_diff); + if (direction_diff < planner_param.vehicle_object_cross_angle_threshold) { + collision_state = CollisionState::IGNORE; + return; + } + } + // Check if ego will pass first const double ego_pass_first_additional_margin = collision_state == CollisionState::EGO_PASS_FIRST @@ -268,7 +287,8 @@ class CrosswalkModule : public SceneModuleInterfaceWithRTC const rclcpp::Time & now, const bool is_ego_yielding, const bool has_traffic_light, const std::optional & collision_point, const uint8_t classification, const PlannerParam & planner_param, const lanelet::BasicPolygon2d & crosswalk_polygon, - const Polygon2d & attention_area) + const Polygon2d & attention_area, + const std::optional & ego_crosswalk_passage_direction) { // update current uuids current_uuids_.push_back(uuid); @@ -292,7 +312,7 @@ class CrosswalkModule : public SceneModuleInterfaceWithRTC // update object state objects.at(uuid).transitState( now, position, vel, is_ego_yielding, collision_point, planner_param, crosswalk_polygon, - is_object_away_from_path); + is_object_away_from_path, ego_crosswalk_passage_direction); objects.at(uuid).collision_point = collision_point; objects.at(uuid).position = position; objects.at(uuid).classification = classification;