diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index 47f036264ed86..a4304a0d7bdec 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -7,7 +7,7 @@ autoware_package() find_package(OpenCV REQUIRED) find_package(magic_enum CONFIG REQUIRED) -set(COMPILE_WITH_OLD_ARCHITECTURE TRUE) +set(COMPILE_WITH_OLD_ARCHITECTURE FALSE) set(common_src src/utilities.cpp @@ -15,6 +15,7 @@ set(common_src src/steering_factor_interface.cpp src/behavior_path_planner_node.cpp src/scene_module/scene_module_visitor.cpp + src/scene_module/lane_change/lane_change_module.cpp src/turn_signal_decider.cpp src/util/avoidance/util.cpp src/util/lane_change/util.cpp @@ -47,7 +48,6 @@ if(COMPILE_WITH_OLD_ARCHITECTURE) src/scene_module/avoidance/avoidance_module.cpp src/scene_module/lane_following/lane_following_module.cpp src/scene_module/lane_change/external_request_lane_change_module.cpp - src/scene_module/lane_change/lane_change_module.cpp src/scene_module/pull_over/pull_over_module.cpp src/scene_module/pull_out/pull_out_module.cpp ${common_src} @@ -60,6 +60,7 @@ if(COMPILE_WITH_OLD_ARCHITECTURE) else() ament_auto_add_library(behavior_path_planner_node SHARED src/planner_manager.cpp + src/scene_module/lane_change/manager.cpp ${common_src} ) 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 f3815ef1cc6e7..1cf0c6b4a363b 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,6 +29,7 @@ #include "behavior_path_planner/scene_module/side_shift/side_shift_module.hpp" #else #include "behavior_path_planner/planner_manager.hpp" +#include "behavior_path_planner/scene_module/lane_change/manager.hpp" #endif #include "behavior_path_planner/steering_factor_interface.hpp" diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index d13f8df4852be..ff1178667b2b7 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -30,6 +30,7 @@ struct BehaviorPathPlannerParameters bool verbose; ModuleConfigParameters config_avoidance; + ModuleConfigParameters config_lane_change; double backward_path_length; double forward_path_length; 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 index e6a00371c898e..489dbfd3b5764 100644 --- 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 @@ -51,13 +51,20 @@ using tier4_planning_msgs::msg::LaneChangeDebugMsgArray; class LaneChangeModule : public SceneModuleInterface { public: +#ifdef USE_OLD_ARCHITECTURE LaneChangeModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters); - +#else + LaneChangeModule( + const std::string & name, rclcpp::Node & node, + const std::shared_ptr & parameters, + const std::shared_ptr & rtc_interface_left, + const std::shared_ptr & rtc_interface_right); +#endif bool isExecutionRequested() const override; bool isExecutionReady() const override; - BT::NodeStatus updateState() override; + ModuleStatus updateState() override; BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; CandidateOutput planCandidate() const override; @@ -68,33 +75,40 @@ class LaneChangeModule : public SceneModuleInterface void acceptVisitor( [[maybe_unused]] const std::shared_ptr & visitor) const override; +#ifndef USE_OLD_ARCHITECTURE + void updateModuleParams(const std::shared_ptr & parameters) + { + parameters_ = parameters; + } +#endif + void publishRTCStatus() override { - rtc_interface_left_.publishCooperateStatus(clock_->now()); - rtc_interface_right_.publishCooperateStatus(clock_->now()); + rtc_interface_left_->publishCooperateStatus(clock_->now()); + rtc_interface_right_->publishCooperateStatus(clock_->now()); } bool isActivated() override { - if (rtc_interface_left_.isRegistered(uuid_left_)) { - return rtc_interface_left_.isActivated(uuid_left_); + if (rtc_interface_left_->isRegistered(uuid_left_)) { + return rtc_interface_left_->isActivated(uuid_left_); } - if (rtc_interface_right_.isRegistered(uuid_right_)) { - return rtc_interface_right_.isActivated(uuid_right_); + if (rtc_interface_right_->isRegistered(uuid_right_)) { + return rtc_interface_right_->isActivated(uuid_right_); } return false; } void lockRTCCommand() override { - rtc_interface_left_.lockCommandUpdate(); - rtc_interface_right_.lockCommandUpdate(); + rtc_interface_left_->lockCommandUpdate(); + rtc_interface_right_->lockCommandUpdate(); } void unlockRTCCommand() override { - rtc_interface_left_.unlockCommandUpdate(); - rtc_interface_right_.unlockCommandUpdate(); + rtc_interface_left_->unlockCommandUpdate(); + rtc_interface_right_->unlockCommandUpdate(); } private: @@ -113,8 +127,8 @@ class LaneChangeModule : public SceneModuleInterface bool is_abort_condition_satisfied_{false}; bool is_activated_ = false; - RTCInterface rtc_interface_left_; - RTCInterface rtc_interface_right_; + std::shared_ptr rtc_interface_left_; + std::shared_ptr rtc_interface_right_; UUID uuid_left_; UUID uuid_right_; UUID candidate_uuid_; @@ -123,14 +137,14 @@ class LaneChangeModule : public SceneModuleInterface void waitApprovalLeft(const double start_distance, const double finish_distance) { - rtc_interface_left_.updateCooperateStatus( + rtc_interface_left_->updateCooperateStatus( uuid_left_, isExecutionReady(), start_distance, finish_distance, clock_->now()); is_waiting_approval_ = true; } void waitApprovalRight(const double start_distance, const double finish_distance) { - rtc_interface_right_.updateCooperateStatus( + rtc_interface_right_->updateCooperateStatus( uuid_right_, isExecutionReady(), start_distance, finish_distance, clock_->now()); is_waiting_approval_ = true; } @@ -138,14 +152,14 @@ class LaneChangeModule : public SceneModuleInterface void updateRTCStatus(const CandidateOutput & candidate) { if (candidate.lateral_shift > 0.0) { - rtc_interface_left_.updateCooperateStatus( + rtc_interface_left_->updateCooperateStatus( uuid_left_, isExecutionReady(), candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change, clock_->now()); candidate_uuid_ = uuid_left_; return; } if (candidate.lateral_shift < 0.0) { - rtc_interface_right_.updateCooperateStatus( + rtc_interface_right_->updateCooperateStatus( uuid_right_, isExecutionReady(), candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change, clock_->now()); candidate_uuid_ = uuid_right_; @@ -159,21 +173,21 @@ class LaneChangeModule : public SceneModuleInterface void removeRTCStatus() override { - rtc_interface_left_.clearCooperateStatus(); - rtc_interface_right_.clearCooperateStatus(); + rtc_interface_left_->clearCooperateStatus(); + rtc_interface_right_->clearCooperateStatus(); } void removePreviousRTCStatusLeft() { - if (rtc_interface_left_.isRegistered(uuid_left_)) { - rtc_interface_left_.removeCooperateStatus(uuid_left_); + if (rtc_interface_left_->isRegistered(uuid_left_)) { + rtc_interface_left_->removeCooperateStatus(uuid_left_); } } void removePreviousRTCStatusRight() { - if (rtc_interface_right_.isRegistered(uuid_right_)) { - rtc_interface_right_.removeCooperateStatus(uuid_right_); + if (rtc_interface_right_->isRegistered(uuid_right_)) { + rtc_interface_right_->removeCooperateStatus(uuid_right_); } } @@ -184,6 +198,7 @@ class LaneChangeModule : public SceneModuleInterface std::pair getSafePath( const lanelet::ConstLanelets & lane_change_lanes, const double check_distance, LaneChangePath & safe_path) const; + PathWithLaneId extendBackwardLength(const PathWithLaneId & original_path) const; void updateLaneChangeStatus(); void generateExtendedDrivableArea(PathWithLaneId & path); 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 new file mode 100644 index 0000000000000..87e5d42688d06 --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp @@ -0,0 +1,57 @@ +// 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__MANAGER_HPP_ +#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__MANAGER_HPP_ + +#include "behavior_path_planner/scene_module/lane_change/lane_change_module.hpp" +#include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp" + +#include + +#include +#include +#include +#include + +namespace behavior_path_planner +{ + +class LaneChangeModuleManager : public SceneModuleManagerInterface +{ +public: + LaneChangeModuleManager( + rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, + std::shared_ptr parameters); + + std::shared_ptr createNewSceneModuleInstance() override + { + return std::make_shared( + name_, *node_, parameters_, rtc_interface_left_, rtc_interface_right_); + } + + void updateModuleParams(const std::vector & parameters) override; + +private: + std::shared_ptr rtc_interface_left_; + std::shared_ptr rtc_interface_right_; + + std::shared_ptr parameters_; + + std::vector> registered_modules_; +}; + +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__MANAGER_HPP_ 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 c71472a24dd4b..10a6bbbf1efc5 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -197,6 +197,17 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod planner_manager_ = std::make_shared(*this, lane_following_param_ptr_, p.verbose); + if (p.config_lane_change.enable_module) { + const std::string module_topic = "lane_change"; + auto manager = std::make_shared( + this, module_topic, p.config_lane_change, lane_change_param_ptr_); + planner_manager_->registerSceneModuleManager(manager); + path_candidate_publishers_.emplace( + module_topic, create_publisher(path_candidate_name_space + module_topic, 1)); + path_reference_publishers_.emplace( + module_topic, create_publisher(path_reference_name_space + module_topic, 1)); + } + mutex_bt_.unlock(); } #endif @@ -229,7 +240,15 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() BehaviorPathPlannerParameters p{}; p.verbose = declare_parameter("verbose"); + { + const std::string ns = "lane_change."; + p.config_lane_change.enable_module = declare_parameter(ns + "enable_module"); + p.config_lane_change.enable_simultaneous_execution = + declare_parameter(ns + "enable_simultaneous_execution"); + p.config_lane_change.priority = declare_parameter(ns + "priority"); + p.config_lane_change.max_module_size = declare_parameter(ns + "max_module_size"); + } // vehicle info const auto vehicle_info = VehicleInfoUtil(*this).getVehicleInfo(); p.vehicle_info = vehicle_info; 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 index cac2dabf9c92c..c5a49944bdf98 100644 --- 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 @@ -44,22 +44,40 @@ namespace behavior_path_planner { using autoware_auto_perception_msgs::msg::ObjectClassification; +#ifdef USE_OLD_ARCHITECTURE LaneChangeModule::LaneChangeModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters) : SceneModuleInterface{name, node}, parameters_{std::move(parameters)}, - rtc_interface_left_(&node, "lane_change_left"), - rtc_interface_right_(&node, "lane_change_right"), uuid_left_{generateUUID()}, uuid_right_{generateUUID()} { + rtc_interface_left_ = std::make_shared(&node, "lane_change_left"); + rtc_interface_right_ = std::make_shared(&node, "lane_change_right"); steering_factor_interface_ptr_ = std::make_unique(&node, "lane_change"); } +#else +LaneChangeModule::LaneChangeModule( + const std::string & name, rclcpp::Node & node, + const std::shared_ptr & parameters, + const std::shared_ptr & rtc_interface_left, + const std::shared_ptr & rtc_interface_right) +: SceneModuleInterface{name, node}, + parameters_{parameters}, + rtc_interface_left_{rtc_interface_left}, + rtc_interface_right_{rtc_interface_right}, + uuid_left_{generateUUID()}, + uuid_right_{generateUUID()} +{ + steering_factor_interface_ptr_ = std::make_unique(&node, "lane_change"); +} +#endif void LaneChangeModule::onEntry() { + std::cerr << "Lane change on entry.\n"; RCLCPP_DEBUG(getLogger(), "LANE_CHANGE onEntry"); - current_state_ = BT::NodeStatus::SUCCESS; + current_state_ = ModuleStatus::SUCCESS; current_lane_change_state_ = LaneChangeStates::Normal; updateLaneChangeStatus(); } @@ -67,13 +85,13 @@ void LaneChangeModule::onEntry() void LaneChangeModule::onExit() { resetParameters(); - current_state_ = BT::NodeStatus::SUCCESS; + current_state_ = ModuleStatus::SUCCESS; RCLCPP_DEBUG(getLogger(), "LANE_CHANGE onExit"); } bool LaneChangeModule::isExecutionRequested() const { - if (current_state_ == BT::NodeStatus::RUNNING) { + if (current_state_ == ModuleStatus::RUNNING) { return true; } @@ -89,7 +107,7 @@ bool LaneChangeModule::isExecutionRequested() const bool LaneChangeModule::isExecutionReady() const { - if (current_state_ == BT::NodeStatus::RUNNING) { + if (current_state_ == ModuleStatus::RUNNING) { return true; } @@ -103,36 +121,36 @@ bool LaneChangeModule::isExecutionReady() const return found_safe_path; } -BT::NodeStatus LaneChangeModule::updateState() +ModuleStatus LaneChangeModule::updateState() { RCLCPP_DEBUG(getLogger(), "LANE_CHANGE updateState"); if (!isValidPath()) { - current_state_ = BT::NodeStatus::SUCCESS; + current_state_ = ModuleStatus::SUCCESS; return current_state_; } const auto is_within_current_lane = lane_change_utils::isEgoWithinOriginalLane( status_.current_lanes, getEgoPose(), planner_data_->parameters); if (isAbortState() && !is_within_current_lane) { - current_state_ = BT::NodeStatus::RUNNING; + current_state_ = ModuleStatus::RUNNING; return current_state_; } if (isAbortConditionSatisfied()) { if ((isNearEndOfLane() && isCurrentSpeedLow()) || !is_within_current_lane) { - current_state_ = BT::NodeStatus::RUNNING; + current_state_ = ModuleStatus::RUNNING; return current_state_; } - current_state_ = BT::NodeStatus::FAILURE; + current_state_ = ModuleStatus::FAILURE; return current_state_; } if (hasFinishedLaneChange()) { - current_state_ = BT::NodeStatus::SUCCESS; + current_state_ = ModuleStatus::SUCCESS; return current_state_; } - current_state_ = BT::NodeStatus::RUNNING; + current_state_ = ModuleStatus::RUNNING; return current_state_; } @@ -162,10 +180,10 @@ BehaviorModuleOutput LaneChangeModule::plan() generateExtendedDrivableArea(path); - prev_approved_path_ = path; - BehaviorModuleOutput output; output.path = std::make_shared(path); + path_reference_ = getPreviousModuleOutput().reference_path; + prev_approved_path_ = *getPreviousModuleOutput().path; updateOutputTurnSignal(output); updateSteeringFactorPtr(output); @@ -235,18 +253,21 @@ CandidateOutput LaneChangeModule::planCandidate() const BehaviorModuleOutput LaneChangeModule::planWaitingApproval() { +#ifdef USE_OLD_ARCHITECTURE const auto is_within_current_lane = lane_change_utils::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_); 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; @@ -739,6 +760,7 @@ void LaneChangeModule::resetParameters() abort_path_ = nullptr; clearWaitingApproval(); + unlockNewModuleLaunch(); removeRTCStatus(); steering_factor_interface_ptr_->clearSteeringFactors(); object_debug_.clear(); 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 new file mode 100644 index 0000000000000..79bc075de2ffe --- /dev/null +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -0,0 +1,51 @@ +// 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/manager.hpp" + +#include + +#include +#include +#include +#include + +namespace behavior_path_planner +{ + +LaneChangeModuleManager::LaneChangeModuleManager( + rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, + std::shared_ptr parameters) +: SceneModuleManagerInterface(node, name, config), parameters_{std::move(parameters)} +{ + rtc_interface_left_ = std::make_shared(node, name + "_left"); + rtc_interface_right_ = std::make_shared(node, name + "_right"); +} + +void LaneChangeModuleManager::updateModuleParams( + [[maybe_unused]] const std::vector & parameters) +{ + using tier4_autoware_utils::updateParam; + + [[maybe_unused]] auto p = parameters_; + + [[maybe_unused]] std::string ns = "lane_change."; + // updateParam(parameters, ns + ..., ...); + + std::for_each(registered_modules_.begin(), registered_modules_.end(), [&p](const auto & m) { + m->updateModuleParams(p); + }); +} + +} // namespace behavior_path_planner