Skip to content

Commit ee93be1

Browse files
takayuki5168takayuki5168
authored
refactor(behavior_velocity_planner): independent of plugin packages (#9760)
Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
1 parent 452e076 commit ee93be1

File tree

9 files changed

+144
-106
lines changed

9 files changed

+144
-106
lines changed

‎planning/behavior_velocity_planner/autoware_behavior_velocity_planner/CMakeLists.txt

+2-1
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,7 @@ autoware_package()
1616
ament_auto_add_library(${PROJECT_NAME}_lib SHARED
1717
src/node.cpp
1818
src/planner_manager.cpp
19+
src/test_utils.cpp
1920
)
2021

2122
rclcpp_components_register_node(${PROJECT_NAME}_lib
@@ -34,7 +35,7 @@ endif()
3435

3536
if(BUILD_TESTING)
3637
ament_add_ros_isolated_gtest(test_${PROJECT_NAME}
37-
test/src/test_node_interface.cpp
38+
test/test_node_interface.cpp
3839
)
3940
target_link_libraries(test_${PROJECT_NAME}
4041
gtest_main

‎planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.hpp ‎planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp

+4-4
Original file line numberDiff line numberDiff line change
@@ -12,12 +12,12 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#ifndef NODE_HPP_
16-
#define NODE_HPP_
15+
#ifndef AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__NODE_HPP_
16+
#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__NODE_HPP_
1717

18+
#include "autoware/behavior_velocity_planner/planner_manager.hpp"
1819
#include "autoware/universe_utils/ros/logger_level_configure.hpp"
1920
#include "autoware/universe_utils/ros/polling_subscriber.hpp"
20-
#include "planner_manager.hpp"
2121

2222
#include <autoware/behavior_velocity_planner_common/planner_data.hpp>
2323
#include <autoware/universe_utils/ros/published_time_publisher.hpp>
@@ -150,4 +150,4 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node
150150
};
151151
} // namespace autoware::behavior_velocity_planner
152152

153-
#endif // NODE_HPP_
153+
#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__NODE_HPP_

‎planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.hpp ‎planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/planner_manager.hpp

+3-3
Original file line numberDiff line numberDiff line change
@@ -12,8 +12,8 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#ifndef PLANNER_MANAGER_HPP_
16-
#define PLANNER_MANAGER_HPP_
15+
#ifndef AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__PLANNER_MANAGER_HPP_
16+
#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__PLANNER_MANAGER_HPP_
1717

1818
#include <autoware/behavior_velocity_planner_common/plugin_interface.hpp>
1919
#include <autoware/behavior_velocity_planner_common/plugin_wrapper.hpp>
@@ -57,4 +57,4 @@ class BehaviorVelocityPlannerManager
5757
};
5858
} // namespace autoware::behavior_velocity_planner
5959

60-
#endif // PLANNER_MANAGER_HPP_
60+
#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__PLANNER_MANAGER_HPP_

‎planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/test_utils.hpp

+47
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,47 @@
1+
// Copyright 2024 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+
#ifndef AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__TEST_UTILS_HPP_
16+
#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__TEST_UTILS_HPP_
17+
18+
#include "autoware/behavior_velocity_planner/node.hpp"
19+
20+
#include <autoware_planning_test_manager/autoware_planning_test_manager.hpp>
21+
22+
#include <memory>
23+
#include <string>
24+
#include <vector>
25+
26+
namespace autoware::behavior_velocity_planner
27+
{
28+
using autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode;
29+
using autoware::planning_test_manager::PlanningInterfaceTestManager;
30+
31+
struct PluginInfo
32+
{
33+
std::string module_name; // e.g. crosswalk
34+
std::string plugin_name; // e.g. autoware::behavior_velocity_planner::CrosswalkModulePlugin
35+
};
36+
37+
std::shared_ptr<PlanningInterfaceTestManager> generateTestManager();
38+
39+
std::shared_ptr<BehaviorVelocityPlannerNode> generateNode(
40+
const std::vector<PluginInfo> & plugin_info_vec);
41+
42+
void publishMandatoryTopics(
43+
std::shared_ptr<PlanningInterfaceTestManager> test_manager,
44+
std::shared_ptr<BehaviorVelocityPlannerNode> test_target_node);
45+
} // namespace autoware::behavior_velocity_planner
46+
47+
#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__TEST_UTILS_HPP_

