From aa045e76ae9ae63b503243d4a21d8b6711ab6fbc Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 16 Feb 2023 19:26:19 +0900 Subject: [PATCH 1/8] has assoc_ids memeber Signed-off-by: Mamoru Sobue --- .../scene_module/intersection/manager.hpp | 14 +- .../intersection/scene_intersection.hpp | 12 +- .../scene_merge_from_private_road.hpp | 12 +- .../include/utilization/util.hpp | 9 ++ .../src/scene_module/intersection/manager.cpp | 124 +++++++----------- .../intersection/scene_intersection.cpp | 7 +- .../scene_merge_from_private_road.cpp | 4 +- .../src/utilization/util.cpp | 29 ++++ 8 files changed, 109 insertions(+), 102 deletions(-) diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp index 01c21d2197202..7e22a2be65d71 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp @@ -44,12 +44,7 @@ class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC std::function &)> getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; - bool hasSameParentLaneletAndTurnDirectionWith( - const lanelet::ConstLanelet & lane, const size_t module_id, - const std::string & turn_direction_lane) const; - - bool hasSameParentLaneletAndTurnDirectionWithRegistered( - const lanelet::ConstLanelet & lane, const std::string & turn_direction) const; + bool hasSameParentLaneletAndTurnDirectionWithRegistered(const lanelet::ConstLanelet & lane) const; }; class MergeFromPrivateModuleManager : public SceneModuleManagerInterface @@ -67,12 +62,7 @@ class MergeFromPrivateModuleManager : public SceneModuleManagerInterface std::function &)> getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; - bool hasSameParentLaneletAndTurnDirectionWith( - const lanelet::ConstLanelet & lane, const size_t module_id, - const std::string & turn_direction_lane) const; - - bool hasSameParentLaneletAndTurnDirectionWithRegistered( - const lanelet::ConstLanelet & lane, const std::string & turn_direction) const; + bool hasSameParentLaneletAndTurnDirectionWithRegistered(const lanelet::ConstLanelet & lane) const; }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index bd3f3c10cf0fa..fd4f10625dbe2 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -33,6 +33,7 @@ #include #include +#include #include #include #include @@ -97,8 +98,8 @@ class IntersectionModule : public SceneModuleInterface IntersectionModule( const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, - const PlannerParam & planner_param, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock); + const PlannerParam & planner_param, const std::set & assoc_ids, + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); /** * @brief plan go-stop velocity at traffic crossing with collision check between reference path @@ -109,14 +110,19 @@ class IntersectionModule : public SceneModuleInterface visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray() override; + const std::set & getAssocIds() const { return assoc_ids_; } + private: - int64_t lane_id_; + const int64_t lane_id_; std::string turn_direction_; bool has_traffic_light_; bool is_go_out_; // Parameter PlannerParam planner_param_; std::optional intersection_lanelets_; + // for an intersection lane l1, its associative lanes are those that share same parent lanelet and + // have same turn_direction + const std::set assoc_ids_; /** * @brief check collision for all lanelet area & predicted objects (call checkPathCollision() as diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp index 6db961152d80e..c631a96c127b8 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp @@ -31,6 +31,7 @@ #include #include +#include #include #include @@ -73,8 +74,8 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface MergeFromPrivateRoadModule( const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, - const PlannerParam & planner_param, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock); + const PlannerParam & planner_param, const std::set & assoc_ids, + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); /** * @brief plan go-stop velocity at traffic crossing with collision check between reference path @@ -85,10 +86,11 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray() override; + const std::set & getAssocIds() const { return assoc_ids_; } + private: - int64_t lane_id_; - std::string turn_direction_; - bool has_traffic_light_; + const int64_t lane_id_; + const std::set assoc_ids_; autoware_auto_planning_msgs::msg::PathWithLaneId extractPathNearExitOfPrivateRoad( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const double extend_length); diff --git a/planning/behavior_velocity_planner/include/utilization/util.hpp b/planning/behavior_velocity_planner/include/utilization/util.hpp index e28d75b653c08..43ffc2c1c43ba 100644 --- a/planning/behavior_velocity_planner/include/utilization/util.hpp +++ b/planning/behavior_velocity_planner/include/utilization/util.hpp @@ -292,6 +292,15 @@ boost::optional insertStopPoint( const geometry_msgs::msg::Point & stop_point, PathWithLaneId & output); boost::optional insertStopPoint( const geometry_msgs::msg::Point & stop_point, const size_t stop_seg_idx, PathWithLaneId & output); + +/* + @brief return 'associatvie' lanes in the intersection. 'associative' means that a lane shares same + or lane-changeable parent lanes with `lane` and has same turn_direction value. + */ +std::set getAssociativeIntersectionLanelets( + lanelet::ConstLanelet lane, const lanelet::LaneletMapPtr lanelet_map, + const lanelet::routing::RoutingGraphPtr routing_graph); + } // namespace planning_utils } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp index f1289afd25b06..14f92827e310a 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp @@ -26,18 +26,6 @@ namespace behavior_velocity_planner { -bool hasSameLanelet(const lanelet::ConstLanelets & lanes1, const lanelet::ConstLanelets & lanes2) -{ - for (const auto & lane1 : lanes1) { - for (const auto & lane2 : lanes2) { - if (lane1.id() == lane2.id()) { - return true; - } - } - } - return false; -} - IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC(node, getModuleName()) { @@ -91,8 +79,11 @@ MergeFromPrivateModuleManager::MergeFromPrivateModuleManager(rclcpp::Node & node void IntersectionModuleManager::launchNewModules( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { - const auto lanelets = planning_utils::getLaneletsOnPath( - path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); + const auto routing_graph = planner_data_->route_handler_->getRoutingGraphPtr(); + const auto lanelet_map = planner_data_->route_handler_->getLaneletMapPtr(); + + const auto lanelets = + planning_utils::getLaneletsOnPath(path, lanelet_map, planner_data_->current_odometry->pose); for (size_t i = 0; i < lanelets.size(); i++) { const auto ll = lanelets.at(i); const auto lane_id = ll.id(); @@ -106,12 +97,14 @@ void IntersectionModuleManager::launchNewModules( continue; } - if (hasSameParentLaneletAndTurnDirectionWithRegistered(ll, turn_direction)) { + if (hasSameParentLaneletAndTurnDirectionWithRegistered(ll)) { continue; } + const auto assoc_ids = + planning_utils::getAssociativeIntersectionLanelets(ll, lanelet_map, routing_graph); registerModule(std::make_shared( - module_id, lane_id, planner_data_, intersection_param_, + module_id, lane_id, planner_data_, intersection_param_, assoc_ids, logger_.get_child("intersection_module"), clock_)); generateUUID(module_id); updateRTCStatus( @@ -122,8 +115,11 @@ void IntersectionModuleManager::launchNewModules( void MergeFromPrivateModuleManager::launchNewModules( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { - const auto lanelets = planning_utils::getLaneletsOnPath( - path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); + const auto routing_graph = planner_data_->route_handler_->getRoutingGraphPtr(); + const auto lanelet_map = planner_data_->route_handler_->getLaneletMapPtr(); + + const auto lanelets = + planning_utils::getLaneletsOnPath(path, lanelet_map, planner_data_->current_odometry->pose); for (size_t i = 0; i < lanelets.size(); i++) { const auto ll = lanelets.at(i); const auto lane_id = ll.id(); @@ -148,12 +144,18 @@ void MergeFromPrivateModuleManager::launchNewModules( continue; } + if (hasSameParentLaneletAndTurnDirectionWithRegistered(ll)) { + continue; + } + if (i + 1 < lanelets.size()) { const auto next_lane = lanelets.at(i + 1); const std::string next_lane_location = next_lane.attributeOr("location", "else"); if (next_lane_location != "private") { + const auto assoc_ids = + planning_utils::getAssociativeIntersectionLanelets(ll, lanelet_map, routing_graph); registerModule(std::make_shared( - module_id, lane_id, planner_data_, merge_from_private_area_param_, + module_id, lane_id, planner_data_, merge_from_private_area_param_, assoc_ids, logger_.get_child("merge_from_private_road_module"), clock_)); continue; } @@ -164,8 +166,10 @@ void MergeFromPrivateModuleManager::launchNewModules( for (auto && conflicting_lanelet : conflicting_lanelets) { const std::string conflicting_attr = conflicting_lanelet.attributeOr("location", "else"); if (conflicting_attr == "urban") { + const auto assoc_ids = + planning_utils::getAssociativeIntersectionLanelets(ll, lanelet_map, routing_graph); registerModule(std::make_shared( - module_id, lane_id, planner_data_, merge_from_private_area_param_, + module_id, lane_id, planner_data_, merge_from_private_area_param_, assoc_ids, logger_.get_child("merge_from_private_road_module"), clock_)); continue; } @@ -182,6 +186,8 @@ IntersectionModuleManager::getModuleExpiredFunction( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); return [this, lane_set](const std::shared_ptr & scene_module) { + const auto intersection_module = std::dynamic_pointer_cast(scene_module); + const auto & assoc_ids = intersection_module->getAssocIds(); for (const auto & lane : lane_set) { const std::string turn_direction = lane.attributeOr("turn_direction", "else"); const auto is_intersection = @@ -189,8 +195,8 @@ IntersectionModuleManager::getModuleExpiredFunction( if (!is_intersection) { continue; } - if (hasSameParentLaneletAndTurnDirectionWith( - lane, scene_module->getModuleId(), turn_direction)) { + + if (assoc_ids.find(lane.id()) != assoc_ids.end() /* contains */) { return false; } } @@ -206,6 +212,9 @@ MergeFromPrivateModuleManager::getModuleExpiredFunction( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); return [this, lane_set](const std::shared_ptr & scene_module) { + const auto merge_from_private_module = + std::dynamic_pointer_cast(scene_module); + const auto & assoc_ids = merge_from_private_module->getAssocIds(); for (const auto & lane : lane_set) { const std::string turn_direction = lane.attributeOr("turn_direction", "else"); const auto is_intersection = @@ -213,8 +222,8 @@ MergeFromPrivateModuleManager::getModuleExpiredFunction( if (!is_intersection) { continue; } - if (hasSameParentLaneletAndTurnDirectionWith( - lane, scene_module->getModuleId(), turn_direction)) { + + if (assoc_ids.find(lane.id()) != assoc_ids.end() /* contains */) { return false; } } @@ -222,68 +231,27 @@ MergeFromPrivateModuleManager::getModuleExpiredFunction( }; } -bool IntersectionModuleManager::hasSameParentLaneletAndTurnDirectionWith( - const lanelet::ConstLanelet & lane, const size_t module_id, - const std::string & turn_direction_lane) const -{ - const auto registered_lane = planner_data_->route_handler_->getLaneletsFromId(module_id); - const auto turn_direction_registered = registered_lane.attributeOr("turn_direction", "else"); - if (turn_direction_registered != turn_direction_lane) { - return false; - } - lanelet::ConstLanelets registered_parents = - planner_data_->route_handler_->getPreviousLanelets(registered_lane); - lanelet::ConstLanelets parents = planner_data_->route_handler_->getPreviousLanelets(lane); - for (const auto & ll : registered_parents) { - auto neighbor_lanes = planner_data_->route_handler_->getLaneChangeableNeighbors(ll); - neighbor_lanes.push_back(ll); - if (hasSameLanelet(parents, neighbor_lanes)) { - return true; - } - } - return false; -} - bool IntersectionModuleManager::hasSameParentLaneletAndTurnDirectionWithRegistered( - const lanelet::ConstLanelet & lane, const std::string & turn_direction) const + const lanelet::ConstLanelet & lane) const { - lanelet::ConstLanelets parents = planner_data_->route_handler_->getPreviousLanelets(lane); - - for (const auto & id : registered_module_id_set_) { - const auto registered_lane = planner_data_->route_handler_->getLaneletsFromId(id); - const auto turn_direction_registered = registered_lane.attributeOr("turn_direction", "else"); - if (turn_direction_registered != turn_direction) { - continue; - } - lanelet::ConstLanelets registered_parents = - planner_data_->route_handler_->getPreviousLanelets(registered_lane); - for (const auto & ll : registered_parents) { - auto neighbor_lanes = planner_data_->route_handler_->getLaneChangeableNeighbors(ll); - neighbor_lanes.push_back(ll); - if (hasSameLanelet(parents, neighbor_lanes)) { - return true; - } + for (const auto & scene_module : scene_modules_) { + const auto intersection_module = std::dynamic_pointer_cast(scene_module); + const auto & assoc_ids = intersection_module->getAssocIds(); + if (assoc_ids.find(lane.id()) != assoc_ids.end()) { + return true; } } return false; } -bool MergeFromPrivateModuleManager::hasSameParentLaneletAndTurnDirectionWith( - const lanelet::ConstLanelet & lane, const size_t module_id, - const std::string & turn_direction) const +bool MergeFromPrivateModuleManager::hasSameParentLaneletAndTurnDirectionWithRegistered( + const lanelet::ConstLanelet & lane) const { - const auto registered_lane = planner_data_->route_handler_->getLaneletsFromId(module_id); - const auto turn_direction_registered = registered_lane.attributeOr("turn_direction", "else"); - if (turn_direction_registered != turn_direction) { - return false; - } - lanelet::ConstLanelets registered_parents = - planner_data_->route_handler_->getPreviousLanelets(registered_lane); - lanelet::ConstLanelets parents = planner_data_->route_handler_->getPreviousLanelets(lane); - for (const auto & ll : registered_parents) { - auto neighbor_lanes = planner_data_->route_handler_->getLaneChangeableNeighbors(ll); - neighbor_lanes.push_back(ll); - if (hasSameLanelet(parents, neighbor_lanes)) { + for (const auto & scene_module : scene_modules_) { + const auto merge_from_private_module = + std::dynamic_pointer_cast(scene_module); + const auto & assoc_ids = merge_from_private_module->getAssocIds(); + if (assoc_ids.find(lane.id()) != assoc_ids.end()) { return true; } } diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index 1dad05f96fb22..4aa837dcf8b89 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -53,9 +53,12 @@ static geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( IntersectionModule::IntersectionModule( const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, - const PlannerParam & planner_param, const rclcpp::Logger logger, + const PlannerParam & planner_param, const std::set & assoc_ids, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), lane_id_(lane_id), is_go_out_(false) +: SceneModuleInterface(module_id, logger, clock), + lane_id_(lane_id), + is_go_out_(false), + assoc_ids_(assoc_ids) { velocity_factor_.init(VelocityFactor::INTERSECTION); planner_param_ = planner_param; diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp index a395fc00d5153..bc7dc3a8e073f 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp @@ -35,9 +35,9 @@ namespace bg = boost::geometry; MergeFromPrivateRoadModule::MergeFromPrivateRoadModule( const int64_t module_id, const int64_t lane_id, [[maybe_unused]] std::shared_ptr planner_data, - const PlannerParam & planner_param, const rclcpp::Logger logger, + const PlannerParam & planner_param, const std::set & assoc_ids, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), lane_id_(lane_id) +: SceneModuleInterface(module_id, logger, clock), lane_id_(lane_id), assoc_ids_(assoc_ids) { velocity_factor_.init(VelocityFactor::MERGE); planner_param_ = planner_param; diff --git a/planning/behavior_velocity_planner/src/utilization/util.cpp b/planning/behavior_velocity_planner/src/utilization/util.cpp index 16434f5f10e9c..2fab91c4d2144 100644 --- a/planning/behavior_velocity_planner/src/utilization/util.cpp +++ b/planning/behavior_velocity_planner/src/utilization/util.cpp @@ -668,5 +668,34 @@ boost::optional insertStopPoint( return tier4_autoware_utils::getPose(output.points.at(insert_idx.get())); } + +std::set getAssociativeIntersectionLanelets( + lanelet::ConstLanelet lane, const lanelet::LaneletMapPtr lanelet_map, + const lanelet::routing::RoutingGraphPtr routing_graph) +{ + const std::string turn_direction = lane.attributeOr("turn_direction", "else"); + if (turn_direction.compare("else") == 0) { + return {}; + } + + const auto parents = routing_graph->previous(lane); + std::set parent_neighbors; + for (const auto & parent : parents) { + const auto neighbors = routing_graph->besides(parent); + for (const auto & neighbor : neighbors) parent_neighbors.insert(neighbor.id()); + } + std::set assocs; + for (const auto & parent_neighbor_id : parent_neighbors) { + const auto parent_neighbor = lanelet_map->laneletLayer.get(parent_neighbor_id); + const auto followings = routing_graph->following(parent_neighbor); + for (const auto & following : followings) { + if (following.attributeOr("turn_direction", "else") == turn_direction) { + assocs.insert(following.id()); + } + } + } + return assocs; +} + } // namespace planning_utils } // namespace behavior_velocity_planner From 76aa7187028383dc834bc0ff68b9bd7a7e3b0892 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 16 Feb 2023 20:03:05 +0900 Subject: [PATCH 2/8] trim path points based on assoc_ids Signed-off-by: Mamoru Sobue --- .../scene_module/intersection/scene_intersection.hpp | 2 +- .../include/scene_module/intersection/util.hpp | 8 +++++--- .../scene_module/intersection/scene_intersection.cpp | 9 ++++----- .../intersection/scene_merge_from_private_road.cpp | 2 +- .../src/scene_module/intersection/util.cpp | 11 ++++++----- 5 files changed, 17 insertions(+), 15 deletions(-) diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index fd4f10625dbe2..3efc6d4750bbd 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -190,7 +190,7 @@ class IntersectionModule : public SceneModuleInterface */ TimeDistanceArray calcIntersectionPassingTime( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, - const int objective_lane_id, const double time_delay) const; + const double time_delay) const; /** * @brief check if the object has a target type for collision check diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp index 6fa4d184239da..e0bf1b2fd3ff2 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp @@ -30,6 +30,7 @@ #include #include #include +#include #include #include #include @@ -43,9 +44,10 @@ std::optional insertPoint( const geometry_msgs::msg::Pose & in_pose, autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path); -bool hasLaneId(const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p, const int id); -std::optional> findLaneIdInterval( - const autoware_auto_planning_msgs::msg::PathWithLaneId & p, const int lane_id); +bool hasLaneIds( + const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p, const std::set & ids); +std::optional> findLaneIdsInterval( + const autoware_auto_planning_msgs::msg::PathWithLaneId & p, const std::set & ids); std::optional getDuplicatedPointIdx( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & point); diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index 4aa837dcf8b89..9d2a80f72fa6a 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -131,7 +131,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * setDistance(std::numeric_limits::lowest()); return false; } - const auto lane_interval_ip_opt = util::findLaneIdInterval(path_ip, lane_id_); + const auto lane_interval_ip_opt = util::findLaneIdsInterval(path_ip, assoc_ids_); if (!lane_interval_ip_opt.has_value()) { RCLCPP_WARN(logger_, "Path has no interval on intersection lane %ld", lane_id_); RCLCPP_DEBUG(logger_, "===== plan end ====="); @@ -385,8 +385,7 @@ bool IntersectionModule::checkCollision( /* check collision between target_objects predicted path and ego lane */ // cut the predicted path at passing_time - const auto time_distance_array = - calcIntersectionPassingTime(path, closest_idx, lane_id_, time_delay); + const auto time_distance_array = calcIntersectionPassingTime(path, closest_idx, time_delay); const double passing_time = time_distance_array.back().first; cutPredictPathWithDuration(&target_objects, passing_time); @@ -530,7 +529,7 @@ Polygon2d IntersectionModule::generateEgoIntersectionLanePolygon( TimeDistanceArray IntersectionModule::calcIntersectionPassingTime( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, - const int objective_lane_id, const double time_delay) const + const double time_delay) const { double dist_sum = 0.0; int assigned_lane_found = false; @@ -542,7 +541,7 @@ TimeDistanceArray IntersectionModule::calcIntersectionPassingTime( auto reference_point = path.points.at(i); reference_point.point.longitudinal_velocity_mps = planner_param_.intersection_velocity; reference_path.points.push_back(reference_point); - bool has_objective_lane_id = util::hasLaneId(path.points.at(i), objective_lane_id); + bool has_objective_lane_id = util::hasLaneIds(path.points.at(i), assoc_ids_); if (assigned_lane_found && !has_objective_lane_id) { break; } diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp index bc7dc3a8e073f..cc6c5d8769ea3 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp @@ -82,7 +82,7 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR setDistance(std::numeric_limits::lowest()); return false; } - const auto lane_interval_ip_opt = util::findLaneIdInterval(path_ip, lane_id_); + const auto lane_interval_ip_opt = util::findLaneIdsInterval(path_ip, assoc_ids_); if (!lane_interval_ip_opt.has_value()) { RCLCPP_WARN(logger_, "Path has no interval on intersection lane %ld", lane_id_); RCLCPP_DEBUG(logger_, "===== plan end ====="); diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp index e4ae13c6ce7ce..7d8731f8f3051 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp @@ -68,24 +68,25 @@ std::optional insertPoint( return insert_idx; } -bool hasLaneId(const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p, const int id) +bool hasLaneIds( + const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p, const std::set & ids) { for (const auto & pid : p.lane_ids) { - if (pid == id) { + if (ids.find(pid) != ids.end()) { return true; } } return false; } -std::optional> findLaneIdInterval( - const autoware_auto_planning_msgs::msg::PathWithLaneId & p, const int lane_id) +std::optional> findLaneIdsInterval( + const autoware_auto_planning_msgs::msg::PathWithLaneId & p, const std::set & ids) { bool found = false; size_t start = 0; size_t end = p.points.size() > 0 ? p.points.size() - 1 : 0; for (size_t i = 0; i < p.points.size(); ++i) { - if (hasLaneId(p.points.at(i), lane_id)) { + if (hasLaneIds(p.points.at(i), ids)) { if (!found) { // found interval for the first time found = true; From 043441014bba57fb78bd59430798f59988ad38be Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 17 Feb 2023 19:58:02 +0900 Subject: [PATCH 3/8] getIntersectionSegmentPolygon should return it as a lanelet object Signed-off-by: Mamoru Sobue --- .../intersection/scene_intersection.hpp | 25 +++-- .../scene_module/intersection/util.hpp | 5 +- .../intersection/scene_intersection.cpp | 97 +++++++++++-------- .../src/scene_module/intersection/util.cpp | 9 +- 4 files changed, 76 insertions(+), 60 deletions(-) diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index 3efc6d4750bbd..f58f3c71615cf 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -141,9 +141,9 @@ class IntersectionModule : public SceneModuleInterface const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const lanelet::ConstLanelets & detection_area_lanelets, const lanelet::ConstLanelets & adjacent_lanelets, - const std::optional & intersection_area, + const std::optional & intersection_area, const Polygon2d & ego_poly, const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, - const int closest_idx, const Polygon2d & stuck_vehicle_detect_area, const double time_delay); + const int closest_idx, const double time_delay); /** * @brief Check if there is a stopped vehicle on the ego-lane. @@ -167,10 +167,9 @@ class IntersectionModule : public SceneModuleInterface * @param ignore_dist ignore distance from the start point of the ego-intersection lane * @return generated polygon */ - Polygon2d generateEgoIntersectionLanePolygon( + Polygon2d generateStuckVehicleDetectAreaPolygon( lanelet::LaneletMapConstPtr lanelet_map_ptr, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, - const double extra_dist, const double ignore_dist) const; + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx) const; /** * @brief Modify objects predicted path. remove path point if the time exceeds timer_thr. @@ -227,11 +226,19 @@ class IntersectionModule : public SceneModuleInterface const double margin = 0); /** - * @brief Get lanes including ego lanelet and next lanelet - * @param lanelet_map_ptr lanelet map - * @param path ego-car lane - * @return ego lanelet and next lanelet + * @brief Get path polygon of intersection part and next lane part + * @return trimmed path polygon */ + Polygon2d getIntersectionAndNextSegmentPolygon( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const double width) const; + + /** + * @brief Get path polygon of intersetion part + * @return trimmed path polygon + */ + Polygon2d getIntersectionSegmentPolygon( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const double width) const; + lanelet::ConstLanelets getEgoLaneWithNextLane( lanelet::LaneletMapConstPtr lanelet_map_ptr, const autoware_auto_planning_msgs::msg::PathWithLaneId & path) const; diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp index e0bf1b2fd3ff2..f512f7500cee5 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp @@ -85,7 +85,7 @@ std::optional generateStopLine( " @param use_stuck_stopline if true, a stop line is generated at the beginning of intersection lane */ std::optional generateStuckStopLine( - const int lane_id, const std::vector & conflicting_areas, + const std::vector & conflicting_areas, const std::shared_ptr & planner_data, const double stop_line_margin, const bool use_stuck_stopline, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, const autoware_auto_planning_msgs::msg::PathWithLaneId & path_ip, const double ip_interval, @@ -101,8 +101,7 @@ std::optional generateStuckStopLine( */ std::optional getFirstPointInsidePolygons( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t lane_interval_start, - const size_t lane_interval_end, const int lane_id, - const std::vector & polygons); + const size_t lane_interval_end, const std::vector & polygons); /** * @brief Get stop point from map if exists diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index 9d2a80f72fa6a..1e91cfc738e00 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -141,7 +141,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * } const auto stuck_line_idx_opt = util::generateStuckStopLine( - lane_id_, conflicting_area, planner_data_, planner_param_.stop_line_margin, + conflicting_area, planner_data_, planner_param_.stop_line_margin, planner_param_.use_stuck_stopline, path, path_ip, interval, lane_interval_ip_opt.value(), logger_.get_child("util")); @@ -192,20 +192,17 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * const auto objects_ptr = planner_data_->predicted_objects; /* check stuck vehicle */ - const double ignore_length = - planner_param_.stuck_vehicle_ignore_dist + planner_data_->vehicle_info_.vehicle_length_m; - const double detect_dist = - planner_param_.stuck_vehicle_detect_dist + planner_data_->vehicle_info_.vehicle_length_m; - const auto stuck_vehicle_detect_area = generateEgoIntersectionLanePolygon( - lanelet_map_ptr, *path, closest_idx, detect_dist, ignore_length); + const auto stuck_vehicle_detect_area = + generateStuckVehicleDetectAreaPolygon(lanelet_map_ptr, *path, closest_idx); const bool is_stuck = checkStuckVehicleInIntersection(objects_ptr, stuck_vehicle_detect_area); /* calculate dynamic collision around detection area */ const double time_delay = is_go_out_ ? 0.0 : (planner_param_.state_transit_margin_time - state_machine_.getDuration()); + const auto ego_poly = getIntersectionSegmentPolygon(*path, 0.0); const bool has_collision = checkCollision( - lanelet_map_ptr, *path, detection_lanelets, adjacent_lanelets, intersection_area, objects_ptr, - closest_idx, stuck_vehicle_detect_area, time_delay); + lanelet_map_ptr, *path, detection_lanelets, adjacent_lanelets, intersection_area, ego_poly, + objects_ptr, closest_idx, time_delay); /* calculate final stop lines */ std::optional stop_line_idx = std::nullopt; @@ -317,25 +314,13 @@ bool IntersectionModule::checkCollision( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const lanelet::ConstLanelets & detection_area_lanelets, const lanelet::ConstLanelets & adjacent_lanelets, - const std::optional & intersection_area, + const std::optional & intersection_area, const Polygon2d & ego_poly, const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, - const int closest_idx, const Polygon2d & stuck_vehicle_detect_area, const double time_delay) + const int closest_idx, const double time_delay) { using lanelet::utils::getArcCoordinates; using lanelet::utils::getPolygonFromArcLength; - /* generate ego-lane polygon */ - const auto ego_lane_poly = lanelet_map_ptr->laneletLayer.get(module_id_).polygon2d(); - Polygon2d ego_poly{}; - for (const auto & p : ego_lane_poly) { - ego_poly.outer().emplace_back(p.x(), p.y()); - } - lanelet::ConstLanelets ego_lane_with_next_lane = getEgoLaneWithNextLane(lanelet_map_ptr, path); - lanelet::ConstLanelet closest_lanelet; - lanelet::utils::query::getClosestLanelet( - ego_lane_with_next_lane, tier4_autoware_utils::getPose(path.points.at(closest_idx).point), - &closest_lanelet); - /* extract target objects */ autoware_auto_perception_msgs::msg::PredictedObjects target_objects; target_objects.header = objects_ptr->header; @@ -345,29 +330,18 @@ bool IntersectionModule::checkCollision( continue; } - // ignore vehicle in ego-lane && behind ego - const auto object_pose = object.kinematics.initial_pose_with_covariance.pose; - const bool is_in_ego_lane = bg::within(to_bg2d(object_pose.position), ego_poly); - if (is_in_ego_lane) { - if (!planning_utils::isAheadOf(object_pose, planner_data_->current_odometry->pose)) { - continue; - } - if ( - planner_param_.enable_front_car_decel_prediction && - checkFrontVehicleDeceleration( - ego_lane_with_next_lane, closest_lanelet, stuck_vehicle_detect_area, object)) - return true; - } - // check direction of objects const auto object_direction = getObjectPoseWithVelocityDirection(object.kinematics); + const auto is_in_adjacent_lanelets = checkAngleForTargetLanelets( + object_direction, adjacent_lanelets, planner_param_.detection_area_margin); + if (is_in_adjacent_lanelets) { + continue; + } + if (intersection_area) { const auto obj_poly = tier4_autoware_utils::toPolygon2d(object); const auto intersection_area_2d = intersection_area.value(); const auto is_in_intersection_area = bg::within(obj_poly, intersection_area_2d); - const auto is_in_adjacent_lanelets = checkAngleForTargetLanelets( - object_direction, adjacent_lanelets, planner_param_.detection_area_margin); - if (is_in_adjacent_lanelets) continue; if (is_in_intersection_area) { target_objects.objects.push_back(object); } else if (checkAngleForTargetLanelets( @@ -484,16 +458,19 @@ bool IntersectionModule::checkCollision( return collision_detected; } -Polygon2d IntersectionModule::generateEgoIntersectionLanePolygon( +Polygon2d IntersectionModule::generateStuckVehicleDetectAreaPolygon( lanelet::LaneletMapConstPtr lanelet_map_ptr, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, - const double extra_dist, const double ignore_dist) const + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx) const { using lanelet::utils::getArcCoordinates; using lanelet::utils::getLaneletLength3d; using lanelet::utils::getPolygonFromArcLength; using lanelet::utils::to2D; + const double extra_dist = + planner_param_.stuck_vehicle_detect_dist + planner_data_->vehicle_info_.vehicle_length_m; + const double ignore_dist = + planner_param_.stuck_vehicle_ignore_dist + planner_data_->vehicle_info_.vehicle_length_m; lanelet::ConstLanelets ego_lane_with_next_lane = getEgoLaneWithNextLane(lanelet_map_ptr, path); const double intersection_exit_length = getLaneletLength3d(ego_lane_with_next_lane.front()); @@ -677,6 +654,40 @@ bool IntersectionModule::checkAngleForTargetLanelets( return false; } +Polygon2d IntersectionModule::getIntersectionAndNextSegmentPolygon( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const double width) const +{ + // NOTE: findLaneIdsInterval returns (start, end) of assoc_ids + const auto ego_lane_interval_opt = util::findLaneIdsInterval(path, assoc_ids_); + if (!ego_lane_interval_opt.has_value()) { + return {}; + } + const auto [ego_start, ego_end] = ego_lane_interval_opt.value(); + const int start = ego_start; + int end = ego_end; + if (ego_end + 1 < path.points.size()) { + const int next_id = path.points.at(ego_end).lane_ids.at(0); + const auto next_lane_interval_opt = util::findLaneIdsInterval(path, {next_id}); + if (next_lane_interval_opt.has_value()) { + const auto [next_start, next_end] = next_lane_interval_opt.value(); + end = next_end; + } + } + return planning_utils::generatePathPolygon(path, start, end, width); +} + +Polygon2d IntersectionModule::getIntersectionSegmentPolygon( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const double width) const +{ + // NOTE: findLaneIdsInterval returns (start, end) of assoc_ids + const auto ego_lane_interval_opt = util::findLaneIdsInterval(path, assoc_ids_); + if (!ego_lane_interval_opt.has_value()) { + return {}; + } + const auto [ego_start, ego_end] = ego_lane_interval_opt.value(); + return planning_utils::generatePathPolygon(path, ego_start, ego_end, width); +} + lanelet::ConstLanelets IntersectionModule::getEgoLaneWithNextLane( lanelet::LaneletMapConstPtr lanelet_map_ptr, const autoware_auto_planning_msgs::msg::PathWithLaneId & path) const diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp index 7d8731f8f3051..c15f088c1fcef 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp @@ -120,8 +120,7 @@ std::optional getDuplicatedPointIdx( std::optional getFirstPointInsidePolygons( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t lane_interval_start, - const size_t lane_interval_end, [[maybe_unused]] const int lane_id, - const std::vector & polygons) + const size_t lane_interval_end, const std::vector & polygons) { std::optional first_idx_inside_lanelet = std::nullopt; for (size_t i = lane_interval_start; i <= lane_interval_end; ++i) { @@ -180,7 +179,7 @@ std::optional generateStopLine( } else { // find the index of the first point that intersects with detection_areas const auto first_inside_detection_idx_ip_opt = getFirstPointInsidePolygons( - path_ip, lane_interval_ip_start, lane_interval_ip_end, lane_id, detection_areas); + path_ip, lane_interval_ip_start, lane_interval_ip_end, detection_areas); // if path is not intersecting with detection_area, skip if (!first_inside_detection_idx_ip_opt.has_value()) { RCLCPP_DEBUG( @@ -255,7 +254,7 @@ std::optional generateStopLine( } std::optional generateStuckStopLine( - const int lane_id, const std::vector & conflicting_areas, + const std::vector & conflicting_areas, const std::shared_ptr & planner_data, const double stop_line_margin, const bool use_stuck_stopline, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, const autoware_auto_planning_msgs::msg::PathWithLaneId & path_ip, const double interval, @@ -267,7 +266,7 @@ std::optional generateStuckStopLine( stuck_stop_line_idx_ip = lane_interval_ip_start; } else { const auto stuck_stop_line_idx_ip_opt = util::getFirstPointInsidePolygons( - path_ip, lane_interval_ip_start, lane_interval_ip_end, lane_id, conflicting_areas); + path_ip, lane_interval_ip_start, lane_interval_ip_end, conflicting_areas); if (!stuck_stop_line_idx_ip_opt.has_value()) { RCLCPP_DEBUG( logger, From 7b3e855f082918c4c14e8dc5885c9f031f2b93d4 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 17 Feb 2023 21:22:19 +0900 Subject: [PATCH 4/8] compiles Signed-off-by: Mamoru Sobue --- .../intersection/scene_intersection.hpp | 15 ++--- .../include/utilization/util.hpp | 3 +- .../intersection/scene_intersection.cpp | 59 +++++++------------ .../src/utilization/util.cpp | 24 ++++++++ 4 files changed, 54 insertions(+), 47 deletions(-) diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index f58f3c71615cf..a275320f64deb 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -141,7 +141,8 @@ class IntersectionModule : public SceneModuleInterface const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const lanelet::ConstLanelets & detection_area_lanelets, const lanelet::ConstLanelets & adjacent_lanelets, - const std::optional & intersection_area, const Polygon2d & ego_poly, + const std::optional & intersection_area, const lanelet::ConstLanelet & ego_lane, + const lanelet::ConstLanelets & ego_lane_with_next_lane, const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, const int closest_idx, const double time_delay); @@ -168,8 +169,8 @@ class IntersectionModule : public SceneModuleInterface * @return generated polygon */ Polygon2d generateStuckVehicleDetectAreaPolygon( - lanelet::LaneletMapConstPtr lanelet_map_ptr, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx) const; + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const lanelet::ConstLanelets & ego_lane_with_next_lane, const int closest_idx) const; /** * @brief Modify objects predicted path. remove path point if the time exceeds timer_thr. @@ -229,20 +230,16 @@ class IntersectionModule : public SceneModuleInterface * @brief Get path polygon of intersection part and next lane part * @return trimmed path polygon */ - Polygon2d getIntersectionAndNextSegmentPolygon( + lanelet::ConstLanelets getEgoLaneWithNextLane( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const double width) const; /** * @brief Get path polygon of intersetion part * @return trimmed path polygon */ - Polygon2d getIntersectionSegmentPolygon( + lanelet::ConstLanelet getEgoLane( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const double width) const; - lanelet::ConstLanelets getEgoLaneWithNextLane( - lanelet::LaneletMapConstPtr lanelet_map_ptr, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) const; - /** * @brief Calculate distance between closest path point and intersection lanelet along path * @param lanelet_map_ptr lanelet map diff --git a/planning/behavior_velocity_planner/include/utilization/util.hpp b/planning/behavior_velocity_planner/include/utilization/util.hpp index 43ffc2c1c43ba..c082272f21907 100644 --- a/planning/behavior_velocity_planner/include/utilization/util.hpp +++ b/planning/behavior_velocity_planner/include/utilization/util.hpp @@ -169,7 +169,8 @@ geometry_msgs::msg::Pose getAheadPose( const autoware_auto_planning_msgs::msg::PathWithLaneId & path); Polygon2d generatePathPolygon( const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width); - +lanelet::ConstLanelet generatePathLanelet( + const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width); double calcJudgeLineDistWithAccLimit( const double velocity, const double max_stop_acceleration, const double delay_response_time); diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index 1e91cfc738e00..3efdd19f73520 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -191,18 +191,22 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * /* get dynamic object */ const auto objects_ptr = planner_data_->predicted_objects; + /* considering lane change in the intersection, these lanelets are generated from the path */ + const auto ego_lane = getEgoLane(*path, planner_data_->vehicle_info_.vehicle_width_m); + const auto ego_lane_with_next_lane = + getEgoLaneWithNextLane(*path, planner_data_->vehicle_info_.vehicle_width_m); + /* check stuck vehicle */ const auto stuck_vehicle_detect_area = - generateStuckVehicleDetectAreaPolygon(lanelet_map_ptr, *path, closest_idx); + generateStuckVehicleDetectAreaPolygon(*path, ego_lane_with_next_lane, closest_idx); const bool is_stuck = checkStuckVehicleInIntersection(objects_ptr, stuck_vehicle_detect_area); /* calculate dynamic collision around detection area */ const double time_delay = is_go_out_ ? 0.0 : (planner_param_.state_transit_margin_time - state_machine_.getDuration()); - const auto ego_poly = getIntersectionSegmentPolygon(*path, 0.0); const bool has_collision = checkCollision( - lanelet_map_ptr, *path, detection_lanelets, adjacent_lanelets, intersection_area, ego_poly, - objects_ptr, closest_idx, time_delay); + lanelet_map_ptr, *path, detection_lanelets, adjacent_lanelets, intersection_area, ego_lane, + ego_lane_with_next_lane, objects_ptr, closest_idx, time_delay); /* calculate final stop lines */ std::optional stop_line_idx = std::nullopt; @@ -314,7 +318,8 @@ bool IntersectionModule::checkCollision( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const lanelet::ConstLanelets & detection_area_lanelets, const lanelet::ConstLanelets & adjacent_lanelets, - const std::optional & intersection_area, const Polygon2d & ego_poly, + const std::optional & intersection_area, const lanelet::ConstLanelet & ego_lane, + const lanelet::ConstLanelets & ego_lane_with_next_lane, const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, const int closest_idx, const double time_delay) { @@ -369,6 +374,7 @@ bool IntersectionModule::checkCollision( calcDistanceUntilIntersectionLanelet(lanelet_map_ptr, path, closest_idx); const double base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + const auto ego_poly = ego_lane.polygon2d().basicPolygon(); // check collision between predicted_path and ego_area bool collision_detected = false; for (const auto & object : target_objects.objects) { @@ -459,8 +465,8 @@ bool IntersectionModule::checkCollision( } Polygon2d IntersectionModule::generateStuckVehicleDetectAreaPolygon( - lanelet::LaneletMapConstPtr lanelet_map_ptr, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx) const + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const lanelet::ConstLanelets & ego_lane_with_next_lane, const int closest_idx) const { using lanelet::utils::getArcCoordinates; using lanelet::utils::getLaneletLength3d; @@ -471,7 +477,6 @@ Polygon2d IntersectionModule::generateStuckVehicleDetectAreaPolygon( planner_param_.stuck_vehicle_detect_dist + planner_data_->vehicle_info_.vehicle_length_m; const double ignore_dist = planner_param_.stuck_vehicle_ignore_dist + planner_data_->vehicle_info_.vehicle_length_m; - lanelet::ConstLanelets ego_lane_with_next_lane = getEgoLaneWithNextLane(lanelet_map_ptr, path); const double intersection_exit_length = getLaneletLength3d(ego_lane_with_next_lane.front()); @@ -654,58 +659,38 @@ bool IntersectionModule::checkAngleForTargetLanelets( return false; } -Polygon2d IntersectionModule::getIntersectionAndNextSegmentPolygon( +lanelet::ConstLanelets IntersectionModule::getEgoLaneWithNextLane( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const double width) const { // NOTE: findLaneIdsInterval returns (start, end) of assoc_ids const auto ego_lane_interval_opt = util::findLaneIdsInterval(path, assoc_ids_); if (!ego_lane_interval_opt.has_value()) { - return {}; + return lanelet::ConstLanelets({}); } const auto [ego_start, ego_end] = ego_lane_interval_opt.value(); - const int start = ego_start; - int end = ego_end; if (ego_end + 1 < path.points.size()) { const int next_id = path.points.at(ego_end).lane_ids.at(0); const auto next_lane_interval_opt = util::findLaneIdsInterval(path, {next_id}); if (next_lane_interval_opt.has_value()) { const auto [next_start, next_end] = next_lane_interval_opt.value(); - end = next_end; + return { + planning_utils::generatePathLanelet(path, ego_start, ego_end, width), + planning_utils::generatePathLanelet(path, next_start, next_end, width)}; } } - return planning_utils::generatePathPolygon(path, start, end, width); + return {planning_utils::generatePathLanelet(path, ego_start, ego_end, width)}; } -Polygon2d IntersectionModule::getIntersectionSegmentPolygon( +lanelet::ConstLanelet IntersectionModule::getEgoLane( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const double width) const { // NOTE: findLaneIdsInterval returns (start, end) of assoc_ids const auto ego_lane_interval_opt = util::findLaneIdsInterval(path, assoc_ids_); if (!ego_lane_interval_opt.has_value()) { - return {}; + return lanelet::ConstLanelet(); } const auto [ego_start, ego_end] = ego_lane_interval_opt.value(); - return planning_utils::generatePathPolygon(path, ego_start, ego_end, width); -} - -lanelet::ConstLanelets IntersectionModule::getEgoLaneWithNextLane( - lanelet::LaneletMapConstPtr lanelet_map_ptr, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) const -{ - const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id_); - const auto last_itr = - std::find_if(path.points.crbegin(), path.points.crend(), [this](const auto & p) { - return std::find(p.lane_ids.begin(), p.lane_ids.end(), lane_id_) != p.lane_ids.end(); - }); - lanelet::ConstLanelets ego_lane_with_next_lane; - if (last_itr.base() != path.points.end()) { - const auto & next_lanelet = - lanelet_map_ptr->laneletLayer.get((*last_itr.base()).lane_ids.front()); - ego_lane_with_next_lane = {assigned_lanelet, next_lanelet}; - } else { - ego_lane_with_next_lane = {assigned_lanelet}; - } - return ego_lane_with_next_lane; + return planning_utils::generatePathLanelet(path, ego_start, ego_end, width); } double IntersectionModule::calcDistanceUntilIntersectionLanelet( diff --git a/planning/behavior_velocity_planner/src/utilization/util.cpp b/planning/behavior_velocity_planner/src/utilization/util.cpp index 2fab91c4d2144..94a5b5ff1a3a4 100644 --- a/planning/behavior_velocity_planner/src/utilization/util.cpp +++ b/planning/behavior_velocity_planner/src/utilization/util.cpp @@ -295,6 +295,30 @@ Polygon2d generatePathPolygon( return ego_area; } +lanelet::ConstLanelet generatePathLanelet( + const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width) +{ + lanelet::Points3d lefts; + for (size_t i = start_idx; i <= end_idx; ++i) { + const double yaw = tf2::getYaw(path.points.at(i).point.pose.orientation); + const double x = path.points.at(i).point.pose.position.x + width * std::sin(yaw); + const double y = path.points.at(i).point.pose.position.y - width * std::cos(yaw); + lefts.emplace_back(x, y, path.points.at(i).point.pose.position.z); + } + lanelet::LineString3d left = lanelet::LineString3d(lanelet::InvalId, lefts); + + lanelet::Points3d rights; + for (size_t i = start_idx; i <= end_idx; ++i) { + const double yaw = tf2::getYaw(path.points.at(i).point.pose.orientation); + const double x = path.points.at(i).point.pose.position.x - width * std::sin(yaw); + const double y = path.points.at(i).point.pose.position.y + width * std::cos(yaw); + rights.emplace_back(x, y, path.points.at(i).point.pose.position.z); + } + lanelet::LineString3d right = lanelet::LineString3d(lanelet::InvalId, rights); + + return lanelet::Lanelet(lanelet::InvalId, left, right); +} + geometry_msgs::msg::Pose transformRelCoordinate2D( const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin) { From 46265eacf8369b52a84774c74932a8a7fe782602 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 20 Feb 2023 10:35:51 +0900 Subject: [PATCH 5/8] adjacent lane is OK, but ego_lane may be invalid lanelet Signed-off-by: Mamoru Sobue --- .../intersection/scene_intersection.hpp | 2 +- .../include/scene_module/intersection/util.hpp | 3 ++- .../include/utilization/util.hpp | 12 ++++++++++++ .../src/scene_module/intersection/debug.cpp | 5 ++++- .../intersection/scene_intersection.cpp | 17 ++++++++++++----- .../scene_merge_from_private_road.cpp | 4 ++-- .../src/scene_module/intersection/util.cpp | 6 +++--- 7 files changed, 36 insertions(+), 13 deletions(-) diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index a275320f64deb..767c5714a5459 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -50,7 +50,6 @@ class IntersectionModule : public SceneModuleInterface { bool stop_required; - geometry_msgs::msg::Pose slow_wall_pose; geometry_msgs::msg::Pose stop_wall_pose; geometry_msgs::msg::Polygon stuck_vehicle_detect_area; geometry_msgs::msg::Polygon candidate_collision_ego_lane_polygon; @@ -58,6 +57,7 @@ class IntersectionModule : public SceneModuleInterface std::vector intersection_detection_lanelets; std::vector detection_area; geometry_msgs::msg::Polygon intersection_area; + lanelet::CompoundPolygon3d ego_lane; std::vector adjacent_area; autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp index f512f7500cee5..d2e8646305042 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp @@ -57,7 +57,8 @@ std::optional getDuplicatedPointIdx( */ IntersectionLanelets getObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const int lane_id, const double detection_area_length, const bool tl_arrow_solid_on = false); + const int lane_id, const std::set & assoc_ids, const double detection_area_length, + const bool tl_arrow_solid_on = false); /** * @brief Generate a stop line and insert it into the path. If the stop line is defined in the map, diff --git a/planning/behavior_velocity_planner/include/utilization/util.hpp b/planning/behavior_velocity_planner/include/utilization/util.hpp index c082272f21907..a7b3b0457544e 100644 --- a/planning/behavior_velocity_planner/include/utilization/util.hpp +++ b/planning/behavior_velocity_planner/include/utilization/util.hpp @@ -302,6 +302,18 @@ std::set getAssociativeIntersectionLanelets( lanelet::ConstLanelet lane, const lanelet::LaneletMapPtr lanelet_map, const lanelet::routing::RoutingGraphPtr routing_graph); +template