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..b2194dbc10a25 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 @@ -30,6 +30,7 @@ #include #include #include +#include #include #include @@ -50,8 +51,7 @@ class AvoidanceModule : public SceneModuleInterface #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::unordered_map > & rtc_interface_ptr_map); #endif bool isExecutionRequested() const override; @@ -72,35 +72,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 +89,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_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_left_; + candidate_uuid_ = uuid_map_.at("left"); return; } if (candidate.lateral_shift < 0.0) { - rtc_interface_right_->updateCooperateStatus( - uuid_right_, isExecutionReady(), candidate.start_distance_to_path_change, + 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_right_; + candidate_uuid_ = uuid_map_.at("right"); return; } @@ -158,7 +124,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_map_.at("left")->updateCooperateStatus( left_shift.uuid, true, start_distance, finish_distance, clock_->now()); if (finish_distance > -1.0e-03) { steering_factor_interface_ptr_->updateSteeringFactor( @@ -172,7 +138,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_map_.at("right")->updateCooperateStatus( right_shift.uuid, true, start_distance, finish_distance, clock_->now()); if (finish_distance > -1.0e-03) { steering_factor_interface_ptr_->updateSteeringFactor( @@ -183,32 +149,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_map_.at("left")->isRegistered(candidate_uuid_)) { + rtc_interface_ptr_map_.at("left")->removeCooperateStatus(candidate_uuid_); + } else if (rtc_interface_ptr_map_.at("right")->isRegistered(candidate_uuid_)) { + rtc_interface_ptr_map_.at("right")->removeCooperateStatus(candidate_uuid_); } } void removePreviousRTCStatusLeft() { - if (rtc_interface_left_->isRegistered(uuid_left_)) { - rtc_interface_left_->removeCooperateStatus(uuid_left_); + if (rtc_interface_ptr_map_.at("left")->isRegistered(uuid_map_.at("left"))) { + rtc_interface_ptr_map_.at("left")->removeCooperateStatus(uuid_map_.at("left")); } } void removePreviousRTCStatusRight() { - if (rtc_interface_right_->isRegistered(uuid_right_)) { - rtc_interface_right_->removeCooperateStatus(uuid_right_); + if (rtc_interface_ptr_map_.at("right")->isRegistered(uuid_map_.at("right"))) { + rtc_interface_ptr_map_.at("right")->removeCooperateStatus(uuid_map_.at("right")); } } 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..c04ce131467b5 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_, rtc_interface_ptr_map_); } 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..a9bfdeff8297d 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 @@ -60,8 +60,8 @@ class LaneChangeModule : public SceneModuleInterface LaneChangeModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, - const std::shared_ptr & rtc_interface, Direction direction, - LaneChangeModuleType type); + const std::unordered_map > & rtc_interface_ptr_map, + Direction direction, LaneChangeModuleType type); #endif bool isExecutionRequested() const override; bool isExecutionReady() const override; @@ -76,36 +76,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 +100,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 +111,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_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_right_->updateCooperateStatus( - uuid_right_, isExecutionReady(), start_distance, finish_distance, clock_->now()); + rtc_interface_ptr_map_.at("right")->updateCooperateStatus( + uuid_map_.at("right"), 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_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_left_; + candidate_uuid_ = uuid_map_.at("left"); return; } if (candidate.lateral_shift < 0.0) { - rtc_interface_right_->updateCooperateStatus( - uuid_right_, isExecutionReady(), candidate.start_distance_to_path_change, + 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_right_; + candidate_uuid_ = uuid_map_.at("right"); return; } @@ -178,23 +145,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_map_.at("left")->isRegistered(uuid_map_.at("left"))) { + rtc_interface_ptr_map_.at("left")->removeCooperateStatus(uuid_map_.at("left")); } } void removePreviousRTCStatusRight() { - if (rtc_interface_right_->isRegistered(uuid_right_)) { - rtc_interface_right_->removeCooperateStatus(uuid_right_); + if (rtc_interface_ptr_map_.at("right")->isRegistered(uuid_map_.at("right"))) { + rtc_interface_ptr_map_.at("right")->removeCooperateStatus(uuid_map_.at("right")); } } #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..a2310a0d9ee3e 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_, rtc_interface_ptr_map_, 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..5591da525bc36 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_, rtc_interface_ptr_map_); } 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..c433815704a77 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 @@ -36,6 +36,7 @@ #include #include #include +#include #include #include @@ -71,7 +72,7 @@ class PullOutModule : public SceneModuleInterface PullOutModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, - const std::shared_ptr & rtc_interface); + const std::unordered_map> & rtc_interface_ptr_map); 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..fbad4d6df736b 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_, rtc_interface_ptr_map_); } 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..3cbac05aff151 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 @@ -35,6 +35,7 @@ #include #include #include +#include #include #include @@ -93,7 +94,7 @@ class PullOverModule : public SceneModuleInterface PullOverModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, - const std::shared_ptr & rtc_interface); + const std::unordered_map> & rtc_interface_ptr_map); 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..92d6863a86340 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 @@ -36,6 +36,7 @@ #include #include #include +#include #include #include @@ -54,14 +55,16 @@ 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::unordered_map> & rtc_interface_ptr_map) : 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} + current_state_{ModuleStatus::SUCCESS}, + rtc_interface_ptr_map_(rtc_interface_ptr_map) { #ifdef USE_OLD_ARCHITECTURE std::string module_ns; @@ -71,6 +74,10 @@ class SceneModuleInterface const auto ns = std::string("~/debug/") + module_ns; pub_debug_marker_ = node.create_publisher(ns, 20); #endif + + for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) { + uuid_map_.emplace(itr->first, generateUUID()); + } } virtual ~SceneModuleInterface() = default; @@ -165,29 +172,29 @@ class SceneModuleInterface /** * @brief Publish status if the module is requested to run */ - virtual void publishRTCStatus() + void publishRTCStatus() { - if (!rtc_interface_ptr_) { - return; + for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) { + if (itr->second) { + itr->second->publishCooperateStatus(clock_->now()); + } } - rtc_interface_ptr_->publishCooperateStatus(clock_->now()); } /** * @brief Return true if the activation command is received */ - virtual bool isActivated() + bool isActivated() { - if (!rtc_interface_ptr_) { - return true; - } - if (rtc_interface_ptr_->isRegistered(uuid_)) { - return rtc_interface_ptr_->isActivated(uuid_); + for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) { + if (itr->second->isRegistered(uuid_map_.at(itr->first))) { + return itr->second->isActivated(uuid_map_.at(itr->first)); + } } return false; } - virtual void publishSteeringFactor() + void publishSteeringFactor() { if (!steering_factor_interface_ptr_) { return; @@ -195,20 +202,22 @@ class SceneModuleInterface steering_factor_interface_ptr_->publishSteeringFactor(clock_->now()); } - virtual void lockRTCCommand() + void lockRTCCommand() { - if (!rtc_interface_ptr_) { - return; + for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) { + if (itr->second) { + itr->second->lockCommandUpdate(); + } } - rtc_interface_ptr_->lockCommandUpdate(); } - virtual void unlockRTCCommand() + void unlockRTCCommand() { - if (!rtc_interface_ptr_) { - return; + for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) { + if (itr->second) { + itr->second->unlockCommandUpdate(); + } } - rtc_interface_ptr_->unlockCommandUpdate(); } /** @@ -262,21 +271,39 @@ class SceneModuleInterface BehaviorModuleOutput previous_module_output_; protected: + // TODO(murooka) Remove this function when BT-based architecture will be removed + std::unordered_map> createRTCInterfaceMap( + rclcpp::Node & node, const std::string & name, const std::vector & rtc_types) + { + std::unordered_map> rtc_interface_ptr_map; + for (const auto & rtc_type : rtc_types) { + const auto snake_case_name = util::convertToSnakeCase(name); + const auto rtc_interface_name = + rtc_type == "" ? 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) { - if (!rtc_interface_ptr_) { - return; + for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) { + if (itr->second) { + itr->second->updateCooperateStatus( + uuid_map_.at(itr->first), isExecutionReady(), start_distance, finish_distance, + clock_->now()); + } } - rtc_interface_ptr_->updateCooperateStatus( - uuid_, isExecutionReady(), start_distance, finish_distance, clock_->now()); } - virtual void removeRTCStatus() + void removeRTCStatus() { - if (!rtc_interface_ptr_) { - return; + for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) { + if (itr->second) { + itr->second->clearCooperateStatus(); + } } - rtc_interface_ptr_->clearCooperateStatus(); } BehaviorModuleOutput getPreviousModuleOutput() const { return previous_module_output_; } @@ -291,8 +318,6 @@ class SceneModuleInterface rclcpp::Clock::SharedPtr clock_; - std::shared_ptr rtc_interface_ptr_; - std::shared_ptr planner_data_; std::unique_ptr steering_factor_interface_ptr_; @@ -300,13 +325,15 @@ class SceneModuleInterface bool is_waiting_approval_; bool is_locked_new_module_launch_; - UUID uuid_; + std::unordered_map uuid_map_; PlanResult path_candidate_; PlanResult path_reference_; ModuleStatus current_state_; + std::unordered_map> rtc_interface_ptr_map_; + mutable MarkerArray debug_marker_; }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp index d0344c4319c2d..f9dfa5315731b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp @@ -40,7 +40,8 @@ class SceneModuleManagerInterface { public: SceneModuleManagerInterface( - rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config) + rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, + const std::vector & rtc_types) : node_(node), clock_(*node->get_clock()), logger_(node->get_logger().get_child(name)), @@ -49,6 +50,14 @@ class SceneModuleManagerInterface priority_(config.priority), enable_simultaneous_execution_(config.enable_simultaneous_execution) { + for (const auto & rtc_type : rtc_types) { + const auto snake_case_name = util::convertToSnakeCase(name); + const auto rtc_interface_name = + rtc_type == "" ? snake_case_name : snake_case_name + "_" + rtc_type; + rtc_interface_ptr_map_.emplace( + rtc_type, std::make_shared(node, rtc_interface_name)); + } + pub_debug_marker_ = node->create_publisher("~/debug/" + name, 20); } @@ -174,6 +183,8 @@ class SceneModuleManagerInterface SceneModulePtr idling_module_; + std::unordered_map> rtc_interface_ptr_map_; + private: size_t max_module_num_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/manager.hpp index c78f8f6fad9bf..71a3691b88748 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/manager.hpp @@ -37,7 +37,7 @@ class SideShiftModuleManager : public SceneModuleManagerInterface std::shared_ptr createNewSceneModuleInstance() override { - return std::make_shared(name_, *node_, parameters_); + return std::make_shared(name_, *node_, parameters_, rtc_interface_ptr_map_); } void updateModuleParams(const std::vector & parameters) override; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp index cd0bc0e7b09d9..bcce8ae46971c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp @@ -26,6 +26,7 @@ #include #include +#include #include #include @@ -39,9 +40,16 @@ using tier4_planning_msgs::msg::LateralOffset; class SideShiftModule : public SceneModuleInterface { public: +#ifdef USE_OLD_ARCHITECTURE SideShiftModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters); +#else + SideShiftModule( + const std::string & name, rclcpp::Node & node, + const std::shared_ptr & parameters, + const std::unordered_map > & rtc_interface_ptr_map); +#endif bool isExecutionRequested() const override; bool isExecutionReady() const override; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp index c0598ed7f04d8..1509595ed9520 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -48,6 +48,7 @@ #include #include #include +#include #include #include @@ -534,6 +535,8 @@ double calcLaneChangeBuffer( lanelet::ConstLanelets getLaneletsFromPath( const PathWithLaneId & path, const std::shared_ptr & route_handler); + +std::string convertToSnakeCase(const std::string & input_str); } // namespace behavior_path_planner::util #endif // BEHAVIOR_PATH_PLANNER__UTILITIES_HPP_ 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..658c942c54a3b 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,17 @@ 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, createRTCInterfaceMap(node, name, {"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::unordered_map> & rtc_interface_ptr_map) +: SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{std::move(parameters)} { using std::placeholders::_1; steering_factor_interface_ptr_ = std::make_unique(&node, "avoidance"); @@ -3250,13 +3240,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_map_.at("left"), 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_map_.at("right"), sl_front.start, sl_back.end}); } - uuid_left_ = generateUUID(); - uuid_right_ = generateUUID(); + uuid_map_.at("left") = generateUUID(); + uuid_map_.at("right") = 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..fba08bd39c8d5 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -26,10 +26,8 @@ namespace behavior_path_planner AvoidanceModuleManager::AvoidanceModuleManager( rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, const std::shared_ptr & parameters) -: SceneModuleManagerInterface(node, name, config), parameters_{parameters} +: SceneModuleManagerInterface(node, name, config, {"left", "right"}), 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 90e1e7ad9dce9..8620258e13b75 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 @@ -33,21 +33,15 @@ namespace behavior_path_planner { #ifdef USE_OLD_ARCHITECTURE -std::string getTopicName(const ExternalRequestLaneChangeModule::Direction & direction) -{ - const std::string direction_name = - direction == ExternalRequestLaneChangeModule::Direction::RIGHT ? "right" : "left"; - return "ext_request_lane_change_" + direction_name; -} - 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, createRTCInterfaceMap(node, name, {""})}, + 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)); + std::make_unique(&node, util::convertToSnakeCase(name)); } void ExternalRequestLaneChangeModule::onEntry() @@ -174,7 +168,9 @@ void ExternalRequestLaneChangeModule::resetPathIfAbort() if (!is_abort_approval_requested_) { RCLCPP_DEBUG(getLogger(), "[abort] uuid is reset to request abort approval."); - uuid_ = generateUUID(); + for (auto itr = uuid_map_.begin(); itr != uuid_map_.end(); ++itr) { + itr->second = 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 f8d7876104e04..36e078687736b 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,22 @@ 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, createRTCInterfaceMap(node, name, {"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::unordered_map > & rtc_interface_ptr_map, + Direction direction, LaneChangeModuleType type) +: SceneModuleInterface{name, node, rtc_interface_ptr_map}, + parameters_{parameters}, + direction_{direction}, + type_{type} { - rtc_interface_ptr_ = rtc_interface; steering_factor_interface_ptr_ = std::make_unique(&node, name); } #endif @@ -224,10 +222,10 @@ void LaneChangeModule::resetPathIfAbort() const auto lateral_shift = lane_change_utils::getLateralShift(*abort_path_); if (lateral_shift > 0.0) { removePreviousRTCStatusRight(); - uuid_right_ = generateUUID(); + uuid_map_.at("right") = generateUUID(); } else if (lateral_shift < 0.0) { removePreviousRTCStatusLeft(); - uuid_left_ = generateUUID(); + uuid_map_.at("left") = 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..f58944aaf3e2b 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 @@ -28,13 +28,11 @@ LaneChangeModuleManager::LaneChangeModuleManager( rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, std::shared_ptr parameters, const Direction direction, const LaneChangeModuleType type) -: SceneModuleManagerInterface(node, name, config), +: SceneModuleManagerInterface(node, name, config, {""}), 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/lane_following/lane_following_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp index ef0fe5b70db74..af60cfabfbe7e 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp @@ -27,7 +27,8 @@ namespace behavior_path_planner LaneFollowingModule::LaneFollowingModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters) -: SceneModuleInterface{name, node}, parameters_{parameters} +// RTCInterface is temporarily registered, but not used. +: SceneModuleInterface{name, node, createRTCInterfaceMap(node, name, {""})}, parameters_{parameters} { initParam(); } 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..9084f461d0125 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 @@ -26,9 +26,8 @@ namespace behavior_path_planner PullOutModuleManager::PullOutModuleManager( rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, const std::shared_ptr & parameters) -: SceneModuleManagerInterface(node, name, config), parameters_{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 ea5b7426c6bb2..0fcf101d068e5 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 @@ -38,11 +38,10 @@ namespace behavior_path_planner PullOutModule::PullOutModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters) -: SceneModuleInterface{name, node}, +: SceneModuleInterface{name, node, createRTCInterfaceMap(node, name, {""})}, 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_); @@ -64,12 +63,11 @@ PullOutModule::PullOutModule( PullOutModule::PullOutModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, - const std::shared_ptr & rtc_interface) -: SceneModuleInterface{name, node}, + const std::unordered_map > & rtc_interface_ptr_map) +: SceneModuleInterface{name, node, rtc_interface_ptr_map}, 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_); @@ -742,7 +740,9 @@ void PullOutModule::checkBackFinished() // request pull_out approval waitApproval(); removeRTCStatus(); - uuid_ = generateUUID(); + for (auto itr = uuid_map_.begin(); itr != uuid_map_.end(); ++itr) { + itr->second = 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..63903745eadd2 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 @@ -26,9 +26,8 @@ namespace behavior_path_planner PullOverModuleManager::PullOverModuleManager( rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, const std::shared_ptr & parameters) -: SceneModuleManagerInterface(node, name, config), parameters_{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 1cb8e0ec8ce1f..993acbea78ec2 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 @@ -49,21 +49,19 @@ namespace behavior_path_planner PullOverModule::PullOverModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters) -: SceneModuleInterface{name, node}, +: SceneModuleInterface{name, node, createRTCInterfaceMap(node, name, {""})}, 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) -: SceneModuleInterface{name, node}, + const std::unordered_map > & rtc_interface_ptr_map) +: SceneModuleInterface{name, node, rtc_interface_ptr_map}, 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"); @@ -528,7 +526,9 @@ BehaviorModuleOutput PullOverModule::plan() waitApproval(); removeRTCStatus(); steering_factor_interface_ptr_->clearSteeringFactors(); - uuid_ = generateUUID(); + for (auto itr = uuid_map_.begin(); itr != uuid_map_.end(); ++itr) { + itr->second = generateUUID(); + } current_state_ = ModuleStatus::SUCCESS; // for breaking loop status_.has_requested_approval = true; } else if (isActivated() && isWaitingApproval()) { diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp b/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp index 373630e4307fa..2ac72eb80598e 100644 --- a/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp @@ -26,7 +26,7 @@ namespace behavior_path_planner SideShiftModuleManager::SideShiftModuleManager( rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, const std::shared_ptr & parameters) -: SceneModuleManagerInterface(node, name, config), parameters_{parameters} +: SceneModuleManagerInterface(node, name, config, {""}), parameters_{parameters} { } diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp index cced21882feb3..d85bd2fd192fa 100644 --- a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp @@ -33,16 +33,22 @@ using motion_utils::findNearestSegmentIndex; using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::getPoint; +#ifdef USE_OLD_ARCHITECTURE SideShiftModule::SideShiftModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters) -: SceneModuleInterface{name, node}, parameters_{parameters} +: SceneModuleInterface{name, node, createRTCInterfaceMap(node, name, {""})}, parameters_{parameters} { using std::placeholders::_1; - -#ifdef USE_OLD_ARCHITECTURE lateral_offset_subscriber_ = node.create_subscription( "~/input/lateral_offset", 1, std::bind(&SideShiftModule::onLateralOffset, this, _1)); +#else +SideShiftModule::SideShiftModule( + const std::string & name, rclcpp::Node & node, + const std::shared_ptr & parameters, + const std::unordered_map > & rtc_interface_ptr_map) +: SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{parameters} +{ #endif // If lateral offset is subscribed, it approves side shift module automatically diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index 76218a925c91d..507dce1ea1068 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -2691,4 +2691,18 @@ lanelet::ConstLanelets getLaneletsFromPath( return lanelets; } + +std::string convertToSnakeCase(const std::string & input_str) +{ + std::string output_str = std::string{static_cast(std::tolower(input_str.at(0)))}; + for (size_t i = 1; i < input_str.length(); ++i) { + const auto input_chr = input_str.at(i); + if (std::isupper(input_chr)) { + output_str += "_" + std::string{static_cast(std::tolower(input_chr))}; + } else { + output_str += input_chr; + } + } + return output_str; +} } // namespace behavior_path_planner::util