From ad69c2851b7b84e12c9f0c3b177fb6a9032bf284 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Fri, 29 Sep 2023 16:35:27 +0900 Subject: [PATCH] refactor(behavior path planner): simplify code by using lambda (#5124) simplify code by using lambda Signed-off-by: Daniel Sanchez --- .../src/behavior_path_planner_node.cpp | 61 +++++++------------ 1 file changed, 21 insertions(+), 40 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 f52e018f2eef7..6e37bd0ec4b2c 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -134,43 +134,34 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod const auto & p = planner_data_->parameters; planner_manager_ = std::make_shared(*this, p.verbose); - const auto register_and_create_publisher = [&](const auto & manager) { - const auto & module_name = manager->name(); - planner_manager_->registerSceneModuleManager(manager); - path_candidate_publishers_.emplace( - module_name, create_publisher(path_candidate_name_space + module_name, 1)); - path_reference_publishers_.emplace( - module_name, create_publisher(path_reference_name_space + module_name, 1)); - }; + const auto register_and_create_publisher = + [&](const auto & manager, const bool create_publishers) { + const auto & module_name = manager->name(); + planner_manager_->registerSceneModuleManager(manager); + if (create_publishers) { + path_candidate_publishers_.emplace( + module_name, create_publisher(path_candidate_name_space + module_name, 1)); + path_reference_publishers_.emplace( + module_name, create_publisher(path_reference_name_space + module_name, 1)); + } + }; if (p.config_start_planner.enable_module) { auto manager = std::make_shared(this, "start_planner", p.config_start_planner); - planner_manager_->registerSceneModuleManager(manager); - path_candidate_publishers_.emplace( - "start_planner", create_publisher(path_candidate_name_space + "start_planner", 1)); - path_reference_publishers_.emplace( - "start_planner", create_publisher(path_reference_name_space + "start_planner", 1)); + register_and_create_publisher(manager, true); } if (p.config_goal_planner.enable_module) { auto manager = std::make_shared(this, "goal_planner", p.config_goal_planner); - planner_manager_->registerSceneModuleManager(manager); - path_candidate_publishers_.emplace( - "goal_planner", create_publisher(path_candidate_name_space + "goal_planner", 1)); - path_reference_publishers_.emplace( - "goal_planner", create_publisher(path_reference_name_space + "goal_planner", 1)); + register_and_create_publisher(manager, true); } if (p.config_side_shift.enable_module) { auto manager = std::make_shared(this, "side_shift", p.config_side_shift); - planner_manager_->registerSceneModuleManager(manager); - path_candidate_publishers_.emplace( - "side_shift", create_publisher(path_candidate_name_space + "side_shift", 1)); - path_reference_publishers_.emplace( - "side_shift", create_publisher(path_reference_name_space + "side_shift", 1)); + register_and_create_publisher(manager, true); } if (p.config_lane_change_left.enable_module) { @@ -178,7 +169,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod auto manager = std::make_shared( this, module_topic, p.config_lane_change_left, route_handler::Direction::LEFT, LaneChangeModuleType::NORMAL); - register_and_create_publisher(manager); + register_and_create_publisher(manager, true); } if (p.config_lane_change_right.enable_module) { @@ -186,7 +177,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod auto manager = std::make_shared( this, module_topic, p.config_lane_change_right, route_handler::Direction::RIGHT, LaneChangeModuleType::NORMAL); - register_and_create_publisher(manager); + register_and_create_publisher(manager, true); } if (p.config_ext_request_lane_change_right.enable_module) { @@ -194,7 +185,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod auto manager = std::make_shared( this, module_topic, p.config_ext_request_lane_change_right, route_handler::Direction::RIGHT, LaneChangeModuleType::EXTERNAL_REQUEST); - register_and_create_publisher(manager); + register_and_create_publisher(manager, true); } if (p.config_ext_request_lane_change_left.enable_module) { @@ -202,35 +193,25 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod auto manager = std::make_shared( this, module_topic, p.config_ext_request_lane_change_left, route_handler::Direction::LEFT, LaneChangeModuleType::EXTERNAL_REQUEST); - register_and_create_publisher(manager); + register_and_create_publisher(manager, true); } if (p.config_avoidance.enable_module) { auto manager = std::make_shared(this, "avoidance", p.config_avoidance); - planner_manager_->registerSceneModuleManager(manager); - path_candidate_publishers_.emplace( - "avoidance", create_publisher(path_candidate_name_space + "avoidance", 1)); - path_reference_publishers_.emplace( - "avoidance", create_publisher(path_reference_name_space + "avoidance", 1)); + register_and_create_publisher(manager, true); } if (p.config_avoidance_by_lc.enable_module) { auto manager = std::make_shared( this, "avoidance_by_lane_change", p.config_avoidance_by_lc); - planner_manager_->registerSceneModuleManager(manager); - path_candidate_publishers_.emplace( - "avoidance_by_lane_change", - create_publisher(path_candidate_name_space + "avoidance_by_lane_change", 1)); - path_reference_publishers_.emplace( - "avoidance_by_lane_change", - create_publisher(path_reference_name_space + "avoidance_by_lane_change", 1)); + register_and_create_publisher(manager, true); } if (p.config_dynamic_avoidance.enable_module) { auto manager = std::make_shared( this, "dynamic_avoidance", p.config_dynamic_avoidance); - planner_manager_->registerSceneModuleManager(manager); + register_and_create_publisher(manager, false); } }