From 620efb765586fc6c4e391f6d203f3186b1caea48 Mon Sep 17 00:00:00 2001 From: taikitanaka Date: Mon, 17 Jan 2022 12:45:01 +0900 Subject: [PATCH 1/8] feat(behavior path): add empty module for enable Signed-off-by: taikitanaka --- planning/behavior_path_planner/CMakeLists.txt | 1 + .../scene_module/empty/empty_module.hpp | 52 +++++++++++++++++++ .../src/scene_module/empty/empty_module.cpp | 44 ++++++++++++++++ 3 files changed, 97 insertions(+) create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/empty/empty_module.hpp create mode 100644 planning/behavior_path_planner/src/scene_module/empty/empty_module.cpp diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index 47840d00caae7..b594cbf19f34c 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -34,6 +34,7 @@ ament_auto_add_library(behavior_path_planner_node SHARED src/scene_module/pull_over/util.cpp src/scene_module/pull_out/pull_out_module.cpp src/scene_module/pull_out/util.cpp + src/scene_module/empty/empty_module.cpp ) target_link_libraries(behavior_path_planner_node diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/empty/empty_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/empty/empty_module.hpp new file mode 100644 index 0000000000000..f812e539e44fb --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/empty/empty_module.hpp @@ -0,0 +1,52 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__EMPTY_MODULE__EMPTY_MODULE_HPP_ +#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__EMPTY_MODULE__EMPTY_MODULE_HPP_ + +#include "behavior_path_planner/scene_module/scene_module_interface.hpp" + +#include +#include + +#include + +#include +#include + +namespace behavior_path_planner +{ +struct EmptyParameters +{ +}; +class EmptyModule : public SceneModuleInterface +{ +public: + EmptyModule(const std::string & name, rclcpp::Node & node, const EmptyParameters & parameters); + bool isExecutionRequested() const override; + bool isExecutionReady() const override; + BT::NodeStatus updateState() override; + BehaviorModuleOutput plan() override; + BehaviorModuleOutput planWaitingApproval() override; + PathWithLaneId planCandidate() const override; + void onEntry() override; + void onExit() override; + +private: + EmptyParameters parameters_; +}; + +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__EMPTY_MODULE__EMPTY_MODULE_HPP_ diff --git a/planning/behavior_path_planner/src/scene_module/empty/empty_module.cpp b/planning/behavior_path_planner/src/scene_module/empty/empty_module.cpp new file mode 100644 index 0000000000000..762e4b2975c8c --- /dev/null +++ b/planning/behavior_path_planner/src/scene_module/empty/empty_module.cpp @@ -0,0 +1,44 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "behavior_path_planner/scene_module/empty/empty_module.hpp" + +#include "behavior_path_planner/utilities.hpp" + +#include +#include + +namespace behavior_path_planner +{ +EmptyModule::EmptyModule( + const std::string & name, rclcpp::Node & node, const EmptyParameters & parameters) +: SceneModuleInterface{name, node}, parameters_{parameters} +{ + approval_handler_.clearWaitApproval(); + current_state_ = BT::NodeStatus::IDLE; +} +bool EmptyModule::isExecutionRequested() const { return false; } +bool EmptyModule::isExecutionReady() const { return false; } +BT::NodeStatus EmptyModule::updateState() { return BT::NodeStatus::SUCCESS; } +BehaviorModuleOutput EmptyModule::plan() { return BehaviorModuleOutput(); } +PathWithLaneId EmptyModule::planCandidate() const +{ + PathWithLaneId path; + return path; +} +BehaviorModuleOutput EmptyModule::planWaitingApproval() { return BehaviorModuleOutput(); } +void EmptyModule::onEntry() { RCLCPP_ERROR(getLogger(), "Dont Entry This"); } +void EmptyModule::onExit() {} + +} // namespace behavior_path_planner From a6475899ff37a93b4f66a8d3504d00b9214acfc6 Mon Sep 17 00:00:00 2001 From: taikitanaka Date: Mon, 17 Jan 2022 13:10:39 +0900 Subject: [PATCH 2/8] feat(behavior path): add enable option for lf ss avoid Signed-off-by: taikitanaka --- .../config/behavior_path_planner.param.yaml | 6 +++ .../behavior_path_planner_node.hpp | 5 +++ .../src/behavior_path_planner_node.cpp | 38 +++++++++++++------ 3 files changed, 38 insertions(+), 11 deletions(-) diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml index a43aac42e6f29..1c1c54400818d 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -9,3 +9,9 @@ drivable_area_height: 50.0 refine_goal_search_radius_range: 7.5 intersection_search_distance: 30.0 + enable_lane_following: true + enable_lane_change: true + enable_side_sift: true + enable_avoidance: true + enable_pull_over: true + enable_pull_out: true diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 2f9f10f9941f8..5d4ab8cc0b112 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -18,6 +18,7 @@ #include "behavior_path_planner/behavior_tree_manager.hpp" #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/scene_module/avoidance/avoidance_module.hpp" +#include "behavior_path_planner/scene_module/empty/empty_module.hpp" #include "behavior_path_planner/scene_module/lane_change/lane_change_module.hpp" #include "behavior_path_planner/scene_module/lane_following/lane_following_module.hpp" #include "behavior_path_planner/scene_module/pull_out/pull_out_module.hpp" @@ -119,6 +120,10 @@ class BehaviorPathPlannerNode : public rclcpp::Node void onMap(const HADMapBin::ConstSharedPtr map_msg); void onRoute(const HADMapRoute::ConstSharedPtr route_msg); + //! register new module + template + void registerModule(const bool enable, const std::string & name, const P & param); + /** * @brief Modify the path points near the goal to smoothly connect the lanelet and the goal point. */ diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 3ed89165c8ba6..5ebe58cdb099a 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -16,6 +16,7 @@ #include "behavior_path_planner/path_utilities.hpp" #include "behavior_path_planner/scene_module/avoidance/avoidance_module.hpp" +#include "behavior_path_planner/scene_module/empty/empty_module.hpp" #include "behavior_path_planner/scene_module/lane_change/lane_change_module.hpp" #include "behavior_path_planner/scene_module/pull_out/pull_out_module.hpp" #include "behavior_path_planner/scene_module/pull_over/pull_over_module.hpp" @@ -84,19 +85,20 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod // behavior tree manager { - bt_manager_ = std::make_shared(*this, getBehaviorTreeManagerParam()); - - auto side_shift_module = - std::make_shared("SideShift", *this, getSideShiftParam()); - bt_manager_->registerSceneModule(side_shift_module); + // get enable parameter of behavior + const bool enable_lane_following = declare_parameter("enable_lane_following", true); + // const bool enable_lane_change = true;//declare_parameter("enable_lane_change", true); + const bool enable_side_sift = declare_parameter("enable_side_sift", true); + const bool enable_avoidance = declare_parameter("enable_avoidance", true); + // const bool enable_pull_over = true;//declare_parameter("enable_pull_over", true); + // const bool enable_pull_out = true;//declare_parameter("enable_pull_out", true); - auto avoidance_module = - std::make_shared("Avoidance", *this, getAvoidanceParam()); - bt_manager_->registerSceneModule(avoidance_module); + bt_manager_ = std::make_shared(*this, getBehaviorTreeManagerParam()); + registerModule(enable_side_sift, "SideShift", getSideShiftParam()); + registerModule(enable_avoidance, "Avoidance", getAvoidanceParam()); - auto lane_following_module = - std::make_shared("LaneFollowing", *this, getLaneFollowingParam()); - bt_manager_->registerSceneModule(lane_following_module); + registerModule( + enable_lane_following, "LaneFollowing", getLaneFollowingParam()); const auto lane_change_param = getLaneChangeParam(); @@ -140,6 +142,20 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod } } +template +void BehaviorPathPlannerNode::registerModule( + const bool enable, const std::string & name, const P & p) +{ + if (enable) { + const auto m = std::make_shared(name, *this, p); + bt_manager_->registerSceneModule(m); + } else { + EmptyParameters ep; + auto e = std::make_shared(name, *this, ep); + bt_manager_->registerSceneModule(e); + } +} + BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() { // ROS parameters From b0059481fc2cebbd6891ec796ef6d7fd232e445e Mon Sep 17 00:00:00 2001 From: taikitanaka Date: Mon, 17 Jan 2022 13:19:53 +0900 Subject: [PATCH 3/8] feat(behavior path): add enable option for lc Signed-off-by: taikitanaka --- .../src/behavior_path_planner_node.cpp | 21 ++++++------------- 1 file changed, 6 insertions(+), 15 deletions(-) diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 5ebe58cdb099a..8936e5f2e1574 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -86,30 +86,21 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod // behavior tree manager { // get enable parameter of behavior - const bool enable_lane_following = declare_parameter("enable_lane_following", true); - // const bool enable_lane_change = true;//declare_parameter("enable_lane_change", true); const bool enable_side_sift = declare_parameter("enable_side_sift", true); const bool enable_avoidance = declare_parameter("enable_avoidance", true); - // const bool enable_pull_over = true;//declare_parameter("enable_pull_over", true); - // const bool enable_pull_out = true;//declare_parameter("enable_pull_out", true); + const bool enable_lane_following = declare_parameter("enable_lane_following", true); + const bool enable_lane_change = true; // declare_parameter("enable_lane_change", true); + // const bool enable_pull_over = true; // declare_parameter("enable_pull_over", true); + // const bool enable_pull_out = true; // declare_parameter("enable_pull_out", true); bt_manager_ = std::make_shared(*this, getBehaviorTreeManagerParam()); registerModule(enable_side_sift, "SideShift", getSideShiftParam()); registerModule(enable_avoidance, "Avoidance", getAvoidanceParam()); - registerModule( enable_lane_following, "LaneFollowing", getLaneFollowingParam()); - const auto lane_change_param = getLaneChangeParam(); - - auto lane_change_module = - std::make_shared("LaneChange", *this, lane_change_param); - bt_manager_->registerSceneModule(lane_change_module); - - auto force_lane_change_module = - std::make_shared("ForceLaneChange", *this, lane_change_param); - bt_manager_->registerSceneModule(force_lane_change_module); - + registerModule(enable_lane_change, "LaneChange", lane_change_param); + registerModule(enable_lane_change, "ForceLaneChange", lane_change_param); bt_manager_->registerForceApproval("ForceLaneChange"); auto pull_over_module = std::make_shared("PullOver", *this, getPullOverParam()); From 30bbd55652333abee0374b8b797efbe6e56779e6 Mon Sep 17 00:00:00 2001 From: taikitanaka Date: Mon, 17 Jan 2022 13:54:46 +0900 Subject: [PATCH 4/8] feat(behavior path): add enable option for pull out Signed-off-by: taikitanaka --- .../src/behavior_path_planner_node.cpp | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 8936e5f2e1574..05c48b5d8c2b9 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -89,9 +89,9 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod const bool enable_side_sift = declare_parameter("enable_side_sift", true); const bool enable_avoidance = declare_parameter("enable_avoidance", true); const bool enable_lane_following = declare_parameter("enable_lane_following", true); - const bool enable_lane_change = true; // declare_parameter("enable_lane_change", true); - // const bool enable_pull_over = true; // declare_parameter("enable_pull_over", true); - // const bool enable_pull_out = true; // declare_parameter("enable_pull_out", true); + const bool enable_lane_change = declare_parameter("enable_lane_change", true); + //[[maybe_unused]]const bool enable_pull_over = declare_parameter("enable_pull_over", true); + const bool enable_pull_out = declare_parameter("enable_pull_out", true); bt_manager_ = std::make_shared(*this, getBehaviorTreeManagerParam()); registerModule(enable_side_sift, "SideShift", getSideShiftParam()); @@ -102,13 +102,11 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod registerModule(enable_lane_change, "LaneChange", lane_change_param); registerModule(enable_lane_change, "ForceLaneChange", lane_change_param); bt_manager_->registerForceApproval("ForceLaneChange"); - + //! pull over can't apply registerModule -> rosparam error auto pull_over_module = std::make_shared("PullOver", *this, getPullOverParam()); bt_manager_->registerSceneModule(pull_over_module); - - auto pull_out_module = std::make_shared("PullOut", *this, getPullOutParam()); - bt_manager_->registerSceneModule(pull_out_module); - + // registerModule(enable_pull_over, "PullOverModule", getPullOverParam()); + registerModule(enable_pull_out, "PullOut", getPullOutParam()); bt_manager_->createBehaviorTree(); } From 73b42a6a48836b4fd97dc0eabbb1af8dd066c554 Mon Sep 17 00:00:00 2001 From: tanaka3 Date: Mon, 17 Jan 2022 16:30:21 +0900 Subject: [PATCH 5/8] feat(behavior path): add enable option for pull over Signed-off-by: tanaka3 --- .../src/behavior_path_planner_node.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 05c48b5d8c2b9..9eb72b81902e3 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -90,7 +90,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod const bool enable_avoidance = declare_parameter("enable_avoidance", true); const bool enable_lane_following = declare_parameter("enable_lane_following", true); const bool enable_lane_change = declare_parameter("enable_lane_change", true); - //[[maybe_unused]]const bool enable_pull_over = declare_parameter("enable_pull_over", true); + const bool enable_pull_over = declare_parameter("enable_pull_over", true); const bool enable_pull_out = declare_parameter("enable_pull_out", true); bt_manager_ = std::make_shared(*this, getBehaviorTreeManagerParam()); @@ -102,10 +102,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod registerModule(enable_lane_change, "LaneChange", lane_change_param); registerModule(enable_lane_change, "ForceLaneChange", lane_change_param); bt_manager_->registerForceApproval("ForceLaneChange"); - //! pull over can't apply registerModule -> rosparam error - auto pull_over_module = std::make_shared("PullOver", *this, getPullOverParam()); - bt_manager_->registerSceneModule(pull_over_module); - // registerModule(enable_pull_over, "PullOverModule", getPullOverParam()); + registerModule(enable_pull_over, "PullOver", getPullOverParam()); registerModule(enable_pull_out, "PullOut", getPullOutParam()); bt_manager_->createBehaviorTree(); } From 18a902bfd28ab0fb5abd2034cec2883179439913 Mon Sep 17 00:00:00 2001 From: tanaka3 Date: Mon, 17 Jan 2022 18:22:49 +0900 Subject: [PATCH 6/8] feat(behavior path): selectable via request Signed-off-by: tanaka3 --- planning/behavior_path_planner/CMakeLists.txt | 1 - .../config/behavior_path_planner.param.yaml | 1 + .../behavior_path_planner_node.hpp | 3 +- .../behavior_path_planner/parameters.hpp | 8 +++ .../scene_module/empty/empty_module.hpp | 52 ------------------- .../src/behavior_path_planner_node.cpp | 46 +++++++--------- .../avoidance/avoidance_module.cpp | 1 + .../src/scene_module/empty/empty_module.cpp | 44 ---------------- .../lane_change/lane_change_module.cpp | 1 + .../lane_following/lane_following_module.cpp | 6 ++- .../scene_module/pull_out/pull_out_module.cpp | 1 + .../pull_over/pull_over_module.cpp | 1 + .../side_shift/side_shift_module.cpp | 1 + 13 files changed, 38 insertions(+), 128 deletions(-) delete mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/empty/empty_module.hpp delete mode 100644 planning/behavior_path_planner/src/scene_module/empty/empty_module.cpp diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index b594cbf19f34c..47840d00caae7 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -34,7 +34,6 @@ ament_auto_add_library(behavior_path_planner_node SHARED src/scene_module/pull_over/util.cpp src/scene_module/pull_out/pull_out_module.cpp src/scene_module/pull_out/util.cpp - src/scene_module/empty/empty_module.cpp ) target_link_libraries(behavior_path_planner_node diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml index 1c1c54400818d..82d6aff37a78b 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -9,6 +9,7 @@ drivable_area_height: 50.0 refine_goal_search_radius_range: 7.5 intersection_search_distance: 30.0 + # select necessary modules from below enable_lane_following: true enable_lane_change: true enable_side_sift: true diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 5d4ab8cc0b112..f7f12e2482ee3 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -18,7 +18,6 @@ #include "behavior_path_planner/behavior_tree_manager.hpp" #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/scene_module/avoidance/avoidance_module.hpp" -#include "behavior_path_planner/scene_module/empty/empty_module.hpp" #include "behavior_path_planner/scene_module/lane_change/lane_change_module.hpp" #include "behavior_path_planner/scene_module/lane_following/lane_following_module.hpp" #include "behavior_path_planner/scene_module/pull_out/pull_out_module.hpp" @@ -122,7 +121,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node //! register new module template - void registerModule(const bool enable, const std::string & name, const P & param); + void registerModule(const std::string & name, const P & param); /** * @brief Modify the path points near the goal to smoothly connect the lanelet and the goal point. diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index fe5788a338dd1..7197b8f8fb0d7 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -44,6 +44,14 @@ struct BehaviorPathPlannerParameters double right_over_hang; double base_link2front; double base_link2rear; + + // is module ready + bool enable_lane_following{true}; + bool enable_side_shift{true}; + bool enable_avoidance{true}; + bool enable_lane_change{true}; + bool enable_pull_over{true}; + bool enable_pull_out{true}; }; #endif // BEHAVIOR_PATH_PLANNER__PARAMETERS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/empty/empty_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/empty/empty_module.hpp deleted file mode 100644 index f812e539e44fb..0000000000000 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/empty/empty_module.hpp +++ /dev/null @@ -1,52 +0,0 @@ -// Copyright 2021 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__EMPTY_MODULE__EMPTY_MODULE_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__EMPTY_MODULE__EMPTY_MODULE_HPP_ - -#include "behavior_path_planner/scene_module/scene_module_interface.hpp" - -#include -#include - -#include - -#include -#include - -namespace behavior_path_planner -{ -struct EmptyParameters -{ -}; -class EmptyModule : public SceneModuleInterface -{ -public: - EmptyModule(const std::string & name, rclcpp::Node & node, const EmptyParameters & parameters); - bool isExecutionRequested() const override; - bool isExecutionReady() const override; - BT::NodeStatus updateState() override; - BehaviorModuleOutput plan() override; - BehaviorModuleOutput planWaitingApproval() override; - PathWithLaneId planCandidate() const override; - void onEntry() override; - void onExit() override; - -private: - EmptyParameters parameters_; -}; - -} // namespace behavior_path_planner - -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__EMPTY_MODULE__EMPTY_MODULE_HPP_ diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 9eb72b81902e3..03a66f5162745 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -16,7 +16,6 @@ #include "behavior_path_planner/path_utilities.hpp" #include "behavior_path_planner/scene_module/avoidance/avoidance_module.hpp" -#include "behavior_path_planner/scene_module/empty/empty_module.hpp" #include "behavior_path_planner/scene_module/lane_change/lane_change_module.hpp" #include "behavior_path_planner/scene_module/pull_out/pull_out_module.hpp" #include "behavior_path_planner/scene_module/pull_over/pull_over_module.hpp" @@ -85,25 +84,16 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod // behavior tree manager { - // get enable parameter of behavior - const bool enable_side_sift = declare_parameter("enable_side_sift", true); - const bool enable_avoidance = declare_parameter("enable_avoidance", true); - const bool enable_lane_following = declare_parameter("enable_lane_following", true); - const bool enable_lane_change = declare_parameter("enable_lane_change", true); - const bool enable_pull_over = declare_parameter("enable_pull_over", true); - const bool enable_pull_out = declare_parameter("enable_pull_out", true); - bt_manager_ = std::make_shared(*this, getBehaviorTreeManagerParam()); - registerModule(enable_side_sift, "SideShift", getSideShiftParam()); - registerModule(enable_avoidance, "Avoidance", getAvoidanceParam()); - registerModule( - enable_lane_following, "LaneFollowing", getLaneFollowingParam()); - const auto lane_change_param = getLaneChangeParam(); - registerModule(enable_lane_change, "LaneChange", lane_change_param); - registerModule(enable_lane_change, "ForceLaneChange", lane_change_param); + registerModule("SideShift", getSideShiftParam()); + registerModule("Avoidance", getAvoidanceParam()); + registerModule("LaneFollowing", getLaneFollowingParam()); + auto lane_change_param = getLaneChangeParam(); + registerModule("LaneChange", lane_change_param); + registerModule("ForceLaneChange", lane_change_param); bt_manager_->registerForceApproval("ForceLaneChange"); - registerModule(enable_pull_over, "PullOver", getPullOverParam()); - registerModule(enable_pull_out, "PullOut", getPullOutParam()); + registerModule("PullOver", getPullOverParam()); + registerModule("PullOut", getPullOutParam()); bt_manager_->createBehaviorTree(); } @@ -129,17 +119,10 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod } template -void BehaviorPathPlannerNode::registerModule( - const bool enable, const std::string & name, const P & p) +void BehaviorPathPlannerNode::registerModule(const std::string & name, const P & p) { - if (enable) { - const auto m = std::make_shared(name, *this, p); - bt_manager_->registerSceneModule(m); - } else { - EmptyParameters ep; - auto e = std::make_shared(name, *this, ep); - bt_manager_->registerSceneModule(e); - } + const auto m = std::make_shared(name, *this, p); + bt_manager_->registerSceneModule(m); } BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() @@ -177,6 +160,13 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.base_link2front = vehicle_info.max_longitudinal_offset_m; p.base_link2rear = p.rear_overhang; + // get enable parameter of behavior + p.enable_lane_change = declare_parameter("enable_lane_change", true); + p.enable_side_shift = declare_parameter("enable_side_shift", true); + p.enable_avoidance = declare_parameter("enable_avoidance", true); + p.enable_lane_following = declare_parameter("enable_lane_following", true); + p.enable_pull_over = declare_parameter("enable_pull_over", true); + p.enable_pull_out = declare_parameter("enable_pull_out", true); return p; } 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 7f7f5bb9120a2..fb5cd3a1599ab 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 @@ -57,6 +57,7 @@ AvoidanceModule::AvoidanceModule( bool AvoidanceModule::isExecutionRequested() const { + if (!planner_data_->parameters.enable_avoidance) return false; DEBUG_PRINT("AVOIDANCE isExecutionRequested"); if (current_state_ == BT::NodeStatus::RUNNING) { diff --git a/planning/behavior_path_planner/src/scene_module/empty/empty_module.cpp b/planning/behavior_path_planner/src/scene_module/empty/empty_module.cpp deleted file mode 100644 index 762e4b2975c8c..0000000000000 --- a/planning/behavior_path_planner/src/scene_module/empty/empty_module.cpp +++ /dev/null @@ -1,44 +0,0 @@ -// Copyright 2021 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "behavior_path_planner/scene_module/empty/empty_module.hpp" - -#include "behavior_path_planner/utilities.hpp" - -#include -#include - -namespace behavior_path_planner -{ -EmptyModule::EmptyModule( - const std::string & name, rclcpp::Node & node, const EmptyParameters & parameters) -: SceneModuleInterface{name, node}, parameters_{parameters} -{ - approval_handler_.clearWaitApproval(); - current_state_ = BT::NodeStatus::IDLE; -} -bool EmptyModule::isExecutionRequested() const { return false; } -bool EmptyModule::isExecutionReady() const { return false; } -BT::NodeStatus EmptyModule::updateState() { return BT::NodeStatus::SUCCESS; } -BehaviorModuleOutput EmptyModule::plan() { return BehaviorModuleOutput(); } -PathWithLaneId EmptyModule::planCandidate() const -{ - PathWithLaneId path; - return path; -} -BehaviorModuleOutput EmptyModule::planWaitingApproval() { return BehaviorModuleOutput(); } -void EmptyModule::onEntry() { RCLCPP_ERROR(getLogger(), "Dont Entry This"); } -void EmptyModule::onExit() {} - -} // namespace behavior_path_planner 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 cd563228ec4eb..3ba830fedc6b0 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 @@ -72,6 +72,7 @@ void LaneChangeModule::onExit() bool LaneChangeModule::isExecutionRequested() const { + if (!planner_data_->parameters.enable_lane_change) return false; if (current_state_ == BT::NodeStatus::RUNNING) { return true; } 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 3382dc899241b..e9e81911e5cef 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 @@ -33,7 +33,11 @@ void LaneFollowingModule::initParam() approval_handler_.clearWaitApproval(); // no need approval } -bool LaneFollowingModule::isExecutionRequested() const { return true; } +bool LaneFollowingModule::isExecutionRequested() const +{ + if (!planner_data_->parameters.enable_lane_following) return false; + return true; +} bool LaneFollowingModule::isExecutionReady() const { return true; } 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 d7afa1e308b27..d3eb2ecc96249 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 @@ -74,6 +74,7 @@ void PullOutModule::onExit() bool PullOutModule::isExecutionRequested() const { + if (!planner_data_->parameters.enable_pull_out) return false; if (current_state_ == BT::NodeStatus::RUNNING) { return true; } 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 2808e6845a157..8c70b9c03c7c9 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 @@ -71,6 +71,7 @@ void PullOverModule::onExit() bool PullOverModule::isExecutionRequested() const { + if (!planner_data_->parameters.enable_pull_over) return false; if (current_state_ == BT::NodeStatus::RUNNING) { return true; } 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 f32dbc8ee7848..38a4310e455a0 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 @@ -97,6 +97,7 @@ void SideShiftModule::setParameters(const SideShiftParameters & parameters) bool SideShiftModule::isExecutionRequested() const { + if (!planner_data_->parameters.enable_side_shift) return false; if (current_state_ == BT::NodeStatus::RUNNING) { return true; } From 23d5c5464bf6a0c3fb89dcab0b506519033272ca Mon Sep 17 00:00:00 2001 From: tanaka3 Date: Mon, 17 Jan 2022 18:57:47 +0900 Subject: [PATCH 7/8] Revert "feat(behavior path): selectable via request" This reverts commit 18a902bfd28ab0fb5abd2034cec2883179439913. --- planning/behavior_path_planner/CMakeLists.txt | 1 + .../config/behavior_path_planner.param.yaml | 1 - .../behavior_path_planner_node.hpp | 3 +- .../behavior_path_planner/parameters.hpp | 8 --- .../scene_module/empty/empty_module.hpp | 52 +++++++++++++++++++ .../src/behavior_path_planner_node.cpp | 46 +++++++++------- .../avoidance/avoidance_module.cpp | 1 - .../src/scene_module/empty/empty_module.cpp | 44 ++++++++++++++++ .../lane_change/lane_change_module.cpp | 1 - .../lane_following/lane_following_module.cpp | 6 +-- .../scene_module/pull_out/pull_out_module.cpp | 1 - .../pull_over/pull_over_module.cpp | 1 - .../side_shift/side_shift_module.cpp | 1 - 13 files changed, 128 insertions(+), 38 deletions(-) create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/empty/empty_module.hpp create mode 100644 planning/behavior_path_planner/src/scene_module/empty/empty_module.cpp diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index 47840d00caae7..b594cbf19f34c 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -34,6 +34,7 @@ ament_auto_add_library(behavior_path_planner_node SHARED src/scene_module/pull_over/util.cpp src/scene_module/pull_out/pull_out_module.cpp src/scene_module/pull_out/util.cpp + src/scene_module/empty/empty_module.cpp ) target_link_libraries(behavior_path_planner_node diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml index 82d6aff37a78b..1c1c54400818d 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -9,7 +9,6 @@ drivable_area_height: 50.0 refine_goal_search_radius_range: 7.5 intersection_search_distance: 30.0 - # select necessary modules from below enable_lane_following: true enable_lane_change: true enable_side_sift: true diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index f7f12e2482ee3..5d4ab8cc0b112 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -18,6 +18,7 @@ #include "behavior_path_planner/behavior_tree_manager.hpp" #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/scene_module/avoidance/avoidance_module.hpp" +#include "behavior_path_planner/scene_module/empty/empty_module.hpp" #include "behavior_path_planner/scene_module/lane_change/lane_change_module.hpp" #include "behavior_path_planner/scene_module/lane_following/lane_following_module.hpp" #include "behavior_path_planner/scene_module/pull_out/pull_out_module.hpp" @@ -121,7 +122,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node //! register new module template - void registerModule(const std::string & name, const P & param); + void registerModule(const bool enable, const std::string & name, const P & param); /** * @brief Modify the path points near the goal to smoothly connect the lanelet and the goal point. diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index 7197b8f8fb0d7..fe5788a338dd1 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -44,14 +44,6 @@ struct BehaviorPathPlannerParameters double right_over_hang; double base_link2front; double base_link2rear; - - // is module ready - bool enable_lane_following{true}; - bool enable_side_shift{true}; - bool enable_avoidance{true}; - bool enable_lane_change{true}; - bool enable_pull_over{true}; - bool enable_pull_out{true}; }; #endif // BEHAVIOR_PATH_PLANNER__PARAMETERS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/empty/empty_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/empty/empty_module.hpp new file mode 100644 index 0000000000000..f812e539e44fb --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/empty/empty_module.hpp @@ -0,0 +1,52 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__EMPTY_MODULE__EMPTY_MODULE_HPP_ +#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__EMPTY_MODULE__EMPTY_MODULE_HPP_ + +#include "behavior_path_planner/scene_module/scene_module_interface.hpp" + +#include +#include + +#include + +#include +#include + +namespace behavior_path_planner +{ +struct EmptyParameters +{ +}; +class EmptyModule : public SceneModuleInterface +{ +public: + EmptyModule(const std::string & name, rclcpp::Node & node, const EmptyParameters & parameters); + bool isExecutionRequested() const override; + bool isExecutionReady() const override; + BT::NodeStatus updateState() override; + BehaviorModuleOutput plan() override; + BehaviorModuleOutput planWaitingApproval() override; + PathWithLaneId planCandidate() const override; + void onEntry() override; + void onExit() override; + +private: + EmptyParameters parameters_; +}; + +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__EMPTY_MODULE__EMPTY_MODULE_HPP_ diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 03a66f5162745..9eb72b81902e3 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -16,6 +16,7 @@ #include "behavior_path_planner/path_utilities.hpp" #include "behavior_path_planner/scene_module/avoidance/avoidance_module.hpp" +#include "behavior_path_planner/scene_module/empty/empty_module.hpp" #include "behavior_path_planner/scene_module/lane_change/lane_change_module.hpp" #include "behavior_path_planner/scene_module/pull_out/pull_out_module.hpp" #include "behavior_path_planner/scene_module/pull_over/pull_over_module.hpp" @@ -84,16 +85,25 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod // behavior tree manager { + // get enable parameter of behavior + const bool enable_side_sift = declare_parameter("enable_side_sift", true); + const bool enable_avoidance = declare_parameter("enable_avoidance", true); + const bool enable_lane_following = declare_parameter("enable_lane_following", true); + const bool enable_lane_change = declare_parameter("enable_lane_change", true); + const bool enable_pull_over = declare_parameter("enable_pull_over", true); + const bool enable_pull_out = declare_parameter("enable_pull_out", true); + bt_manager_ = std::make_shared(*this, getBehaviorTreeManagerParam()); - registerModule("SideShift", getSideShiftParam()); - registerModule("Avoidance", getAvoidanceParam()); - registerModule("LaneFollowing", getLaneFollowingParam()); - auto lane_change_param = getLaneChangeParam(); - registerModule("LaneChange", lane_change_param); - registerModule("ForceLaneChange", lane_change_param); + registerModule(enable_side_sift, "SideShift", getSideShiftParam()); + registerModule(enable_avoidance, "Avoidance", getAvoidanceParam()); + registerModule( + enable_lane_following, "LaneFollowing", getLaneFollowingParam()); + const auto lane_change_param = getLaneChangeParam(); + registerModule(enable_lane_change, "LaneChange", lane_change_param); + registerModule(enable_lane_change, "ForceLaneChange", lane_change_param); bt_manager_->registerForceApproval("ForceLaneChange"); - registerModule("PullOver", getPullOverParam()); - registerModule("PullOut", getPullOutParam()); + registerModule(enable_pull_over, "PullOver", getPullOverParam()); + registerModule(enable_pull_out, "PullOut", getPullOutParam()); bt_manager_->createBehaviorTree(); } @@ -119,10 +129,17 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod } template -void BehaviorPathPlannerNode::registerModule(const std::string & name, const P & p) +void BehaviorPathPlannerNode::registerModule( + const bool enable, const std::string & name, const P & p) { - const auto m = std::make_shared(name, *this, p); - bt_manager_->registerSceneModule(m); + if (enable) { + const auto m = std::make_shared(name, *this, p); + bt_manager_->registerSceneModule(m); + } else { + EmptyParameters ep; + auto e = std::make_shared(name, *this, ep); + bt_manager_->registerSceneModule(e); + } } BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() @@ -160,13 +177,6 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.base_link2front = vehicle_info.max_longitudinal_offset_m; p.base_link2rear = p.rear_overhang; - // get enable parameter of behavior - p.enable_lane_change = declare_parameter("enable_lane_change", true); - p.enable_side_shift = declare_parameter("enable_side_shift", true); - p.enable_avoidance = declare_parameter("enable_avoidance", true); - p.enable_lane_following = declare_parameter("enable_lane_following", true); - p.enable_pull_over = declare_parameter("enable_pull_over", true); - p.enable_pull_out = declare_parameter("enable_pull_out", true); return p; } 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 fb5cd3a1599ab..7f7f5bb9120a2 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 @@ -57,7 +57,6 @@ AvoidanceModule::AvoidanceModule( bool AvoidanceModule::isExecutionRequested() const { - if (!planner_data_->parameters.enable_avoidance) return false; DEBUG_PRINT("AVOIDANCE isExecutionRequested"); if (current_state_ == BT::NodeStatus::RUNNING) { diff --git a/planning/behavior_path_planner/src/scene_module/empty/empty_module.cpp b/planning/behavior_path_planner/src/scene_module/empty/empty_module.cpp new file mode 100644 index 0000000000000..762e4b2975c8c --- /dev/null +++ b/planning/behavior_path_planner/src/scene_module/empty/empty_module.cpp @@ -0,0 +1,44 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "behavior_path_planner/scene_module/empty/empty_module.hpp" + +#include "behavior_path_planner/utilities.hpp" + +#include +#include + +namespace behavior_path_planner +{ +EmptyModule::EmptyModule( + const std::string & name, rclcpp::Node & node, const EmptyParameters & parameters) +: SceneModuleInterface{name, node}, parameters_{parameters} +{ + approval_handler_.clearWaitApproval(); + current_state_ = BT::NodeStatus::IDLE; +} +bool EmptyModule::isExecutionRequested() const { return false; } +bool EmptyModule::isExecutionReady() const { return false; } +BT::NodeStatus EmptyModule::updateState() { return BT::NodeStatus::SUCCESS; } +BehaviorModuleOutput EmptyModule::plan() { return BehaviorModuleOutput(); } +PathWithLaneId EmptyModule::planCandidate() const +{ + PathWithLaneId path; + return path; +} +BehaviorModuleOutput EmptyModule::planWaitingApproval() { return BehaviorModuleOutput(); } +void EmptyModule::onEntry() { RCLCPP_ERROR(getLogger(), "Dont Entry This"); } +void EmptyModule::onExit() {} + +} // namespace behavior_path_planner 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 3ba830fedc6b0..cd563228ec4eb 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 @@ -72,7 +72,6 @@ void LaneChangeModule::onExit() bool LaneChangeModule::isExecutionRequested() const { - if (!planner_data_->parameters.enable_lane_change) return false; if (current_state_ == BT::NodeStatus::RUNNING) { return true; } 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 e9e81911e5cef..3382dc899241b 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 @@ -33,11 +33,7 @@ void LaneFollowingModule::initParam() approval_handler_.clearWaitApproval(); // no need approval } -bool LaneFollowingModule::isExecutionRequested() const -{ - if (!planner_data_->parameters.enable_lane_following) return false; - return true; -} +bool LaneFollowingModule::isExecutionRequested() const { return true; } bool LaneFollowingModule::isExecutionReady() const { return true; } 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 d3eb2ecc96249..d7afa1e308b27 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 @@ -74,7 +74,6 @@ void PullOutModule::onExit() bool PullOutModule::isExecutionRequested() const { - if (!planner_data_->parameters.enable_pull_out) return false; if (current_state_ == BT::NodeStatus::RUNNING) { return true; } 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 8c70b9c03c7c9..2808e6845a157 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 @@ -71,7 +71,6 @@ void PullOverModule::onExit() bool PullOverModule::isExecutionRequested() const { - if (!planner_data_->parameters.enable_pull_over) return false; if (current_state_ == BT::NodeStatus::RUNNING) { return true; } 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 38a4310e455a0..f32dbc8ee7848 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 @@ -97,7 +97,6 @@ void SideShiftModule::setParameters(const SideShiftParameters & parameters) bool SideShiftModule::isExecutionRequested() const { - if (!planner_data_->parameters.enable_side_shift) return false; if (current_state_ == BT::NodeStatus::RUNNING) { return true; } From 9285cf65fdfdd46833fcaa2742a54d77594a8731 Mon Sep 17 00:00:00 2001 From: tanaka3 Date: Mon, 17 Jan 2022 19:03:39 +0900 Subject: [PATCH 8/8] chore(behavior path): lane following always true Signed-off-by: tanaka3 --- .../config/behavior_path_planner.param.yaml | 1 - .../behavior_path_planner/src/behavior_path_planner_node.cpp | 5 ++--- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml index 1c1c54400818d..23001838381fe 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -9,7 +9,6 @@ drivable_area_height: 50.0 refine_goal_search_radius_range: 7.5 intersection_search_distance: 30.0 - enable_lane_following: true enable_lane_change: true enable_side_sift: true enable_avoidance: true diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 9eb72b81902e3..046b9d483fb2c 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -88,7 +88,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod // get enable parameter of behavior const bool enable_side_sift = declare_parameter("enable_side_sift", true); const bool enable_avoidance = declare_parameter("enable_avoidance", true); - const bool enable_lane_following = declare_parameter("enable_lane_following", true); const bool enable_lane_change = declare_parameter("enable_lane_change", true); const bool enable_pull_over = declare_parameter("enable_pull_over", true); const bool enable_pull_out = declare_parameter("enable_pull_out", true); @@ -96,8 +95,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod bt_manager_ = std::make_shared(*this, getBehaviorTreeManagerParam()); registerModule(enable_side_sift, "SideShift", getSideShiftParam()); registerModule(enable_avoidance, "Avoidance", getAvoidanceParam()); - registerModule( - enable_lane_following, "LaneFollowing", getLaneFollowingParam()); + // lane following should always be enabled + registerModule(true, "LaneFollowing", getLaneFollowingParam()); const auto lane_change_param = getLaneChangeParam(); registerModule(enable_lane_change, "LaneChange", lane_change_param); registerModule(enable_lane_change, "ForceLaneChange", lane_change_param);