From 8e2dca6cbe7c844dff255af4a8065c55ac139c82 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Mon, 28 Nov 2022 23:38:57 +0900 Subject: [PATCH] fix(behavior_path_planner): use SUCCESS instead of IDLE when onExit Signed-off-by: kosuke55 --- .../scene_module/scene_module_bt_node_interface.hpp | 2 +- .../scene_module/scene_module_interface.hpp | 2 +- .../src/scene_module/avoidance/avoidance_module.cpp | 2 +- .../src/scene_module/lane_following/lane_following_module.cpp | 2 +- .../src/scene_module/pull_out/pull_out_module.cpp | 2 +- .../src/scene_module/pull_over/pull_over_module.cpp | 1 - .../src/scene_module/side_shift/side_shift_module.cpp | 2 +- 7 files changed, 6 insertions(+), 7 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_bt_node_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_bt_node_interface.hpp index 5838d775c39be..e3578928782f3 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_bt_node_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_bt_node_interface.hpp @@ -31,7 +31,7 @@ struct SceneModuleStatus bool is_requested{false}; bool is_execution_ready{false}; bool is_waiting_approval{false}; - BT::NodeStatus status{BT::NodeStatus::IDLE}; + BT::NodeStatus status{BT::NodeStatus::SUCCESS}; }; class SceneModuleBTNodeInterface : public BT::CoroActionNode 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 d238e8c06908f..d86461911a0c2 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 @@ -87,7 +87,7 @@ class SceneModuleInterface clock_{node.get_clock()}, uuid_(generateUUID()), is_waiting_approval_{false}, - current_state_{BT::NodeStatus::IDLE} + current_state_{BT::NodeStatus::SUCCESS} { std::string module_ns; module_ns.resize(name.size()); 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 4e619d8141d00..63dde0f3cf373 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 @@ -2645,7 +2645,7 @@ void AvoidanceModule::onExit() { DEBUG_PRINT("AVOIDANCE onExit"); initVariables(); - current_state_ = BT::NodeStatus::IDLE; + current_state_ = BT::NodeStatus::SUCCESS; clearWaitingApproval(); removeRTCStatus(); steering_factor_interface_ptr_->clearSteeringFactors(); 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 c05c06d893d45..3c2c0da5f0634 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 @@ -65,7 +65,7 @@ void LaneFollowingModule::onEntry() void LaneFollowingModule::onExit() { initParam(); - current_state_ = BT::NodeStatus::IDLE; + current_state_ = BT::NodeStatus::SUCCESS; RCLCPP_DEBUG(getLogger(), "LANE_FOLLOWING onExit"); } 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 fc0317a0a8125..0dca71c1a895f 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 @@ -100,7 +100,7 @@ void PullOutModule::onExit() clearWaitingApproval(); removeRTCStatus(); steering_factor_interface_ptr_->clearSteeringFactors(); - current_state_ = BT::NodeStatus::IDLE; + current_state_ = BT::NodeStatus::SUCCESS; RCLCPP_DEBUG(getLogger(), "PULL_OUT onExit"); } 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 0d6b6b5ea46ec..55b1a5ebe17c1 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 @@ -184,7 +184,6 @@ void PullOverModule::onExit() steering_factor_interface_ptr_->clearSteeringFactors(); // A child node must never return IDLE - // https://github.com/BehaviorTree/BehaviorTree.CPP/blob/master/include/behaviortree_cpp_v3/basic_types.h#L34 current_state_ = BT::NodeStatus::SUCCESS; } 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 77315ba631ac8..6ed656acddf05 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 @@ -63,7 +63,7 @@ void SideShiftModule::onExit() // write me... initVariables(); - current_state_ = BT::NodeStatus::IDLE; + current_state_ = BT::NodeStatus::SUCCESS; } void SideShiftModule::setParameters(const SideShiftParameters & parameters)