fix(behavior_path_planner): fix isActivated behavior (autowarefoundation#3663)

satoshi-ota · rej55 · kyoichi-sugahara · web-flow · commit 5055ae2b8d54 · 2023-05-11T22:22:44.000+09:00
* fix(behavior_path_planner): return FALSE when the uuid is not found

Signed-off-by: satoshi-ota &lt;satoshi.ota928@gmail.com&gt;

* fix(behavior_path_planner): fix isActivated behavior

Signed-off-by: Fumiya Watanabe &lt;rej55.g@gmail.com&gt;

* keep rtc_interface_ptr_map_ empty for lane_following and side_shift

Signed-off-by: kyoichi-sugahara &lt;kyoichi.sugahara@tier4.jp&gt;

---------

Signed-off-by: satoshi-ota &lt;satoshi.ota928@gmail.com&gt;
Signed-off-by: Fumiya Watanabe &lt;rej55.g@gmail.com&gt;
Signed-off-by: kyoichi-sugahara &lt;kyoichi.sugahara@tier4.jp&gt;
Co-authored-by: Fumiya Watanabe &lt;rej55.g@gmail.com&gt;
Co-authored-by: kyoichi-sugahara &lt;kyoichi.sugahara@tier4.jp&gt;
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
@@ -235,12 +235,16 @@ class SceneModuleInterface
    */
   bool isActivated()
   {
+    if (rtc_interface_ptr_map_.empty()) {
+      return true;
+    }
+
     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 true;
+    return false;
   }
 
   void publishSteeringFactor()
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
@@ -26,7 +26,7 @@ namespace behavior_path_planner
 {
 LaneFollowingModule::LaneFollowingModule(const std::string & name, rclcpp::Node & node)
 // RTCInterface is temporarily registered, but not used.
-: SceneModuleInterface{name, node, createRTCInterfaceMap(node, name, {""})}
+: SceneModuleInterface{name, node, {}}
 {
   initParam();
 }
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
@@ -26,7 +26,7 @@ namespace behavior_path_planner
 SideShiftModuleManager::SideShiftModuleManager(
   rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config,
   const std::shared_ptr<SideShiftParameters> & 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
@@ -39,7 +39,7 @@ using tier4_autoware_utils::getPoint;
 SideShiftModule::SideShiftModule(
   const std::string & name, rclcpp::Node & node,
   const std::shared_ptr<SideShiftParameters> & parameters)
-: SceneModuleInterface{name, node, createRTCInterfaceMap(node, name, {""})}, parameters_{parameters}
+: SceneModuleInterface{name, node, {}}, parameters_{parameters}
 {
   using std::placeholders::_1;
   lateral_offset_subscriber_ = node.create_subscription<LateralOffset>(

Original file line number	Diff line number	Diff line change
`@@ -235,12 +235,16 @@ class SceneModuleInterface`
`235`	`235`	`*/`
`236`	`236`	`bool isActivated()`
`237`	`237`	`{`
	`238`	`+ if (rtc_interface_ptr_map_.empty()) {`
	`239`	`+ return true;`
	`240`	`+ }`
	`241`	`+`
`238`	`242`	`for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) {`
`239`	`243`	`if (itr->second->isRegistered(uuid_map_.at(itr->first))) {`
`240`	`244`	`return itr->second->isActivated(uuid_map_.at(itr->first));`
`241`	`245`	`}`
`242`	`246`	`}`
`243`		`- return true;`
	`247`	`+ return false;`
`244`	`248`	`}`
`245`	`249`
`246`	`250`	`void publishSteeringFactor()`
Original file line number	Diff line number	Diff line change
`@@ -26,7 +26,7 @@ namespace behavior_path_planner`
`26`	`26`	`{`
`27`	`27`	`LaneFollowingModule::LaneFollowingModule(const std::string & name, rclcpp::Node & node)`
`28`	`28`	`// RTCInterface is temporarily registered, but not used.`
`29`		`-: SceneModuleInterface{name, node, createRTCInterfaceMap(node, name, {""})}`
	`29`	`+: SceneModuleInterface{name, node, {}}`
`30`	`30`	`{`
`31`	`31`	`initParam();`
`32`	`32`	`}`
Original file line number	Diff line number	Diff line change
`@@ -39,7 +39,7 @@ using tier4_autoware_utils::getPoint;`
`39`	`39`	`SideShiftModule::SideShiftModule(`
`40`	`40`	`const std::string & name, rclcpp::Node & node,`
`41`	`41`	`const std::shared_ptr<SideShiftParameters> & parameters)`
`42`		`-: SceneModuleInterface{name, node, createRTCInterfaceMap(node, name, {""})}, parameters_{parameters}`
	`42`	`+: SceneModuleInterface{name, node, {}}, parameters_{parameters}`
`43`	`43`	`{`
`44`	`44`	`using std::placeholders::_1;`
`45`	`45`	`lateral_offset_subscriber_ = node.create_subscription<LateralOffset>(`