From 5ee3bc2d5a92cf8aaee012e7cc6d3921ba48fd05 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Sun, 19 Nov 2023 18:06:18 +0900 Subject: [PATCH] fix(crosswalk): don't stop in front of the crosswalk if vehicle stuck in intersection Signed-off-by: satoshi-ota --- .../config/crosswalk.param.yaml | 1 + .../util.hpp | 2 +- .../src/manager.cpp | 19 ++++++++++++------- .../src/scene_crosswalk.cpp | 19 ++++++++++++++++--- .../src/scene_crosswalk.hpp | 10 +++++++--- .../src/util.cpp | 14 +++++++------- .../src/manager.cpp | 2 +- 7 files changed, 45 insertions(+), 22 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index a4f9e9d7ce23d..762639950b2c5 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -27,6 +27,7 @@ # param for stuck vehicles stuck_vehicle: + enable_stuck_check_in_intersection: false # [-] flag to enable stuck vehicle checking for crosswalk which is in intersection stuck_vehicle_velocity: 1.0 # [m/s] maximum velocity threshold whether the vehicle is stuck max_stuck_vehicle_lateral_offset: 2.0 # [m] maximum lateral offset for stuck vehicle position should be looked stuck_vehicle_attention_range: 10.0 # [m] the detection area is defined as X meters behind the crosswalk 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 6c70e7a7f6dc0..1cf6a0dd5021a 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 @@ -82,7 +82,7 @@ struct DebugData std::vector> obj_polygons; }; -std::vector getCrosswalksOnPath( +std::vector> getCrosswalksOnPath( const geometry_msgs::msg::Pose & current_pose, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index dd8dc95a8ad3c..c743d440bee85 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -63,6 +63,8 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) cp.no_relax_velocity = getOrDeclareParameter(node, ns + ".slow_down.no_relax_velocity"); // param for stuck vehicle + cp.enable_stuck_check_in_intersection = + getOrDeclareParameter(node, ns + ".stuck_vehicle.enable_stuck_check_in_intersection"); cp.stuck_vehicle_velocity = getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_velocity"); cp.max_stuck_vehicle_lateral_offset = @@ -129,8 +131,9 @@ void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) const auto rh = planner_data_->route_handler_; const auto launch = [this, &path]( - const auto lane_id, const std::optional & reg_elem_id) { - if (isModuleRegistered(lane_id)) { + const auto road_lanelet_id, const auto crosswalk_lanelet_id, + const std::optional & reg_elem_id) { + if (isModuleRegistered(crosswalk_lanelet_id)) { return; } @@ -141,10 +144,12 @@ void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) // NOTE: module_id is always a lane id so that isModuleRegistered works correctly in the case // where both regulatory element and non-regulatory element crosswalks exist. registerModule(std::make_shared( - node_, lane_id, reg_elem_id, lanelet_map_ptr, p, logger, clock_)); - generateUUID(lane_id); + node_, road_lanelet_id, crosswalk_lanelet_id, reg_elem_id, lanelet_map_ptr, p, logger, + clock_)); + generateUUID(crosswalk_lanelet_id); updateRTCStatus( - getUUID(lane_id), true, std::numeric_limits::lowest(), path.header.stamp); + getUUID(crosswalk_lanelet_id), true, std::numeric_limits::lowest(), + path.header.stamp); }; const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( @@ -152,14 +157,14 @@ void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) for (const auto & crosswalk : crosswalk_leg_elem_map) { // NOTE: The former id is a lane id, and the latter one is a regulatory element's id. - launch(crosswalk.first->crosswalkLanelet().id(), crosswalk.first->id()); + launch(crosswalk.second.id(), crosswalk.first->crosswalkLanelet().id(), crosswalk.first->id()); } const auto crosswalk_lanelets = getCrosswalksOnPath( planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr()); for (const auto & crosswalk : crosswalk_lanelets) { - launch(crosswalk.id(), std::nullopt); + launch(crosswalk.first, crosswalk.second.id(), std::nullopt); } } diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 56f5072a2ade9..5cd2a1305ff86 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -176,9 +176,10 @@ tier4_debug_msgs::msg::StringStamped createStringStampedMessage( } // namespace CrosswalkModule::CrosswalkModule( - rclcpp::Node & node, const int64_t module_id, const std::optional & reg_elem_id, - const lanelet::LaneletMapPtr & lanelet_map_ptr, const PlannerParam & planner_param, - const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) + rclcpp::Node & node, const int64_t lane_id, const int64_t module_id, + const std::optional & reg_elem_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, + const PlannerParam & planner_param, const rclcpp::Logger & logger, + const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), module_id_(module_id), planner_param_(planner_param), @@ -200,6 +201,8 @@ CrosswalkModule::CrosswalkModule( crosswalk_ = lanelet_map_ptr->laneletLayer.get(module_id); } + road_ = lanelet_map_ptr->laneletLayer.get(lane_id); + collision_info_pub_ = node.create_publisher("~/debug/collision_info", 1); } @@ -803,6 +806,16 @@ std::optional CrosswalkModule::checkStopForStuckVehicles( return {}; } + // skip stuck vehicle checking for crosswalk which is in intersection. + if (!p.enable_stuck_check_in_intersection) { + std::string turn_direction = road_.attributeOr("turn_direction", "else"); + if (turn_direction == "right" || turn_direction == "left" || turn_direction == "straight") { + if (!road_.regulatoryElementsAs().empty()) { + return {}; + } + } + } + for (const auto & object : objects) { if (!isVehicle(object)) { continue; diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 0768101179857..ef6d01fc43c23 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -121,6 +121,7 @@ class CrosswalkModule : public SceneModuleInterface double max_slow_down_accel; double no_relax_velocity; // param for stuck vehicle + bool enable_stuck_check_in_intersection{false}; double stuck_vehicle_velocity; double max_stuck_vehicle_lateral_offset; double stuck_vehicle_attention_range; @@ -299,9 +300,10 @@ class CrosswalkModule : public SceneModuleInterface }; CrosswalkModule( - rclcpp::Node & node, const int64_t module_id, const std::optional & reg_elem_id, - const lanelet::LaneletMapPtr & lanelet_map_ptr, const PlannerParam & planner_param, - const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); + rclcpp::Node & node, const int64_t lane_id, const int64_t module_id, + const std::optional & reg_elem_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, + const PlannerParam & planner_param, const rclcpp::Logger & logger, + const rclcpp::Clock::SharedPtr clock); bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; @@ -406,6 +408,8 @@ class CrosswalkModule : public SceneModuleInterface lanelet::ConstLanelet crosswalk_; + lanelet::ConstLanelet road_; + lanelet::ConstLineStrings3d stop_lines_; // Parameter diff --git a/planning/behavior_velocity_crosswalk_module/src/util.cpp b/planning/behavior_velocity_crosswalk_module/src/util.cpp index 47bbef7111da0..756eb520a76c5 100644 --- a/planning/behavior_velocity_crosswalk_module/src/util.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/util.cpp @@ -53,19 +53,19 @@ using tier4_autoware_utils::createPoint; using tier4_autoware_utils::Line2d; using tier4_autoware_utils::Point2d; -std::vector getCrosswalksOnPath( +std::vector> getCrosswalksOnPath( const geometry_msgs::msg::Pose & current_pose, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, const std::shared_ptr & overall_graphs) { - std::vector crosswalks; + std::vector> crosswalks; // Add current lane id const auto nearest_lane_id = behavior_velocity_planner::planning_utils::getNearestLaneId(path, lanelet_map, current_pose); - std::vector unique_lane_ids; + std::vector unique_lane_ids; if (nearest_lane_id) { // Add subsequent lane_ids from nearest lane_id unique_lane_ids = behavior_velocity_planner::planning_utils::getSubsequentLaneIdsSetOnPath( @@ -81,24 +81,24 @@ std::vector getCrosswalksOnPath( constexpr int PEDESTRIAN_GRAPH_ID = 1; const auto conflicting_crosswalks = overall_graphs->conflictingInGraph(ll, PEDESTRIAN_GRAPH_ID); for (const auto & crosswalk : conflicting_crosswalks) { - crosswalks.push_back(crosswalk); + crosswalks.emplace_back(lane_id, crosswalk); } } return crosswalks; } -std::set getCrosswalkIdSetOnPath( +std::set getCrosswalkIdSetOnPath( const geometry_msgs::msg::Pose & current_pose, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, const std::shared_ptr & overall_graphs) { - std::set crosswalk_id_set; + std::set crosswalk_id_set; for (const auto & crosswalk : getCrosswalksOnPath(current_pose, path, lanelet_map, overall_graphs)) { - crosswalk_id_set.insert(crosswalk.id()); + crosswalk_id_set.insert(crosswalk.second.id()); } return crosswalk_id_set; diff --git a/planning/behavior_velocity_walkway_module/src/manager.cpp b/planning/behavior_velocity_walkway_module/src/manager.cpp index 8995c2362a8cd..d427e57009cf6 100644 --- a/planning/behavior_velocity_walkway_module/src/manager.cpp +++ b/planning/behavior_velocity_walkway_module/src/manager.cpp @@ -75,7 +75,7 @@ void WalkwayModuleManager::launchNewModules(const PathWithLaneId & path) planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr()); for (const auto & crosswalk : crosswalk_lanelets) { - launch(crosswalk, false); + launch(crosswalk.second, false); } }