fix(behavior_path_planner): return node status in each scene modules (a…

…utowarefoundation#2577) * fix(behavior_path_planner): return node status in each scene modules Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> * fix(behavior_path_planner): fix typo Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> * Update planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com> Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com>
tier4 · Dec 28, 2022 · 67672da · 67672da
1 parent 211152b
commit 67672da
Show file tree

Hide file tree

Showing 4 changed files with 18 additions and 1 deletion.
diff --git a/...vior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp b/...vior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp
@@ -71,6 +71,10 @@ class PullOutModule : public SceneModuleInterface
   bool isExecutionRequested() const override;
   bool isExecutionReady() const override;
   BT::NodeStatus updateState() override;
+  BT::NodeStatus getNodeStatusWhileWaitingApproval() const override
+  {
+    return BT::NodeStatus::SUCCESS;
+  }
   BehaviorModuleOutput plan() override;
   BehaviorModuleOutput planWaitingApproval() override;
   CandidateOutput planCandidate() const override;

diff --git a/...or_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp b/...or_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp
@@ -79,6 +79,10 @@ class PullOverModule : public SceneModuleInterface
   bool isExecutionRequested() const override;
   bool isExecutionReady() const override;
   BT::NodeStatus updateState() override;
+  BT::NodeStatus getNodeStatusWhileWaitingApproval() const override
+  {
+    return BT::NodeStatus::SUCCESS;
+  }
   void onTimer();
   bool planWithEfficientPath();
   bool planWithCloseGoal();

diff --git a/...havior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp b/...havior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp
@@ -103,6 +103,15 @@ class SceneModuleInterface
    */
   virtual BT::NodeStatus updateState() = 0;
 
+  /**
+   * @brief If the module plan customized reference path while waiting approval, it should output
+   * SUCCESS. Otherwise, it should output FAILURE to check execution request of next module.
+   */
+  virtual BT::NodeStatus getNodeStatusWhileWaitingApproval() const
+  {
+    return BT::NodeStatus::FAILURE;
+  }
+
   /**
    * @brief Return true if the module has request for execution (not necessarily feasible)
    */

diff --git a/planning/behavior_path_planner/src/scene_module/scene_module_bt_node_interface.cpp b/planning/behavior_path_planner/src/scene_module/scene_module_bt_node_interface.cpp
@@ -68,7 +68,7 @@ BT::NodeStatus SceneModuleBTNodeInterface::tick()
       // std::exit(EXIT_FAILURE);  // TODO(Horibe) do appropriate handing
     }
     scene_module_->unlockRTCCommand();
-    return BT::NodeStatus::FAILURE;
+    return scene_module_->getNodeStatusWhileWaitingApproval();
   }
 
   while (rclcpp::ok()) {