fix bug

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

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

planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp

@@ -30,6 +30,7 @@
 #include <algorithm>
 #include <memory>
 #include <string>
+#include <unordered_map>
 #include <utility>
 #include <vector>
 
@@ -49,7 +50,8 @@ class AvoidanceModule : public SceneModuleInterface
     const std::string & name, rclcpp::Node & node, std::shared_ptr<AvoidanceParameters> parameters);
 #else
   AvoidanceModule(
-    const std::string & name, rclcpp::Node & node, std::shared_ptr<AvoidanceParameters> parameters);
+    const std::string & name, rclcpp::Node & node, std::shared_ptr<AvoidanceParameters> parameters,
+    const std::unordered_map<std::string, std::shared_ptr<RTCInterface> > & rtc_interface_ptr_map);
 #endif
 
   bool isExecutionRequested() const override;
@@ -94,17 +96,17 @@ class AvoidanceModule : public SceneModuleInterface
   void updateCandidateRTCStatus(const CandidateOutput & candidate)
   {
     if (candidate.lateral_shift > 0.0) {
-      rtc_interface_ptr_vec_.at(0)->updateCooperateStatus(
-        uuid_vec_.at(0), 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_vec_.at(0);
+      candidate_uuid_ = uuid_map_.at("left");
       return;
     }
     if (candidate.lateral_shift < 0.0) {
-      rtc_interface_ptr_vec_.at(1)->updateCooperateStatus(
-        uuid_vec_.at(1), 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_vec_.at(1);
+      candidate_uuid_ = uuid_map_.at("right");
       return;
     }
 
@@ -122,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_ptr_vec_.at(0)->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(
@@ -136,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_ptr_vec_.at(1)->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(
@@ -149,24 +151,24 @@ class AvoidanceModule : public SceneModuleInterface
 
   void removeCandidateRTCStatus()
   {
-    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_);
+    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_ptr_vec_.at(0)->isRegistered(uuid_vec_.at(0))) {
-      rtc_interface_ptr_vec_.at(0)->removeCooperateStatus(uuid_vec_.at(0));
+    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_ptr_vec_.at(1)->isRegistered(uuid_vec_.at(1))) {
-      rtc_interface_ptr_vec_.at(1)->removeCooperateStatus(uuid_vec_.at(1));
+    if (rtc_interface_ptr_map_.at("right")->isRegistered(uuid_map_.at("right"))) {
+      rtc_interface_ptr_map_.at("right")->removeCooperateStatus(uuid_map_.at("right"));
     }
   }
 

planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/manager.hpp

@@ -38,7 +38,7 @@ class AvoidanceModuleManager : public SceneModuleManagerInterface
 
   std::shared_ptr<SceneModuleInterface> createNewSceneModuleInstance() override
   {
-    return std::make_shared<AvoidanceModule>(name_, *node_, parameters_, {"left", "right"});
+    return std::make_shared<AvoidanceModule>(name_, *node_, parameters_, rtc_interface_ptr_map_);
   }
 
   void updateModuleParams(const std::vector<rclcpp::Parameter> & parameters) override;

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp

@@ -59,8 +59,9 @@ class LaneChangeModule : public SceneModuleInterface
 #else
   LaneChangeModule(
     const std::string & name, rclcpp::Node & node,
-    const std::shared_ptr<LaneChangeParameters> & parameters, Direction direction,
-    LaneChangeModuleType type);
+    const std::shared_ptr<LaneChangeParameters> & parameters,
+    const std::unordered_map<std::string, std::shared_ptr<RTCInterface> > & rtc_interface_ptr_map,
+    Direction direction, LaneChangeModuleType type);
 #endif
   bool isExecutionRequested() const override;
   bool isExecutionReady() const override;
@@ -110,32 +111,32 @@ class LaneChangeModule : public SceneModuleInterface
 #ifdef USE_OLD_ARCHITECTURE
   void waitApprovalLeft(const double start_distance, const double finish_distance)
   {
-    rtc_interface_ptr_vec_.at(0)->updateCooperateStatus(
-      uuid_vec_.at(0), 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_ptr_vec_.at(1)->updateCooperateStatus(
-      uuid_vec_.at(1), 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_ptr_vec_.at(0)->updateCooperateStatus(
-        uuid_vec_.at(0), 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_vec_.at(0);
+      candidate_uuid_ = uuid_map_.at("left");
       return;
     }
     if (candidate.lateral_shift < 0.0) {
-      rtc_interface_ptr_vec_.at(1)->updateCooperateStatus(
-        uuid_vec_.at(1), 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_vec_.at(1);
+      candidate_uuid_ = uuid_map_.at("right");
       return;
     }
 
@@ -146,15 +147,15 @@ class LaneChangeModule : public SceneModuleInterface
 
   void removePreviousRTCStatusLeft()
   {
-    if (rtc_interface_ptr_vec_.at(0)->isRegistered(uuid_vec_.at(0))) {
-      rtc_interface_ptr_vec_.at(0)->removeCooperateStatus(uuid_vec_.at(0));
+    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_ptr_vec_.at(1)->isRegistered(uuid_vec_.at(1))) {
-      rtc_interface_ptr_vec_.at(1)->removeCooperateStatus(uuid_vec_.at(1));
+    if (rtc_interface_ptr_map_.at("right")->isRegistered(uuid_map_.at("right"))) {
+      rtc_interface_ptr_map_.at("right")->removeCooperateStatus(uuid_map_.at("right"));
     }
   }
 #endif

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp

@@ -39,7 +39,7 @@ class LaneChangeModuleManager : public SceneModuleManagerInterface
   std::shared_ptr<SceneModuleInterface> createNewSceneModuleInstance() override
   {
     return std::make_shared<LaneChangeModule>(
-      name_, *node_, parameters_, {"left", "right"}, direction_, type_);
+      name_, *node_, parameters_, rtc_interface_ptr_map_, direction_, type_);
   }
 
   void updateModuleParams(const std::vector<rclcpp::Parameter> & parameters) override;

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<SceneModuleInterface> createNewSceneModuleInstance() override
   {
-    return std::make_shared<PullOutModule>(name_, *node_, parameters_);
+    return std::make_shared<PullOutModule>(name_, *node_, parameters_, rtc_interface_ptr_map_);
   }
 
   void updateModuleParams(const std::vector<rclcpp::Parameter> & parameters) override;

planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp

@@ -36,6 +36,7 @@
 #include <deque>
 #include <memory>
 #include <string>
+#include <unordered_map>
 #include <utility>
 #include <vector>
 
@@ -70,7 +71,8 @@ class PullOutModule : public SceneModuleInterface
 #else
   PullOutModule(
     const std::string & name, rclcpp::Node & node,
-    const std::shared_ptr<PullOutParameters> & parameters);
+    const std::shared_ptr<PullOutParameters> & parameters,
+    const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map);
 
   void updateModuleParams(const std::shared_ptr<PullOutParameters> & parameters)
   {

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<SceneModuleInterface> createNewSceneModuleInstance() override
   {
-    return std::make_shared<PullOverModule>(name_, *node_, parameters_);
+    return std::make_shared<PullOverModule>(name_, *node_, parameters_, rtc_interface_ptr_map_);
   }
 
   void updateModuleParams(const std::vector<rclcpp::Parameter> & parameters) override;

planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp

@@ -35,6 +35,7 @@
 #include <limits>
 #include <memory>
 #include <string>
+#include <unordered_map>
 #include <utility>
 #include <vector>
 
@@ -92,7 +93,8 @@ class PullOverModule : public SceneModuleInterface
 #else
   PullOverModule(
     const std::string & name, rclcpp::Node & node,
-    const std::shared_ptr<PullOverParameters> & parameters);
+    const std::shared_ptr<PullOverParameters> & parameters,
+    const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map);
 
   void updateModuleParams(const std::shared_ptr<PullOverParameters> & parameters)
   {

planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp

@@ -36,6 +36,7 @@
 #include <memory>
 #include <random>
 #include <string>
+#include <unordered_map>
 #include <utility>
 #include <vector>
 
@@ -55,13 +56,15 @@ class SceneModuleInterface
 {
 public:
   SceneModuleInterface(
-    const std::string & name, rclcpp::Node & node, const std::vector<std::string> rtc_types = {""})
+    const std::string & name, rclcpp::Node & node,
+    const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & 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},
-    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,14 +74,8 @@ class SceneModuleInterface
     const auto ns = std::string("~/debug/") + module_ns;
     pub_debug_marker_ = node.create_publisher<MarkerArray>(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<RTCInterface>(&node, name));
-      } else {
-        rtc_interface_ptr_vec_.push_back(
-          std::make_shared<RTCInterface>(&node, name + "_" + rtc_type));
-      }
+    for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) {
+      uuid_map_.emplace(itr->first, generateUUID);
     }
 #endif
   }
@@ -177,9 +174,9 @@ class SceneModuleInterface
    */
   virtual void publishRTCStatus()
   {
-    for (const auto & rtc_interface_ptr : rtc_interface_ptr_vec_) {
-      if (rtc_interface_ptr) {
-        rtc_interface_ptr->publishCooperateStatus(clock_->now());
+    for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) {
+      if (itr->second) {
+        itr->second->publishCooperateStatus(clock_->now());
       }
     }
   }
@@ -189,9 +186,9 @@ class SceneModuleInterface
    */
   virtual bool isActivated()
   {
-    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));
+    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;
@@ -207,18 +204,18 @@ class SceneModuleInterface
 
   virtual void lockRTCCommand()
   {
-    for (const auto & rtc_interface_ptr : rtc_interface_ptr_vec_) {
-      if (rtc_interface_ptr) {
-        rtc_interface_ptr->lockCommandUpdate();
+    for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) {
+      if (itr->second) {
+        itr->second->lockCommandUpdate();
       }
     }
   }
 
   virtual void unlockRTCCommand()
   {
-    for (const auto & rtc_interface_ptr : rtc_interface_ptr_vec_) {
-      if (rtc_interface_ptr) {
-        rtc_interface_ptr->unlockCommandUpdate();
+    for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) {
+      if (itr->second) {
+        itr->second->unlockCommandUpdate();
       }
     }
   }
@@ -276,19 +273,20 @@ class SceneModuleInterface
 protected:
   void updateRTCStatus(const double start_distance, const double finish_distance)
   {
-    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());
+    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());
       }
     }
   }
 
   virtual void removeRTCStatus()
   {
-    for (const auto & rtc_interface_ptr : rtc_interface_ptr_vec_) {
-      if (rtc_interface_ptr) {
-        rtc_interface_ptr->clearCooperateStatus();
+    for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) {
+      if (itr->second) {
+        itr->second->clearCooperateStatus();
       }
     }
   }
@@ -305,22 +303,22 @@ class SceneModuleInterface
 
   rclcpp::Clock::SharedPtr clock_;
 
-  std::vector<std::shared_ptr<RTCInterface>> rtc_interface_ptr_vec_;
-
   std::shared_ptr<const PlannerData> planner_data_;
 
   std::unique_ptr<SteeringFactorInterface> steering_factor_interface_ptr_;
 
   bool is_waiting_approval_;
   bool is_locked_new_module_launch_;
 
-  std::vector<UUID> uuid_vec_;
+  std::unordered_map<std::string, UUID> uuid_map_;
 
   PlanResult path_candidate_;
   PlanResult path_reference_;
 
   ModuleStatus current_state_;
 
+  std::unordered_map<std::string, std::shared_ptr<RTCInterface>> rtc_interface_ptr_map_;
+
   mutable MarkerArray debug_marker_;
 };