From 52597251832cd3139904b29ee20a0f6bbde3a3ee Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sat, 1 Apr 2023 06:02:53 +0900 Subject: [PATCH] refactor(behavior_path_planner): unify rtc_interface instance Signed-off-by: Takayuki Murooka --- .../avoidance/avoidance_module.hpp | 76 +++++-------------- .../scene_module/avoidance/manager.hpp | 7 +- .../lane_change/lane_change_module.hpp | 72 ++++-------------- .../scene_module/lane_change/manager.hpp | 4 +- .../scene_module/pull_out/manager.hpp | 4 +- .../scene_module/pull_out/pull_out_module.hpp | 3 +- .../scene_module/pull_over/manager.hpp | 4 +- .../pull_over/pull_over_module.hpp | 3 +- .../scene_module/scene_module_interface.hpp | 64 ++++++++++------ .../avoidance/avoidance_module.cpp | 26 ++----- .../src/scene_module/avoidance/manager.cpp | 2 - .../external_request_lane_change_module.cpp | 9 ++- .../lane_change/lane_change_module.cpp | 20 ++--- .../src/scene_module/lane_change/manager.cpp | 2 - .../src/scene_module/pull_out/manager.cpp | 1 - .../scene_module/pull_out/pull_out_module.cpp | 9 +-- .../src/scene_module/pull_over/manager.cpp | 1 - .../pull_over/pull_over_module.cpp | 9 +-- 18 files changed, 107 insertions(+), 209 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index f982890c8528c..699a095223c5a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -49,9 +49,7 @@ class AvoidanceModule : public SceneModuleInterface const std::string & name, rclcpp::Node & node, std::shared_ptr parameters); #else AvoidanceModule( - const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, - std::shared_ptr & rtc_interface_left, - std::shared_ptr & rtc_interface_right); + const std::string & name, rclcpp::Node & node, std::shared_ptr parameters); #endif bool isExecutionRequested() const override; @@ -72,35 +70,6 @@ class AvoidanceModule : public SceneModuleInterface parameters_ = parameters; } #endif - - void publishRTCStatus() override - { - 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_right_->isRegistered(uuid_right_)) { - return rtc_interface_right_->isActivated(uuid_right_); - } - return false; - } - - void lockRTCCommand() override - { - rtc_interface_left_->lockCommandUpdate(); - rtc_interface_right_->lockCommandUpdate(); - } - - void unlockRTCCommand() override - { - rtc_interface_left_->unlockCommandUpdate(); - rtc_interface_right_->unlockCommandUpdate(); - } std::shared_ptr get_debug_msg_array() const; private: @@ -118,29 +87,24 @@ class AvoidanceModule : public SceneModuleInterface PathShifter path_shifter_; - std::shared_ptr rtc_interface_left_; - std::shared_ptr rtc_interface_right_; - RegisteredShiftLineArray left_shift_array_; RegisteredShiftLineArray right_shift_array_; UUID candidate_uuid_; - UUID uuid_left_; - UUID uuid_right_; void updateCandidateRTCStatus(const CandidateOutput & candidate) { if (candidate.lateral_shift > 0.0) { - rtc_interface_left_->updateCooperateStatus( - uuid_left_, isExecutionReady(), candidate.start_distance_to_path_change, + rtc_interface_ptr_vec_.at(0)->updateCooperateStatus( + uuid_vec_.at(0), isExecutionReady(), candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change, clock_->now()); - candidate_uuid_ = uuid_left_; + candidate_uuid_ = uuid_vec_.at(0); return; } if (candidate.lateral_shift < 0.0) { - rtc_interface_right_->updateCooperateStatus( - uuid_right_, isExecutionReady(), candidate.start_distance_to_path_change, + rtc_interface_ptr_vec_.at(1)->updateCooperateStatus( + uuid_vec_.at(1), isExecutionReady(), candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change, clock_->now()); - candidate_uuid_ = uuid_right_; + candidate_uuid_ = uuid_vec_.at(1); return; } @@ -158,7 +122,7 @@ class AvoidanceModule : public SceneModuleInterface calcSignedArcLength(path.points, ego_position, left_shift.start_pose.position); const double finish_distance = calcSignedArcLength(path.points, ego_position, left_shift.finish_pose.position); - rtc_interface_left_->updateCooperateStatus( + rtc_interface_ptr_vec_.at(0)->updateCooperateStatus( left_shift.uuid, true, start_distance, finish_distance, clock_->now()); if (finish_distance > -1.0e-03) { steering_factor_interface_ptr_->updateSteeringFactor( @@ -172,7 +136,7 @@ class AvoidanceModule : public SceneModuleInterface calcSignedArcLength(path.points, ego_position, right_shift.start_pose.position); const double finish_distance = calcSignedArcLength(path.points, ego_position, right_shift.finish_pose.position); - rtc_interface_right_->updateCooperateStatus( + rtc_interface_ptr_vec_.at(1)->updateCooperateStatus( right_shift.uuid, true, start_distance, finish_distance, clock_->now()); if (finish_distance > -1.0e-03) { steering_factor_interface_ptr_->updateSteeringFactor( @@ -183,32 +147,26 @@ class AvoidanceModule : public SceneModuleInterface } } - void removeRTCStatus() override - { - rtc_interface_left_->clearCooperateStatus(); - rtc_interface_right_->clearCooperateStatus(); - } - void removeCandidateRTCStatus() { - if (rtc_interface_left_->isRegistered(candidate_uuid_)) { - rtc_interface_left_->removeCooperateStatus(candidate_uuid_); - } else if (rtc_interface_right_->isRegistered(candidate_uuid_)) { - rtc_interface_right_->removeCooperateStatus(candidate_uuid_); + if (rtc_interface_ptr_vec_.at(0)->isRegistered(candidate_uuid_)) { + rtc_interface_ptr_vec_.at(0)->removeCooperateStatus(candidate_uuid_); + } else if (rtc_interface_ptr_vec_.at(1)->isRegistered(candidate_uuid_)) { + rtc_interface_ptr_vec_.at(1)->removeCooperateStatus(candidate_uuid_); } } void removePreviousRTCStatusLeft() { - if (rtc_interface_left_->isRegistered(uuid_left_)) { - rtc_interface_left_->removeCooperateStatus(uuid_left_); + if (rtc_interface_ptr_vec_.at(0)->isRegistered(uuid_vec_.at(0))) { + rtc_interface_ptr_vec_.at(0)->removeCooperateStatus(uuid_vec_.at(0)); } } void removePreviousRTCStatusRight() { - if (rtc_interface_right_->isRegistered(uuid_right_)) { - rtc_interface_right_->removeCooperateStatus(uuid_right_); + if (rtc_interface_ptr_vec_.at(1)->isRegistered(uuid_vec_.at(1))) { + rtc_interface_ptr_vec_.at(1)->removeCooperateStatus(uuid_vec_.at(1)); } } diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/manager.hpp index 1a731a5589c1d..55c13fd32614c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/manager.hpp @@ -38,17 +38,12 @@ class AvoidanceModuleManager : public SceneModuleManagerInterface std::shared_ptr createNewSceneModuleInstance() override { - return std::make_shared( - name_, *node_, parameters_, rtc_interface_left_, rtc_interface_right_); + return std::make_shared(name_, *node_, parameters_, {"left", "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_; 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 12f9c1155e423..3e1f91951a2b3 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 @@ -59,8 +59,7 @@ class LaneChangeModule : public SceneModuleInterface #else LaneChangeModule( const std::string & name, rclcpp::Node & node, - const std::shared_ptr & parameters, - const std::shared_ptr & rtc_interface, Direction direction, + const std::shared_ptr & parameters, Direction direction, LaneChangeModuleType type); #endif bool isExecutionRequested() const override; @@ -76,36 +75,7 @@ class LaneChangeModule : public SceneModuleInterface void acceptVisitor( [[maybe_unused]] const std::shared_ptr & visitor) const override; -#ifdef USE_OLD_ARCHITECTURE - void publishRTCStatus() override - { - 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_right_->isRegistered(uuid_right_)) { - return rtc_interface_right_->isActivated(uuid_right_); - } - return false; - } - - void lockRTCCommand() override - { - rtc_interface_left_->lockCommandUpdate(); - rtc_interface_right_->lockCommandUpdate(); - } - - void unlockRTCCommand() override - { - rtc_interface_left_->unlockCommandUpdate(); - rtc_interface_right_->unlockCommandUpdate(); - } -#else +#ifndef USE_OLD_ARCHITECTURE void updateModuleParams(const std::shared_ptr & parameters) { parameters_ = parameters; @@ -129,10 +99,6 @@ class LaneChangeModule : public SceneModuleInterface bool is_activated_{false}; #ifdef USE_OLD_ARCHITECTURE - std::shared_ptr rtc_interface_left_; - std::shared_ptr rtc_interface_right_; - UUID uuid_left_; - UUID uuid_right_; UUID candidate_uuid_; #else Direction direction_{Direction::NONE}; @@ -144,32 +110,32 @@ class LaneChangeModule : public SceneModuleInterface #ifdef USE_OLD_ARCHITECTURE void waitApprovalLeft(const double start_distance, const double finish_distance) { - rtc_interface_left_->updateCooperateStatus( - uuid_left_, isExecutionReady(), start_distance, finish_distance, clock_->now()); + rtc_interface_ptr_vec_.at(0)->updateCooperateStatus( + uuid_vec_.at(0), 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( - uuid_right_, isExecutionReady(), start_distance, finish_distance, clock_->now()); + rtc_interface_ptr_vec_.at(1)->updateCooperateStatus( + uuid_vec_.at(1), isExecutionReady(), start_distance, finish_distance, clock_->now()); is_waiting_approval_ = true; } void updateRTCStatus(const CandidateOutput & candidate) { if (candidate.lateral_shift > 0.0) { - rtc_interface_left_->updateCooperateStatus( - uuid_left_, isExecutionReady(), candidate.start_distance_to_path_change, + rtc_interface_ptr_vec_.at(0)->updateCooperateStatus( + uuid_vec_.at(0), isExecutionReady(), candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change, clock_->now()); - candidate_uuid_ = uuid_left_; + candidate_uuid_ = uuid_vec_.at(0); return; } if (candidate.lateral_shift < 0.0) { - rtc_interface_right_->updateCooperateStatus( - uuid_right_, isExecutionReady(), candidate.start_distance_to_path_change, + rtc_interface_ptr_vec_.at(1)->updateCooperateStatus( + uuid_vec_.at(1), isExecutionReady(), candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change, clock_->now()); - candidate_uuid_ = uuid_right_; + candidate_uuid_ = uuid_vec_.at(1); return; } @@ -178,23 +144,17 @@ class LaneChangeModule : public SceneModuleInterface "Direction is UNKNOWN, start_distance = " << candidate.start_distance_to_path_change); } - void removeRTCStatus() override - { - 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_ptr_vec_.at(0)->isRegistered(uuid_vec_.at(0))) { + rtc_interface_ptr_vec_.at(0)->removeCooperateStatus(uuid_vec_.at(0)); } } void removePreviousRTCStatusRight() { - if (rtc_interface_right_->isRegistered(uuid_right_)) { - rtc_interface_right_->removeCooperateStatus(uuid_right_); + if (rtc_interface_ptr_vec_.at(1)->isRegistered(uuid_vec_.at(1))) { + rtc_interface_ptr_vec_.at(1)->removeCooperateStatus(uuid_vec_.at(1)); } } #endif 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 666a8c54ddd32..55008b5d821a2 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 @@ -39,14 +39,12 @@ class LaneChangeModuleManager : public SceneModuleManagerInterface std::shared_ptr createNewSceneModuleInstance() override { return std::make_shared( - name_, *node_, parameters_, rtc_interface_, direction_, type_); + name_, *node_, parameters_, {"left", "right"}, direction_, type_); } void updateModuleParams(const std::vector & parameters) override; private: - std::shared_ptr rtc_interface_; - std::shared_ptr parameters_; std::vector> registered_modules_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/manager.hpp index 8c29c37927813..f5702330d6bd7 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/manager.hpp @@ -36,7 +36,7 @@ class PullOutModuleManager : public SceneModuleManagerInterface std::shared_ptr createNewSceneModuleInstance() override { - return std::make_shared(name_, *node_, parameters_, rtc_interface_); + return std::make_shared(name_, *node_, parameters_); } void updateModuleParams(const std::vector & parameters) override; @@ -44,8 +44,6 @@ class PullOutModuleManager : public SceneModuleManagerInterface private: std::shared_ptr parameters_; - std::shared_ptr rtc_interface_; - std::vector> registered_modules_; }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp index 2e1b580b4c4e4..220d02a8ab49a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp @@ -70,8 +70,7 @@ class PullOutModule : public SceneModuleInterface #else PullOutModule( const std::string & name, rclcpp::Node & node, - const std::shared_ptr & parameters, - const std::shared_ptr & rtc_interface); + const std::shared_ptr & parameters); void updateModuleParams(const std::shared_ptr & parameters) { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/manager.hpp index dbbd6d1f0393f..dc4ea4618c0ba 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/manager.hpp @@ -36,7 +36,7 @@ class PullOverModuleManager : public SceneModuleManagerInterface std::shared_ptr createNewSceneModuleInstance() override { - return std::make_shared(name_, *node_, parameters_, rtc_interface_); + return std::make_shared(name_, *node_, parameters_); } void updateModuleParams(const std::vector & parameters) override; @@ -44,8 +44,6 @@ class PullOverModuleManager : public SceneModuleManagerInterface private: std::shared_ptr parameters_; - std::shared_ptr rtc_interface_; - std::vector> registered_modules_; }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp index d0005ba6d522a..e4f9d0fb7971a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp @@ -92,8 +92,7 @@ class PullOverModule : public SceneModuleInterface #else PullOverModule( const std::string & name, rclcpp::Node & node, - const std::shared_ptr & parameters, - const std::shared_ptr & rtc_interface); + const std::shared_ptr & parameters); void updateModuleParams(const std::shared_ptr & parameters) { 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 4d452e0c2e55b..34f91a4ae0db6 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 @@ -54,13 +54,13 @@ using PlanResult = PathWithLaneId::SharedPtr; class SceneModuleInterface { public: - SceneModuleInterface(const std::string & name, rclcpp::Node & node) + SceneModuleInterface( + const std::string & name, rclcpp::Node & node, const std::vector rtc_types = {""}) : name_{name}, logger_{node.get_logger().get_child(name)}, clock_{node.get_clock()}, is_waiting_approval_{false}, is_locked_new_module_launch_{false}, - uuid_(generateUUID()), current_state_{ModuleStatus::SUCCESS} { #ifdef USE_OLD_ARCHITECTURE @@ -70,6 +70,16 @@ class SceneModuleInterface const auto ns = std::string("~/debug/") + module_ns; pub_debug_marker_ = node.create_publisher(ns, 20); + + for (const auto & rtc_type : rtc_types) { + uuid_vec_.push_back(generateUUID()); + if (rtc_type == "") { + rtc_interface_ptr_vec_.push_back(std::make_shared(&node, name)); + } else { + rtc_interface_ptr_vec_.push_back( + std::make_shared(&node, name + "_" + rtc_type)); + } + } #endif } @@ -167,10 +177,11 @@ class SceneModuleInterface */ virtual void publishRTCStatus() { - if (!rtc_interface_ptr_) { - return; + for (const auto & rtc_interface_ptr : rtc_interface_ptr_vec_) { + if (rtc_interface_ptr) { + rtc_interface_ptr->publishCooperateStatus(clock_->now()); + } } - rtc_interface_ptr_->publishCooperateStatus(clock_->now()); } /** @@ -178,11 +189,10 @@ class SceneModuleInterface */ virtual bool isActivated() { - if (!rtc_interface_ptr_) { - return true; - } - if (rtc_interface_ptr_->isRegistered(uuid_)) { - return rtc_interface_ptr_->isActivated(uuid_); + for (size_t i = 0; i < rtc_interface_ptr_vec_.size(); ++i) { + if (rtc_interface_ptr_vec_.at(i)->isRegistered(uuid_vec_.at(i))) { + return rtc_interface_ptr_vec_.at(i)->isActivated(uuid_vec_.at(i)); + } } return false; } @@ -197,18 +207,20 @@ class SceneModuleInterface virtual void lockRTCCommand() { - if (!rtc_interface_ptr_) { - return; + for (const auto & rtc_interface_ptr : rtc_interface_ptr_vec_) { + if (rtc_interface_ptr) { + rtc_interface_ptr->lockCommandUpdate(); + } } - rtc_interface_ptr_->lockCommandUpdate(); } virtual void unlockRTCCommand() { - if (!rtc_interface_ptr_) { - return; + for (const auto & rtc_interface_ptr : rtc_interface_ptr_vec_) { + if (rtc_interface_ptr) { + rtc_interface_ptr->unlockCommandUpdate(); + } } - rtc_interface_ptr_->unlockCommandUpdate(); } /** @@ -264,19 +276,21 @@ class SceneModuleInterface protected: void updateRTCStatus(const double start_distance, const double finish_distance) { - if (!rtc_interface_ptr_) { - return; + for (size_t i = 0; i < rtc_interface_ptr_vec_.size(); ++i) { + if (rtc_interface_ptr_vec_.at(i)) { + rtc_interface_ptr_vec_.at(i)->updateCooperateStatus( + uuid_vec_.at(i), isExecutionReady(), start_distance, finish_distance, clock_->now()); + } } - rtc_interface_ptr_->updateCooperateStatus( - uuid_, isExecutionReady(), start_distance, finish_distance, clock_->now()); } virtual void removeRTCStatus() { - if (!rtc_interface_ptr_) { - return; + for (const auto & rtc_interface_ptr : rtc_interface_ptr_vec_) { + if (rtc_interface_ptr) { + rtc_interface_ptr->clearCooperateStatus(); + } } - rtc_interface_ptr_->clearCooperateStatus(); } BehaviorModuleOutput getPreviousModuleOutput() const { return previous_module_output_; } @@ -291,7 +305,7 @@ class SceneModuleInterface rclcpp::Clock::SharedPtr clock_; - std::shared_ptr rtc_interface_ptr_; + std::vector> rtc_interface_ptr_vec_; std::shared_ptr planner_data_; @@ -300,7 +314,7 @@ class SceneModuleInterface bool is_waiting_approval_; bool is_locked_new_module_launch_; - UUID uuid_; + std::vector uuid_vec_; PlanResult path_candidate_; PlanResult path_reference_; diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index ce779d97bebc4..7db0dd1d4fd04 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -80,27 +80,15 @@ void pushUniqueVector(T & base_vector, const T & additional_vector) #ifdef USE_OLD_ARCHITECTURE AvoidanceModule::AvoidanceModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters) -: SceneModuleInterface{name, node}, - parameters_{std::move(parameters)}, - uuid_left_{generateUUID()}, - uuid_right_{generateUUID()} +: SceneModuleInterface{name, node, {"left", "right"}}, parameters_{std::move(parameters)} { using std::placeholders::_1; - rtc_interface_left_ = std::make_shared(&node, "avoidance_left"), - rtc_interface_right_ = std::make_shared(&node, "avoidance_right"), steering_factor_interface_ptr_ = std::make_unique(&node, "avoidance"); } #else AvoidanceModule::AvoidanceModule( - const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, - std::shared_ptr & rtc_interface_left, - std::shared_ptr & rtc_interface_right) -: SceneModuleInterface{name, node}, - parameters_{std::move(parameters)}, - rtc_interface_left_{rtc_interface_left}, - rtc_interface_right_{rtc_interface_right}, - uuid_left_{generateUUID()}, - uuid_right_{generateUUID()} + const std::string & name, rclcpp::Node & node, std::shared_ptr parameters) +: SceneModuleInterface{name, node}, parameters_{std::move(parameters)} { using std::placeholders::_1; steering_factor_interface_ptr_ = std::make_unique(&node, "avoidance"); @@ -3250,13 +3238,13 @@ void AvoidanceModule::addShiftLineIfApproved(const AvoidLineArray & shift_lines) const auto sl_back = shift_lines.back(); if (getRelativeLengthFromPath(sl) > 0.0) { - left_shift_array_.push_back({uuid_left_, sl_front.start, sl_back.end}); + left_shift_array_.push_back({uuid_vec_.at(0), sl_front.start, sl_back.end}); } else if (getRelativeLengthFromPath(sl) < 0.0) { - right_shift_array_.push_back({uuid_right_, sl_front.start, sl_back.end}); + right_shift_array_.push_back({uuid_vec_.at(1), sl_front.start, sl_back.end}); } - uuid_left_ = generateUUID(); - uuid_right_ = generateUUID(); + uuid_vec_.at(0) = generateUUID(); + uuid_vec_.at(1) = generateUUID(); candidate_uuid_ = generateUUID(); lockNewModuleLaunch(); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index 0940842274381..5e501386d66fd 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -28,8 +28,6 @@ AvoidanceModuleManager::AvoidanceModuleManager( const std::shared_ptr & parameters) : SceneModuleManagerInterface(node, name, config), parameters_{parameters} { - rtc_interface_left_ = std::make_shared(node, name + "_left"); - rtc_interface_right_ = std::make_shared(node, name + "_right"); } void AvoidanceModuleManager::updateModuleParams(const std::vector & parameters) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp index 78bd5daa0bcd2..697a1619e3070 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp @@ -43,9 +43,10 @@ std::string getTopicName(const ExternalRequestLaneChangeModule::Direction & dire ExternalRequestLaneChangeModule::ExternalRequestLaneChangeModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, const Direction & direction) -: SceneModuleInterface{name, node}, parameters_{std::move(parameters)}, direction_{direction} +: SceneModuleInterface{name, node, {getTopicName(direction)}}, + parameters_{std::move(parameters)}, + direction_{direction} { - rtc_interface_ptr_ = std::make_shared(&node, getTopicName(direction)); steering_factor_interface_ptr_ = std::make_unique(&node, getTopicName(direction)); } @@ -174,7 +175,9 @@ void ExternalRequestLaneChangeModule::resetPathIfAbort() if (!is_abort_approval_requested_) { RCLCPP_DEBUG(getLogger(), "[abort] uuid is reset to request abort approval."); - uuid_ = generateUUID(); + for (auto & uuid : uuid_vec_) { + uuid = generateUUID(); + } is_abort_approval_requested_ = true; is_abort_path_approved_ = false; return; 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 190178d6d5bd8..0ee96f3a8fb24 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 @@ -39,24 +39,20 @@ 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)}, - uuid_left_{generateUUID()}, - uuid_right_{generateUUID()} +: SceneModuleInterface{name, node, {"left", "right"}}, parameters_{std::move(parameters)} { - 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, Direction direction, - LaneChangeModuleType type) -: SceneModuleInterface{name, node}, parameters_{parameters}, direction_{direction}, type_{type} + const std::vector & rtc_types, Direction direction, LaneChangeModuleType type) +: SceneModuleInterface{name, node, rtc_types}, + parameters_{parameters}, + direction_{direction}, + type_{type} { - rtc_interface_ptr_ = rtc_interface; steering_factor_interface_ptr_ = std::make_unique(&node, name); } #endif @@ -224,10 +220,10 @@ void LaneChangeModule::resetPathIfAbort() const auto lateral_shift = lane_change_utils::getLateralShift(*abort_path_); if (lateral_shift > 0.0) { removePreviousRTCStatusRight(); - uuid_right_ = generateUUID(); + uuid_vec_.at(1) = generateUUID(); } else if (lateral_shift < 0.0) { removePreviousRTCStatusLeft(); - uuid_left_ = generateUUID(); + uuid_vec_.at(0) = generateUUID(); } #else removeRTCStatus(); 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 458ab20970e37..4649c049b9b58 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 @@ -32,9 +32,7 @@ LaneChangeModuleManager::LaneChangeModuleManager( parameters_{std::move(parameters)}, direction_{direction}, type_{type} - { - rtc_interface_ = std::make_shared(node, name); } void LaneChangeModuleManager::updateModuleParams( diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/manager.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/manager.cpp index d02dbdfeeaddb..d305714cf20a6 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/manager.cpp @@ -28,7 +28,6 @@ PullOutModuleManager::PullOutModuleManager( const std::shared_ptr & parameters) : SceneModuleManagerInterface(node, name, config), parameters_{parameters} { - rtc_interface_ = std::make_shared(node, name); } void PullOutModuleManager::updateModuleParams( diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp index f1572500ba458..aef516f54dac3 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp @@ -42,7 +42,6 @@ PullOutModule::PullOutModule( parameters_{parameters}, vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()} { - rtc_interface_ptr_ = std::make_shared(&node, "pull_out"); steering_factor_interface_ptr_ = std::make_unique(&node, "pull_out"); lane_departure_checker_ = std::make_shared(); lane_departure_checker_->setVehicleInfo(vehicle_info_); @@ -63,13 +62,11 @@ PullOutModule::PullOutModule( #else PullOutModule::PullOutModule( const std::string & name, rclcpp::Node & node, - const std::shared_ptr & parameters, - const std::shared_ptr & rtc_interface) + const std::shared_ptr & parameters) : SceneModuleInterface{name, node}, parameters_{parameters}, vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()} { - rtc_interface_ptr_ = rtc_interface; steering_factor_interface_ptr_ = std::make_unique(&node, "pull_out"); lane_departure_checker_ = std::make_shared(); lane_departure_checker_->setVehicleInfo(vehicle_info_); @@ -736,7 +733,9 @@ void PullOutModule::checkBackFinished() // request pull_out approval waitApproval(); removeRTCStatus(); - uuid_ = generateUUID(); + for (auto & uuid : uuid_vec_) { + uuid = generateUUID(); + } current_state_ = ModuleStatus::SUCCESS; // for breaking loop } } diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/manager.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/manager.cpp index 40efddcef157e..6e244e8ad6cae 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/manager.cpp @@ -28,7 +28,6 @@ PullOverModuleManager::PullOverModuleManager( const std::shared_ptr & parameters) : SceneModuleManagerInterface(node, name, config), parameters_{parameters} { - rtc_interface_ = std::make_shared(node, name); } void PullOverModuleManager::updateModuleParams( diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp index b2f5dfae39425..fd92cb021e0f7 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp @@ -53,17 +53,14 @@ PullOverModule::PullOverModule( parameters_{parameters}, vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()} { - rtc_interface_ptr_ = std::make_shared(&node, "pull_over"); #else PullOverModule::PullOverModule( const std::string & name, rclcpp::Node & node, - const std::shared_ptr & parameters, - const std::shared_ptr & rtc_interface) + const std::shared_ptr & parameters) : SceneModuleInterface{name, node}, parameters_{parameters}, vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()} { - rtc_interface_ptr_ = rtc_interface; #endif steering_factor_interface_ptr_ = std::make_unique(&node, "pull_over"); @@ -525,7 +522,9 @@ BehaviorModuleOutput PullOverModule::plan() waitApproval(); removeRTCStatus(); steering_factor_interface_ptr_->clearSteeringFactors(); - uuid_ = generateUUID(); + for (auto & uuid : uuid_vec_) { + uuid = generateUUID(); + } current_state_ = ModuleStatus::SUCCESS; // for breaking loop status_.has_requested_approval = true; } else if (isActivated() && isWaitingApproval()) {