|
| 1 | +// Copyright 2023 TIER IV, Inc. |
| 2 | +// |
| 3 | +// Licensed under the Apache License, Version 2.0 (the "License"); |
| 4 | +// you may not use this file except in compliance with the License. |
| 5 | +// You may obtain a copy of the License at |
| 6 | +// |
| 7 | +// http://www.apache.org/licenses/LICENSE-2.0 |
| 8 | +// |
| 9 | +// Unless required by applicable law or agreed to in writing, software |
| 10 | +// distributed under the License is distributed on an "AS IS" BASIS, |
| 11 | +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 12 | +// See the License for the specific language governing permissions and |
| 13 | +// limitations under the License. |
| 14 | + |
| 15 | +#include "ament_index_cpp/get_package_share_directory.hpp" |
| 16 | +#include "behavior_path_planner/behavior_path_planner_node.hpp" |
| 17 | +#include "planning_interface_test_manager/planning_interface_test_manager.hpp" |
| 18 | +#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" |
| 19 | + |
| 20 | +#include <gtest/gtest.h> |
| 21 | + |
| 22 | +#include <cmath> |
| 23 | +#include <vector> |
| 24 | + |
| 25 | +using behavior_path_planner::BehaviorPathPlannerNode; |
| 26 | +using planning_test_utils::PlanningInterfaceTestManager; |
| 27 | + |
| 28 | +std::shared_ptr<PlanningInterfaceTestManager> generateTestManager() |
| 29 | +{ |
| 30 | + auto test_manager = std::make_shared<PlanningInterfaceTestManager>(); |
| 31 | + |
| 32 | + // set subscriber with topic name: behavior_path_planner → test_node_ |
| 33 | + test_manager->setPathWithLaneIdSubscriber("behavior_path_planner/output/path"); |
| 34 | + |
| 35 | + // set behavior_path_planner's input topic name(this topic is changed to test node) |
| 36 | + test_manager->setRouteInputTopicName("behavior_path_planner/input/route"); |
| 37 | + |
| 38 | + test_manager->setInitialPoseTopicName("behavior_path_planner/input/odometry"); |
| 39 | + |
| 40 | + return test_manager; |
| 41 | +} |
| 42 | + |
| 43 | +std::shared_ptr<BehaviorPathPlannerNode> generateNode() |
| 44 | +{ |
| 45 | + auto node_options = rclcpp::NodeOptions{}; |
| 46 | + const auto planning_test_utils_dir = |
| 47 | + ament_index_cpp::get_package_share_directory("planning_test_utils"); |
| 48 | + const auto behavior_path_planner_dir = |
| 49 | + ament_index_cpp::get_package_share_directory("behavior_path_planner"); |
| 50 | + |
| 51 | + std::vector<std::string> module_names; |
| 52 | + module_names.emplace_back("behavior_path_planner::SideShiftModuleManager"); |
| 53 | + |
| 54 | + std::vector<rclcpp::Parameter> params; |
| 55 | + params.emplace_back("launch_modules", module_names); |
| 56 | + node_options.parameter_overrides(params); |
| 57 | + |
| 58 | + test_utils::updateNodeOptions( |
| 59 | + node_options, {planning_test_utils_dir + "/config/test_common.param.yaml", |
| 60 | + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", |
| 61 | + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", |
| 62 | + behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", |
| 63 | + behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", |
| 64 | + behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", |
| 65 | + ament_index_cpp::get_package_share_directory("behavior_path_side_shift_module") + |
| 66 | + "/config/side_shift.param.yaml"}); |
| 67 | + |
| 68 | + return std::make_shared<BehaviorPathPlannerNode>(node_options); |
| 69 | +} |
| 70 | + |
| 71 | +void publishMandatoryTopics( |
| 72 | + std::shared_ptr<PlanningInterfaceTestManager> test_manager, |
| 73 | + std::shared_ptr<BehaviorPathPlannerNode> test_target_node) |
| 74 | +{ |
| 75 | + // publish necessary topics from test_manager |
| 76 | + test_manager->publishInitialPose(test_target_node, "behavior_path_planner/input/odometry"); |
| 77 | + test_manager->publishAcceleration(test_target_node, "behavior_path_planner/input/accel"); |
| 78 | + test_manager->publishPredictedObjects(test_target_node, "behavior_path_planner/input/perception"); |
| 79 | + test_manager->publishOccupancyGrid( |
| 80 | + test_target_node, "behavior_path_planner/input/occupancy_grid_map"); |
| 81 | + test_manager->publishLaneDrivingScenario( |
| 82 | + test_target_node, "behavior_path_planner/input/scenario"); |
| 83 | + test_manager->publishMap(test_target_node, "behavior_path_planner/input/vector_map"); |
| 84 | + test_manager->publishCostMap(test_target_node, "behavior_path_planner/input/costmap"); |
| 85 | + test_manager->publishOperationModeState(test_target_node, "system/operation_mode/state"); |
| 86 | + test_manager->publishLateralOffset( |
| 87 | + test_target_node, "behavior_path_planner/input/lateral_offset"); |
| 88 | +} |
| 89 | + |
| 90 | +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) |
| 91 | +{ |
| 92 | + rclcpp::init(0, nullptr); |
| 93 | + auto test_manager = generateTestManager(); |
| 94 | + auto test_target_node = generateNode(); |
| 95 | + |
| 96 | + publishMandatoryTopics(test_manager, test_target_node); |
| 97 | + |
| 98 | + // test for normal trajectory |
| 99 | + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); |
| 100 | + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); |
| 101 | + |
| 102 | + // test with empty route |
| 103 | + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalRoute(test_target_node)); |
| 104 | + rclcpp::shutdown(); |
| 105 | +} |
| 106 | + |
| 107 | +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) |
| 108 | +{ |
| 109 | + rclcpp::init(0, nullptr); |
| 110 | + |
| 111 | + auto test_manager = generateTestManager(); |
| 112 | + auto test_target_node = generateNode(); |
| 113 | + publishMandatoryTopics(test_manager, test_target_node); |
| 114 | + |
| 115 | + // test for normal trajectory |
| 116 | + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); |
| 117 | + |
| 118 | + // make sure behavior_path_planner is running |
| 119 | + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); |
| 120 | + |
| 121 | + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testRouteWithInvalidEgoPose(test_target_node)); |
| 122 | + |
| 123 | + rclcpp::shutdown(); |
| 124 | +} |
0 commit comments