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 2cca72e919722..a732d91ff586e 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 @@ -336,13 +336,13 @@ 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; - }; + // 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); + // 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) { @@ -352,18 +352,18 @@ 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; - } - } + // 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); @@ -1166,10 +1166,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 f32ee96623822..bdf2e8146d250 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;