‎planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml

-13
Original file line numberDiff line numberDiff line change
@@ -64,19 +64,6 @@
6464

6565
<test_depend>ament_cmake_ros</test_depend>
6666
<test_depend>ament_lint_auto</test_depend>
67-
<test_depend>autoware_behavior_velocity_blind_spot_module</test_depend>
68-
<test_depend>autoware_behavior_velocity_crosswalk_module</test_depend>
69-
<test_depend>autoware_behavior_velocity_detection_area_module</test_depend>
70-
<test_depend>autoware_behavior_velocity_intersection_module</test_depend>
71-
<test_depend>autoware_behavior_velocity_no_drivable_lane_module</test_depend>
72-
<test_depend>autoware_behavior_velocity_no_stopping_area_module</test_depend>
73-
<test_depend>autoware_behavior_velocity_occlusion_spot_module</test_depend>
74-
<test_depend>autoware_behavior_velocity_run_out_module</test_depend>
75-
<test_depend>autoware_behavior_velocity_speed_bump_module</test_depend>
76-
<test_depend>autoware_behavior_velocity_stop_line_module</test_depend>
77-
<test_depend>autoware_behavior_velocity_traffic_light_module</test_depend>
78-
<test_depend>autoware_behavior_velocity_virtual_traffic_light_module</test_depend>
79-
<test_depend>autoware_behavior_velocity_walkway_module</test_depend>
8067
<test_depend>autoware_lint_common</test_depend>
8168
<!--<test_depend>autoware_behavior_velocity_template_module</test_depend>-->
8269

‎planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,7 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#include "node.hpp"
15+
#include "autoware/behavior_velocity_planner/node.hpp"
1616

1717
#include <autoware/behavior_velocity_planner_common/utilization/path_utilization.hpp>
1818
#include <autoware/motion_utils/trajectory/path_with_lane_id.hpp>

‎planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,7 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#include "planner_manager.hpp"
15+
#include "autoware/behavior_velocity_planner/planner_manager.hpp"
1616

1717
#include <autoware/motion_utils/trajectory/interpolation.hpp>
1818
#include <autoware/motion_utils/trajectory/trajectory.hpp>

‎planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp ‎planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/test_utils.cpp

+25-83
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
// Copyright 2023 Tier IV, Inc.
1+
// Copyright 2024 TIER IV, Inc.
22
//
33
// Licensed under the Apache License, Version 2.0 (the "License");
44
// you may not use this file except in compliance with the License.
@@ -12,22 +12,18 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#include "node.hpp"
15+
#include "autoware/behavior_velocity_planner/test_utils.hpp"
1616

1717
#include <ament_index_cpp/get_package_share_directory.hpp>
1818
#include <autoware_planning_test_manager/autoware_planning_test_manager.hpp>
1919
#include <autoware_test_utils/autoware_test_utils.hpp>
2020

21-
#include <gtest/gtest.h>
22-
23-
#include <cmath>
2421
#include <memory>
2522
#include <string>
2623
#include <vector>
2724

