From dc5baadbdeeedfeada4236e045d134b894154b46 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Mon, 8 May 2023 10:50:30 +0900 Subject: [PATCH] refactor(avoidance_by_lane_change): reuse lane change class (#3577) * refactor(avoidance_by_lane_change): reuse lane change class Signed-off-by: Muhammad Zulfaqar Azmi * fix wrong direction Signed-off-by: Muhammad Zulfaqar * fix conflict Signed-off-by: Muhammad Zulfaqar * fix rtc paramters Signed-off-by: Muhammad Zulfaqar * Revert "fix rtc paramters" This reverts commit 16be270af3225cbe9e826a89b048a6dd044e98b9. * fix rtc Signed-off-by: Muhammad Zulfaqar Azmi * fix pre-commit Signed-off-by: Muhammad Zulfaqar Azmi * fix pre-commit and move updateRTCStatus to cpp file Signed-off-by: Muhammad Zulfaqar Azmi * COMPILE_WITH_OLD_ARCHITECTURE=TRUE Signed-off-by: Muhammad Zulfaqar Azmi --------- Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Muhammad Zulfaqar Signed-off-by: Mingyu Li --- planning/behavior_path_planner/CMakeLists.txt | 3 +- .../behavior_path_planner_node.hpp | 2 - .../lane_change/avoidance_by_lane_change.hpp | 62 +++ .../scene_module/lane_change/base_class.hpp | 16 +- .../scene_module/lane_change/interface.hpp | 30 +- .../scene_module/lane_change/manager.hpp | 19 +- .../scene_module/lane_change/normal.hpp | 5 +- .../utils/avoidance_by_lc/module_data.hpp | 45 -- .../lane_change/lane_change_module_data.hpp | 23 +- .../utils/lane_change/utils.hpp | 16 - .../src/behavior_path_planner_node.cpp | 8 +- .../scene_module/avoidance_by_lc/module.cpp | 387 ------------------ .../lane_change/avoidance_by_lane_change.cpp | 224 ++++++++++ .../scene_module/lane_change/interface.cpp | 70 ++++ .../src/scene_module/lane_change/manager.cpp | 30 ++ .../src/scene_module/lane_change/normal.cpp | 40 +- .../src/utils/lane_change/utils.cpp | 230 ----------- 17 files changed, 487 insertions(+), 723 deletions(-) create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp delete mode 100644 planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance_by_lc/module_data.hpp create mode 100644 planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index 8cad60065540b..0de6dc33582ea 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -14,7 +14,6 @@ set(common_src src/behavior_path_planner_node.cpp src/scene_module/scene_module_visitor.cpp src/scene_module/avoidance/avoidance_module.cpp - src/scene_module/avoidance_by_lc/module.cpp src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp src/scene_module/pull_out/pull_out_module.cpp src/scene_module/goal_planner/goal_planner_module.cpp @@ -22,6 +21,7 @@ set(common_src src/scene_module/lane_change/interface.cpp src/scene_module/lane_change/normal.cpp src/scene_module/lane_change/external_request.cpp + src/scene_module/lane_change/avoidance_by_lane_change.cpp src/turn_signal_decider.cpp src/utils/utils.cpp src/utils/path_utils.cpp @@ -66,7 +66,6 @@ else() ament_auto_add_library(behavior_path_planner_node SHARED src/planner_manager.cpp src/scene_module/avoidance/manager.cpp - src/scene_module/avoidance_by_lc/manager.cpp src/scene_module/dynamic_avoidance/manager.cpp src/scene_module/pull_out/manager.cpp src/scene_module/goal_planner/manager.cpp diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 037c470c23dc0..2c1ef1038554e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -29,7 +29,6 @@ #else #include "behavior_path_planner/planner_manager.hpp" #include "behavior_path_planner/scene_module/avoidance/manager.hpp" -#include "behavior_path_planner/scene_module/avoidance_by_lc/manager.hpp" #include "behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp" #include "behavior_path_planner/scene_module/goal_planner/manager.hpp" #include "behavior_path_planner/scene_module/lane_change/manager.hpp" @@ -40,7 +39,6 @@ #include "behavior_path_planner/steering_factor_interface.hpp" #include "behavior_path_planner/turn_signal_decider.hpp" #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" -#include "behavior_path_planner/utils/avoidance_by_lc/module_data.hpp" #include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" #include "behavior_path_planner/utils/lane_following/module_data.hpp" diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp new file mode 100644 index 0000000000000..e58a8c07850cf --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp @@ -0,0 +1,62 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__AVOIDANCE_BY_LANE_CHANGE_HPP_ +#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__AVOIDANCE_BY_LANE_CHANGE_HPP_ + +#include "behavior_path_planner/scene_module/lane_change/normal.hpp" + +#include +#include + +namespace behavior_path_planner +{ +using autoware_auto_planning_msgs::msg::PathWithLaneId; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::Twist; +using tier4_planning_msgs::msg::LaneChangeDebugMsg; +using tier4_planning_msgs::msg::LaneChangeDebugMsgArray; +using AvoidanceDebugData = DebugData; + +class AvoidanceByLaneChange : public NormalLaneChange +{ +public: + AvoidanceByLaneChange( + const std::shared_ptr & parameters, + std::shared_ptr avoidance_parameters, + std::shared_ptr avoidance_by_lane_change_parameters); + + void updateSpecialData() override; + + std::pair getSafePath(LaneChangePath & safe_path) const override; + +private: + std::shared_ptr avoidance_parameters_; + std::shared_ptr avoidance_by_lane_change_parameters_; + + AvoidancePlanningData calcAvoidancePlanningData(AvoidanceDebugData & debug) const; + AvoidancePlanningData avoidance_data_; + mutable AvoidanceDebugData avoidance_debug_data_; + + ObjectDataArray registered_objects_; + mutable ObjectDataArray stopped_objects_; + + ObjectData createObjectData( + const AvoidancePlanningData & data, const PredictedObject & object) const; + + void fillAvoidanceTargetObjects(AvoidancePlanningData & data, AvoidanceDebugData & debug) const; +}; +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__AVOIDANCE_BY_LANE_CHANGE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp index de5955f123940..011431f5017b3 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp @@ -55,7 +55,7 @@ class LaneChangeBase LaneChangeBase( std::shared_ptr parameters, LaneChangeModuleType type, Direction direction) - : parameters_{std::move(parameters)}, direction_{direction}, type_{type} + : lane_change_parameters_{std::move(parameters)}, direction_{direction}, type_{type} { prev_module_reference_path_ = std::make_shared(); prev_module_path_ = std::make_shared(); @@ -107,6 +107,8 @@ class LaneChangeBase } } + virtual void updateSpecialData() {} + const LaneChangeStatus & getLaneChangeStatus() const { return status_; } LaneChangePath getLaneChangePath() const @@ -120,7 +122,7 @@ class LaneChangeBase bool isAbortState() const { - if (!parameters_->enable_abort_lane_change) { + if (!lane_change_parameters_->enable_abort_lane_change) { return false; } @@ -167,7 +169,7 @@ class LaneChangeBase Direction getDirection() const { - if (direction_ == Direction::NONE) { + if (direction_ == Direction::NONE && !status_.lane_change_path.path.points.empty()) { const auto lateral_shift = utils::lane_change::getLateralShift(status_.lane_change_path); return lateral_shift > 0.0 ? Direction::LEFT : Direction::RIGHT; } @@ -187,7 +189,8 @@ class LaneChangeBase virtual bool getLaneChangePaths( const lanelet::ConstLanelets & original_lanelets, - const lanelet::ConstLanelets & target_lanelets, LaneChangePaths * candidate_paths) const = 0; + const lanelet::ConstLanelets & target_lanelets, Direction direction, + LaneChangePaths * candidate_paths) const = 0; virtual std::vector getDrivableLanes() const = 0; @@ -197,7 +200,8 @@ class LaneChangeBase virtual bool isValidPath(const PathWithLaneId & path) const = 0; - lanelet::ConstLanelets getLaneChangeLanes(const lanelet::ConstLanelets & current_lanes) const; + virtual lanelet::ConstLanelets getLaneChangeLanes( + const lanelet::ConstLanelets & current_lanes, Direction direction) const = 0; bool isNearEndOfLane() const { @@ -222,7 +226,7 @@ class LaneChangeBase LaneChangeStates current_lane_change_state_{}; - std::shared_ptr parameters_{}; + std::shared_ptr lane_change_parameters_{}; std::shared_ptr abort_path_{}; std::shared_ptr planner_data_{}; std::shared_ptr prev_module_reference_path_{}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp index b098eecdec231..653c95b060163 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp @@ -16,6 +16,7 @@ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__INTERFACE_HPP_ #include "behavior_path_planner/marker_util/lane_change/debug.hpp" +#include "behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp" #include "behavior_path_planner/scene_module/lane_change/base_class.hpp" #include "behavior_path_planner/scene_module/lane_change/external_request.hpp" #include "behavior_path_planner/scene_module/lane_change/normal.hpp" @@ -74,6 +75,8 @@ class LaneChangeInterface : public SceneModuleInterface ModuleStatus updateState() override; + void updateData() override; + BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; @@ -113,6 +116,22 @@ class LaneChangeInterface : public SceneModuleInterface bool is_abort_approval_requested_{false}; }; +class AvoidanceByLaneChangeInterface : public LaneChangeInterface +{ +public: + AvoidanceByLaneChangeInterface( + const std::string & name, rclcpp::Node & node, + const std::shared_ptr & parameters, + const std::shared_ptr & avoidance_parameters, + const std::shared_ptr & avoidance_by_lane_change_parameters, + const std::unordered_map > & rtc_interface_ptr_map); + + ModuleStatus updateState() override; + +protected: + void updateRTCStatus(const double start_distance, const double finish_distance) override; +}; + class LaneChangeBTInterface : public LaneChangeInterface { public: @@ -150,16 +169,7 @@ class LaneChangeBTModule : public LaneChangeBTInterface const std::shared_ptr & parameters); protected: - void updateRTCStatus(const double start_distance, const double finish_distance) override - { - const auto direction = std::invoke([&]() -> std::string { - const auto dir = module_type_->getDirection(); - return (dir == Direction::LEFT) ? "left" : "right"; - }); - - rtc_interface_ptr_map_.at(direction)->updateCooperateStatus( - uuid_map_.at(direction), isExecutionReady(), start_distance, finish_distance, clock_->now()); - } + void updateRTCStatus(const double start_distance, const double finish_distance) override; }; class ExternalRequestLaneChangeLeftBTModule : public LaneChangeBTInterface diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp index 39452d6367fae..ebee8f3d3a07a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp @@ -42,7 +42,7 @@ class LaneChangeModuleManager : public SceneModuleManagerInterface void updateModuleParams(const std::vector & parameters) override; -private: +protected: std::shared_ptr parameters_; std::vector> registered_modules_; @@ -52,6 +52,23 @@ class LaneChangeModuleManager : public SceneModuleManagerInterface LaneChangeModuleType type_; }; +class AvoidanceByLaneChangeModuleManager : public LaneChangeModuleManager +{ +public: + AvoidanceByLaneChangeModuleManager( + rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, + std::shared_ptr parameters, + std::shared_ptr avoidance_parameters, + std::shared_ptr avoidance_by_lane_change_parameters); + + std::shared_ptr createNewSceneModuleInstance() override; + + // void updateModuleParams(const std::vector & parameters) override; + +private: + std::shared_ptr avoidance_parameters_; + std::shared_ptr avoidance_by_lane_change_parameters_; +}; } // namespace behavior_path_planner #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__MANAGER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index 11a7574bb075e..c4744b0ccbd38 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -68,7 +68,8 @@ class NormalLaneChange : public LaneChangeBase protected: lanelet::ConstLanelets getCurrentLanes() const override; - lanelet::ConstLanelets getLaneChangeLanes(const lanelet::ConstLanelets & current_lanes) const; + lanelet::ConstLanelets getLaneChangeLanes( + const lanelet::ConstLanelets & current_lanes, Direction direction) const override; int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const override; @@ -79,7 +80,7 @@ class NormalLaneChange : public LaneChangeBase bool getLaneChangePaths( const lanelet::ConstLanelets & original_lanelets, - const lanelet::ConstLanelets & target_lanelets, + const lanelet::ConstLanelets & target_lanelets, Direction direction, LaneChangePaths * candidate_paths) const override; std::vector getDrivableLanes() const override; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance_by_lc/module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance_by_lc/module_data.hpp deleted file mode 100644 index e556c56eac8d7..0000000000000 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance_by_lc/module_data.hpp +++ /dev/null @@ -1,45 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE_BY_LC__MODULE_DATA_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE_BY_LC__MODULE_DATA_HPP_ - -#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" -#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" - -#include - -namespace behavior_path_planner -{ - -struct AvoidanceByLCParameters -{ - // avoidance - std::shared_ptr avoidance; - - // lane change - std::shared_ptr lane_change; - - // execute if the target object number is larger than this param. - size_t execute_object_num{1}; - - // execute only when the target object longitudinal distance is larger than this param. - double execute_object_longitudinal_margin{0.0}; - - // execute only when lane change end point is before the object. - bool execute_only_when_lane_change_finish_before_object{false}; -}; - -} // namespace behavior_path_planner - -#endif // BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE_BY_LC__MODULE_DATA_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index b19e7469c0600..e81f8821b25ed 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -14,10 +14,12 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ +#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" #include "lanelet2_core/geometry/Lanelet.h" #include "autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp" +#include #include #include @@ -79,7 +81,26 @@ struct LaneChangeTargetObjectIndices std::vector other_lane{}; }; -enum class LaneChangeModuleType { NORMAL = 0, EXTERNAL_REQUEST }; +enum class LaneChangeModuleType { + NORMAL = 0, + EXTERNAL_REQUEST, + AVOIDANCE_BY_LANE_CHANGE, +}; + +struct AvoidanceByLCParameters +{ + std::shared_ptr avoidance{}; + std::shared_ptr lane_change{}; + + // execute if the target object number is larger than this param. + size_t execute_object_num{1}; + + // execute only when the target object longitudinal distance is larger than this param. + double execute_object_longitudinal_margin{0.0}; + + // execute only when lane change end point is before the object. + bool execute_only_when_lane_change_finish_before_object{false}; +}; } // namespace behavior_path_planner #endif // BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp index 43514b011c6fb..8702e7a8747a0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp @@ -79,22 +79,6 @@ std::optional constructCandidatePath( const BehaviorPathPlannerParameters & common_parameter, const LaneChangeParameters & lane_change_param); -bool getLaneChangePaths( - const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets, - const lanelet::ConstLanelets & target_lanelets, const Pose & pose, const Twist & twist, - const PredictedObjects::ConstSharedPtr dynamic_objects, - const BehaviorPathPlannerParameters & common_parameter, const LaneChangeParameters & parameter, - const double check_length, LaneChangePaths * candidate_paths, - std::unordered_map * debug_data); - -bool getLaneChangePaths( - const PathWithLaneId & original_path, const RouteHandler & route_handler, - const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, - const Pose & pose, const Twist & twist, const PredictedObjects::ConstSharedPtr dynamic_objects, - const BehaviorPathPlannerParameters & common_parameter, const LaneChangeParameters & parameter, - const double check_length, const Direction direction, LaneChangePaths * candidate_paths, - std::unordered_map * debug_data); - bool isLaneChangePathSafe( const LaneChangePath & lane_change_path, const PredictedObjects::ConstSharedPtr dynamic_objects, const LaneChangeTargetObjectIndices & dynamic_object_indices, const Pose & current_pose, 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 e58c71f00fc62..bb491086992f0 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -286,8 +286,9 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod } if (p.config_avoidance_by_lc.enable_module) { - auto manager = std::make_shared( - this, "avoidance_by_lane_change", p.config_avoidance_by_lc, avoidance_by_lc_param_ptr_); + auto manager = std::make_shared( + this, "avoidance_by_lane_change", p.config_avoidance_by_lc, lane_change_param_ptr_, + avoidance_param_ptr_, avoidance_by_lc_param_ptr_); planner_manager_->registerSceneModuleManager(manager); path_candidate_publishers_.emplace( "avoidance_by_lane_change", @@ -491,6 +492,9 @@ AvoidanceByLCParameters BehaviorPathPlannerNode::getAvoidanceByLCParam( declare_parameter(ns + "execute_only_when_lane_change_finish_before_object"); } + if (p.execute_object_num < 1) { + RCLCPP_WARN_STREAM(get_logger(), "execute_object_num cannot be lesser than 1."); + } return p; } diff --git a/planning/behavior_path_planner/src/scene_module/avoidance_by_lc/module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance_by_lc/module.cpp index e3f032d3d0be3..c76034d03cecb 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance_by_lc/module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance_by_lc/module.cpp @@ -55,406 +55,21 @@ AvoidanceByLCModule::AvoidanceByLCModule( : SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{parameters} { } - -void AvoidanceByLCModule::processOnEntry() -{ -#ifndef USE_OLD_ARCHITECTURE - waitApproval(); -#endif - current_lane_change_state_ = LaneChangeStates::Normal; - updateLaneChangeStatus(); -} - -void AvoidanceByLCModule::processOnExit() -{ - resetParameters(); -} - -bool AvoidanceByLCModule::isExecutionRequested() const -{ - if (current_state_ == ModuleStatus::RUNNING) { - return true; - } - -#ifdef USE_OLD_ARCHITECTURE - const auto current_lanes = utils::getCurrentLanes(planner_data_); -#else - const auto current_lanes = - utils::getCurrentLanesFromPath(*getPreviousModuleOutput().reference_path, planner_data_); -#endif - const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); - - LaneChangePath selected_path; - const auto [found_valid_path, found_safe_path] = - getSafePath(lane_change_lanes, check_distance_, selected_path); - - if (!found_valid_path) { - return false; - } - - const auto object_num = avoidance_data_.target_objects.size(); - if (parameters_->execute_object_num > object_num) { - return false; - } - - const auto to_front_object_distance = avoidance_data_.target_objects.front().longitudinal; - if (parameters_->execute_object_longitudinal_margin > to_front_object_distance) { - return false; - } - - const auto to_lane_change_end_distance = calcSignedArcLength( - selected_path.path.points, getEgoPose().position, selected_path.shift_line.end.position); - const auto lane_change_finish_before_object = - to_front_object_distance > to_lane_change_end_distance; - if ( - !lane_change_finish_before_object && - parameters_->execute_only_when_lane_change_finish_before_object) { - return false; - } - - return true; -} - -bool AvoidanceByLCModule::isExecutionReady() const -{ - if (current_state_ == ModuleStatus::RUNNING) { - return true; - } - -#ifdef USE_OLD_ARCHITECTURE - const auto current_lanes = utils::getCurrentLanes(planner_data_); -#else - const auto current_lanes = - utils::getCurrentLanesFromPath(*getPreviousModuleOutput().reference_path, planner_data_); -#endif - const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); - - LaneChangePath selected_path; - const auto [found_valid_path, found_safe_path] = - getSafePath(lane_change_lanes, check_distance_, selected_path); - - return found_safe_path; -} - -void AvoidanceByLCModule::updateData() -{ - debug_data_ = DebugData(); - avoidance_data_ = calcAvoidancePlanningData(debug_data_); - - utils::avoidance::updateRegisteredObject( - registered_objects_, avoidance_data_.target_objects, parameters_->avoidance); - utils::avoidance::compensateDetectionLost( - registered_objects_, avoidance_data_.target_objects, avoidance_data_.other_objects); - - std::sort( - avoidance_data_.target_objects.begin(), avoidance_data_.target_objects.end(), - [](auto a, auto b) { return a.longitudinal < b.longitudinal; }); -} - -AvoidancePlanningData AvoidanceByLCModule::calcAvoidancePlanningData(DebugData & debug) const -{ - AvoidancePlanningData data; - - // reference pose - data.reference_pose = getEgoPose(); - - data.reference_path_rough = *getPreviousModuleOutput().path; - - data.reference_path = utils::resamplePathWithSpline( - data.reference_path_rough, parameters_->avoidance->resample_interval_for_planning); - - // lanelet info -#ifdef USE_OLD_ARCHITECTURE - data.current_lanelets = utils::getCurrentLanes(planner_data_); -#else - data.current_lanelets = - utils::getCurrentLanesFromPath(*getPreviousModuleOutput().reference_path, planner_data_); -#endif - - // target objects for avoidance - fillAvoidanceTargetObjects(data, debug); - - return data; -} - -void AvoidanceByLCModule::fillAvoidanceTargetObjects( - AvoidancePlanningData & data, DebugData & debug) const -{ - const auto expanded_lanelets = lanelet::utils::getExpandedLanelets( - data.current_lanelets, parameters_->avoidance->detection_area_left_expand_dist, - parameters_->avoidance->detection_area_right_expand_dist * (-1.0)); - - const auto [object_within_target_lane, object_outside_target_lane] = - utils::separateObjectsByLanelets(*planner_data_->dynamic_object, expanded_lanelets); - - for (const auto & object : object_outside_target_lane.objects) { - ObjectData other_object; - other_object.object = object; - other_object.reason = "OutOfTargetArea"; - data.other_objects.push_back(other_object); - } - - ObjectDataArray objects; - for (const auto & object : object_within_target_lane.objects) { - objects.push_back(createObjectData(data, object)); - } - - utils::avoidance::filterTargetObjects( - objects, data, debug, planner_data_, parameters_->avoidance); -} - -ObjectData AvoidanceByLCModule::createObjectData( - const AvoidancePlanningData & data, const PredictedObject & object) const -{ - using boost::geometry::return_centroid; - - const auto & path_points = data.reference_path.points; - const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; - const auto object_closest_index = findNearestIndex(path_points, object_pose.position); - const auto object_closest_pose = path_points.at(object_closest_index).point.pose; - - ObjectData object_data{}; - - object_data.object = object; - - // Calc envelop polygon. - utils::avoidance::fillObjectEnvelopePolygon( - object_data, registered_objects_, object_closest_pose, parameters_->avoidance); - - // calc object centroid. - object_data.centroid = return_centroid(object_data.envelope_poly); - - // Calc moving time. - utils::avoidance::fillObjectMovingTime(object_data, stopped_objects_, parameters_->avoidance); - - // Calc lateral deviation from path to target object. - object_data.lateral = calcLateralDeviation(object_closest_pose, object_pose.position); - - // Find the footprint point closest to the path, set to object_data.overhang_distance. - object_data.overhang_dist = utils::avoidance::calcEnvelopeOverhangDistance( - object_data, object_closest_pose, object_data.overhang_pose.position); - - // Check whether the the ego should avoid the object. - const auto & vehicle_width = planner_data_->parameters.vehicle_width; - const auto safety_margin = - 0.5 * vehicle_width + parameters_->avoidance->lateral_passable_safety_buffer; - object_data.avoid_required = - (utils::avoidance::isOnRight(object_data) && - std::abs(object_data.overhang_dist) < safety_margin) || - (!utils::avoidance::isOnRight(object_data) && object_data.overhang_dist < safety_margin); - - return object_data; -} - -ModuleStatus AvoidanceByLCModule::updateState() -{ - RCLCPP_DEBUG(getLogger(), "AVOIDANCE_BY_LC updateState"); - if (!isValidPath()) { - current_state_ = ModuleStatus::SUCCESS; - return current_state_; - } - - if (isWaitingApproval()) { - const auto object_num = avoidance_data_.target_objects.size(); - if (parameters_->execute_object_num > object_num) { - current_state_ = ModuleStatus::SUCCESS; - return current_state_; - } - - const auto to_front_object_distance = avoidance_data_.target_objects.front().longitudinal; - if (parameters_->execute_object_longitudinal_margin > to_front_object_distance) { - current_state_ = ModuleStatus::FAILURE; - return current_state_; - } - - const auto to_lane_change_end_distance = calcSignedArcLength( - status_.lane_change_path.path.points, getEgoPose().position, - status_.lane_change_path.shift_line.end.position); - const auto lane_change_finish_before_object = - to_front_object_distance > to_lane_change_end_distance; - if ( - !lane_change_finish_before_object && - parameters_->execute_only_when_lane_change_finish_before_object) { - current_state_ = ModuleStatus::FAILURE; - return current_state_; - } - } - - const auto is_within_current_lane = utils::lane_change::isEgoWithinOriginalLane( - status_.current_lanes, getEgoPose(), planner_data_->parameters); - if (isAbortState() && !is_within_current_lane) { - current_state_ = ModuleStatus::RUNNING; - return current_state_; - } - - if (isAbortConditionSatisfied()) { - if ((isNearEndOfLane() && isCurrentVelocityLow()) || !is_within_current_lane) { - current_state_ = ModuleStatus::RUNNING; - return current_state_; - } - - current_state_ = ModuleStatus::FAILURE; - return current_state_; - } - - if (hasFinishedLaneChange()) { - current_state_ = ModuleStatus::SUCCESS; - return current_state_; - } - current_state_ = ModuleStatus::RUNNING; - return current_state_; -} - -BehaviorModuleOutput AvoidanceByLCModule::plan() -{ - resetPathCandidate(); - resetPathReference(); - is_activated_ = isActivated(); - - PathWithLaneId path = status_.lane_change_path.path; - if (!isValidPath(path)) { - status_.is_valid_path = false; - return BehaviorModuleOutput{}; - } else { - status_.is_valid_path = true; - } - - if ((is_abort_condition_satisfied_ && isNearEndOfLane() && isCurrentVelocityLow())) { - const auto stop_point = utils::insertStopPoint(0.1, path); - } - - if (isAbortState()) { - resetPathIfAbort(); - if (is_activated_) { - path = abort_path_->path; - } - } - - generateExtendedDrivableArea(path); - - BehaviorModuleOutput output; - output.path = std::make_shared(path); -#ifdef USE_OLD_ARCHITECTURE - path_reference_ = getPreviousModuleOutput().reference_path; - prev_approved_path_ = path; -#else - const auto reference_path = - utils::getCenterLinePathFromRootLanelet(status_.lane_change_lanes.front(), planner_data_); - output.reference_path = std::make_shared(reference_path); - path_reference_ = std::make_shared(reference_path); - prev_approved_path_ = *getPreviousModuleOutput().path; -#endif - updateOutputTurnSignal(output); - - updateSteeringFactorPtr(output); - clearWaitingApproval(); - - return output; -} - -void AvoidanceByLCModule::resetPathIfAbort() -{ - if (!is_abort_approval_requested_) { -#ifdef USE_OLD_ARCHITECTURE - const auto lateral_shift = utils::lane_change::getLateralShift(*abort_path_); - if (lateral_shift > 0.0) { - removePreviousRTCStatusRight(); - uuid_map_.at("right") = generateUUID(); - } else if (lateral_shift < 0.0) { - removePreviousRTCStatusLeft(); - uuid_map_.at("left") = generateUUID(); - } -#else - removeRTCStatus(); -#endif - RCLCPP_DEBUG(getLogger(), "[abort] uuid is reset to request abort approval."); - is_abort_approval_requested_ = true; - is_abort_path_approved_ = false; - return; - } - - if (isActivated()) { - RCLCPP_DEBUG(getLogger(), "[abort] isActivated() is true. set is_abort_path_approved to true."); - is_abort_path_approved_ = true; - clearWaitingApproval(); - } else { - RCLCPP_DEBUG(getLogger(), "[abort] isActivated() is False."); - is_abort_path_approved_ = false; - waitApproval(); - } -} - -CandidateOutput AvoidanceByLCModule::planCandidate() const -{ - CandidateOutput output; - - LaneChangePath selected_path; - // Get lane change lanes -#ifdef USE_OLD_ARCHITECTURE - const auto current_lanes = utils::getCurrentLanes(planner_data_); -#else - const auto current_lanes = - utils::getCurrentLanesFromPath(*getPreviousModuleOutput().reference_path, planner_data_); -#endif - const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); - - if (lane_change_lanes.empty()) { - return output; - } - -#ifdef USE_OLD_ARCHITECTURE - [[maybe_unused]] const auto [found_valid_path, found_safe_path] = - getSafePath(lane_change_lanes, check_distance_, selected_path); -#else - selected_path = status_.lane_change_path; -#endif - - selected_path.path.header = planner_data_->route_handler->getRouteHeader(); - - if (isAbortState()) { - selected_path = *abort_path_; - } - - if (selected_path.path.points.empty()) { - return output; - } - - output.path_candidate = selected_path.path; - output.lateral_shift = utils::lane_change::getLateralShift(selected_path); - output.start_distance_to_path_change = motion_utils::calcSignedArcLength( - selected_path.path.points, getEgoPose().position, selected_path.shift_line.start.position); - output.finish_distance_to_path_change = motion_utils::calcSignedArcLength( - selected_path.path.points, getEgoPose().position, selected_path.shift_line.end.position); - - updateSteeringFactorPtr(output, selected_path); - return output; -} - BehaviorModuleOutput AvoidanceByLCModule::planWaitingApproval() { -#ifdef USE_OLD_ARCHITECTURE - const auto is_within_current_lane = utils::lane_change::isEgoWithinOriginalLane( - status_.current_lanes, getEgoPose(), planner_data_->parameters); - if (is_within_current_lane) { - prev_approved_path_ = getReferencePath(); - } -#else prev_approved_path_ = *getPreviousModuleOutput().path; -#endif BehaviorModuleOutput out; out.path = std::make_shared(prev_approved_path_); out.reference_path = getPreviousModuleOutput().reference_path; out.turn_signal_info = getPreviousModuleOutput().turn_signal_info; // for new architecture -#ifndef USE_OLD_ARCHITECTURE const auto current_lanes = utils::getCurrentLanesFromPath(*getPreviousModuleOutput().reference_path, planner_data_); const auto drivable_lanes = utils::generateDrivableLanes(current_lanes); const auto target_drivable_lanes = getNonOverlappingExpandedLanes(*out.path, drivable_lanes); out.drivable_area_info.drivable_lanes = utils::combineDrivableLanes( getPreviousModuleOutput().drivable_area_info.drivable_lanes, target_drivable_lanes); -#endif if (!avoidance_data_.target_objects.empty()) { const auto to_front_object_distance = avoidance_data_.target_objects.front().longitudinal; @@ -467,9 +82,7 @@ BehaviorModuleOutput AvoidanceByLCModule::planWaitingApproval() getEgoPosition(), to_front_object_distance - lane_change_buffer, 0.0, *out.path, stop_pose_); } -#ifndef USE_OLD_ARCHITECTURE updateLaneChangeStatus(); -#endif const auto candidate = planCandidate(); path_candidate_ = std::make_shared(candidate.path_candidate); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp new file mode 100644 index 0000000000000..17fc83370d750 --- /dev/null +++ b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp @@ -0,0 +1,224 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp" + +#include "behavior_path_planner/utils/avoidance/utils.hpp" +#include "behavior_path_planner/utils/path_utils.hpp" + +#include + +#include + +namespace behavior_path_planner +{ +AvoidanceByLaneChange::AvoidanceByLaneChange( + const std::shared_ptr & parameters, + std::shared_ptr avoidance_parameters, + std::shared_ptr avoidance_by_lane_change_parameters) +: NormalLaneChange(parameters, LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE, Direction::NONE), + avoidance_parameters_(std::move(avoidance_parameters)), + avoidance_by_lane_change_parameters_(std::move(avoidance_by_lane_change_parameters)) +{ +} + +std::pair AvoidanceByLaneChange::getSafePath(LaneChangePath & safe_path) const +{ + const auto & avoidance_objects = avoidance_data_.target_objects; + const auto execute_object_num = avoidance_by_lane_change_parameters_->execute_object_num; + + if (avoidance_objects.size() < execute_object_num) { + return {false, false}; + } + + const auto current_lanes = getCurrentLanes(); + + if (current_lanes.empty()) { + return {false, false}; + } + + const auto & o_front = avoidance_objects.front(); + + // check distance from ego to o_front vs acceptable longitudinal margin + const auto execute_object_longitudinal_margin = + avoidance_by_lane_change_parameters_->execute_object_longitudinal_margin; + if (execute_object_longitudinal_margin > o_front.longitudinal) { + return {false, false}; + } + + const auto direction = utils::avoidance::isOnRight(o_front) ? Direction::LEFT : Direction::RIGHT; + const auto target_lanes = getLaneChangeLanes(current_lanes, direction); + + if (target_lanes.empty()) { + return {false, false}; + } + + // find candidate paths + LaneChangePaths valid_paths{}; + const auto found_safe_path = + getLaneChangePaths(current_lanes, target_lanes, direction, &valid_paths); + + if (valid_paths.empty()) { + return {false, false}; + } + + if (found_safe_path) { + safe_path = valid_paths.back(); + } else { + // force candidate + safe_path = valid_paths.front(); + } + + const auto to_lane_change_end_distance = motion_utils::calcSignedArcLength( + safe_path.path.points, getEgoPose().position, safe_path.shift_line.end.position); + const auto lane_change_finish_before_object = o_front.longitudinal > to_lane_change_end_distance; + const auto execute_only_when_lane_change_finish_before_object = + avoidance_by_lane_change_parameters_->execute_only_when_lane_change_finish_before_object; + if (!lane_change_finish_before_object && execute_only_when_lane_change_finish_before_object) { + return {false, found_safe_path}; + } + return {true, found_safe_path}; +} + +void AvoidanceByLaneChange::updateSpecialData() +{ + avoidance_debug_data_ = DebugData(); + avoidance_data_ = calcAvoidancePlanningData(avoidance_debug_data_); + + utils::avoidance::updateRegisteredObject( + registered_objects_, avoidance_data_.target_objects, + avoidance_by_lane_change_parameters_->avoidance); + utils::avoidance::compensateDetectionLost( + registered_objects_, avoidance_data_.target_objects, avoidance_data_.other_objects); + + std::sort( + avoidance_data_.target_objects.begin(), avoidance_data_.target_objects.end(), + [](auto a, auto b) { return a.longitudinal < b.longitudinal; }); +} + +AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData( + AvoidanceDebugData & debug) const +{ + AvoidancePlanningData data; + + // reference pose + data.reference_pose = getEgoPose(); + + data.reference_path_rough = *prev_module_path_; + + const auto resample_interval = + avoidance_by_lane_change_parameters_->avoidance->resample_interval_for_planning; + data.reference_path = utils::resamplePathWithSpline(data.reference_path_rough, resample_interval); + + data.current_lanelets = getCurrentLanes(); + + // get related objects from dynamic_objects, and then separates them as target objects and non + // target objects + fillAvoidanceTargetObjects(data, debug); + + return data; +} + +void AvoidanceByLaneChange::fillAvoidanceTargetObjects( + AvoidancePlanningData & data, DebugData & debug) const +{ + const auto left_expand_dist = avoidance_parameters_->detection_area_left_expand_dist; + const auto right_expand_dist = avoidance_parameters_->detection_area_right_expand_dist; + + const auto expanded_lanelets = lanelet::utils::getExpandedLanelets( + data.current_lanelets, left_expand_dist, right_expand_dist * (-1.0)); + + const auto [object_within_target_lane, object_outside_target_lane] = + utils::separateObjectsByLanelets(*planner_data_->dynamic_object, expanded_lanelets); + + // Assume that the maximum allocation for data.other object is the sum of + // objects_within_target_lane and object_outside_target_lane. The maximum allocation for + // data.target_objects is equal to object_within_target_lane + { + const auto other_objects_size = + object_within_target_lane.objects.size() + object_outside_target_lane.objects.size(); + data.other_objects.reserve(other_objects_size); + data.target_objects.reserve(object_within_target_lane.objects.size()); + } + + { + const auto & objects = object_outside_target_lane.objects; + std::transform( + objects.cbegin(), objects.cend(), std::back_inserter(data.other_objects), + [](const auto & object) { + ObjectData other_object; + other_object.object = object; + other_object.reason = "OutOfTargetArea"; + return other_object; + }); + } + + ObjectDataArray target_lane_objects; + target_lane_objects.reserve(object_within_target_lane.objects.size()); + { + const auto & objects = object_within_target_lane.objects; + std::transform( + objects.cbegin(), objects.cend(), std::back_inserter(target_lane_objects), + [&](const auto & object) { return createObjectData(data, object); }); + } + + utils::avoidance::filterTargetObjects( + target_lane_objects, data, debug, planner_data_, avoidance_parameters_); +} + +ObjectData AvoidanceByLaneChange::createObjectData( + const AvoidancePlanningData & data, const PredictedObject & object) const +{ + using boost::geometry::return_centroid; + + const auto & path_points = data.reference_path.points; + const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; + const auto object_closest_index = + motion_utils::findNearestIndex(path_points, object_pose.position); + const auto object_closest_pose = path_points.at(object_closest_index).point.pose; + + ObjectData object_data{}; + + object_data.object = object; + + // Calc envelop polygon. + utils::avoidance::fillObjectEnvelopePolygon( + object_data, registered_objects_, object_closest_pose, avoidance_parameters_); + + // calc object centroid. + object_data.centroid = return_centroid(object_data.envelope_poly); + + // Calc moving time. + utils::avoidance::fillObjectMovingTime(object_data, stopped_objects_, avoidance_parameters_); + + // Calc lateral deviation from path to target object. + object_data.lateral = + tier4_autoware_utils::calcLateralDeviation(object_closest_pose, object_pose.position); + + // Find the footprint point closest to the path, set to object_data.overhang_distance. + object_data.overhang_dist = utils::avoidance::calcEnvelopeOverhangDistance( + object_data, object_closest_pose, object_data.overhang_pose.position); + + // Check whether the the ego should avoid the object. + const auto vehicle_width = planner_data_->parameters.vehicle_width; + const auto safety_margin = + 0.5 * vehicle_width + avoidance_parameters_->lateral_passable_safety_buffer; + object_data.avoid_required = + (utils::avoidance::isOnRight(object_data) && + std::abs(object_data.overhang_dist) < safety_margin) || + (!utils::avoidance::isOnRight(object_data) && object_data.overhang_dist < safety_margin); + + return object_data; +} +} // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index f222b8f3f5519..9889a94648942 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -110,6 +110,13 @@ ModuleStatus LaneChangeInterface::updateState() return current_state_; } +void LaneChangeInterface::updateData() +{ + module_type_->setPreviousModulePaths( + getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); + module_type_->updateSpecialData(); +} + BehaviorModuleOutput LaneChangeInterface::plan() { resetPathCandidate(); @@ -310,6 +317,57 @@ void SceneModuleVisitor::visitLaneChangeInterface(const LaneChangeInterface * in lane_change_visitor_ = interface->get_debug_msg_array(); } +AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface( + const std::string & name, rclcpp::Node & node, + const std::shared_ptr & parameters, + const std::shared_ptr & avoidance_parameters, + const std::shared_ptr & avoidance_by_lane_change_parameters, + const std::unordered_map > & rtc_interface_ptr_map) +: LaneChangeInterface{ + name, node, parameters, rtc_interface_ptr_map, + std::make_unique( + parameters, avoidance_parameters, avoidance_by_lane_change_parameters)} +{ +} + +ModuleStatus AvoidanceByLaneChangeInterface::updateState() +{ + if (!module_type_->isValidPath()) { + current_state_ = ModuleStatus::FAILURE; + return current_state_; + } + + if (module_type_->isAbortState()) { + current_state_ = ModuleStatus::RUNNING; + return current_state_; + } + + if (module_type_->isCancelConditionSatisfied()) { + current_state_ = ModuleStatus::FAILURE; + return current_state_; + } + + if (module_type_->hasFinishedLaneChange()) { + current_state_ = ModuleStatus::SUCCESS; + return current_state_; + } + + current_state_ = ModuleStatus::RUNNING; + return current_state_; +} + +void AvoidanceByLaneChangeInterface::updateRTCStatus( + const double start_distance, const double finish_distance) +{ + const auto direction = std::invoke([&]() -> std::string { + const auto dir = module_type_->getDirection(); + return (dir == Direction::LEFT) ? "left" : "right"; + }); + + rtc_interface_ptr_map_.at(direction)->updateCooperateStatus( + uuid_map_.at(direction), isExecutionReady(), start_distance, finish_distance, clock_->now()); +} + LaneChangeBTInterface::LaneChangeBTInterface( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, @@ -414,6 +472,18 @@ LaneChangeBTModule::LaneChangeBTModule( std::make_unique(parameters, LaneChangeModuleType::NORMAL, Direction::NONE)} { } + +void LaneChangeBTModule::updateRTCStatus(const double start_distance, const double finish_distance) +{ + const auto direction = std::invoke([&]() -> std::string { + const auto dir = module_type_->getDirection(); + return (dir == Direction::LEFT) ? "left" : "right"; + }); + + rtc_interface_ptr_map_.at(direction)->updateCooperateStatus( + uuid_map_.at(direction), isExecutionReady(), start_distance, finish_distance, clock_->now()); +} + ExternalRequestLaneChangeLeftBTModule::ExternalRequestLaneChangeLeftBTModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index ec935edb91d9e..69260c97fcc02 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -24,6 +24,7 @@ namespace behavior_path_planner { using route_handler::Direction; +using utils::convertToSnakeCase; LaneChangeModuleManager::LaneChangeModuleManager( rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, std::shared_ptr parameters, const Direction direction, @@ -62,4 +63,33 @@ void LaneChangeModuleManager::updateModuleParams( }); } +AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( + rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, + std::shared_ptr parameters, + std::shared_ptr avoidance_parameters, + std::shared_ptr avoidance_by_lane_change_parameters) +: LaneChangeModuleManager( + node, name, config, std::move(parameters), Direction::NONE, + LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE), + avoidance_parameters_(std::move(avoidance_parameters)), + avoidance_by_lane_change_parameters_(std::move(avoidance_by_lane_change_parameters)) +{ + rtc_interface_ptr_map_.clear(); + const std::vector rtc_types = {"left", "right"}; + for (const auto & rtc_type : rtc_types) { + const auto snake_case_name = convertToSnakeCase(name); + const std::string rtc_interface_name = snake_case_name + "_" + rtc_type; + rtc_interface_ptr_map_.emplace( + rtc_type, std::make_shared(node, rtc_interface_name)); + } +} + +std::shared_ptr +AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance() +{ + return std::make_shared( + name_, *node_, parameters_, avoidance_parameters_, avoidance_by_lane_change_parameters_, + rtc_interface_ptr_map_); +} + } // namespace behavior_path_planner 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 3ec87716f8035..419a02470ff8b 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 @@ -59,18 +59,19 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) const auto current_lanes = getCurrentLanes(); if (current_lanes.empty()) { - return std::make_pair(false, false); + return {false, false}; } - const auto target_lanes = getLaneChangeLanes(current_lanes); + const auto target_lanes = getLaneChangeLanes(current_lanes, direction_); if (target_lanes.empty()) { - return std::make_pair(false, false); + return {false, false}; } // find candidate paths LaneChangePaths valid_paths{}; - const auto found_safe_path = getLaneChangePaths(current_lanes, target_lanes, &valid_paths); + const auto found_safe_path = + getLaneChangePaths(current_lanes, target_lanes, direction_, &valid_paths); if (valid_paths.empty()) { return {false, false}; @@ -133,7 +134,7 @@ bool NormalLaneChange::hasFinishedLaneChange() const const auto & lane_change_end = status_.lane_change_path.shift_line.end; const double dist_to_lane_change_end = motion_utils::calcSignedArcLength( lane_change_path.points, current_pose.position, lane_change_end.position); - return dist_to_lane_change_end + parameters_->lane_change_finish_judge_buffer < 0.0; + return dist_to_lane_change_end + lane_change_parameters_->lane_change_finish_judge_buffer < 0.0; } PathWithLaneId NormalLaneChange::getReferencePath() const @@ -145,7 +146,7 @@ bool NormalLaneChange::isCancelConditionSatisfied() { current_lane_change_state_ = LaneChangeStates::Normal; - if (!parameters_->enable_cancel_lane_change) { + if (!lane_change_parameters_->enable_cancel_lane_change) { return false; } @@ -167,7 +168,7 @@ bool NormalLaneChange::isCancelConditionSatisfied() return true; } - if (!parameters_->enable_abort_lane_change) { + if (!lane_change_parameters_->enable_abort_lane_change) { return false; } @@ -182,7 +183,7 @@ bool NormalLaneChange::isAbortConditionSatisfied(const Pose & pose) const auto & common_parameters = planner_data_->parameters; const auto found_abort_path = utils::lane_change::getAbortPaths( - planner_data_, status_.lane_change_path, pose, common_parameters, *parameters_); + planner_data_, status_.lane_change_path, pose, common_parameters, *lane_change_parameters_); if (!found_abort_path && !is_abort_path_approved_) { current_lane_change_state_ = LaneChangeStates::Stop; @@ -235,7 +236,7 @@ lanelet::ConstLanelets NormalLaneChange::getCurrentLanes() const } lanelet::ConstLanelets NormalLaneChange::getLaneChangeLanes( - const lanelet::ConstLanelets & current_lanes) const + const lanelet::ConstLanelets & current_lanes, Direction direction) const { if (current_lanes.empty()) { return {}; @@ -256,7 +257,7 @@ lanelet::ConstLanelets NormalLaneChange::getLaneChangeLanes( route_handler->getLaneletSequence(current_lane, getEgoPose(), 0.0, lane_change_prepare_length); const auto lane_change_lane = utils::lane_change::getLaneChangeTargetLane( - *getRouteHandler(), current_lanes, type_, direction_); + *getRouteHandler(), current_lanes, type_, direction); const auto lane_change_lane_length = std::max(lane_change_lane_length_, getEgoVelocity() * 10.0); if (lane_change_lane) { @@ -297,7 +298,7 @@ PathWithLaneId NormalLaneChange::getPrepareSegment( bool NormalLaneChange::getLaneChangePaths( const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, - LaneChangePaths * candidate_paths) const + Direction direction, LaneChangePaths * candidate_paths) const { object_debug_.clear(); if (original_lanelets.empty() || target_lanelets.empty()) { @@ -314,7 +315,7 @@ bool NormalLaneChange::getLaneChangePaths( const auto prepare_duration = common_parameter.lane_change_prepare_duration; const auto minimum_prepare_length = common_parameter.minimum_prepare_length; const auto minimum_lane_changing_velocity = common_parameter.minimum_lane_changing_velocity; - const auto lane_change_sampling_num = parameters_->lane_change_sampling_num; + const auto lane_change_sampling_num = lane_change_parameters_->lane_change_sampling_num; // get velocity const auto current_velocity = getEgoTwist().linear.x; @@ -425,7 +426,7 @@ bool NormalLaneChange::getLaneChangePaths( const double s_goal = lanelet::utils::getArcCoordinates(target_lanelets, route_handler.getGoalPose()).length; if ( - s_start + lane_changing_length + parameters_->lane_change_finish_judge_buffer + + s_start + lane_changing_length + lane_change_parameters_->lane_change_finish_judge_buffer + next_lane_change_buffer > s_goal) { RCLCPP_DEBUG( @@ -470,7 +471,7 @@ bool NormalLaneChange::getLaneChangePaths( const auto candidate_path = utils::lane_change::constructCandidatePath( prepare_segment, target_segment, target_lane_reference_path, shift_line, original_lanelets, target_lanelets, sorted_lane_ids, acceleration, lc_length, lc_velocity, common_parameter, - *parameters_); + *lane_change_parameters_); if (!candidate_path) { RCLCPP_DEBUG( @@ -481,7 +482,7 @@ bool NormalLaneChange::getLaneChangePaths( const auto is_valid = utils::lane_change::hasEnoughLength( *candidate_path, original_lanelets, target_lanelets, getEgoPose(), route_handler, - minimum_lane_changing_velocity, common_parameter, direction_); + minimum_lane_changing_velocity, common_parameter, direction); if (!is_valid) { RCLCPP_DEBUG( @@ -497,13 +498,14 @@ bool NormalLaneChange::getLaneChangePaths( route_handler, target_lanelets, getEgoPose(), check_length_); dynamic_object_indices = utils::lane_change::filterObjectIndices( {*candidate_path}, *dynamic_objects, backward_target_lanes_for_object_filtering, - getEgoPose(), common_parameter.forward_path_length, *parameters_, lateral_buffer); + getEgoPose(), common_parameter.forward_path_length, *lane_change_parameters_, + lateral_buffer); } candidate_paths->push_back(*candidate_path); const auto is_safe = utils::lane_change::isLaneChangePathSafe( *candidate_path, dynamic_objects, dynamic_object_indices, getEgoPose(), getEgoTwist(), - common_parameter, *parameters_, common_parameter.expected_front_deceleration, + common_parameter, *lane_change_parameters_, common_parameter.expected_front_deceleration, common_parameter.expected_rear_deceleration, ego_pose_before_collision, object_debug_, acceleration); @@ -528,7 +530,7 @@ bool NormalLaneChange::isApprovedPathSafe(Pose & ego_pose_before_collision) cons const auto current_twist = getEgoTwist(); const auto & dynamic_objects = planner_data_->dynamic_object; const auto & common_parameters = planner_data_->parameters; - const auto & lane_change_parameters = *parameters_; + const auto & lane_change_parameters = *lane_change_parameters_; const auto & route_handler = planner_data_->route_handler; const auto & path = status_.lane_change_path; @@ -545,7 +547,7 @@ bool NormalLaneChange::isApprovedPathSafe(Pose & ego_pose_before_collision) cons return utils::lane_change::isLaneChangePathSafe( path, dynamic_objects, dynamic_object_indices, current_pose, current_twist, common_parameters, - *parameters_, common_parameters.expected_front_deceleration_for_abort, + *lane_change_parameters_, common_parameters.expected_front_deceleration_for_abort, common_parameters.expected_rear_deceleration_for_abort, ego_pose_before_collision, debug_data, status_.lane_change_path.acceleration); } diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index 67cdd72511585..aa4868c02792e 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -194,236 +194,6 @@ std::optional constructCandidatePath( return std::optional{candidate_path}; } -#ifdef USE_OLD_ARCHITECTURE -bool getLaneChangePaths( - const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets, - const lanelet::ConstLanelets & target_lanelets, const Pose & pose, const Twist & twist, - const PredictedObjects::ConstSharedPtr dynamic_objects, - const BehaviorPathPlannerParameters & common_parameter, const LaneChangeParameters & parameter, - const double check_length, LaneChangePaths * candidate_paths, - std::unordered_map * debug_data) -#else -bool getLaneChangePaths( - const PathWithLaneId & original_path, const RouteHandler & route_handler, - const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, - const Pose & pose, const Twist & twist, const PredictedObjects::ConstSharedPtr dynamic_objects, - const BehaviorPathPlannerParameters & common_parameter, const LaneChangeParameters & parameter, - const double check_length, const Direction direction, LaneChangePaths * candidate_paths, - std::unordered_map * debug_data) -#endif -{ - debug_data->clear(); - if (original_lanelets.empty() || target_lanelets.empty()) { - return false; - } - - Pose ego_pose_before_collision{}; - - // rename parameter - const auto backward_path_length = common_parameter.backward_path_length; - const auto forward_path_length = common_parameter.forward_path_length; - const auto prepare_duration = common_parameter.lane_change_prepare_duration; - const auto minimum_lane_changing_velocity = common_parameter.minimum_lane_changing_velocity; - const auto lane_change_sampling_num = parameter.lane_change_sampling_num; - - // get velocity - const auto current_velocity = twist.linear.x; - - // compute maximum_deceleration - const auto maximum_deceleration = std::invoke( - [&minimum_lane_changing_velocity, ¤t_velocity, &common_parameter, ¶meter]() { - const double min_a = (minimum_lane_changing_velocity - current_velocity) / - common_parameter.lane_change_prepare_duration; - return std::clamp( - min_a, -std::abs(common_parameter.min_acc), -std::numeric_limits::epsilon()); - }); - - const auto acceleration_resolution = std::abs(maximum_deceleration) / lane_change_sampling_num; - - const auto target_length = - utils::getArcLengthToTargetLanelet(original_lanelets, target_lanelets.front(), pose); - -#ifdef USE_OLD_ARCHITECTURE - const auto shift_intervals = - route_handler.getLateralIntervalsToPreferredLane(target_lanelets.back()); -#else - const auto get_opposite_direction = - (direction == Direction::RIGHT) ? Direction::LEFT : Direction::RIGHT; - const auto shift_intervals = route_handler.getLateralIntervalsToPreferredLane( - target_lanelets.back(), get_opposite_direction); -#endif - const auto next_lane_change_buffer = - utils::calcMinimumLaneChangeLength(common_parameter, shift_intervals); - - const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanelets.back()); - - const auto dist_to_end_of_current_lanes = utils::getDistanceToEndOfLane(pose, original_lanelets); - - [[maybe_unused]] const auto arc_position_from_current = - lanelet::utils::getArcCoordinates(original_lanelets, pose); - const auto arc_position_from_target = lanelet::utils::getArcCoordinates(target_lanelets, pose); - - const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanelets); - - const auto sorted_lane_ids = getSortedLaneIds( - route_handler, original_lanelets, target_lanelets, arc_position_from_target.distance); - const auto lateral_buffer = calcLateralBufferForFiltering(common_parameter.vehicle_width, 0.5); - - LaneChangeTargetObjectIndices dynamic_object_indices; - - candidate_paths->reserve(lane_change_sampling_num); - for (double sampled_acc = 0.0; sampled_acc >= maximum_deceleration; - sampled_acc -= acceleration_resolution) { - const auto prepare_velocity = - std::max(current_velocity + sampled_acc * prepare_duration, minimum_lane_changing_velocity); - - // compute actual acceleration - const double acceleration = (prepare_velocity - current_velocity) / prepare_duration; - - // get path on original lanes - const double prepare_length = - current_velocity * prepare_duration + 0.5 * acceleration * std::pow(prepare_duration, 2); - - if (prepare_length < target_length) { - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "prepare length is shorter than distance to target lane!!"); - break; - } - -#ifdef USE_OLD_ARCHITECTURE - const auto prepare_segment = getPrepareSegment( - route_handler, original_lanelets, arc_position_from_current.length, backward_path_length, - prepare_length, prepare_velocity); -#else - const auto prepare_segment = getPrepareSegment( - original_path, original_lanelets, pose, backward_path_length, prepare_length, - std::max(prepare_velocity, minimum_lane_changing_velocity)); -#endif - - if (prepare_segment.points.empty()) { - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "prepare segment is empty!!"); - continue; - } - - // lane changing start pose is at the end of prepare segment - const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose; - - const auto target_length_from_lane_change_start_pose = utils::getArcLengthToTargetLanelet( - original_lanelets, target_lanelets.front(), lane_changing_start_pose); - // In new architecture, there is a possibility that the lane change start pose is behind of the - // target lanelet, even if the condition prepare_length > target_length is satisfied. In - // that case, the lane change shouldn't be executed. - if (target_length_from_lane_change_start_pose > 0.0) { - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "[only new arch] lane change start pose is behind target lanelet!!"); - break; - } - - // calculate shift length - const auto shift_length = - lanelet::utils::getLateralDistanceToClosestLanelet(target_lanelets, lane_changing_start_pose); - - // we assume constant velocity during lane change - const auto lane_changing_velocity = prepare_velocity; - const auto lane_changing_length = - calcLaneChangingLength(lane_changing_velocity, shift_length, common_parameter); - - if (lane_changing_length + prepare_length > dist_to_end_of_current_lanes) { - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "lane changing path too long"); - continue; - } - - const auto target_segment = getTargetSegment( - route_handler, target_lanelets, forward_path_length, lane_changing_start_pose, - target_lane_length, lane_changing_length, lane_changing_velocity, next_lane_change_buffer); - - if (target_segment.points.empty()) { - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "target segment is empty!! something wrong..."); - continue; - } - - const auto resample_interval = - calcLaneChangeResampleInterval(lane_changing_length, lane_changing_velocity); - - const auto lc_length = LaneChangePhaseInfo{prepare_length, lane_changing_length}; - const auto target_lane_reference_path = getReferencePathFromTargetLane( - route_handler, target_lanelets, lane_changing_start_pose, target_lane_length, - lc_length.lane_changing, forward_path_length, resample_interval, is_goal_in_route, - next_lane_change_buffer); - - if (target_lane_reference_path.points.empty()) { - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "target_lane_reference_path is empty!!"); - continue; - } - - const auto shift_line = getLaneChangingShiftLine( - prepare_segment, target_segment, target_lane_reference_path, shift_length); - - const auto lc_velocity = LaneChangePhaseInfo{prepare_velocity, lane_changing_velocity}; - - const auto candidate_path = constructCandidatePath( - prepare_segment, target_segment, target_lane_reference_path, shift_line, original_lanelets, - target_lanelets, sorted_lane_ids, acceleration, lc_length, lc_velocity, common_parameter, - parameter); - - if (!candidate_path) { - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "no candidate path!!"); - continue; - } - -#ifdef USE_OLD_ARCHITECTURE - const auto is_valid = hasEnoughLength( - *candidate_path, original_lanelets, target_lanelets, pose, route_handler, - minimum_lane_changing_velocity, common_parameter); -#else - const auto is_valid = hasEnoughLength( - *candidate_path, original_lanelets, target_lanelets, pose, route_handler, - minimum_lane_changing_velocity, common_parameter, direction); -#endif - - if (!is_valid) { - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "invalid candidate path!!"); - continue; - } - - if (candidate_paths->empty()) { - // only compute dynamic object indices once - const auto backward_target_lanes_for_object_filtering = - utils::lane_change::getBackwardLanelets(route_handler, target_lanelets, pose, check_length); - dynamic_object_indices = filterObjectIndices( - {*candidate_path}, *dynamic_objects, backward_target_lanes_for_object_filtering, pose, - common_parameter.forward_path_length, parameter, lateral_buffer); - } - candidate_paths->push_back(*candidate_path); - - const auto is_safe = isLaneChangePathSafe( - *candidate_path, dynamic_objects, dynamic_object_indices, pose, twist, common_parameter, - parameter, common_parameter.expected_front_deceleration, - common_parameter.expected_rear_deceleration, ego_pose_before_collision, *debug_data, - acceleration); - - if (is_safe) { - return true; - } - } - - return false; -} - bool hasEnoughLength( const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, [[maybe_unused]] const lanelet::ConstLanelets & target_lanes, const Pose & current_pose,