diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index 6559f39278fdc..240963309e9a2 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -63,6 +63,7 @@ # param for target object filtering object_filtering: crosswalk_attention_range: 1.0 # [m] the detection area is defined as -X meters before the crosswalk to +X meters behind the crosswalk + vehicle_object_cross_angle_threshold: 0.5 # [rad] threshold judging object is crossing walkway as a pedestrian or passing over as a vehicle target_object: unknown: true # [-] whether to look and stop by UNKNOWN objects bicycle: true # [-] whether to look and stop by BICYCLE objects diff --git a/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp b/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp index 3f4af4a5a1138..77de7d241c05c 100644 --- a/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp +++ b/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp @@ -50,6 +50,8 @@ enum class CollisionState { YIELD, EGO_PASS_FIRST, EGO_PASS_LATER, IGNORE }; struct CollisionPoint { geometry_msgs::msg::Point collision_point{}; + std::optional crosswalk_passage_direction{ + std::nullopt}; // denote obj is passing the crosswalk along the vehicle lane double time_to_collision{}; double time_to_vehicle{}; }; diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index cb0311ffe8ebb..b8190cb6124e7 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -116,6 +116,8 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) // param for target area & object cp.crosswalk_attention_range = getOrDeclareParameter(node, ns + ".object_filtering.crosswalk_attention_range"); + cp.vehicle_object_cross_angle_threshold = getOrDeclareParameter( + node, ns + ".object_filtering.vehicle_object_cross_angle_threshold"); cp.look_unknown = getOrDeclareParameter(node, ns + ".object_filtering.target_object.unknown"); cp.look_bicycle = diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 239eddca1fed9..69440aaddc2d0 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -25,6 +25,7 @@ #include #include +#include #include #include @@ -316,6 +317,8 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( const double ego_vel = planner_data_->current_velocity->twist.linear.x; const double ego_acc = planner_data_->current_acceleration->accel.accel.linear.x; + const std::optional ego_crosswalk_passage_direction = + findEgoPassageDirectionAlongPath(sparse_resample_path); const auto base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; const auto dist_ego_to_stop = calcSignedArcLength(ego_path.points, ego_pos, p_stop_line->first) + p_stop_line->second; @@ -349,25 +352,44 @@ 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> nearest_stop_info; std::vector stop_factor_points; for (const auto & object : object_info_manager_.getObject()) { - const auto & collision_point = object.collision_point; - if (collision_point) { + const auto & collision_point_opt = object.collision_point; + if (collision_point_opt) { + const auto & collision_point = collision_point_opt.value(); const auto & collision_state = object.collision_state; if (collision_state != CollisionState::YIELD) { continue; } + if ( + isVehicleType(object.classification) && ego_crosswalk_passage_direction && + collision_point.crosswalk_passage_direction) { + const double direction_diff = tier4_autoware_utils::normalizeRadian( + collision_point.crosswalk_passage_direction.value() - + ego_crosswalk_passage_direction.value()); + RCLCPP_INFO( + rclcpp::get_logger("temp"), + "collision_point.crosswalk_passage_direction = %f, ego_crosswalk_passage_direction = %f, " + "direction_diff = %f", + collision_point.crosswalk_passage_direction.value(), + ego_crosswalk_passage_direction.value(), direction_diff); + if (std::fabs(direction_diff) < planner_param_.vehicle_object_cross_angle_threshold) { + continue; + } + } stop_factor_points.push_back(object.position); const auto dist_ego2cp = - calcSignedArcLength( - sparse_resample_path.points, ego_pos, collision_point->collision_point) - + calcSignedArcLength(sparse_resample_path.points, ego_pos, collision_point.collision_point) - planner_param_.stop_distance_from_object; if (!nearest_stop_info || dist_ego2cp - base_link2front < nearest_stop_info->second) { nearest_stop_info = - std::make_pair(collision_point->collision_point, dist_ego2cp - base_link2front); + std::make_pair(collision_point.collision_point, dist_ego2cp - base_link2front); } } } @@ -580,6 +602,70 @@ std::pair CrosswalkModule::clampAttentionRangeByNeighborCrosswal return std::make_pair(clamped_near_attention_range, clamped_far_attention_range); } +std::optional CrosswalkModule::findEgoPassageDirectionAlongPath( + const PathWithLaneId & path) const +{ + auto findIntersectPoint = [&](const lanelet::ConstLineString3d line) + -> std::optional> { + const auto line_start = + tier4_autoware_utils::createPoint(line.front().x(), line.front().y(), line.front().z()); + const auto line_end = + tier4_autoware_utils::createPoint(line.back().x(), line.back().y(), line.back().z()); + for (unsigned i = 0; i < path.points.size() - 1; ++i) { + const auto & start = path.points.at(i).point.pose.position; + const auto & end = path.points.at(i + 1).point.pose.position; + if (const auto intersect = tier4_autoware_utils::intersect(line_start, line_end, start, end); + intersect.has_value()) { + return std::make_optional(std::make_pair(i, intersect.value())); + } + } + return std::nullopt; + }; + const auto intersect_pt1 = findIntersectPoint(crosswalk_.leftBound()); + const auto intersect_pt2 = findIntersectPoint(crosswalk_.rightBound()); + + if (!intersect_pt1 || !intersect_pt2) { + return std::nullopt; + } + const auto idx1 = intersect_pt1.value().first, idx2 = intersect_pt2.value().first; + const auto & front = idx1 > idx2 ? intersect_pt2.value().second : intersect_pt1.value().second; + const auto & back = idx1 > idx2 ? intersect_pt1.value().second : intersect_pt2.value().second; + return std::atan2(back.y - front.y, back.x - front.x); +} + +std::optional CrosswalkModule::findObjectPassageDirectionAlongVehicleLane( + const autoware_auto_perception_msgs::msg::PredictedPath & path) const +{ + using tier4_autoware_utils::Segment2d; + + auto findIntersectPoint = [&](const lanelet::ConstLineString3d line) + -> std::optional> { + const auto line_start = + tier4_autoware_utils::createPoint(line.front().x(), line.front().y(), line.front().z()); + const auto line_end = + tier4_autoware_utils::createPoint(line.back().x(), line.back().y(), line.back().z()); + for (unsigned i = 0; i < path.path.size() - 1; ++i) { + const auto & start = path.path.at(i).position; + const auto & end = path.path.at(i + 1).position; + if (const auto intersect = tier4_autoware_utils::intersect(line_start, line_end, start, end); + intersect.has_value()) { + return std::make_optional(std::make_pair(i, intersect.value())); + } + } + return std::nullopt; + }; + const auto intersect_pt1 = findIntersectPoint(crosswalk_.leftBound()); + const auto intersect_pt2 = findIntersectPoint(crosswalk_.rightBound()); + + if (!intersect_pt1 || !intersect_pt2) { + return std::nullopt; + } + const auto idx1 = intersect_pt1.value().first, idx2 = intersect_pt2.value().first; + const auto & front = idx1 > idx2 ? intersect_pt2.value().second : intersect_pt1.value().second; + const auto & back = idx1 > idx2 ? intersect_pt1.value().second : intersect_pt2.value().second; + return std::atan2(back.y - front.y, back.x - front.x); +} + std::optional CrosswalkModule::getCollisionPoint( const PathWithLaneId & ego_path, const PredictedObject & object, const std::pair & crosswalk_attention_range, const Polygon2d & attention_area) @@ -625,6 +711,8 @@ std::optional CrosswalkModule::getCollisionPoint( // Calculate multi-step object polygon, and reset start_idx const size_t start_idx_with_margin = std::max(static_cast(start_idx) - 1, 0); const size_t end_idx_with_margin = std::min(i + 1, obj_path.path.size() - 1); + const auto object_crosswalk_passage_direction = + findObjectPassageDirectionAlongVehicleLane(obj_path); const auto obj_multi_step_polygon = createMultiStepPolygon( obj_path.path, obj_polygon, start_idx_with_margin, end_idx_with_margin); is_start_idx_initialized = false; @@ -660,7 +748,8 @@ std::optional CrosswalkModule::getCollisionPoint( if (dist_ego2cp < minimum_stop_dist) { minimum_stop_dist = dist_ego2cp; nearest_collision_point = createCollisionPoint( - intersection_center_point, dist_ego2cp, dist_obj2cp, ego_vel, obj_vel); + intersection_center_point, dist_ego2cp, dist_obj2cp, ego_vel, obj_vel, + object_crosswalk_passage_direction); debug_data_.obj_polygons.push_back( toGeometryPointVector(obj_multi_step_polygon, ego_pos.z)); } @@ -679,17 +768,18 @@ std::optional CrosswalkModule::getCollisionPoint( CollisionPoint CrosswalkModule::createCollisionPoint( const geometry_msgs::msg::Point & nearest_collision_point, const double dist_ego2cp, const double dist_obj2cp, const geometry_msgs::msg::Vector3 & ego_vel, - const geometry_msgs::msg::Vector3 & obj_vel) const + const geometry_msgs::msg::Vector3 & obj_vel, + const std::optional object_crosswalk_passage_direction) const { constexpr double min_ego_velocity = 1.38; // [m/s] - - const auto & base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + const auto base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; const auto estimated_velocity = std::hypot(obj_vel.x, obj_vel.y); const auto velocity = std::max(planner_param_.min_object_velocity, estimated_velocity); CollisionPoint collision_point{}; collision_point.collision_point = nearest_collision_point; + collision_point.crosswalk_passage_direction = object_crosswalk_passage_direction; collision_point.time_to_collision = std::max(0.0, dist_ego2cp - planner_param_.stop_distance_from_object - base_link2front) / std::max(ego_vel.x, min_ego_velocity); @@ -931,7 +1021,8 @@ void CrosswalkModule::updateObjectState( getCollisionPoint(sparse_resample_path, object, crosswalk_attention_range, attention_area); 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, planner_param_, crosswalk_.polygon2d().basicPolygon()); + has_traffic_light, collision_point, object.classification.front().label, planner_param_, + crosswalk_.polygon2d().basicPolygon()); if (collision_point) { const auto collision_state = object_info_manager_.getCollisionState(obj_uuid); diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index ef6d01fc43c23..7f020fbe5422c 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -150,6 +150,7 @@ class CrosswalkModule : public SceneModuleInterface double traffic_light_state_timeout; // param for target area & object double crosswalk_attention_range; + double vehicle_object_cross_angle_threshold; bool look_unknown; bool look_bicycle; bool look_motorcycle; @@ -160,6 +161,7 @@ class CrosswalkModule : public SceneModuleInterface { CollisionState collision_state{}; std::optional time_to_start_stopped{std::nullopt}; + uint8_t classification{ObjectClassification::UNKNOWN}; geometry_msgs::msg::Point position{}; std::optional collision_point{}; @@ -242,8 +244,8 @@ class CrosswalkModule : public SceneModuleInterface void update( const std::string & uuid, const geometry_msgs::msg::Point & position, const double vel, const rclcpp::Time & now, const bool is_ego_yielding, const bool has_traffic_light, - const std::optional & collision_point, const PlannerParam & planner_param, - const lanelet::BasicPolygon2d & crosswalk_polygon) + const std::optional & collision_point, const uint8_t classification, + const PlannerParam & planner_param, const lanelet::BasicPolygon2d & crosswalk_polygon) { // update current uuids current_uuids_.push_back(uuid); @@ -265,6 +267,7 @@ class CrosswalkModule : public SceneModuleInterface crosswalk_polygon); objects.at(uuid).collision_point = collision_point; objects.at(uuid).position = position; + objects.at(uuid).classification = classification; } void finalize() { @@ -330,6 +333,11 @@ class CrosswalkModule : public SceneModuleInterface const std::vector & path_intersects, const std::optional & stop_pose) const; + std::optional findEgoPassageDirectionAlongPath( + const PathWithLaneId & sparse_resample_path) const; + std::optional findObjectPassageDirectionAlongVehicleLane( + const autoware_auto_perception_msgs::msg::PredictedPath & path) const; + std::optional getCollisionPoint( const PathWithLaneId & ego_path, const PredictedObject & object, const std::pair & crosswalk_attention_range, const Polygon2d & attention_area); @@ -366,7 +374,8 @@ class CrosswalkModule : public SceneModuleInterface CollisionPoint createCollisionPoint( const geometry_msgs::msg::Point & nearest_collision_point, const double dist_ego2cp, const double dist_obj2cp, const geometry_msgs::msg::Vector3 & ego_vel, - const geometry_msgs::msg::Vector3 & obj_vel) const; + const geometry_msgs::msg::Vector3 & obj_vel, + const std::optional object_crosswalk_passage_direction) const; float calcTargetVelocity( const geometry_msgs::msg::Point & stop_point, const PathWithLaneId & ego_path) const;