28-
using autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode;
29-
using autoware::planning_test_manager::PlanningInterfaceTestManager;
30-
25+
namespace autoware::behavior_velocity_planner
26+
{
3127
std::shared_ptr<PlanningInterfaceTestManager> generateTestManager()
3228
{
3329
auto test_manager = std::make_shared<PlanningInterfaceTestManager>();
@@ -45,7 +41,8 @@ std::shared_ptr<PlanningInterfaceTestManager> generateTestManager()
4541
return test_manager;
4642
}
4743

48-
std::shared_ptr<BehaviorVelocityPlannerNode> generateNode()
44+
std::shared_ptr<BehaviorVelocityPlannerNode> generateNode(
45+
const std::vector<PluginInfo> & plugin_info_vec)
4946
{
5047
auto node_options = rclcpp::NodeOptions{};
5148

@@ -64,49 +61,29 @@ std::shared_ptr<BehaviorVelocityPlannerNode> generateNode()
6461
return package_path + "/config/" + module + ".param.yaml";
6562
};
6663

67-
std::vector<std::string> module_names;
68-
module_names.emplace_back("autoware::behavior_velocity_planner::CrosswalkModulePlugin");
69-
module_names.emplace_back("autoware::behavior_velocity_planner::WalkwayModulePlugin");
70-
module_names.emplace_back("autoware::behavior_velocity_planner::TrafficLightModulePlugin");
71-
module_names.emplace_back("autoware::behavior_velocity_planner::IntersectionModulePlugin");
72-
module_names.emplace_back("autoware::behavior_velocity_planner::MergeFromPrivateModulePlugin");
73-
module_names.emplace_back("autoware::behavior_velocity_planner::BlindSpotModulePlugin");
74-
module_names.emplace_back("autoware::behavior_velocity_planner::DetectionAreaModulePlugin");
75-
module_names.emplace_back("autoware::behavior_velocity_planner::VirtualTrafficLightModulePlugin");
76-
module_names.emplace_back("autoware::behavior_velocity_planner::NoStoppingAreaModulePlugin");
77-
module_names.emplace_back("autoware::behavior_velocity_planner::StopLineModulePlugin");
78-
module_names.emplace_back("autoware::behavior_velocity_planner::OcclusionSpotModulePlugin");
79-
module_names.emplace_back("autoware::behavior_velocity_planner::RunOutModulePlugin");
80-
module_names.emplace_back("autoware::behavior_velocity_planner::SpeedBumpModulePlugin");
81-
module_names.emplace_back("autoware::behavior_velocity_planner::NoDrivableLaneModulePlugin");
64+
std::vector<std::string> plugin_names;
65+
for (const auto & plugin_info : plugin_info_vec) {
66+
plugin_names.emplace_back(plugin_info.plugin_name);
67+
}
8268

8369
std::vector<rclcpp::Parameter> params;
84-
params.emplace_back("launch_modules", module_names);
70+
params.emplace_back("launch_modules", plugin_names);
8571
params.emplace_back("is_simulation", false);
8672
node_options.parameter_overrides(params);
8773

88-
autoware::test_utils::updateNodeOptions(
89-
node_options,
90-
{autoware_test_utils_dir + "/config/test_common.param.yaml",
91-
autoware_test_utils_dir + "/config/test_nearest_search.param.yaml",
92-
autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml",
93-
velocity_smoother_dir + "/config/default_velocity_smoother.param.yaml",
94-
velocity_smoother_dir + "/config/Analytical.param.yaml",
95-
behavior_velocity_planner_common_dir + "/config/behavior_velocity_planner_common.param.yaml",
96-
behavior_velocity_planner_dir + "/config/behavior_velocity_planner.param.yaml",
97-
get_behavior_velocity_module_config("blind_spot"),
98-
get_behavior_velocity_module_config("crosswalk"),
99-
get_behavior_velocity_module_config("walkway"),
100-
get_behavior_velocity_module_config("detection_area"),
101-
get_behavior_velocity_module_config("intersection"),
102-
get_behavior_velocity_module_config("no_stopping_area"),
103-
get_behavior_velocity_module_config("occlusion_spot"),
104-
get_behavior_velocity_module_config("run_out"),
105-
get_behavior_velocity_module_config("speed_bump"),
106-
get_behavior_velocity_module_config("stop_line"),
107-
get_behavior_velocity_module_config("traffic_light"),
108-
get_behavior_velocity_module_config("virtual_traffic_light"),
109-
get_behavior_velocity_module_config("no_drivable_lane")});
74+
auto yaml_files = std::vector<std::string>{
75+
autoware_test_utils_dir + "/config/test_common.param.yaml",
76+
autoware_test_utils_dir + "/config/test_nearest_search.param.yaml",
77+
autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml",
78+
velocity_smoother_dir + "/config/default_velocity_smoother.param.yaml",
79+
velocity_smoother_dir + "/config/Analytical.param.yaml",
80+
behavior_velocity_planner_common_dir + "/config/behavior_velocity_planner_common.param.yaml",
81+
behavior_velocity_planner_dir + "/config/behavior_velocity_planner.param.yaml"};
82+
for (const auto & plugin_info : plugin_info_vec) {
83+
yaml_files.push_back(get_behavior_velocity_module_config(plugin_info.module_name));
84+
}
85+
86+
autoware::test_utils::updateNodeOptions(node_options, yaml_files);
11087

11188
// TODO(Takagi, Isamu): set launch_modules
11289
// TODO(Kyoichi Sugahara) set to true launch_virtual_traffic_light
@@ -141,39 +118,4 @@ void publishMandatoryTopics(
141118
test_manager->publishOccupancyGrid(
142119
test_target_node, "behavior_velocity_planner_node/input/occupancy_grid");
143120
}
144-
145-
TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID)
146-
{
147-
rclcpp::init(0, nullptr);
148-
auto test_manager = generateTestManager();
149-
auto test_target_node = generateNode();
150-
151-
publishMandatoryTopics(test_manager, test_target_node);
152-
153-
// test with nominal path_with_lane_id
154-
ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node));
155-
EXPECT_GE(test_manager->getReceivedTopicNum(), 1);
156-
157-
// test with empty path_with_lane_id
158-
ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node));
159-
rclcpp::shutdown();
160-
}
161-
162-
TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose)
163-
{
164-
rclcpp::init(0, nullptr);
165-
166-
auto test_manager = generateTestManager();
167-
auto test_target_node = generateNode();
168-
publishMandatoryTopics(test_manager, test_target_node);
169-
170-
// test for normal trajectory
171-
ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node));
172-
173-
// make sure behavior_path_planner is running
174-
EXPECT_GE(test_manager->getReceivedTopicNum(), 1);
175-
176-
ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node));
177-
178-
rclcpp::shutdown();
179-
}
121+
} // namespace autoware::behavior_velocity_planner

