diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp index 4e741a2b3db2c..30a7b4853471c 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp @@ -67,7 +67,7 @@ class LaneChangeInterface : public SceneModuleInterface bool isExecutionReady() const override; - bool isRootLaneletToBeUpdated() const override + bool isCurrentRouteLaneletToBeReset() const override { return getCurrentStatus() == ModuleStatus::SUCCESS; } diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index ee32add05dd51..8ae0582828834 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -406,7 +406,7 @@ void NormalLaneChange::insertStopPoint( PathWithLaneId NormalLaneChange::getReferencePath() const { - return utils::getCenterLinePathFromRootLanelet( + return utils::getCenterLinePathFromLanelet( status_.lane_change_path.info.target_lanes.front(), planner_data_); } diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_manager_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_manager_design.md index 5af70a4f98e29..7188c03bd2604 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_manager_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_manager_design.md @@ -532,67 +532,33 @@ When the manager removes succeeded modules, the last added module's output is us ## Reference path generation -The root reference path is generated from the centerline of the **lanelet sequence** that obtained from the **root lanelet**, and it is not only used as an input to the first added module of approved modules stack, but also used as the output of `behavior_path_planner` if none of the modules are running. +The reference path is generated from the centerline of the **lanelet sequence** obtained from the **current route lanelet**, and it is not only used as an input to the first added module of approved modules stack, but also used as the output of `behavior_path_planner` if none of the modules are running.
![root_generation](../image/manager/root_generation.svg){width=500}
root reference path generation
-The root lanelet is the closest lanelet within the route, and the update timing is based on Ego's operation mode state. +The **current route lanelet** keeps track of the route lanelet currently followed by the planner. +It is initialized as the closest lanelet within the route. +It is then updated as ego travels along the route such that (1) it follows the previous **current route lanelet** and (2) it is the closest lanelet within the route. -- the state is `OperationModeState::AUTONOMOUS`: Update only when the ego moves to right or left lane by lane change module. -- the state is **NOT** `OperationModeState::AUTONOMOUS`: Update at the beginning of every planning cycle. +The **current route lanelet** can be reset to the closest lanelet within the route, ignoring whether it follows the previous **current route lanelet** . -![root_lanelet](../image/manager/root_lanelet.svg) +![current_route_lanelet](../image/manager/current_route_lanelet.svg) The manager needs to know the ego behavior and then generate a root reference path from the lanes that Ego should follow. -For example, during autonomous driving, even if Ego moves into the next lane in order to avoid a parked vehicle, the target lanes that Ego should follow will **NOT** change because Ego will return to the original lane after the avoidance maneuver. Therefore, the manager does **NOT** update **root lanelet** even if the avoidance maneuver is finished. +For example, during autonomous driving, even if Ego moves into the next lane in order to avoid a parked vehicle, the target lanes that Ego should follow will **NOT** change because Ego will return to the original lane after the avoidance maneuver. Therefore, the manager does **NOT** reset the **current route lanelet**, even if the avoidance maneuver is finished. ![avoidance](../image/manager/avoidance.svg) -On the other hand, if the lane change is successful, the manager updates **root lanelet** because the lane that Ego should follow changes. +On the other hand, if the lane change is successful, the manager resets the **current route lanelet** because the lane that Ego should follow changes. ![lane_change](../image/manager/lane_change.svg) -In addition, while manual driving, the manager always updates **root lanelet** because the pilot may move to an adjacent lane regardless of the decision of the autonomous driving system. - -```c++ - /** - * @brief get reference path from root_lanelet_ centerline. - * @param planner data. - * @return reference path. - */ - BehaviorModuleOutput getReferencePath(const std::shared_ptr & data) const - { - const auto & route_handler = data->route_handler; - const auto & pose = data->self_odometry->pose.pose; - const auto p = data->parameters; - - constexpr double extra_margin = 10.0; - const auto backward_length = - std::max(p.backward_path_length, p.backward_path_length + extra_margin); - - const auto lanelet_sequence = route_handler->getLaneletSequence( - root_lanelet_.value(), pose, backward_length, std::numeric_limits::max()); - - lanelet::ConstLanelet closest_lane{}; - if (lanelet::utils::query::getClosestLaneletWithConstrains( - lanelet_sequence, pose, &closest_lane, p.ego_nearest_dist_threshold, - p.ego_nearest_yaw_threshold)) { - return utils::getReferencePath(closest_lane, data); - } - - if (lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose, &closest_lane)) { - return utils::getReferencePath(closest_lane, data); - } - - return {}; // something wrong. - } -``` - -Code is [here](https://github.com/autowarefoundation/autoware.universe/blob/b1734916e3efd9786507a271e0fe829dd37476c8/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp#L202-L227) +In addition, while manually driving (i.e., either the `OperationModeState` is different from `AUTONOMOUS` or the Autoware control is not engaged), the manager resets the **current route lanelet** at each iteration because the ego vehicle may move to an adjacent lane regardless of the decision of the autonomous driving system. +The only exception is when a module is already approved, allowing testing the module's behavior while manually driving. ## Drivable area generation diff --git a/planning/behavior_path_planner/image/manager/avoidance.svg b/planning/behavior_path_planner/image/manager/avoidance.svg index b61231aa69b17..ea69166abee5f 100644 --- a/planning/behavior_path_planner/image/manager/avoidance.svg +++ b/planning/behavior_path_planner/image/manager/avoidance.svg @@ -1,80 +1,373 @@ + - - - - - - - - - -
-
-
- Ego + + + + + + + + + + + + + +
+
+
+ lanelet +
-
- - Ego - - - - - - -
-
-
- ego's driving lane + + + + + + + + + + + + +
+
+
+ lanelet +
-
- - ego's driving lane - - - - - - -
-
-
- obstacle + + + + + + + + + + + + +
+
+
+ + current route lanelet +
+ +
-
- - obstacle - + + + + + + + + + + + + +
+
+
+ lanelet +
+
+
+ + + + + + + + + + + + + + + +
+
+
+ root reference path +
+
+
+ + + + + + + + + + + + + + + + + + +
+
+
+ Ego +
+
+
+ + + + + + + + + + + + + + + +
+
+
+ lanelet sequence +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Ego +
+
+
+ + + + + + + + + + + + +
+
+
+ current route lanelet +
+
+
+ + + + + + + + + + + + +
+
+
+ obstacle +
+
+
+ + + + + + + + + + + + + + + + + + + - - - - Text is not SVG - cannot display - - diff --git a/planning/behavior_path_planner/image/manager/current_route_lanelet.svg b/planning/behavior_path_planner/image/manager/current_route_lanelet.svg new file mode 100644 index 0000000000000..bbeaf17db79d1 --- /dev/null +++ b/planning/behavior_path_planner/image/manager/current_route_lanelet.svg @@ -0,0 +1,147 @@ + + + + + + + + + + + + + + + +
+
+
+ current route lanelet +
+
+
+ + + + + + + + + + + + + +
+
+
+ driving lane data +
+
+
+ + + + + + + + + + + + + +
+
+
+ root reference path +
+
+
+ + + + + + + + + + + + +
+
+
+ generate root reference path +
+
+
+ + + + + + + diff --git a/planning/behavior_path_planner/image/manager/lane_change.svg b/planning/behavior_path_planner/image/manager/lane_change.svg index b0a01d6b9daae..ba7cccddda523 100644 --- a/planning/behavior_path_planner/image/manager/lane_change.svg +++ b/planning/behavior_path_planner/image/manager/lane_change.svg @@ -1,212 +1,274 @@ + + - - - - - - - - - -
-
-
- ego's driving lane -
-
-
- - ego's driving lane - - - - - - -
-
-
- Ego -
-
-
- - Ego - - - - - - - - -
-
-
- before lane change + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ current route lanelet +
-
- - before lane change - - - - - - -
-
-
- after lane change + + + + + + + + + + + + +
+
+
+ Ego +
-
- - after lane change - - - - - - - -
-
-
- root lanelet + + + + + + + + + + + + + + + + + + +
+
+
+ before lane change +
-
- - root lanelet - - - - - - - -
-
-
- root lanelet (UPDATED) + + + + + + + + + + + + +
+
+
+ after lane change +
-
- - root lanelet (UPDATED) - - - - - - - -
-
-
- lanelet + + + + + + + + + + + + + + + +
+
+
+
+ current route lanelet +
+
+
+ (updated) +
+
+
-
- - lanelet - - - - - - -
-
-
- lanelet + + + + + + + + + + + + + + + +
+
+
+ lanelet +
-
- - lanelet - - - - - - - -
-
-
- Ego + + + + + + + + + + + + + + + +
+
+
+ Ego +
-
- - Ego - + + + + + + + - - - - Text is not SVG - cannot display - - diff --git a/planning/behavior_path_planner/image/manager/root_lanelet.svg b/planning/behavior_path_planner/image/manager/root_lanelet.svg deleted file mode 100644 index 802eb1613e77b..0000000000000 --- a/planning/behavior_path_planner/image/manager/root_lanelet.svg +++ /dev/null @@ -1,166 +0,0 @@ - - - - - - - - - - -
-
-
- root lanelet -
-
-
- - root lanelet - - - - - - -
-
-
- lanelet -
-
-
- - lanelet - - - - - - -
-
-
- lanelet -
-
-
- - lanelet - - - - - - -
-
-
- lanelet -
-
-
- - lanelet - - - - - - - -
-
-
- root reference path -
-
-
- - root refe... - - - - - - - - -
-
-
- Ego -
-
-
- - Ego - - - - - - - -
-
-
- lanelet sequence -
-
-
- - lanelet sequence - - - - - - - - - - Text is not SVG - cannot display - - - diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp index 5e773eba96aee..9054435a33912 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp @@ -137,7 +137,7 @@ class PlannerManager std::for_each(manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->reset(); }); approved_module_ptrs_.clear(); candidate_module_ptrs_.clear(); - root_lanelet_ = std::nullopt; + current_route_lanelet_ = std::nullopt; resetProcessingTime(); } @@ -239,13 +239,6 @@ class PlannerManager */ bool hasCandidateModules() const { return !candidate_module_ptrs_.empty(); } - /** - * @brief reset root lanelet. if there are approved modules, don't reset root lanelet. - * @param planner data. - * @details this function is called only when it is in disengage and drive by manual. - */ - void resetRootLanelet(const std::shared_ptr & data); - /** * @brief show planner manager internal condition. */ @@ -261,6 +254,18 @@ class PlannerManager */ std::shared_ptr getDebugMsg(); + /** + * @brief reset the current route lanelet to be the closest lanelet within the route + * @param planner data. + */ + void resetCurrentRouteLanelet(const std::shared_ptr & data) + { + lanelet::ConstLanelet ret{}; + data->route_handler->getClosestLaneletWithinRoute(data->self_odometry->pose.pose, &ret); + RCLCPP_DEBUG(logger_, "update current route lanelet. id:%ld", ret.id()); + current_route_lanelet_ = ret; + } + private: /** * @brief run the module and publish RTC status. @@ -300,11 +305,16 @@ class PlannerManager BehaviorModuleOutput & output, const std::shared_ptr & data) const; /** - * @brief get reference path from root_lanelet_ centerline. + * @brief get reference path from current_route_lanelet_ centerline. * @param planner data. * @return reference path. */ - BehaviorModuleOutput getReferencePath(const std::shared_ptr & data) const; + BehaviorModuleOutput getReferencePath(const std::shared_ptr & data); + + /** + * @brief publish the root reference path and current route lanelet + */ + void publishDebugRootReferencePath(const BehaviorModuleOutput & reference_path); /** * @brief stop and unregister the module from manager. @@ -361,19 +371,6 @@ class PlannerManager candidate_module_ptrs_.clear(); } - /** - * @brief get current root lanelet. the lanelet is used for reference path generation. - * @param planner data. - * @return root lanelet. - */ - lanelet::ConstLanelet updateRootLanelet(const std::shared_ptr & data) const - { - lanelet::ConstLanelet ret{}; - data->route_handler->getClosestLaneletWithinRoute(data->self_odometry->pose.pose, &ret); - RCLCPP_DEBUG(logger_, "update start lanelet. id:%ld", ret.id()); - return ret; - } - /** * @brief get manager pointer from module name. * @param module pointer. @@ -447,7 +444,7 @@ class PlannerManager static std::string getNames(const std::vector & modules); - std::optional root_lanelet_{std::nullopt}; + std::optional current_route_lanelet_{std::nullopt}; std::vector manager_ptrs_; diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 7531c1bd8d0dc..d3fc0b11a9c52 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -367,10 +367,8 @@ void BehaviorPathPlannerNode::run() const bool is_first_time = !(planner_data_->route_handler->isHandlerReady()); if (route_ptr) { planner_data_->route_handler->setRoute(*route_ptr); - planner_manager_->resetRootLanelet(planner_data_); - // uuid is not changed when rerouting with modified goal, - // in this case do not need to rest modules. + // in this case do not need to reset modules. const bool has_same_route_id = planner_data_->prev_route_id && route_ptr->uuid == planner_data_->prev_route_id; // Reset behavior tree when new route is received, @@ -380,14 +378,13 @@ void BehaviorPathPlannerNode::run() planner_manager_->reset(); planner_data_->prev_modified_goal.reset(); } + planner_manager_->resetCurrentRouteLanelet(planner_data_); } - const auto controlled_by_autoware_autonomously = planner_data_->operation_mode->mode == OperationModeState::AUTONOMOUS && planner_data_->operation_mode->is_autoware_control_enabled; - if (!controlled_by_autoware_autonomously) { - planner_manager_->resetRootLanelet(planner_data_); - } + if (!controlled_by_autoware_autonomously && !planner_manager_->hasApprovedModules()) + planner_manager_->resetCurrentRouteLanelet(planner_data_); // run behavior planner const auto output = planner_manager_->run(planner_data_); diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index c60bc53a1129e..8e8b9a809cf6a 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -89,9 +89,7 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da stop_watch_.tic("total_time"); debug_info_.clear(); - if (!root_lanelet_) { - root_lanelet_ = updateRootLanelet(data); - } + if (!current_route_lanelet_) resetCurrentRouteLanelet(data); std::for_each( manager_ptrs_.begin(), manager_ptrs_.end(), [&data](const auto & m) { m->setData(data); }); @@ -416,8 +414,7 @@ BehaviorModuleOutput PlannerManager::runKeepLastModules( return output; } -BehaviorModuleOutput PlannerManager::getReferencePath( - const std::shared_ptr & data) const +BehaviorModuleOutput PlannerManager::getReferencePath(const std::shared_ptr & data) { const auto & route_handler = data->route_handler; const auto & pose = data->self_odometry->pose.pose; @@ -428,20 +425,41 @@ BehaviorModuleOutput PlannerManager::getReferencePath( std::max(p.backward_path_length, p.backward_path_length + extra_margin); const auto lanelet_sequence = route_handler->getLaneletSequence( - root_lanelet_.value(), pose, backward_length, std::numeric_limits::max()); + current_route_lanelet_.value(), pose, backward_length, p.forward_path_length); lanelet::ConstLanelet closest_lane{}; - if (lanelet::utils::query::getClosestLaneletWithConstrains( - lanelet_sequence, pose, &closest_lane, p.ego_nearest_dist_threshold, - p.ego_nearest_yaw_threshold)) { - return utils::getReferencePath(closest_lane, data); - } - - if (lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose, &closest_lane)) { - return utils::getReferencePath(closest_lane, data); - } + const auto could_calculate_closest_lanelet = + lanelet::utils::query::getClosestLaneletWithConstrains( + lanelet_sequence, pose, &closest_lane, p.ego_nearest_dist_threshold, + p.ego_nearest_yaw_threshold) || + lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose, &closest_lane); + + if (could_calculate_closest_lanelet) + current_route_lanelet_ = closest_lane; + else + resetCurrentRouteLanelet(data); + + const auto reference_path = utils::getReferencePath(*current_route_lanelet_, data); + publishDebugRootReferencePath(reference_path); + return reference_path; +} - return {}; // something wrong. +void PlannerManager::publishDebugRootReferencePath(const BehaviorModuleOutput & reference_path) +{ + using visualization_msgs::msg::Marker; + MarkerArray array; + Marker m = tier4_autoware_utils::createDefaultMarker( + "map", clock_.now(), "root_reference_path", 0UL, Marker::LINE_STRIP, + tier4_autoware_utils::createMarkerScale(1.0, 1.0, 1.0), + tier4_autoware_utils::createMarkerColor(1.0, 0.0, 0.0, 1.0)); + for (const auto & p : reference_path.path.points) m.points.push_back(p.point.pose.position); + array.markers.push_back(m); + m.points.clear(); + m.id = 1UL; + for (const auto & p : current_route_lanelet_->polygon3d().basicPolygon()) + m.points.emplace_back().set__x(p.x()).set__y(p.y()).set__z(p.z()); + array.markers.push_back(m); + debug_publisher_ptr_->publish("root_reference_path", array); } SceneModulePtr PlannerManager::selectHighestPriorityModule( @@ -770,7 +788,8 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrisRootLaneletToBeUpdated(); - })) { - root_lanelet_ = updateRootLanelet(data); - } + return m->isCurrentRouteLaneletToBeReset(); + })) + resetCurrentRouteLanelet(data); std::for_each(itr, approved_module_ptrs_.end(), [&](auto & m) { debug_info_.emplace_back(m, Action::DELETE, "From Approved"); @@ -875,62 +893,6 @@ void PlannerManager::updateCandidateModules( sortByPriority(candidate_module_ptrs_); } -void PlannerManager::resetRootLanelet(const std::shared_ptr & data) -{ - if (!root_lanelet_) { - root_lanelet_ = updateRootLanelet(data); - return; - } - - // when lane change module is running, don't update root lanelet. - const bool is_lane_change_running = std::invoke([&]() { - const auto lane_change_itr = std::find_if( - approved_module_ptrs_.begin(), approved_module_ptrs_.end(), - [](const auto & m) { return m->isRootLaneletToBeUpdated(); }); - return lane_change_itr != approved_module_ptrs_.end(); - }); - if (is_lane_change_running) { - return; - } - - const auto root_lanelet = updateRootLanelet(data); - - // if root_lanelet is not route lanelets, reset root lanelet. - // this can be caused by rerouting. - const auto & route_handler = data->route_handler; - if (!route_handler->isRouteLanelet(root_lanelet_.value())) { - root_lanelet_ = root_lanelet; - return; - } - - // check ego is in same lane - if (root_lanelet_.value().id() == root_lanelet.id()) { - return; - } - - // check ego is in next lane - const auto next_lanelets = route_handler->getRoutingGraphPtr()->following(root_lanelet_.value()); - for (const auto & next : next_lanelets) { - if (next.id() == root_lanelet.id()) { - return; - } - } - - if (!approved_module_ptrs_.empty()) { - return; - } - - for (const auto & m : candidate_module_ptrs_) { - if (m->isLockedNewModuleLaunch()) { - return; - } - } - - root_lanelet_ = root_lanelet; - - RCLCPP_INFO_EXPRESSION(logger_, verbose_, "change ego's following lane. reset."); -} - void PlannerManager::print() const { const auto get_status = [](const auto & m) { diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp index 1a8c8241abe1a..8a8d7068aee55 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp @@ -259,7 +259,7 @@ class SceneModuleInterface return is_waiting_approval_ || current_state_ == ModuleStatus::WAITING_APPROVAL; } - virtual bool isRootLaneletToBeUpdated() const { return false; } + virtual bool isCurrentRouteLaneletToBeReset() const { return false; } bool isLockedNewModuleLaunch() const { return is_locked_new_module_launch_; } diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp index dcee7696933dd..9054fb20c3828 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp @@ -262,8 +262,8 @@ std::vector getTargetLaneletPolygons( const lanelet::ConstLanelets & lanelets, const Pose & pose, const double check_length, const std::string & target_type); -PathWithLaneId getCenterLinePathFromRootLanelet( - const lanelet::ConstLanelet & root_lanelet, +PathWithLaneId getCenterLinePathFromLanelet( + const lanelet::ConstLanelet & current_route_lanelet, const std::shared_ptr & planner_data); // route handler diff --git a/planning/behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner_common/src/utils/utils.cpp index 308cf9c4fed2c..5fe21016fcb46 100644 --- a/planning/behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/utils.cpp @@ -1114,16 +1114,15 @@ std::shared_ptr generateCenterLinePath( return centerline_path; } -PathWithLaneId getCenterLinePathFromRootLanelet( - const lanelet::ConstLanelet & root_lanelet, - const std::shared_ptr & planner_data) +PathWithLaneId getCenterLinePathFromLanelet( + const lanelet::ConstLanelet & lanelet, const std::shared_ptr & planner_data) { const auto & route_handler = planner_data->route_handler; const auto & current_pose = planner_data->self_odometry->pose.pose; const auto & p = planner_data->parameters; const auto reference_lanes = route_handler->getLaneletSequence( - root_lanelet, current_pose, p.backward_path_length, p.forward_path_length); + lanelet, current_pose, p.backward_path_length, p.forward_path_length); return getCenterLinePath( *route_handler, reference_lanes, current_pose, p.backward_path_length, p.forward_path_length,