From 1b6cc51fefefae23491d31071a968f86c16ef9d8 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Thu, 14 Dec 2023 16:03:09 +0900 Subject: [PATCH] refactor(lane_change): separate lane change and external request (#5850) * refactor(lane_change): separate lane change and external request Signed-off-by: Muhammad Zulfaqar Azmi * refactored external request lane change Signed-off-by: Muhammad Zulfaqar Azmi * separate on external request Signed-off-by: Muhammad Zulfaqar Azmi * style(pre-commit): autofix * delete external request Signed-off-by: Muhammad Zulfaqar Azmi * fix interface Signed-off-by: Muhammad Zulfaqar Azmi * fix external lane change couldn't be initialize Signed-off-by: Muhammad Zulfaqar Azmi * fix documents Signed-off-by: Muhammad Zulfaqar Azmi * fix link in README Signed-off-by: Muhammad Zulfaqar Azmi * fix based on comments Signed-off-by: Zulfaqar Azmi * add ament auto package Signed-off-by: Zulfaqar Azmi * Update planning/behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> * Update planning/behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> * fix test Signed-off-by: Muhammad Zulfaqar Azmi * fix AbLC test Signed-off-by: Muhammad Zulfaqar Azmi --------- Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Zulfaqar Azmi Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> --- planning/.pages | 2 +- .../interface.hpp | 2 +- .../manager.hpp | 2 +- .../scene.hpp | 2 +- .../package.xml | 1 + ...t_behavior_path_planner_node_interface.cpp | 5 +- .../CMakeLists.txt | 25 ++++ .../manager.hpp | 54 ++++++++ .../scene.hpp} | 8 +- .../package.xml | 39 ++++++ .../plugins.xml | 4 + .../src/manager.cpp | 49 +++++++ .../src/scene.cpp} | 2 +- ...t_behavior_path_planner_node_interface.cpp | 128 ++++++++++++++++++ .../CMakeLists.txt | 29 ++++ .../README.md} | 36 ++--- .../config}/lane_change.param.yaml | 0 .../images}/lane_change-abort.png | Bin .../images}/lane_change-cancel.png | Bin .../lane_change-candidate_path_samples.png | Bin .../lane_change-cant_cancel_no_abort.png | Bin .../images}/lane_change-debug-1.png | Bin .../images}/lane_change-debug-2.png | Bin .../images}/lane_change-debug-3.png | Bin ...nable_collision_check_at_prepare_phase.png | Bin .../images}/lane_change-intersection_case.png | Bin .../lane_change-lane_change_phases.png | Bin .../lane_change-stop_at_terminal.drawio.svg | 0 ...hange-stop_at_terminal_no_block.drawio.svg | 0 ...hange-stop_far_from_target_lane.drawio.svg | 0 ...ane_change-stop_not_at_terminal.drawio.svg | 0 ..._at_terminal_no_blocking_object.drawio.svg | 0 .../lane_change-when_cannot_change_lanes.png | Bin .../images}/lane_change.drawio.svg | 0 .../images}/lane_objects.drawio.svg | 0 .../interface.hpp | 17 ++- .../manager.hpp | 31 +---- .../scene.hpp} | 8 +- .../utils}/base_class.hpp | 12 +- .../utils/data_structs.hpp} | 6 +- .../utils/markers.hpp} | 12 +- .../utils/path.hpp} | 10 +- .../utils}/utils.hpp | 12 +- .../package.xml | 38 ++++++ .../plugins.xml | 6 + .../src}/interface.cpp | 7 +- .../src}/manager.cpp | 17 +-- .../src/scene.cpp} | 4 +- .../src/utils/markers.cpp} | 2 +- .../src/utils}/utils.cpp | 6 +- ...t_behavior_path_planner_node_interface.cpp | 126 +++++++++++++++++ .../test/test_lane_change_utils.cpp | 3 +- planning/behavior_path_planner/CMakeLists.txt | 14 -- planning/behavior_path_planner/plugins.xml | 4 - ...t_behavior_path_planner_node_interface.cpp | 7 +- 55 files changed, 585 insertions(+), 145 deletions(-) create mode 100644 planning/behavior_path_external_request_lane_change_module/CMakeLists.txt create mode 100644 planning/behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/manager.hpp rename planning/{behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request.hpp => behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/scene.hpp} (79%) create mode 100644 planning/behavior_path_external_request_lane_change_module/package.xml create mode 100644 planning/behavior_path_external_request_lane_change_module/plugins.xml create mode 100644 planning/behavior_path_external_request_lane_change_module/src/manager.cpp rename planning/{behavior_path_planner/src/scene_module/lane_change/external_request.cpp => behavior_path_external_request_lane_change_module/src/scene.cpp} (92%) create mode 100644 planning/behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp create mode 100644 planning/behavior_path_lane_change_module/CMakeLists.txt rename planning/{behavior_path_planner/docs/behavior_path_planner_lane_change_design.md => behavior_path_lane_change_module/README.md} (96%) rename planning/{behavior_path_planner/config/lane_change => behavior_path_lane_change_module/config}/lane_change.param.yaml (100%) rename planning/{behavior_path_planner/image/lane_change/cancel_and_abort => behavior_path_lane_change_module/images}/lane_change-abort.png (100%) rename planning/{behavior_path_planner/image/lane_change/cancel_and_abort => behavior_path_lane_change_module/images}/lane_change-cancel.png (100%) rename planning/{behavior_path_planner/image/lane_change => behavior_path_lane_change_module/images}/lane_change-candidate_path_samples.png (100%) rename planning/{behavior_path_planner/image/lane_change/cancel_and_abort => behavior_path_lane_change_module/images}/lane_change-cant_cancel_no_abort.png (100%) rename planning/{behavior_path_planner/image/lane_change => behavior_path_lane_change_module/images}/lane_change-debug-1.png (100%) rename planning/{behavior_path_planner/image/lane_change => behavior_path_lane_change_module/images}/lane_change-debug-2.png (100%) rename planning/{behavior_path_planner/image/lane_change => behavior_path_lane_change_module/images}/lane_change-debug-3.png (100%) rename planning/{behavior_path_planner/image/lane_change => behavior_path_lane_change_module/images}/lane_change-enable_collision_check_at_prepare_phase.png (100%) rename planning/{behavior_path_planner/image/lane_change => behavior_path_lane_change_module/images}/lane_change-intersection_case.png (100%) rename planning/{behavior_path_planner/image/lane_change => behavior_path_lane_change_module/images}/lane_change-lane_change_phases.png (100%) rename planning/{behavior_path_planner/image/lane_change => behavior_path_lane_change_module/images}/lane_change-stop_at_terminal.drawio.svg (100%) rename planning/{behavior_path_planner/image/lane_change => behavior_path_lane_change_module/images}/lane_change-stop_at_terminal_no_block.drawio.svg (100%) rename planning/{behavior_path_planner/image/lane_change => behavior_path_lane_change_module/images}/lane_change-stop_far_from_target_lane.drawio.svg (100%) rename planning/{behavior_path_planner/image/lane_change => behavior_path_lane_change_module/images}/lane_change-stop_not_at_terminal.drawio.svg (100%) rename planning/{behavior_path_planner/image/lane_change => behavior_path_lane_change_module/images}/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg (100%) rename planning/{behavior_path_planner/image/lane_change => behavior_path_lane_change_module/images}/lane_change-when_cannot_change_lanes.png (100%) rename planning/{behavior_path_planner/image/lane_change => behavior_path_lane_change_module/images}/lane_change.drawio.svg (100%) rename planning/{behavior_path_planner/image/lane_change => behavior_path_lane_change_module/images}/lane_objects.drawio.svg (100%) rename planning/{behavior_path_planner/include/behavior_path_planner/scene_module/lane_change => behavior_path_lane_change_module/include/behavior_path_lane_change_module}/interface.hpp (87%) rename planning/{behavior_path_planner/include/behavior_path_planner/scene_module/lane_change => behavior_path_lane_change_module/include/behavior_path_lane_change_module}/manager.hpp (68%) rename planning/{behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp => behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp} (96%) rename planning/{behavior_path_planner/include/behavior_path_planner/scene_module/lane_change => behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils}/base_class.hpp (95%) rename planning/{behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp => behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp} (96%) rename planning/{behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp => behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp} (80%) rename planning/{behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_path.hpp => behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp} (82%) rename planning/{behavior_path_planner/include/behavior_path_planner/utils/lane_change => behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils}/utils.hpp (96%) create mode 100644 planning/behavior_path_lane_change_module/package.xml create mode 100644 planning/behavior_path_lane_change_module/plugins.xml rename planning/{behavior_path_planner/src/scene_module/lane_change => behavior_path_lane_change_module/src}/interface.cpp (98%) rename planning/{behavior_path_planner/src/scene_module/lane_change => behavior_path_lane_change_module/src}/manager.cpp (95%) rename planning/{behavior_path_planner/src/scene_module/lane_change/normal.cpp => behavior_path_lane_change_module/src/scene.cpp} (99%) rename planning/{behavior_path_planner/src/marker_utils/lane_change/debug.cpp => behavior_path_lane_change_module/src/utils/markers.cpp} (98%) rename planning/{behavior_path_planner/src/utils/lane_change => behavior_path_lane_change_module/src/utils}/utils.cpp (99%) create mode 100644 planning/behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp rename planning/{behavior_path_planner => behavior_path_lane_change_module}/test/test_lane_change_utils.cpp (95%) diff --git a/planning/.pages b/planning/.pages index 7cbd36d2ca2d0..6ae8abe240d9b 100644 --- a/planning/.pages +++ b/planning/.pages @@ -14,7 +14,7 @@ nav: - 'Avoidance by Lane Change': planning/behavior_path_planner/docs/behavior_path_planner_avoidance_by_lane_change_design - 'Dynamic Avoidance': planning/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design - 'Goal Planner': planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design - - 'Lane Change': planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design + - 'Lane Change': planning/behavior_path_lane_change_module - 'Side Shift': planning/behavior_path_planner/docs/behavior_path_planner_side_shift_design - 'Start Planner': planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design - 'Behavior Velocity Planner': diff --git a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/interface.hpp b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/interface.hpp index 0bc08ccd400ca..016a6b2977701 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/interface.hpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/interface.hpp @@ -17,7 +17,7 @@ #include "behavior_path_avoidance_by_lane_change_module/data_structs.hpp" #include "behavior_path_avoidance_by_lane_change_module/scene.hpp" -#include "behavior_path_planner/scene_module/lane_change/interface.hpp" +#include "behavior_path_lane_change_module/interface.hpp" #include diff --git a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/manager.hpp b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/manager.hpp index b1beae9c6fc03..d996555365166 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/manager.hpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/manager.hpp @@ -17,7 +17,7 @@ #include "behavior_path_avoidance_by_lane_change_module/data_structs.hpp" #include "behavior_path_avoidance_by_lane_change_module/interface.hpp" -#include "behavior_path_planner/scene_module/lane_change/manager.hpp" +#include "behavior_path_lane_change_module/manager.hpp" #include diff --git a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp index 1e667c207c64f..f22291c99b8e7 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp @@ -16,7 +16,7 @@ #define BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__SCENE_HPP_ #include "behavior_path_avoidance_by_lane_change_module/data_structs.hpp" -#include "behavior_path_planner/scene_module/lane_change/normal.hpp" +#include "behavior_path_lane_change_module/scene.hpp" #include diff --git a/planning/behavior_path_avoidance_by_lane_change_module/package.xml b/planning/behavior_path_avoidance_by_lane_change_module/package.xml index da51dd9f235dc..3a4f42f2d2a6c 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/package.xml +++ b/planning/behavior_path_avoidance_by_lane_change_module/package.xml @@ -19,6 +19,7 @@ eigen3_cmake_module behavior_path_avoidance_module + behavior_path_lane_change_module behavior_path_planner behavior_path_planner_common motion_utils diff --git a/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp index 2264669fdcdbe..3041ae7e1112e 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -47,6 +47,8 @@ std::shared_ptr generateNode() ament_index_cpp::get_package_share_directory("planning_test_utils"); const auto behavior_path_planner_dir = ament_index_cpp::get_package_share_directory("behavior_path_planner"); + const auto behavior_path_lane_change_module_dir = + ament_index_cpp::get_package_share_directory("behavior_path_lane_change_module"); std::vector module_names; module_names.emplace_back("behavior_path_planner::AvoidanceByLaneChangeModuleManager"); @@ -63,8 +65,7 @@ std::shared_ptr generateNode() behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", - ament_index_cpp::get_package_share_directory("behavior_path_planner") + - "/config/lane_change/lane_change.param.yaml", + behavior_path_lane_change_module_dir + "/config/lane_change.param.yaml", ament_index_cpp::get_package_share_directory("behavior_path_avoidance_module") + "/config/avoidance.param.yaml", ament_index_cpp::get_package_share_directory("behavior_path_avoidance_by_lane_change_module") + diff --git a/planning/behavior_path_external_request_lane_change_module/CMakeLists.txt b/planning/behavior_path_external_request_lane_change_module/CMakeLists.txt new file mode 100644 index 0000000000000..86dc0ccb61330 --- /dev/null +++ b/planning/behavior_path_external_request_lane_change_module/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_path_external_request_lane_change_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_path_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/manager.cpp + src/scene.cpp +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gmock(test_${PROJECT_NAME} + test/test_behavior_path_planner_node_interface.cpp + ) + + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ) + + target_include_directories(test_${PROJECT_NAME} PRIVATE src) +endif() + +ament_auto_package() diff --git a/planning/behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/manager.hpp b/planning/behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/manager.hpp new file mode 100644 index 0000000000000..dfcc4f61d8241 --- /dev/null +++ b/planning/behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/manager.hpp @@ -0,0 +1,54 @@ +// Copyright 2023 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_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__MANAGER_HPP_ +#define BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__MANAGER_HPP_ + +#include "behavior_path_lane_change_module/manager.hpp" +#include "route_handler/route_handler.hpp" + +#include + +#include +#include + +namespace behavior_path_planner +{ +class ExternalRequestLaneChangeRightModuleManager : public LaneChangeModuleManager +{ +public: + ExternalRequestLaneChangeRightModuleManager() + : LaneChangeModuleManager( + "external_request_lane_change_right", route_handler::Direction::RIGHT, + LaneChangeModuleType::EXTERNAL_REQUEST) + { + } + std::unique_ptr createNewSceneModuleInstance() override; +}; + +class ExternalRequestLaneChangeLeftModuleManager : public LaneChangeModuleManager +{ +public: + ExternalRequestLaneChangeLeftModuleManager() + + : LaneChangeModuleManager( + "external_request_lane_change_left", route_handler::Direction::LEFT, + LaneChangeModuleType::EXTERNAL_REQUEST) + { + } + std::unique_ptr createNewSceneModuleInstance() override; +}; +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__MANAGER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request.hpp b/planning/behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/scene.hpp similarity index 79% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request.hpp rename to planning/behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/scene.hpp index d9c77d6db337e..c23d6f2f3996c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request.hpp +++ b/planning/behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/scene.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__EXTERNAL_REQUEST_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__EXTERNAL_REQUEST_HPP_ +#ifndef BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__SCENE_HPP_ +#define BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__SCENE_HPP_ -#include "behavior_path_planner/scene_module/lane_change/normal.hpp" +#include "behavior_path_lane_change_module/scene.hpp" #include @@ -35,4 +35,4 @@ class ExternalRequestLaneChange : public NormalLaneChange }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__EXTERNAL_REQUEST_HPP_ +#endif // BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__SCENE_HPP_ diff --git a/planning/behavior_path_external_request_lane_change_module/package.xml b/planning/behavior_path_external_request_lane_change_module/package.xml new file mode 100644 index 0000000000000..16216aa1f71e9 --- /dev/null +++ b/planning/behavior_path_external_request_lane_change_module/package.xml @@ -0,0 +1,39 @@ + + + + behavior_path_external_request_lane_change_module + 0.1.0 + The behavior_path_external_request_lane_change_module package + + Fumiya Watanabe + Zulfaqar Azmi + Kosuke Takeuchi + Tomoya Kimura + Shumpei Wakabayashi + Tomohito Ando + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + behavior_path_lane_change_module + behavior_path_planner + behavior_path_planner_common + motion_utils + pluginlib + rclcpp + rtc_interface + tier4_autoware_utils + tier4_planning_msgs + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_path_external_request_lane_change_module/plugins.xml b/planning/behavior_path_external_request_lane_change_module/plugins.xml new file mode 100644 index 0000000000000..f3cc686837c38 --- /dev/null +++ b/planning/behavior_path_external_request_lane_change_module/plugins.xml @@ -0,0 +1,4 @@ + + + + diff --git a/planning/behavior_path_external_request_lane_change_module/src/manager.cpp b/planning/behavior_path_external_request_lane_change_module/src/manager.cpp new file mode 100644 index 0000000000000..3cfe7aa51f0f1 --- /dev/null +++ b/planning/behavior_path_external_request_lane_change_module/src/manager.cpp @@ -0,0 +1,49 @@ +// Copyright 2023 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_external_request_lane_change_module/manager.hpp" + +#include "behavior_path_external_request_lane_change_module/scene.hpp" +#include "behavior_path_lane_change_module/interface.hpp" + +namespace behavior_path_planner +{ + +std::unique_ptr +ExternalRequestLaneChangeRightModuleManager::createNewSceneModuleInstance() +{ + return std::make_unique( + name_, *node_, parameters_, rtc_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_, + std::make_unique(parameters_, direction_)); +} + +std::unique_ptr +ExternalRequestLaneChangeLeftModuleManager::createNewSceneModuleInstance() +{ + return std::make_unique( + name_, *node_, parameters_, rtc_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_, + std::make_unique(parameters_, direction_)); +} + +} // namespace behavior_path_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_path_planner::ExternalRequestLaneChangeRightModuleManager, + behavior_path_planner::SceneModuleManagerInterface) +PLUGINLIB_EXPORT_CLASS( + behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager, + behavior_path_planner::SceneModuleManagerInterface) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/external_request.cpp b/planning/behavior_path_external_request_lane_change_module/src/scene.cpp similarity index 92% rename from planning/behavior_path_planner/src/scene_module/lane_change/external_request.cpp rename to planning/behavior_path_external_request_lane_change_module/src/scene.cpp index 3a304a9b5bb53..913266bf79fa3 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/external_request.cpp +++ b/planning/behavior_path_external_request_lane_change_module/src/scene.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/lane_change/external_request.hpp" +#include "behavior_path_external_request_lane_change_module/scene.hpp" #include diff --git a/planning/behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp new file mode 100644 index 0000000000000..ff5be388704da --- /dev/null +++ b/planning/behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -0,0 +1,128 @@ +// Copyright 2023 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 "ament_index_cpp/get_package_share_directory.hpp" +#include "behavior_path_planner/behavior_path_planner_node.hpp" +#include "planning_interface_test_manager/planning_interface_test_manager.hpp" +#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" + +#include + +#include +#include + +using behavior_path_planner::BehaviorPathPlannerNode; +using planning_test_utils::PlanningInterfaceTestManager; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + + // set subscriber with topic name: behavior_path_planner → test_node_ + test_manager->setPathWithLaneIdSubscriber("behavior_path_planner/output/path"); + + // set behavior_path_planner's input topic name(this topic is changed to test node) + test_manager->setRouteInputTopicName("behavior_path_planner/input/route"); + + test_manager->setInitialPoseTopicName("behavior_path_planner/input/odometry"); + + return test_manager; +} + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto behavior_path_planner_dir = + ament_index_cpp::get_package_share_directory("behavior_path_planner"); + const auto behavior_path_lane_change_module_dir = + ament_index_cpp::get_package_share_directory("behavior_path_lane_change_module"); + + std::vector module_names; + module_names.emplace_back("behavior_path_planner::ExternalRequestLaneChangeRightModuleManager"); + module_names.emplace_back("behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager"); + + std::vector params; + params.emplace_back("launch_modules", module_names); + node_options.parameter_overrides(params); + + test_utils::updateNodeOptions( + node_options, { + planning_test_utils_dir + "/config/test_common.param.yaml", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", + behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", + behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", + behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", + behavior_path_lane_change_module_dir + "/config/lane_change.param.yaml", + }); + + return std::make_shared(node_options); +} + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ + // publish necessary topics from test_manager + test_manager->publishInitialPose(test_target_node, "behavior_path_planner/input/odometry"); + test_manager->publishAcceleration(test_target_node, "behavior_path_planner/input/accel"); + test_manager->publishPredictedObjects(test_target_node, "behavior_path_planner/input/perception"); + test_manager->publishOccupancyGrid( + test_target_node, "behavior_path_planner/input/occupancy_grid_map"); + test_manager->publishLaneDrivingScenario( + test_target_node, "behavior_path_planner/input/scenario"); + test_manager->publishMap(test_target_node, "behavior_path_planner/input/vector_map"); + test_manager->publishCostMap(test_target_node, "behavior_path_planner/input/costmap"); + test_manager->publishOperationModeState(test_target_node, "system/operation_mode/state"); + test_manager->publishLateralOffset( + test_target_node, "behavior_path_planner/input/lateral_offset"); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty route + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalRoute(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testRouteWithInvalidEgoPose(test_target_node)); + + rclcpp::shutdown(); +} diff --git a/planning/behavior_path_lane_change_module/CMakeLists.txt b/planning/behavior_path_lane_change_module/CMakeLists.txt new file mode 100644 index 0000000000000..e5bce68ebfa44 --- /dev/null +++ b/planning/behavior_path_lane_change_module/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_path_lane_change_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_path_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/interface.cpp + src/manager.cpp + src/scene.cpp + src/utils/markers.cpp + src/utils/utils.cpp +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gmock(test_${PROJECT_NAME} + test/test_behavior_path_planner_node_interface.cpp + test/test_lane_change_utils.cpp + ) + + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ) + + target_include_directories(test_${PROJECT_NAME} PRIVATE src) +endif() + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md b/planning/behavior_path_lane_change_module/README.md similarity index 96% rename from planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md rename to planning/behavior_path_lane_change_module/README.md index ee74f700c4d42..a1fa2068b9ab5 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md +++ b/planning/behavior_path_lane_change_module/README.md @@ -15,7 +15,7 @@ The Lane Change module is activated when lane change is needed and can be safely The lane change candidate path is divided into two phases: preparation and lane-changing. The following figure illustrates each phase of the lane change candidate path. -![lane-change-phases](../image/lane_change/lane_change-lane_change_phases.png) +![lane-change-phases](./images/lane_change-lane_change_phases.png) ### Preparation phase @@ -62,7 +62,7 @@ Note that when the `current_velocity` is lower than `minimum_lane_changing_veloc The following figure illustrates when `longitudinal_acceleration_sampling_num = 4`. Assuming that `maximum_deceleration = 1.0` then `a0 == 0.0 == no deceleration`, `a1 == 0.25`, `a2 == 0.5`, `a3 == 0.75` and `a4 == 1.0 == maximum_deceleration`. `a0` is the expected lane change trajectories should ego vehicle do not decelerate, and `a1`'s path is the expected lane change trajectories should ego vehicle decelerate at `0.25 m/s^2`. -![path_samples](../image/lane_change/lane_change-candidate_path_samples.png) +![path_samples](./images/lane_change-candidate_path_samples.png) Which path will be chosen will depend on validity and collision check. @@ -155,21 +155,21 @@ stop #### Candidate Path's Safety check -See [safety check utils explanation](../docs/behavior_path_planner_safety_check.md) +See [safety check utils explanation](../behavior_path_planner/docs/behavior_path_planner_safety_check.md) #### Objects selection and classification First, we divide the target objects into obstacles in the target lane, obstacles in the current lane, and obstacles in other lanes. Target lane indicates the lane that the ego vehicle is going to reach after the lane change and current lane mean the current lane where the ego vehicle is following before the lane change. Other lanes are lanes that do not belong to the target and current lanes. The following picture describes objects on each lane. Note that users can remove objects either on current and other lanes from safety check by changing the flag, which are `check_objects_on_current_lanes` and `check_objects_on_other_lanes`. -![object lanes](../image/lane_change/lane_objects.drawio.svg) +![object lanes](./images/lane_objects.drawio.svg) -Furthermore, to change lanes behind a vehicle waiting at a traffic light, we skip the safety check for the stopping vehicles near the traffic light.　The explanation for parked car detection is written in [documentation for avoidance module](../docs/behavior_path_planner_avoidance_design.md). +Furthermore, to change lanes behind a vehicle waiting at a traffic light, we skip the safety check for the stopping vehicles near the traffic light.　The explanation for parked car detection is written in [documentation for avoidance module](../behavior_path_planner/docs/behavior_path_planner_avoidance_design.md). ##### Collision check in prepare phase The ego vehicle may need to secure ample inter-vehicle distance ahead of the target vehicle before attempting a lane change. The flag `enable_collision_check_at_prepare_phase` can be enabled to gain this behavior. The following image illustrates the differences between the `false` and `true` cases. -![enable collision check at prepare phase](../image/lane_change/lane_change-enable_collision_check_at_prepare_phase.png) +![enable collision check at prepare phase](./images/lane_change-enable_collision_check_at_prepare_phase.png) The parameter `prepare_phase_ignore_target_speed_thresh` can be configured to ignore the prepare phase collision check for targets whose speeds are less than a specific threshold, such as stationary or very slow-moving objects. @@ -184,7 +184,7 @@ minimum_lane_change_distance = minimum_prepare_length + minimum_lane_changing_ve The following figure illustrates when the lane is blocked in multiple lane changes cases. -![multiple-lane-changes](../image/lane_change/lane_change-when_cannot_change_lanes.png) +![multiple-lane-changes](./images/lane_change-when_cannot_change_lanes.png) #### Stopping position when an object exists ahead @@ -195,25 +195,25 @@ The position to be stopped depends on the situation, such as when the lane chang Regardless of the presence or absence of objects in the lane change target lane, stop by keeping the distance necessary for lane change to the object ahead. -![stop_at_terminal_no_block](../image/lane_change/lane_change-stop_at_terminal_no_block.drawio.svg) +![stop_at_terminal_no_block](./images/lane_change-stop_at_terminal_no_block.drawio.svg) -![stop_at_terminal](../image/lane_change/lane_change-stop_at_terminal.drawio.svg) +![stop_at_terminal](./images/lane_change-stop_at_terminal.drawio.svg) ##### When the ego vehicle is not near the end of the lane change If there are NO objects in the lane change section of the target lane, stop by keeping the distance necessary for lane change to the object ahead. -![stop_not_at_terminal_no_blocking_object](../image/lane_change/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg) +![stop_not_at_terminal_no_blocking_object](./images/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg) If there are objects in the lane change section of the target lane, stop WITHOUT keeping the distance necessary for lane change to the object ahead. -![stop_not_at_terminal](../image/lane_change/lane_change-stop_not_at_terminal.drawio.svg) +![stop_not_at_terminal](./images/lane_change-stop_not_at_terminal.drawio.svg) ##### When the target lane is far away When the target lane for lane change is far away and not next to the current lane, do not keep the distance necessary for lane change to the object ahead. -![stop_far_from_target_lane](../image/lane_change/lane_change-stop_far_from_target_lane.drawio.svg) +![stop_far_from_target_lane](./images/lane_change-stop_far_from_target_lane.drawio.svg) ### Lane Change When Stuck @@ -292,19 +292,19 @@ The function can be enabled by setting `enable_on_prepare_phase` to `true`. The following image illustrates the cancel process. -![cancel](../image/lane_change/cancel_and_abort/lane_change-cancel.png) +![cancel](./images/lane_change-cancel.png) #### Abort Assume the ego vehicle has already departed from the current lane. In that case, it is dangerous to cancel the path, and it will cause the ego vehicle to change the heading direction abruptly. In this case, planning a trajectory that allows the ego vehicle to return to the current path while minimizing the heading changes is necessary. In this case, the lane change module will generate an abort path. The following images show an example of the abort path. Do note that the function DOESN'T GUARANTEE a safe abort process, as it didn't check the presence of the surrounding objects and/or their reactions. The function can be enable manually by setting both `enable_on_prepare_phase` and `enable_on_lane_changing_phase` to `true`. The parameter `max_lateral_jerk` need to be set to a high value in order for it to work. -![abort](../image/lane_change/cancel_and_abort/lane_change-abort.png) +![abort](./images/lane_change-abort.png) #### Stop/Cruise The last behavior will also occur if the ego vehicle has departed from the current lane. If the abort function is disabled or the abort is no longer possible, the ego vehicle will attempt to stop or transition to the obstacle cruise mode. Do note that the module DOESN'T GUARANTEE safe maneuver due to the unexpected behavior that might've occurred during these critical scenarios. The following images illustrate the situation. -![stop](../image/lane_change/cancel_and_abort/lane_change-cant_cancel_no_abort.png) +![stop](./images/lane_change-cant_cancel_no_abort.png) ## Parameters @@ -451,11 +451,11 @@ Then add the marker in `rviz2`. -![debug](../image/lane_change/lane_change-debug-1.png) +![debug](./images/lane_change-debug-1.png) -![debug2](../image/lane_change/lane_change-debug-2.png) +![debug2](./images/lane_change-debug-2.png) -![debug3](../image/lane_change/lane_change-debug-3.png) +![debug3](./images/lane_change-debug-3.png) Available information diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_lane_change_module/config/lane_change.param.yaml similarity index 100% rename from planning/behavior_path_planner/config/lane_change/lane_change.param.yaml rename to planning/behavior_path_lane_change_module/config/lane_change.param.yaml diff --git a/planning/behavior_path_planner/image/lane_change/cancel_and_abort/lane_change-abort.png b/planning/behavior_path_lane_change_module/images/lane_change-abort.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/cancel_and_abort/lane_change-abort.png rename to planning/behavior_path_lane_change_module/images/lane_change-abort.png diff --git a/planning/behavior_path_planner/image/lane_change/cancel_and_abort/lane_change-cancel.png b/planning/behavior_path_lane_change_module/images/lane_change-cancel.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/cancel_and_abort/lane_change-cancel.png rename to planning/behavior_path_lane_change_module/images/lane_change-cancel.png diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-candidate_path_samples.png b/planning/behavior_path_lane_change_module/images/lane_change-candidate_path_samples.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-candidate_path_samples.png rename to planning/behavior_path_lane_change_module/images/lane_change-candidate_path_samples.png diff --git a/planning/behavior_path_planner/image/lane_change/cancel_and_abort/lane_change-cant_cancel_no_abort.png b/planning/behavior_path_lane_change_module/images/lane_change-cant_cancel_no_abort.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/cancel_and_abort/lane_change-cant_cancel_no_abort.png rename to planning/behavior_path_lane_change_module/images/lane_change-cant_cancel_no_abort.png diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-debug-1.png b/planning/behavior_path_lane_change_module/images/lane_change-debug-1.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-debug-1.png rename to planning/behavior_path_lane_change_module/images/lane_change-debug-1.png diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-debug-2.png b/planning/behavior_path_lane_change_module/images/lane_change-debug-2.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-debug-2.png rename to planning/behavior_path_lane_change_module/images/lane_change-debug-2.png diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-debug-3.png b/planning/behavior_path_lane_change_module/images/lane_change-debug-3.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-debug-3.png rename to planning/behavior_path_lane_change_module/images/lane_change-debug-3.png diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-enable_collision_check_at_prepare_phase.png b/planning/behavior_path_lane_change_module/images/lane_change-enable_collision_check_at_prepare_phase.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-enable_collision_check_at_prepare_phase.png rename to planning/behavior_path_lane_change_module/images/lane_change-enable_collision_check_at_prepare_phase.png diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-intersection_case.png b/planning/behavior_path_lane_change_module/images/lane_change-intersection_case.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-intersection_case.png rename to planning/behavior_path_lane_change_module/images/lane_change-intersection_case.png diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-lane_change_phases.png b/planning/behavior_path_lane_change_module/images/lane_change-lane_change_phases.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-lane_change_phases.png rename to planning/behavior_path_lane_change_module/images/lane_change-lane_change_phases.png diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-stop_at_terminal.drawio.svg b/planning/behavior_path_lane_change_module/images/lane_change-stop_at_terminal.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-stop_at_terminal.drawio.svg rename to planning/behavior_path_lane_change_module/images/lane_change-stop_at_terminal.drawio.svg diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-stop_at_terminal_no_block.drawio.svg b/planning/behavior_path_lane_change_module/images/lane_change-stop_at_terminal_no_block.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-stop_at_terminal_no_block.drawio.svg rename to planning/behavior_path_lane_change_module/images/lane_change-stop_at_terminal_no_block.drawio.svg diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-stop_far_from_target_lane.drawio.svg b/planning/behavior_path_lane_change_module/images/lane_change-stop_far_from_target_lane.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-stop_far_from_target_lane.drawio.svg rename to planning/behavior_path_lane_change_module/images/lane_change-stop_far_from_target_lane.drawio.svg diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-stop_not_at_terminal.drawio.svg b/planning/behavior_path_lane_change_module/images/lane_change-stop_not_at_terminal.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-stop_not_at_terminal.drawio.svg rename to planning/behavior_path_lane_change_module/images/lane_change-stop_not_at_terminal.drawio.svg diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg b/planning/behavior_path_lane_change_module/images/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg rename to planning/behavior_path_lane_change_module/images/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-when_cannot_change_lanes.png b/planning/behavior_path_lane_change_module/images/lane_change-when_cannot_change_lanes.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-when_cannot_change_lanes.png rename to planning/behavior_path_lane_change_module/images/lane_change-when_cannot_change_lanes.png diff --git a/planning/behavior_path_planner/image/lane_change/lane_change.drawio.svg b/planning/behavior_path_lane_change_module/images/lane_change.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change.drawio.svg rename to planning/behavior_path_lane_change_module/images/lane_change.drawio.svg diff --git a/planning/behavior_path_planner/image/lane_change/lane_objects.drawio.svg b/planning/behavior_path_lane_change_module/images/lane_objects.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_objects.drawio.svg rename to planning/behavior_path_lane_change_module/images/lane_objects.drawio.svg diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp similarity index 87% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp rename to planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp index b7cef007d6d9f..fd9a979ee0f76 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// Copyright 2023 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. @@ -12,14 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__INTERFACE_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__INTERFACE_HPP_ +#ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__INTERFACE_HPP_ +#define BEHAVIOR_PATH_LANE_CHANGE_MODULE__INTERFACE_HPP_ -#include "behavior_path_planner/scene_module/lane_change/base_class.hpp" -#include "behavior_path_planner/scene_module/lane_change/external_request.hpp" -#include "behavior_path_planner/scene_module/lane_change/normal.hpp" -#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" -#include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" +#include "behavior_path_lane_change_module/scene.hpp" +#include "behavior_path_lane_change_module/utils/base_class.hpp" +#include "behavior_path_lane_change_module/utils/data_structs.hpp" +#include "behavior_path_lane_change_module/utils/path.hpp" #include "behavior_path_planner_common/interface/scene_module_interface.hpp" #include "behavior_path_planner_common/turn_signal_decider.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -146,4 +145,4 @@ class LaneChangeInterface : public SceneModuleInterface }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__INTERFACE_HPP_ +#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__INTERFACE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/manager.hpp similarity index 68% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp rename to planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/manager.hpp index 024c6685b28c4..b34f691bd2b0d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/manager.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__MANAGER_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__MANAGER_HPP_ +#ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__MANAGER_HPP_ +#define BEHAVIOR_PATH_LANE_CHANGE_MODULE__MANAGER_HPP_ -#include "behavior_path_planner/scene_module/lane_change/interface.hpp" +#include "behavior_path_lane_change_module/utils/data_structs.hpp" #include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include "route_handler/route_handler.hpp" @@ -23,7 +23,6 @@ #include #include -#include #include namespace behavior_path_planner @@ -72,28 +71,6 @@ class LaneChangeLeftModuleManager : public LaneChangeModuleManager { } }; - -class ExternalRequestLaneChangeRightModuleManager : public LaneChangeModuleManager -{ -public: - ExternalRequestLaneChangeRightModuleManager() - : LaneChangeModuleManager( - "external_request_lane_change_right", route_handler::Direction::RIGHT, - LaneChangeModuleType::EXTERNAL_REQUEST) - { - } -}; - -class ExternalRequestLaneChangeLeftModuleManager : public LaneChangeModuleManager -{ -public: - ExternalRequestLaneChangeLeftModuleManager() - : LaneChangeModuleManager( - "external_request_lane_change_left", route_handler::Direction::LEFT, - LaneChangeModuleType::EXTERNAL_REQUEST) - { - } -}; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__MANAGER_HPP_ +#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__MANAGER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp similarity index 96% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp rename to planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp index 61caf7b2b393f..50766621b2f44 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp @@ -11,10 +11,10 @@ // 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__LANE_CHANGE__NORMAL_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__NORMAL_HPP_ +#ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__SCENE_HPP_ +#define BEHAVIOR_PATH_LANE_CHANGE_MODULE__SCENE_HPP_ -#include "behavior_path_planner/scene_module/lane_change/base_class.hpp" +#include "behavior_path_lane_change_module/utils/base_class.hpp" #include #include @@ -180,4 +180,4 @@ class NormalLaneChange : public LaneChangeBase double stop_time_{0.0}; }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__NORMAL_HPP_ +#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__SCENE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp similarity index 95% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp rename to planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp index be49ec5b2e740..a7a35f07e5f30 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp @@ -11,12 +11,12 @@ // 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__LANE_CHANGE__BASE_CLASS_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__BASE_CLASS_HPP_ +#ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_ +#define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_ -#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" -#include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" -#include "behavior_path_planner/utils/lane_change/utils.hpp" +#include "behavior_path_lane_change_module/utils/data_structs.hpp" +#include "behavior_path_lane_change_module/utils/path.hpp" +#include "behavior_path_lane_change_module/utils/utils.hpp" #include "behavior_path_planner_common/interface/scene_module_interface.hpp" #include "behavior_path_planner_common/turn_signal_decider.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -269,4 +269,4 @@ class LaneChangeBase mutable rclcpp::Clock clock_{RCL_ROS_TIME}; }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__BASE_CLASS_HPP_ +#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp similarity index 96% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp rename to planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp index debeef5ec573f..dd7760d31eaa7 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp @@ -11,8 +11,8 @@ // 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__UTILS__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ +#ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_ +#define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_ #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -211,4 +211,4 @@ struct PathSafetyStatus }; } // namespace behavior_path_planner::data::lane_change -#endif // BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ +#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp similarity index 80% rename from planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp rename to planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp index 96f0851ed33c0..d6deecd4f46fa 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__MARKER_UTILS__LANE_CHANGE__DEBUG_HPP_ -#define BEHAVIOR_PATH_PLANNER__MARKER_UTILS__LANE_CHANGE__DEBUG_HPP_ +#ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ +#define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ -#include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" -#include "behavior_path_planner_common/marker_utils/utils.hpp" +#include "behavior_path_lane_change_module/utils/path.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include + #include -#include #include namespace marker_utils::lane_change_markers @@ -38,4 +38,4 @@ MarkerArray showFilteredObjects( const ExtendedPredictedObjects & target_lane_objects, const ExtendedPredictedObjects & other_lane_objects, const std::string & ns); } // namespace marker_utils::lane_change_markers -#endif // BEHAVIOR_PATH_PLANNER__MARKER_UTILS__LANE_CHANGE__DEBUG_HPP_ +#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_path.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp similarity index 82% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_path.hpp rename to planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp index c98f62c47860c..9e42b49635b82 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_path.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// 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. @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_PATH_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_PATH_HPP_ +#ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ +#define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ -#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" +#include "behavior_path_lane_change_module/utils/data_structs.hpp" #include "behavior_path_planner_common/turn_signal_decider.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -50,4 +50,4 @@ struct LaneChangeStatus }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_PATH_HPP_ +#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp similarity index 96% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp rename to planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp index f99ed043eb5dd..de4db37571122 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// 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. @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_ +#ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ +#define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ -#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" -#include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" +#include "behavior_path_lane_change_module/utils/data_structs.hpp" +#include "behavior_path_lane_change_module/utils/path.hpp" #include "behavior_path_planner_common/parameters.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner_common/utils/utils.hpp" @@ -208,4 +208,4 @@ lanelet::ConstLanelets generateExpandedLanelets( const double right_offset); } // namespace behavior_path_planner::utils::lane_change -#endif // BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_ +#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_lane_change_module/package.xml b/planning/behavior_path_lane_change_module/package.xml new file mode 100644 index 0000000000000..fb4bef9535a48 --- /dev/null +++ b/planning/behavior_path_lane_change_module/package.xml @@ -0,0 +1,38 @@ + + + + behavior_path_lane_change_module + 0.1.0 + The behavior_path_lane_change_module package + + Fumiya Watanabe + Zulfaqar Azmi + Kosuke Takeuchi + Tomoya Kimura + Shumpei Wakabayashi + Tomohito Ando + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + behavior_path_planner + behavior_path_planner_common + motion_utils + pluginlib + rclcpp + rtc_interface + tier4_autoware_utils + tier4_planning_msgs + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_path_lane_change_module/plugins.xml b/planning/behavior_path_lane_change_module/plugins.xml new file mode 100644 index 0000000000000..a598bc8176938 --- /dev/null +++ b/planning/behavior_path_lane_change_module/plugins.xml @@ -0,0 +1,6 @@ + + + + + + diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp similarity index 98% rename from planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp rename to planning/behavior_path_lane_change_module/src/interface.cpp index 101dd43919361..1e80067842152 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/lane_change/interface.hpp" +#include "behavior_path_lane_change_module/interface.hpp" -#include "behavior_path_planner/marker_utils/lane_change/debug.hpp" -#include "behavior_path_planner/utils/lane_change/utils.hpp" +#include "behavior_path_lane_change_module/utils/markers.hpp" +#include "behavior_path_lane_change_module/utils/utils.hpp" #include "behavior_path_planner_common/interface/scene_module_interface.hpp" #include "behavior_path_planner_common/interface/scene_module_visitor.hpp" #include "behavior_path_planner_common/marker_utils/utils.hpp" @@ -26,7 +26,6 @@ #include #include #include -#include namespace behavior_path_planner { diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_lane_change_module/src/manager.cpp similarity index 95% rename from planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp rename to planning/behavior_path_lane_change_module/src/manager.cpp index 658ffb06c25fe..e4405a31a360d 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_lane_change_module/src/manager.cpp @@ -12,8 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/lane_change/manager.hpp" +#include "behavior_path_lane_change_module/manager.hpp" +#include "behavior_path_lane_change_module/interface.hpp" #include "tier4_autoware_utils/ros/parameter.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" @@ -262,16 +263,10 @@ void LaneChangeModuleManager::init(rclcpp::Node * node) std::unique_ptr LaneChangeModuleManager::createNewSceneModuleInstance() { - if (type_ == LaneChangeModuleType::NORMAL) { - return std::make_unique( - name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_, - std::make_unique(parameters_, LaneChangeModuleType::NORMAL, direction_)); - } return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, objects_of_interest_marker_interface_ptr_map_, - std::make_unique(parameters_, direction_)); + std::make_unique(parameters_, LaneChangeModuleType::NORMAL, direction_)); } void LaneChangeModuleManager::updateModuleParams(const std::vector & parameters) @@ -298,9 +293,3 @@ PLUGINLIB_EXPORT_CLASS( PLUGINLIB_EXPORT_CLASS( behavior_path_planner::LaneChangeLeftModuleManager, behavior_path_planner::SceneModuleManagerInterface) -PLUGINLIB_EXPORT_CLASS( - behavior_path_planner::ExternalRequestLaneChangeRightModuleManager, - behavior_path_planner::SceneModuleManagerInterface) -PLUGINLIB_EXPORT_CLASS( - behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager, - behavior_path_planner::SceneModuleManagerInterface) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp similarity index 99% rename from planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp rename to planning/behavior_path_lane_change_module/src/scene.cpp index ebb41ae63dcbe..21a600d11d83d 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/lane_change/normal.hpp" +#include "behavior_path_lane_change_module/scene.hpp" -#include "behavior_path_planner/utils/lane_change/utils.hpp" +#include "behavior_path_lane_change_module/utils/utils.hpp" #include "behavior_path_planner_common/marker_utils/utils.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" diff --git a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp b/planning/behavior_path_lane_change_module/src/utils/markers.cpp similarity index 98% rename from planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp rename to planning/behavior_path_lane_change_module/src/utils/markers.cpp index c2a640f989b50..719acba405a68 100644 --- a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/markers.cpp @@ -16,7 +16,7 @@ #include "behavior_path_planner_common/marker_utils/utils.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include +#include #include #include diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp similarity index 99% rename from planning/behavior_path_planner/src/utils/lane_change/utils.cpp rename to planning/behavior_path_lane_change_module/src/utils/utils.cpp index 8ba820231d7cc..b5a4fd15b4c6b 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/lane_change/utils.hpp" +#include "behavior_path_lane_change_module/utils/utils.hpp" -#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" -#include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" +#include "behavior_path_lane_change_module/utils/data_structs.hpp" +#include "behavior_path_lane_change_module/utils/path.hpp" #include "behavior_path_planner_common/marker_utils/utils.hpp" #include "behavior_path_planner_common/parameters.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" diff --git a/planning/behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp new file mode 100644 index 0000000000000..7a606b5d126ce --- /dev/null +++ b/planning/behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -0,0 +1,126 @@ +// Copyright 2023 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 "ament_index_cpp/get_package_share_directory.hpp" +#include "behavior_path_planner/behavior_path_planner_node.hpp" +#include "planning_interface_test_manager/planning_interface_test_manager.hpp" +#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" + +#include + +#include +#include + +using behavior_path_planner::BehaviorPathPlannerNode; +using planning_test_utils::PlanningInterfaceTestManager; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + + // set subscriber with topic name: behavior_path_planner → test_node_ + test_manager->setPathWithLaneIdSubscriber("behavior_path_planner/output/path"); + + // set behavior_path_planner's input topic name(this topic is changed to test node) + test_manager->setRouteInputTopicName("behavior_path_planner/input/route"); + + test_manager->setInitialPoseTopicName("behavior_path_planner/input/odometry"); + + return test_manager; +} + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto behavior_path_planner_dir = + ament_index_cpp::get_package_share_directory("behavior_path_planner"); + const auto behavior_path_lane_change_module_dir = + ament_index_cpp::get_package_share_directory("behavior_path_lane_change_module"); + + std::vector module_names; + module_names.emplace_back("behavior_path_planner::LaneChangeRightModuleManager"); + module_names.emplace_back("behavior_path_planner::LaneChangeLeftModuleManager"); + + std::vector params; + params.emplace_back("launch_modules", module_names); + node_options.parameter_overrides(params); + + test_utils::updateNodeOptions( + node_options, {planning_test_utils_dir + "/config/test_common.param.yaml", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", + behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", + behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", + behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", + behavior_path_lane_change_module_dir + "/config/lane_change.param.yaml"}); + + return std::make_shared(node_options); +} + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ + // publish necessary topics from test_manager + test_manager->publishInitialPose(test_target_node, "behavior_path_planner/input/odometry"); + test_manager->publishAcceleration(test_target_node, "behavior_path_planner/input/accel"); + test_manager->publishPredictedObjects(test_target_node, "behavior_path_planner/input/perception"); + test_manager->publishOccupancyGrid( + test_target_node, "behavior_path_planner/input/occupancy_grid_map"); + test_manager->publishLaneDrivingScenario( + test_target_node, "behavior_path_planner/input/scenario"); + test_manager->publishMap(test_target_node, "behavior_path_planner/input/vector_map"); + test_manager->publishCostMap(test_target_node, "behavior_path_planner/input/costmap"); + test_manager->publishOperationModeState(test_target_node, "system/operation_mode/state"); + test_manager->publishLateralOffset( + test_target_node, "behavior_path_planner/input/lateral_offset"); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty route + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalRoute(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testRouteWithInvalidEgoPose(test_target_node)); + + rclcpp::shutdown(); +} diff --git a/planning/behavior_path_planner/test/test_lane_change_utils.cpp b/planning/behavior_path_lane_change_module/test/test_lane_change_utils.cpp similarity index 95% rename from planning/behavior_path_planner/test/test_lane_change_utils.cpp rename to planning/behavior_path_lane_change_module/test/test_lane_change_utils.cpp index 477af32893b0b..1f90114df8045 100644 --- a/planning/behavior_path_planner/test/test_lane_change_utils.cpp +++ b/planning/behavior_path_lane_change_module/test/test_lane_change_utils.cpp @@ -11,8 +11,7 @@ // 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/utils/lane_change/lane_change_module_data.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" +#include "behavior_path_lane_change_module/utils/data_structs.hpp" #include #include diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index 56456b08aac1e..3e7a46f4899c3 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -14,13 +14,7 @@ ament_auto_add_library(${PROJECT_NAME}_lib SHARED src/behavior_path_planner_node.cpp src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp src/scene_module/dynamic_avoidance/manager.cpp - src/scene_module/lane_change/interface.cpp - src/scene_module/lane_change/normal.cpp - src/scene_module/lane_change/external_request.cpp - src/scene_module/lane_change/manager.cpp - src/utils/lane_change/utils.cpp src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp - src/marker_utils/lane_change/debug.cpp ) target_include_directories(${PROJECT_NAME}_lib SYSTEM PUBLIC @@ -46,14 +40,6 @@ if(BUILD_TESTING) ${PROJECT_NAME}_lib ) - ament_add_ros_isolated_gmock(test_${CMAKE_PROJECT_NAME}_lane_change_module - test/test_lane_change_utils.cpp - ) - - target_link_libraries(test_${CMAKE_PROJECT_NAME}_lane_change_module - ${PROJECT_NAME}_lib - ) - ament_add_ros_isolated_gtest(test_${PROJECT_NAME}_node_interface test/test_${PROJECT_NAME}_node_interface.cpp ) diff --git a/planning/behavior_path_planner/plugins.xml b/planning/behavior_path_planner/plugins.xml index 2028b0d571e68..a16f3dd7bddee 100644 --- a/planning/behavior_path_planner/plugins.xml +++ b/planning/behavior_path_planner/plugins.xml @@ -1,7 +1,3 @@ - - - - diff --git a/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp index bab77f9141a00..f2c22f774fc77 100644 --- a/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp @@ -50,10 +50,6 @@ std::shared_ptr generateNode() std::vector module_names; module_names.emplace_back("behavior_path_planner::DynamicAvoidanceModuleManager"); - module_names.emplace_back("behavior_path_planner::LaneChangeRightModuleManager"); - module_names.emplace_back("behavior_path_planner::LaneChangeLeftModuleManager"); - module_names.emplace_back("behavior_path_planner::ExternalRequestLaneChangeRightModuleManager"); - module_names.emplace_back("behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager"); std::vector params; params.emplace_back("launch_modules", module_names); @@ -67,8 +63,7 @@ std::shared_ptr generateNode() behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", - behavior_path_planner_dir + "/config/dynamic_avoidance/dynamic_avoidance.param.yaml", - behavior_path_planner_dir + "/config/lane_change/lane_change.param.yaml"}); + behavior_path_planner_dir + "/config/dynamic_avoidance/dynamic_avoidance.param.yaml"}); return std::make_shared(node_options); }