diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index fd616ece9812e..9b39a21adaf24 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -185,6 +185,7 @@ class AvoidanceModule : public SceneModuleInterface ShiftedPath prev_output_; ShiftedPath prev_linear_shift_path_; // used for shift point check PathWithLaneId prev_reference_; + lanelet::ConstLanelets prev_driving_lanes_; // for raw_shift_line registration AvoidLineArray registered_raw_shift_lines_; diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index ff9c4439068ea..04248900a5aee 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -76,6 +76,25 @@ void pushUniqueVector(T & base_vector, const T & additional_vector) { base_vector.insert(base_vector.end(), additional_vector.begin(), additional_vector.end()); } + +bool isDrivingSameLane( + const lanelet::ConstLanelets & previous_lanes, const lanelet::ConstLanelets & current_lanes) +{ + std::multiset prev_ids{}; + std::multiset curr_ids{}; + std::multiset same_ids{}; + + std::for_each( + previous_lanes.begin(), previous_lanes.end(), [&](const auto & l) { prev_ids.insert(l.id()); }); + std::for_each( + current_lanes.begin(), current_lanes.end(), [&](const auto & l) { curr_ids.insert(l.id()); }); + + std::set_intersection( + prev_ids.begin(), prev_ids.end(), curr_ids.begin(), curr_ids.end(), + std::inserter(same_ids, same_ids.end())); + + return !same_ids.empty(); +} } // namespace #ifdef USE_OLD_ARCHITECTURE @@ -164,9 +183,15 @@ ModuleStatus AvoidanceModule::updateState() current_state_ = ModuleStatus::RUNNING; } + if (!isDrivingSameLane(prev_driving_lanes_, avoidance_data_.current_lanelets)) { + RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 500, "previous module lane is updated."); + current_state_ = ModuleStatus::SUCCESS; + } + DEBUG_PRINT( "is_plan_running = %d, has_avoidance_target = %d", is_plan_running, has_avoidance_target); + prev_driving_lanes_ = avoidance_data_.current_lanelets; return current_state_; } @@ -3125,6 +3150,10 @@ void AvoidanceModule::updateData() if (prev_reference_.points.empty()) { prev_reference_ = *getPreviousModuleOutput().path; } + if (prev_driving_lanes_.empty()) { + prev_driving_lanes_ = + utils::getCurrentLanesFromPath(*getPreviousModuleOutput().reference_path, planner_data_); + } #endif debug_data_ = DebugData(); @@ -3181,6 +3210,7 @@ void AvoidanceModule::initVariables() prev_output_ = ShiftedPath(); prev_linear_shift_path_ = ShiftedPath(); prev_reference_ = PathWithLaneId(); + prev_driving_lanes_.clear(); path_shifter_ = PathShifter{}; debug_data_ = DebugData(); @@ -3366,8 +3396,7 @@ void AvoidanceModule::updateDebugMarker( add(createOtherObjectsMarkerArray(data.other_objects, std::string("NotNeedAvoidance"))); add(makeOverhangToRoadShoulderMarkerArray(data.target_objects, "overhang")); - add(createOverhangFurthestLineStringMarkerArray( - debug.bounds, "farthest_linestring_from_overhang", 1.0, 0.0, 1.0)); + add(createOverhangFurthestLineStringMarkerArray(debug.bounds, "bounds", 1.0, 0.0, 1.0)); add(createUnsafeObjectsMarkerArray(debug.unsafe_objects, "unsafe_objects")); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 87498aa971979..f9b51a2694bdc 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -103,6 +103,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() output.turn_signal_info = updateOutputTurnSignal(); if (isAbortState()) { + output.reference_path = std::make_shared(prev_module_reference_path_); return output; } diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 5591416d5adf8..b9f1ad482efc4 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -675,21 +675,58 @@ void filterTargetObjects( o.overhang_lanelet = overhang_lanelet; lanelet::BasicPoint3d overhang_basic_pose( o.overhang_pose.position.x, o.overhang_pose.position.y, o.overhang_pose.position.z); + const bool get_left = isOnRight(o) && parameters->enable_avoidance_over_same_direction; const bool get_right = !isOnRight(o) && parameters->enable_avoidance_over_same_direction; + const bool get_opposite = parameters->enable_avoidance_over_opposite_direction; - const auto target_lines = rh->getFurthestLinestring( - overhang_lanelet, get_right, get_left, - parameters->enable_avoidance_over_opposite_direction); + lanelet::ConstLineString3d target_line{}; + { + const auto lines = + rh->getFurthestLinestring(overhang_lanelet, get_right, get_left, get_opposite, true); + if (isOnRight(o)) { + o.to_road_shoulder_distance = + distance2d(to2D(overhang_basic_pose), to2D(lines.back().basicLineString())); + debug.bounds.push_back(lines.back()); + } else { + o.to_road_shoulder_distance = + distance2d(to2D(overhang_basic_pose), to2D(lines.front().basicLineString())); + debug.bounds.push_back(lines.front()); + } + } - if (isOnRight(o)) { - o.to_road_shoulder_distance = - distance2d(to2D(overhang_basic_pose), to2D(target_lines.back().basicLineString())); - debug.bounds.push_back(target_lines.back()); - } else { - o.to_road_shoulder_distance = - distance2d(to2D(overhang_basic_pose), to2D(target_lines.front().basicLineString())); - debug.bounds.push_back(target_lines.front()); + lanelet::ConstLanelets previous_lanelet{}; + if (rh->getPreviousLaneletsWithinRoute(overhang_lanelet, &previous_lanelet)) { + const auto lines = rh->getFurthestLinestring( + previous_lanelet.front(), get_right, get_left, get_opposite, true); + if (isOnRight(o)) { + const auto d = + distance2d(to2D(overhang_basic_pose), to2D(lines.back().basicLineString())); + o.to_road_shoulder_distance = std::min(d, o.to_road_shoulder_distance); + debug.bounds.push_back(lines.back()); + } else { + const auto d = + distance2d(to2D(overhang_basic_pose), to2D(lines.front().basicLineString())); + o.to_road_shoulder_distance = std::min(d, o.to_road_shoulder_distance); + debug.bounds.push_back(lines.front()); + } + } + + lanelet::ConstLanelet next_lanelet{}; + if (rh->getNextLaneletWithinRoute(overhang_lanelet, &next_lanelet)) { + const auto lines = + rh->getFurthestLinestring(next_lanelet, get_right, get_left, get_opposite, true); + if (isOnRight(o)) { + const auto d = + distance2d(to2D(overhang_basic_pose), to2D(lines.back().basicLineString())); + o.to_road_shoulder_distance = std::min(d, o.to_road_shoulder_distance); + debug.bounds.push_back(lines.back()); + } else { + const auto d = + distance2d(to2D(overhang_basic_pose), to2D(lines.front().basicLineString())); + o.to_road_shoulder_distance = std::min(d, o.to_road_shoulder_distance); + debug.bounds.push_back(lines.front()); + } } } diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index e8eaa8ccac28b..90093361e79ef 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -105,6 +105,8 @@ class RouteHandler // for lanelet bool getPreviousLaneletsWithinRoute( const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelets * prev_lanelets) const; + bool getNextLaneletWithinRoute( + const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * next_lanelet) const; bool isDeadEndLanelet(const lanelet::ConstLanelet & lanelet) const; lanelet::ConstLanelets getLaneletsFromPoint(const lanelet::ConstPoint3d & point) const; lanelet::ConstLanelets getLaneChangeableNeighbors(const lanelet::ConstLanelet & lanelet) const; @@ -117,7 +119,7 @@ class RouteHandler * @return vector of lanelet having same direction if true */ boost::optional getRightLanelet( - const lanelet::ConstLanelet & lanelet) const; + const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const; /** * @brief Check if same-direction lane is available at the left side of the lanelet @@ -127,7 +129,7 @@ class RouteHandler * @return vector of lanelet having same direction if true */ boost::optional getLeftLanelet( - const lanelet::ConstLanelet & lanelet) const; + const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const; lanelet::ConstLanelets getNextLanelets(const lanelet::ConstLanelet & lanelet) const; lanelet::ConstLanelets getPreviousLanelets(const lanelet::ConstLanelet & lanelet) const; @@ -189,7 +191,8 @@ class RouteHandler * @param the lanelet of interest * @return vector of lanelet having same direction if true */ - lanelet::ConstLanelet getMostRightLanelet(const lanelet::ConstLanelet & lanelet) const; + lanelet::ConstLanelet getMostRightLanelet( + const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const; /** * @brief Check if same-direction lane is available at the left side of the lanelet @@ -198,7 +201,8 @@ class RouteHandler * @param the lanelet of interest * @return vector of lanelet having same direction if true */ - lanelet::ConstLanelet getMostLeftLanelet(const lanelet::ConstLanelet & lanelet) const; + lanelet::ConstLanelet getMostLeftLanelet( + const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const; /** * @brief Searches the furthest linestring to the right side of the lanelet @@ -207,7 +211,7 @@ class RouteHandler * @return right most linestring of the lane with same direction */ lanelet::ConstLineString3d getRightMostSameDirectionLinestring( - const lanelet::ConstLanelet & lanelet) const noexcept; + const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const noexcept; /** * @brief Searches the furthest linestring to the right side of the lanelet @@ -216,7 +220,7 @@ class RouteHandler * @return right most linestring */ lanelet::ConstLineString3d getRightMostLinestring( - const lanelet::ConstLanelet & lanelet) const noexcept; + const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const noexcept; /** * @brief Searches the furthest linestring to the left side of the lanelet @@ -225,7 +229,7 @@ class RouteHandler * @return left most linestring of the lane with same direction */ lanelet::ConstLineString3d getLeftMostSameDirectionLinestring( - const lanelet::ConstLanelet & lanelet) const noexcept; + const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const noexcept; /** * @brief Searches the furthest linestring to the left side of the lanelet @@ -234,7 +238,7 @@ class RouteHandler * @return left most linestring */ lanelet::ConstLineString3d getLeftMostLinestring( - const lanelet::ConstLanelet & lanelet) const noexcept; + const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const noexcept; /** * @brief Return furthest linestring on both side of the lanelet @@ -246,7 +250,7 @@ class RouteHandler */ lanelet::ConstLineStrings3d getFurthestLinestring( const lanelet::ConstLanelet & lanelet, bool is_right = true, bool is_left = true, - bool is_opposite = true) const noexcept; + bool is_opposite = true, bool enable_same_root = false) const noexcept; /** * Retrieves a sequence of lanelets before the given lanelet. @@ -379,8 +383,6 @@ class RouteHandler bool isBijectiveConnection( const lanelet::ConstLanelets & lanelet_section1, const lanelet::ConstLanelets & lanelet_section2) const; - bool getNextLaneletWithinRoute( - const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * next_lanelet) const; bool getPreviousLaneletWithinRouteExceptGoal( const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * prev_lanelet) const; bool getNextLaneletWithinRouteExceptStart( diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 17cfdbdfff05e..31df6d5870f56 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -911,7 +911,7 @@ bool RouteHandler::isBijectiveConnection( } boost::optional RouteHandler::getRightLanelet( - const lanelet::ConstLanelet & lanelet) const + const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const { // routable lane const auto & right_lane = routing_graph_ptr_->right(lanelet); @@ -921,6 +921,38 @@ boost::optional RouteHandler::getRightLanelet( // non-routable lane (e.g. lane change infeasible) const auto & adjacent_right_lane = routing_graph_ptr_->adjacentRight(lanelet); + if (adjacent_right_lane) { + return adjacent_right_lane; + } + + // same root right lanelet + if (!enable_same_root) { + return adjacent_right_lane; + } + + lanelet::ConstLanelet next_lanelet; + if (!getNextLaneletWithinRoute(lanelet, &next_lanelet)) { + return adjacent_right_lane; + } + + lanelet::ConstLanelets prev_lanelet; + if (!getPreviousLaneletsWithinRoute(lanelet, &prev_lanelet)) { + return adjacent_right_lane; + } + + const auto next_right_lane = getRightLanelet(next_lanelet, false); + if (!next_right_lane) { + return adjacent_right_lane; + } + + for (const auto & lane : getNextLanelets(prev_lanelet.front())) { + for (const auto & target_lane : getNextLanelets(lane)) { + if (next_right_lane.get().id() == target_lane.id()) { + return lane; + } + } + } + return adjacent_right_lane; } @@ -936,7 +968,7 @@ bool RouteHandler::getLeftLaneletWithinRoute( } boost::optional RouteHandler::getLeftLanelet( - const lanelet::ConstLanelet & lanelet) const + const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const { // routable lane const auto & left_lane = routing_graph_ptr_->left(lanelet); @@ -946,6 +978,38 @@ boost::optional RouteHandler::getLeftLanelet( // non-routable lane (e.g. lane change infeasible) const auto & adjacent_left_lane = routing_graph_ptr_->adjacentLeft(lanelet); + if (adjacent_left_lane) { + return adjacent_left_lane; + } + + // same root right lanelet + if (!enable_same_root) { + return adjacent_left_lane; + } + + lanelet::ConstLanelet next_lanelet; + if (!getNextLaneletWithinRoute(lanelet, &next_lanelet)) { + return adjacent_left_lane; + } + + lanelet::ConstLanelets prev_lanelet; + if (!getPreviousLaneletsWithinRoute(lanelet, &prev_lanelet)) { + return adjacent_left_lane; + } + + const auto next_left_lane = getLeftLanelet(next_lanelet, false); + if (!next_left_lane) { + return adjacent_left_lane; + } + + for (const auto & lane : getNextLanelets(prev_lanelet.front())) { + for (const auto & target_lane : getNextLanelets(lane)) { + if (next_left_lane.get().id() == target_lane.id()) { + return lane; + } + } + } + return adjacent_left_lane; } @@ -1075,113 +1139,118 @@ lanelet::Lanelets RouteHandler::getLeftOppositeLanelets(const lanelet::ConstLane return opposite_lanelets; } -lanelet::ConstLanelet RouteHandler::getMostRightLanelet(const lanelet::ConstLanelet & lanelet) const +lanelet::ConstLanelet RouteHandler::getMostRightLanelet( + const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const { // recursively compute the width of the lanes - const auto & same = getRightLanelet(lanelet); + const auto & same = getRightLanelet(lanelet, enable_same_root); if (same) { - return getMostRightLanelet(same.get()); + return getMostRightLanelet(same.get(), enable_same_root); } + return lanelet; } -lanelet::ConstLanelet RouteHandler::getMostLeftLanelet(const lanelet::ConstLanelet & lanelet) const +lanelet::ConstLanelet RouteHandler::getMostLeftLanelet( + const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const { // recursively compute the width of the lanes - const auto & same = getLeftLanelet(lanelet); + const auto & same = getLeftLanelet(lanelet, enable_same_root); if (same) { - return getMostLeftLanelet(same.get()); + return getMostLeftLanelet(same.get(), enable_same_root); } + return lanelet; } lanelet::ConstLineString3d RouteHandler::getRightMostSameDirectionLinestring( - const lanelet::ConstLanelet & lanelet) const noexcept + const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const noexcept { // recursively compute the width of the lanes - const auto & same = getRightLanelet(lanelet); + const auto & same = getRightLanelet(lanelet, enable_same_root); if (same) { - return getRightMostSameDirectionLinestring(same.get()); + return getRightMostSameDirectionLinestring(same.get(), enable_same_root); } + return lanelet.rightBound(); } lanelet::ConstLineString3d RouteHandler::getRightMostLinestring( - const lanelet::ConstLanelet & lanelet) const noexcept + const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const noexcept { - const auto & same = getRightLanelet(lanelet); + const auto & same = getRightLanelet(lanelet, enable_same_root); const auto & opposite = getRightOppositeLanelets(lanelet); if (!same && opposite.empty()) { return lanelet.rightBound(); } if (same) { - return getRightMostLinestring(same.get()); + return getRightMostLinestring(same.get(), enable_same_root); } if (!opposite.empty()) { - return getLeftMostLinestring(lanelet::ConstLanelet(opposite.front())); + return getLeftMostLinestring(lanelet::ConstLanelet(opposite.front()), false); } return lanelet.rightBound(); } lanelet::ConstLineString3d RouteHandler::getLeftMostSameDirectionLinestring( - const lanelet::ConstLanelet & lanelet) const noexcept + const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const noexcept { // recursively compute the width of the lanes - const auto & same = getLeftLanelet(lanelet); + const auto & same = getLeftLanelet(lanelet, enable_same_root); if (same) { - return getLeftMostSameDirectionLinestring(same.get()); + return getLeftMostSameDirectionLinestring(same.get(), enable_same_root); } + return lanelet.leftBound(); } lanelet::ConstLineString3d RouteHandler::getLeftMostLinestring( - const lanelet::ConstLanelet & lanelet) const noexcept + const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const noexcept { // recursively compute the width of the lanes - const auto & same = getLeftLanelet(lanelet); + const auto & same = getLeftLanelet(lanelet, enable_same_root); const auto & opposite = getLeftOppositeLanelets(lanelet); - if (!same && opposite.empty()) { return lanelet.leftBound(); } if (same) { - return getLeftMostLinestring(same.get()); + return getLeftMostLinestring(same.get(), enable_same_root); } if (!opposite.empty()) { - return getRightMostLinestring(lanelet::ConstLanelet(opposite.front())); + return getRightMostLinestring(lanelet::ConstLanelet(opposite.front()), false); } return lanelet.leftBound(); } lanelet::ConstLineStrings3d RouteHandler::getFurthestLinestring( - const lanelet::ConstLanelet & lanelet, bool is_right, bool is_left, - bool is_opposite) const noexcept + const lanelet::ConstLanelet & lanelet, bool is_right, bool is_left, bool is_opposite, + bool enable_same_root) const noexcept { lanelet::ConstLineStrings3d linestrings; linestrings.reserve(2); if (is_right && is_opposite) { - linestrings.emplace_back(getRightMostLinestring(lanelet)); + linestrings.emplace_back(getRightMostLinestring(lanelet, enable_same_root)); } else if (is_right && !is_opposite) { - linestrings.emplace_back(getRightMostSameDirectionLinestring(lanelet)); + linestrings.emplace_back(getRightMostSameDirectionLinestring(lanelet, enable_same_root)); } else { linestrings.emplace_back(lanelet.rightBound()); } if (is_left && is_opposite) { - linestrings.emplace_back(getLeftMostLinestring(lanelet)); + linestrings.emplace_back(getLeftMostLinestring(lanelet, enable_same_root)); } else if (is_left && !is_opposite) { - linestrings.emplace_back(getLeftMostSameDirectionLinestring(lanelet)); + linestrings.emplace_back(getLeftMostSameDirectionLinestring(lanelet, enable_same_root)); } else { linestrings.emplace_back(lanelet.leftBound()); }