From e24acc4fef1660865be647ee5b5a3ac317a16d4d Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Wed, 26 Apr 2023 11:15:32 +0900 Subject: [PATCH 1/3] refactor(lane_change): move for lane change module to interface Signed-off-by: Muhammad Zulfaqar Azmi --- planning/behavior_path_planner/CMakeLists.txt | 2 - .../behavior_path_planner_node.hpp | 1 - .../scene_module/lane_change/base_class.hpp | 8 +- .../scene_module/lane_change/bt_normal.hpp | 59 ------- .../lane_change/external_request.hpp | 1 - .../scene_module/lane_change/interface.hpp | 20 +++ .../lane_change/lane_change_module.hpp | 156 ------------------ .../scene_module/lane_change/normal.hpp | 32 ++++ .../scene_module/scene_module_interface.hpp | 4 +- .../src/behavior_path_planner_node.cpp | 2 +- .../scene_module/lane_change/interface.cpp | 8 + .../src/scene_module/lane_change/normal.cpp | 99 +++++++++++ 12 files changed, 168 insertions(+), 224 deletions(-) delete mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/bt_normal.hpp delete mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index f3e0583c49df7..0c27e5fd835d8 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -19,9 +19,7 @@ set(common_src src/scene_module/goal_planner/goal_planner_module.cpp src/scene_module/side_shift/side_shift_module.cpp src/scene_module/lane_change/interface.cpp - src/scene_module/lane_change/lane_change_module.cpp src/scene_module/lane_change/normal.cpp - src/scene_module/lane_change/bt_normal.cpp src/scene_module/lane_change/external_request.cpp src/turn_signal_decider.cpp src/utils/utils.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 843c0cfdab20f..7024a78ae274b 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 @@ -22,7 +22,6 @@ #include "behavior_path_planner/behavior_tree_manager.hpp" #include "behavior_path_planner/scene_module/avoidance/avoidance_module.hpp" #include "behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp" -#include "behavior_path_planner/scene_module/lane_change/lane_change_module.hpp" #include "behavior_path_planner/scene_module/lane_following/lane_following_module.hpp" #include "behavior_path_planner/scene_module/pull_out/pull_out_module.hpp" #include "behavior_path_planner/scene_module/side_shift/side_shift_module.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 67de521511569..de5955f123940 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 @@ -53,15 +53,19 @@ class LaneChangeBase { public: LaneChangeBase( - const std::shared_ptr & parameters, LaneChangeModuleType type, + std::shared_ptr parameters, LaneChangeModuleType type, Direction direction) - : parameters_{parameters}, direction_{direction}, type_{type} + : parameters_{std::move(parameters)}, direction_{direction}, type_{type} { prev_module_reference_path_ = std::make_shared(); prev_module_path_ = std::make_shared(); prev_drivable_lanes_ = std::make_shared>(); } + LaneChangeBase(const LaneChangeBase &) = delete; + LaneChangeBase(LaneChangeBase &&) = delete; + LaneChangeBase & operator=(const LaneChangeBase &) = delete; + LaneChangeBase & operator=(LaneChangeBase &&) = delete; virtual ~LaneChangeBase() = default; virtual void updateLaneChangeStatus() = 0; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/bt_normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/bt_normal.hpp deleted file mode 100644 index feb560b6d8ac2..0000000000000 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/bt_normal.hpp +++ /dev/null @@ -1,59 +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__SCENE_MODULE__LANE_CHANGE__BT_NORMAL_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__BT_NORMAL_HPP_ - -#include "behavior_path_planner/scene_module/lane_change/normal.hpp" - -#include -#include -#include -#include - -namespace behavior_path_planner -{ -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using geometry_msgs::msg::Point; -using geometry_msgs::msg::Pose; -using geometry_msgs::msg::Twist; -using marker_utils::CollisionCheckDebugMap; -using route_handler::Direction; -using tier4_planning_msgs::msg::LaneChangeDebugMsg; -using tier4_planning_msgs::msg::LaneChangeDebugMsgArray; - -class NormalLaneChangeBT : public NormalLaneChange -{ -public: - NormalLaneChangeBT( - const std::shared_ptr & parameters, LaneChangeModuleType type, - Direction direction); - - ~NormalLaneChangeBT() override = default; - - PathWithLaneId getReferencePath() const override; - -protected: - lanelet::ConstLanelets getCurrentLanes() const override; - - int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const override; - - PathWithLaneId getPrepareSegment( - const lanelet::ConstLanelets & current_lanes, const double arc_length_from_current, - const double backward_path_length, const double prepare_length, - const double prepare_velocity) const override; - - std::vector getDrivableLanes() const override; -}; -} // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__BT_NORMAL_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request.hpp index 6c115536bbb5b..51a71ef7cd061 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request.hpp @@ -15,7 +15,6 @@ #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__EXTERNAL_REQUEST_HPP_ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__EXTERNAL_REQUEST_HPP_ -#include "behavior_path_planner/scene_module/lane_change/bt_normal.hpp" #include "behavior_path_planner/scene_module/lane_change/normal.hpp" #include 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 23c7b71f5bd4d..b098eecdec231 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 @@ -142,6 +142,26 @@ class LaneChangeBTInterface : public LaneChangeInterface bool is_activated_{false}; }; +class LaneChangeBTModule : public LaneChangeBTInterface +{ +public: + LaneChangeBTModule( + const std::string & name, rclcpp::Node & node, + 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()); + } +}; + class ExternalRequestLaneChangeLeftBTModule : public LaneChangeBTInterface { public: diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp deleted file mode 100644 index 0ed8195c6a6c9..0000000000000 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp +++ /dev/null @@ -1,156 +0,0 @@ -// Copyright 2021 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__LANE_CHANGE_MODULE_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_MODULE_HPP_ - -#include "behavior_path_planner/marker_util/lane_change/debug.hpp" -#include "behavior_path_planner/scene_module/lane_change/bt_normal.hpp" -#include "behavior_path_planner/scene_module/scene_module_interface.hpp" -#include "behavior_path_planner/turn_signal_decider.hpp" -#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" -#include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" -#include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" - -#include - -#include "tier4_planning_msgs/msg/detail/lane_change_debug_msg_array__struct.hpp" -#include "tier4_planning_msgs/msg/lane_change_debug_msg.hpp" -#include "tier4_planning_msgs/msg/lane_change_debug_msg_array.hpp" -#include -#include -#include - -#include - -#include -#include -#include -#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; - -class LaneChangeModule : public SceneModuleInterface -{ -public: - LaneChangeModule( - const std::string & name, rclcpp::Node & node, - std::shared_ptr parameters); - - void processOnEntry() override; - void processOnExit() override; - - bool isExecutionRequested() const override; - - bool isExecutionReady() const override; - ModuleStatus updateState() override; - - BehaviorModuleOutput plan() override; - BehaviorModuleOutput planWaitingApproval() override; - CandidateOutput planCandidate() const override; - - std::shared_ptr get_debug_msg_array() const; - - void acceptVisitor( - [[maybe_unused]] const std::shared_ptr & visitor) const override; - - void setData(const std::shared_ptr & data) override; - -private: - std::shared_ptr parameters_; - - std::unique_ptr module_type_; - - void resetPathIfAbort(); - -protected: - void setObjectDebugVisualization() const; - - void updateSteeringFactorPtr(const BehaviorModuleOutput & output); - - void updateSteeringFactorPtr( - const CandidateOutput & output, const LaneChangePath & selected_path) const; - - mutable LaneChangeDebugMsgArray lane_change_debug_msg_array_; - - PathWithLaneId prev_approved_path_; - - void clearAbortApproval() { is_abort_path_approved_ = false; } - - bool is_abort_path_approved_{false}; - - bool is_abort_approval_requested_{false}; - bool is_activated_{false}; - - UUID candidate_uuid_; - - void waitApprovalLeft(const double start_distance, const double finish_distance) - { - rtc_interface_ptr_map_.at("left")->updateCooperateStatus( - uuid_map_.at("left"), isExecutionReady(), start_distance, finish_distance, clock_->now()); - is_waiting_approval_ = true; - } - - void waitApprovalRight(const double start_distance, const double finish_distance) - { - rtc_interface_ptr_map_.at("right")->updateCooperateStatus( - uuid_map_.at("right"), isExecutionReady(), start_distance, finish_distance, clock_->now()); - } - - void updateRTCStatus(const CandidateOutput & candidate) - { - if (candidate.lateral_shift > 0.0) { - rtc_interface_ptr_map_.at("left")->updateCooperateStatus( - uuid_map_.at("left"), isExecutionReady(), candidate.start_distance_to_path_change, - candidate.finish_distance_to_path_change, clock_->now()); - candidate_uuid_ = uuid_map_.at("left"); - return; - } - if (candidate.lateral_shift < 0.0) { - rtc_interface_ptr_map_.at("right")->updateCooperateStatus( - uuid_map_.at("right"), isExecutionReady(), candidate.start_distance_to_path_change, - candidate.finish_distance_to_path_change, clock_->now()); - candidate_uuid_ = uuid_map_.at("right"); - return; - } - - RCLCPP_WARN_STREAM( - getLogger(), - "Direction is UNKNOWN, start_distance = " << candidate.start_distance_to_path_change); - } - - void removeRTCStatus() - { - const auto direction = std::invoke([&]() -> std::string { - const auto dir = module_type_->getDirection(); - return (dir == Direction::LEFT) ? "left" : "right"; - }); - - if (rtc_interface_ptr_map_.at(direction)->isRegistered(uuid_map_.at(direction))) { - rtc_interface_ptr_map_.at(direction)->removeCooperateStatus(uuid_map_.at(direction)); - } - - uuid_map_.at(direction) = generateUUID(); - } -}; -} // namespace behavior_path_planner - -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_MODULE_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 ff270f0e456c0..11a7574bb075e 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 @@ -39,6 +39,10 @@ class NormalLaneChange : public LaneChangeBase const std::shared_ptr & parameters, LaneChangeModuleType type, Direction direction); + NormalLaneChange(const NormalLaneChange &) = delete; + NormalLaneChange(NormalLaneChange &&) = delete; + NormalLaneChange & operator=(const NormalLaneChange &) = delete; + NormalLaneChange & operator=(NormalLaneChange &&) = delete; ~NormalLaneChange() override = default; void updateLaneChangeStatus() override; @@ -86,5 +90,33 @@ class NormalLaneChange : public LaneChangeBase bool isValidPath(const PathWithLaneId & path) const override; }; + +class NormalLaneChangeBT : public NormalLaneChange +{ +public: + NormalLaneChangeBT( + const std::shared_ptr & parameters, LaneChangeModuleType type, + Direction direction); + + NormalLaneChangeBT(const NormalLaneChangeBT &) = delete; + NormalLaneChangeBT(NormalLaneChangeBT &&) = delete; + NormalLaneChangeBT & operator=(const NormalLaneChangeBT &) = delete; + NormalLaneChangeBT & operator=(NormalLaneChangeBT &&) = delete; + ~NormalLaneChangeBT() override = default; + + PathWithLaneId getReferencePath() const override; + +protected: + lanelet::ConstLanelets getCurrentLanes() const override; + + int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const override; + + PathWithLaneId getPrepareSegment( + const lanelet::ConstLanelets & current_lanes, const double arc_length_from_current, + const double backward_path_length, const double prepare_length, + const double prepare_velocity) const override; + + std::vector getDrivableLanes() const override; +}; } // namespace behavior_path_planner #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__NORMAL_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp index 525577535be4e..1b9f837d5a90d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp @@ -390,14 +390,14 @@ class SceneModuleInterface for (const auto & rtc_type : rtc_types) { const auto snake_case_name = utils::convertToSnakeCase(name); const auto rtc_interface_name = - rtc_type == "" ? snake_case_name : snake_case_name + "_" + rtc_type; + rtc_type.empty() ? snake_case_name : (snake_case_name + "_" + rtc_type); rtc_interface_ptr_map.emplace( rtc_type, std::make_shared(&node, rtc_interface_name)); } return rtc_interface_ptr_map; } - void updateRTCStatus(const double start_distance, const double finish_distance) + virtual void updateRTCStatus(const double start_distance, const double finish_distance) { for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) { if (itr->second) { 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 d688f684f3ed2..82c0c73f52b12 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -172,7 +172,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod bt_manager_->registerSceneModule(ext_request_lane_change_left_module); auto lane_change_module = - std::make_shared("LaneChange", *this, lane_change_param_ptr_); + std::make_shared("LaneChange", *this, lane_change_param_ptr_); path_candidate_publishers_.emplace( "LaneChange", create_publisher(path_candidate_name_space + "lane_change", 1)); bt_manager_->registerSceneModule(lane_change_module); 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 88db79400eb18..f222b8f3f5519 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 @@ -406,6 +406,14 @@ void SceneModuleVisitor::visitLaneChangeBTInterface(const LaneChangeBTInterface external_request_lane_change_bt_visitor_ = module->get_debug_msg_array(); } +LaneChangeBTModule::LaneChangeBTModule( + const std::string & name, rclcpp::Node & node, + const std::shared_ptr & parameters) +: LaneChangeBTInterface{ + name, node, parameters, createRTCInterfaceMap(node, name, {"left", "right"}), + std::make_unique(parameters, LaneChangeModuleType::NORMAL, Direction::NONE)} +{ +} 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/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 4b20048a82042..494aa680c7c96 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 @@ -625,4 +625,103 @@ bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const return true; } + +NormalLaneChangeBT::NormalLaneChangeBT( + const std::shared_ptr & parameters, LaneChangeModuleType type, + Direction direction) +: NormalLaneChange(parameters, type, direction) +{ +} + +PathWithLaneId NormalLaneChangeBT::getReferencePath() const +{ + PathWithLaneId reference_path; + if (!utils::lane_change::isEgoWithinOriginalLane( + status_.current_lanes, getEgoPose(), planner_data_->parameters)) { + return reference_path; + } + + const auto & route_handler = getRouteHandler(); + const auto & current_pose = getEgoPose(); + const auto & common_parameters = planner_data_->parameters; + + // Set header + reference_path.header = getRouteHeader(); + + const auto current_lanes = getCurrentLanes(); + + if (current_lanes.empty()) { + return reference_path; + } + + reference_path = utils::getCenterLinePath( + *route_handler, current_lanes, current_pose, common_parameters.backward_path_length, + common_parameters.forward_path_length, common_parameters); + + const auto shift_intervals = + route_handler->getLateralIntervalsToPreferredLane(current_lanes.back()); + const double lane_change_buffer = + utils::calcMinimumLaneChangeLength(common_parameters, shift_intervals); + + reference_path = utils::setDecelerationVelocity( + *route_handler, reference_path, current_lanes, parameters_->prepare_duration, + lane_change_buffer); + + const auto & dp = planner_data_->drivable_area_expansion_parameters; + + const auto drivable_lanes = utils::generateDrivableLanes(current_lanes); + const auto shorten_lanes = utils::cutOverlappedLanes(reference_path, drivable_lanes); + const auto expanded_lanes = utils::expandLanelets( + shorten_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, + dp.drivable_area_types_to_skip); + utils::generateDrivableArea( + reference_path, expanded_lanes, common_parameters.vehicle_length, planner_data_); + + return reference_path; +} + +lanelet::ConstLanelets NormalLaneChangeBT::getCurrentLanes() const +{ + return utils::getCurrentLanes(planner_data_); +} + +int NormalLaneChangeBT::getNumToPreferredLane(const lanelet::ConstLanelet & lane) const +{ + return std::abs(getRouteHandler()->getNumLaneToPreferredLane(lane)); +} + +PathWithLaneId NormalLaneChangeBT::getPrepareSegment( + const lanelet::ConstLanelets & current_lanes, const double arc_length_from_current, + const double backward_path_length, const double prepare_length, + const double prepare_velocity) const +{ + if (current_lanes.empty()) { + return PathWithLaneId(); + } + + const double s_start = arc_length_from_current - backward_path_length; + const double s_end = arc_length_from_current + prepare_length; + + RCLCPP_DEBUG( + rclcpp::get_logger("behavior_path_planner") + .get_child("lane_change") + .get_child("util") + .get_child("getPrepareSegment"), + "start: %f, end: %f", s_start, s_end); + + PathWithLaneId prepare_segment = + getRouteHandler()->getCenterLinePath(current_lanes, s_start, s_end); + + prepare_segment.points.back().point.longitudinal_velocity_mps = std::min( + prepare_segment.points.back().point.longitudinal_velocity_mps, + static_cast(prepare_velocity)); + + return prepare_segment; +} + +std::vector NormalLaneChangeBT::getDrivableLanes() const +{ + return utils::lane_change::generateDrivableLanes( + *getRouteHandler(), status_.current_lanes, status_.lane_change_lanes); +} } // namespace behavior_path_planner From d4e0397c0581a2154f31d345c8619e11566dde5f Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Thu, 27 Apr 2023 12:05:04 +0900 Subject: [PATCH 2/3] fix build error Signed-off-by: Muhammad Zulfaqar Azmi --- .../src/scene_module/lane_change/normal.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 494aa680c7c96..adf9d4d57af80 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 @@ -664,7 +664,7 @@ PathWithLaneId NormalLaneChangeBT::getReferencePath() const utils::calcMinimumLaneChangeLength(common_parameters, shift_intervals); reference_path = utils::setDecelerationVelocity( - *route_handler, reference_path, current_lanes, parameters_->prepare_duration, + *route_handler, reference_path, current_lanes, common_parameters.lane_change_prepare_duration, lane_change_buffer); const auto & dp = planner_data_->drivable_area_expansion_parameters; From 274abd0b39ad10e4f228573de41c6ea1405e5b3a Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Thu, 27 Apr 2023 12:07:44 +0900 Subject: [PATCH 3/3] fix conflict --- .../scene_module/lane_change/bt_normal.cpp | 129 ------- .../lane_change/lane_change_module.cpp | 323 ------------------ 2 files changed, 452 deletions(-) delete mode 100644 planning/behavior_path_planner/src/scene_module/lane_change/bt_normal.cpp delete mode 100644 planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/bt_normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/bt_normal.cpp deleted file mode 100644 index 78d2e9196dce5..0000000000000 --- a/planning/behavior_path_planner/src/scene_module/lane_change/bt_normal.cpp +++ /dev/null @@ -1,129 +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. - -#include "behavior_path_planner/scene_module/lane_change/bt_normal.hpp" - -#include "behavior_path_planner/utils/lane_change/utils.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" - -#include - -#include -#include -#include -#include -#include -#include - -namespace behavior_path_planner -{ -NormalLaneChangeBT::NormalLaneChangeBT( - const std::shared_ptr & parameters, LaneChangeModuleType type, - Direction direction) -: NormalLaneChange(parameters, type, direction) -{ -} - -PathWithLaneId NormalLaneChangeBT::getReferencePath() const -{ - PathWithLaneId reference_path; - if (!utils::lane_change::isEgoWithinOriginalLane( - status_.current_lanes, getEgoPose(), planner_data_->parameters)) { - return reference_path; - } - - const auto & route_handler = getRouteHandler(); - const auto & current_pose = getEgoPose(); - const auto & common_parameters = planner_data_->parameters; - - // Set header - reference_path.header = getRouteHeader(); - - const auto current_lanes = getCurrentLanes(); - - if (current_lanes.empty()) { - return reference_path; - } - - reference_path = utils::getCenterLinePath( - *route_handler, current_lanes, current_pose, common_parameters.backward_path_length, - common_parameters.forward_path_length, common_parameters); - - const auto shift_intervals = - route_handler->getLateralIntervalsToPreferredLane(current_lanes.back()); - const double lane_change_buffer = - utils::calcMinimumLaneChangeLength(common_parameters, shift_intervals); - - reference_path = utils::setDecelerationVelocity( - *route_handler, reference_path, current_lanes, common_parameters.lane_change_prepare_duration, - lane_change_buffer); - - const auto & dp = planner_data_->drivable_area_expansion_parameters; - - const auto drivable_lanes = utils::generateDrivableLanes(current_lanes); - const auto shorten_lanes = utils::cutOverlappedLanes(reference_path, drivable_lanes); - const auto expanded_lanes = utils::expandLanelets( - shorten_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, - dp.drivable_area_types_to_skip); - utils::generateDrivableArea( - reference_path, expanded_lanes, common_parameters.vehicle_length, planner_data_); - - return reference_path; -} - -lanelet::ConstLanelets NormalLaneChangeBT::getCurrentLanes() const -{ - return utils::getCurrentLanes(planner_data_); -} - -int NormalLaneChangeBT::getNumToPreferredLane(const lanelet::ConstLanelet & lane) const -{ - return std::abs(getRouteHandler()->getNumLaneToPreferredLane(lane)); -} - -PathWithLaneId NormalLaneChangeBT::getPrepareSegment( - const lanelet::ConstLanelets & current_lanes, const double arc_length_from_current, - const double backward_path_length, const double prepare_length, - const double prepare_velocity) const -{ - if (current_lanes.empty()) { - return PathWithLaneId(); - } - - const double s_start = arc_length_from_current - backward_path_length; - const double s_end = arc_length_from_current + prepare_length; - - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner") - .get_child("lane_change") - .get_child("util") - .get_child("getPrepareSegment"), - "start: %f, end: %f", s_start, s_end); - - PathWithLaneId prepare_segment = - getRouteHandler()->getCenterLinePath(current_lanes, s_start, s_end); - - prepare_segment.points.back().point.longitudinal_velocity_mps = std::min( - prepare_segment.points.back().point.longitudinal_velocity_mps, - static_cast(prepare_velocity)); - - return prepare_segment; -} - -std::vector NormalLaneChangeBT::getDrivableLanes() const -{ - return utils::lane_change::generateDrivableLanes( - *getRouteHandler(), status_.current_lanes, status_.lane_change_lanes); -} -} // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp deleted file mode 100644 index e4034180ccb73..0000000000000 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ /dev/null @@ -1,323 +0,0 @@ -// Copyright 2021 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/lane_change_module.hpp" - -#include "behavior_path_planner/scene_module/scene_module_visitor.hpp" -#include "behavior_path_planner/turn_signal_decider.hpp" -#include "behavior_path_planner/utils/lane_change/utils.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" -#include "behavior_path_planner/utils/utils.hpp" - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -namespace behavior_path_planner -{ -using autoware_auto_perception_msgs::msg::ObjectClassification; - -LaneChangeModule::LaneChangeModule( - const std::string & name, rclcpp::Node & node, std::shared_ptr parameters) -: SceneModuleInterface{name, node, createRTCInterfaceMap(node, name, {"left", "right"})}, - module_type_{ - std::make_unique(parameters, LaneChangeModuleType::NORMAL, Direction::NONE)} -{ -} - -void LaneChangeModule::processOnEntry() -{ - module_type_->updateLaneChangeStatus(); -} - -void LaneChangeModule::processOnExit() -{ - module_type_->resetParameters(); - debug_marker_.markers.clear(); - resetPathCandidate(); -} - -bool LaneChangeModule::isExecutionRequested() const -{ - if (current_state_ == ModuleStatus::RUNNING) { - return true; - } - - LaneChangePath selected_path; - const auto [found_valid_path, found_safe_path] = module_type_->getSafePath(selected_path); - - return found_valid_path; -} - -bool LaneChangeModule::isExecutionReady() const -{ - if (current_state_ == ModuleStatus::RUNNING) { - return true; - } - - LaneChangePath selected_path; - const auto [found_valid_path, found_safe_path] = module_type_->getSafePath(selected_path); - - return found_safe_path; -} - -ModuleStatus LaneChangeModule::updateState() -{ - RCLCPP_DEBUG(getLogger(), "%s updateState", name().c_str()); - - 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_; -} - -BehaviorModuleOutput LaneChangeModule::plan() -{ - resetPathCandidate(); - resetPathReference(); - is_activated_ = isActivated(); - - if (!module_type_->isValidPath()) { - return {}; - } - - if (module_type_->isAbortState()) { - resetPathIfAbort(); - } - - auto output = module_type_->generateOutput(); - - path_reference_ = getPreviousModuleOutput().reference_path; - prev_approved_path_ = *output.path; - - updateSteeringFactorPtr(output); - clearWaitingApproval(); - - return output; -} - -BehaviorModuleOutput LaneChangeModule::planWaitingApproval() -{ - const auto path = module_type_->getReferencePath(); - if (!path.points.empty()) { - prev_approved_path_ = module_type_->getReferencePath(); - } - - BehaviorModuleOutput out; - out.path = std::make_shared(prev_approved_path_); - out.reference_path = getPreviousModuleOutput().reference_path; - out.turn_signal_info = getPreviousModuleOutput().turn_signal_info; - - module_type_->extendOutputDrivableArea(out); - - const auto candidate = planCandidate(); - path_candidate_ = std::make_shared(candidate.path_candidate); - - path_reference_ = getPreviousModuleOutput().reference_path; - updateRTCStatus(candidate); - waitApproval(); - is_abort_path_approved_ = false; - - return out; -} - -CandidateOutput LaneChangeModule::planCandidate() const -{ - LaneChangePath selected_path; - - if (module_type_->isAbortState()) { - selected_path = module_type_->getLaneChangePath(); - } else { - [[maybe_unused]] const auto [found_valid_path, found_safe_path] = - module_type_->getSafePath(selected_path); - } - - selected_path.path.header = module_type_->getRouteHeader(); - - if (selected_path.path.points.empty()) { - return {}; - } - - CandidateOutput 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, module_type_->getEgoPosition(), - selected_path.shift_line.start.position); - output.finish_distance_to_path_change = motion_utils::calcSignedArcLength( - selected_path.path.points, module_type_->getEgoPosition(), - selected_path.shift_line.end.position); - - updateSteeringFactorPtr(output, selected_path); - return output; -} - -void LaneChangeModule::resetPathIfAbort() -{ - if (!module_type_->isAbortState()) { - return; - } - - if (!is_abort_approval_requested_) { - removeRTCStatus(); - 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(); - } -} - -void LaneChangeModule::setData(const std::shared_ptr & data) -{ - module_type_->setData(data); -} - -void LaneChangeModule::setObjectDebugVisualization() const -{ - using marker_utils::lane_change_markers::showAllValidLaneChangePath; - using marker_utils::lane_change_markers::showLerpedPose; - using marker_utils::lane_change_markers::showObjectInfo; - using marker_utils::lane_change_markers::showPolygon; - using marker_utils::lane_change_markers::showPolygonPose; - - const auto debug_data = module_type_->getDebugData(); - const auto debug_valid_path = module_type_->getDebugValidPath(); - - debug_marker_.markers.clear(); - const auto add = [this](const MarkerArray & added) { - tier4_autoware_utils::appendMarkerArray(added, &debug_marker_); - }; - - add(showObjectInfo(debug_data, "object_debug_info")); - add(showLerpedPose(debug_data, "lerp_pose_before_true")); - add(showPolygonPose(debug_data, "expected_pose")); - add(showPolygon(debug_data, "lerped_polygon")); - add(showAllValidLaneChangePath(debug_valid_path, "lane_change_valid_paths")); -} - -std::shared_ptr LaneChangeModule::get_debug_msg_array() const -{ - const auto debug_data = module_type_->getDebugData(); - LaneChangeDebugMsgArray debug_msg_array; - debug_msg_array.lane_change_info.reserve(debug_data.size()); - for (const auto & [uuid, debug_data] : debug_data) { - LaneChangeDebugMsg debug_msg; - debug_msg.object_id = uuid; - debug_msg.allow_lane_change = debug_data.allow_lane_change; - debug_msg.is_front = debug_data.is_front; - debug_msg.relative_distance = debug_data.relative_to_ego; - debug_msg.failed_reason = debug_data.failed_reason; - debug_msg.velocity = debug_data.object_twist.linear.x; - debug_msg_array.lane_change_info.push_back(debug_msg); - } - lane_change_debug_msg_array_ = debug_msg_array; - - lane_change_debug_msg_array_.header.stamp = clock_->now(); - return std::make_shared(lane_change_debug_msg_array_); -} - -void LaneChangeModule::updateSteeringFactorPtr(const BehaviorModuleOutput & output) -{ - const auto current_pose = module_type_->getEgoPose(); - const auto status = module_type_->getLaneChangeStatus(); - const double start_distance = motion_utils::calcSignedArcLength( - output.path->points, current_pose.position, status.lane_change_path.shift_line.start.position); - const double finish_distance = motion_utils::calcSignedArcLength( - output.path->points, current_pose.position, status.lane_change_path.shift_line.end.position); - - const auto turn_signal_info = output.turn_signal_info; - const uint16_t steering_factor_direction = - std::invoke([this, &start_distance, &finish_distance, &turn_signal_info]() { - if (turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { - waitApprovalLeft(start_distance, finish_distance); - return SteeringFactor::LEFT; - } - if (turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_RIGHT) { - waitApprovalRight(start_distance, finish_distance); - return SteeringFactor::RIGHT; - } - return SteeringFactor::UNKNOWN; - }); - - // TODO(tkhmy) add handle status TRYING - steering_factor_interface_ptr_->updateSteeringFactor( - {status.lane_change_path.shift_line.start, status.lane_change_path.shift_line.end}, - {start_distance, finish_distance}, SteeringFactor::LANE_CHANGE, steering_factor_direction, - SteeringFactor::TURNING, ""); -} - -void LaneChangeModule::updateSteeringFactorPtr( - const CandidateOutput & output, const LaneChangePath & selected_path) const -{ - const uint16_t steering_factor_direction = std::invoke([&output]() { - if (output.lateral_shift > 0.0) { - return SteeringFactor::LEFT; - } - return SteeringFactor::RIGHT; - }); - - steering_factor_interface_ptr_->updateSteeringFactor( - {selected_path.shift_line.start, selected_path.shift_line.end}, - {output.start_distance_to_path_change, output.finish_distance_to_path_change}, - SteeringFactor::LANE_CHANGE, steering_factor_direction, SteeringFactor::APPROACHING, ""); -} - -void LaneChangeModule::acceptVisitor(const std::shared_ptr & visitor) const -{ - if (visitor) { - visitor->visitLaneChangeModule(this); - } -} - -void SceneModuleVisitor::visitLaneChangeModule(const LaneChangeModule * module) const -{ - lane_change_visitor_ = module->get_debug_msg_array(); -} -} // namespace behavior_path_planner