‎planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/test_node_interface.cpp

+61
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,61 @@
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 "autoware/behavior_velocity_planner/test_utils.hpp"
16+
17+
#include <gtest/gtest.h>
18+
19+
#include <cmath>
20+
#include <memory>
21+
#include <string>
22+
#include <vector>
23+
24+
namespace autoware::behavior_velocity_planner
25+
{
26+
TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID)
27+
{
28+
rclcpp::init(0, nullptr);
29+
auto test_manager = autoware::behavior_velocity_planner::generateTestManager();
30+
auto test_target_node = autoware::behavior_velocity_planner::generateNode({});
31+
32+
autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node);
33+
34+
// test with nominal path_with_lane_id
35+
ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node));
36+
EXPECT_GE(test_manager->getReceivedTopicNum(), 1);
37+
38+
// test with empty path_with_lane_id
39+
ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node));
40+
rclcpp::shutdown();
41+
}
42+
43+
TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose)
44+
{
45+
rclcpp::init(0, nullptr);
46+
47+
auto test_manager = autoware::behavior_velocity_planner::generateTestManager();
48+
auto test_target_node = autoware::behavior_velocity_planner::generateNode({});
49+
autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node);
50+
51+
// test for normal trajectory
52+
ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node));
53+
54+
// make sure behavior_path_planner is running
55+
EXPECT_GE(test_manager->getReceivedTopicNum(), 1);
56+
57+
ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node));
58+
59+
rclcpp::shutdown();
60+
}
61+
} // namespace autoware::behavior_velocity_planner

0 commit comments

Comments
 (0)