Skip to content

Commit 0d87929

Browse files
SakodaShintaroSakodaShintaro
committed
Merge remote-tracking branch 'origin/beta/v0.41' into cherry-pick/bus_stop_area
2 parents 5efef23 + e7dda0f commit 0d87929

File tree

31 files changed

+635
-833
lines changed

31 files changed

+635
-833
lines changed

‎planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp

+26-12
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,7 @@
2121
#include <gtest/gtest.h>
2222

2323
#include <memory>
24+
#include <string>
2425
#include <vector>
2526

2627
using autoware::freespace_planner::FreespacePlannerNode;
@@ -29,10 +30,8 @@ using autoware::planning_test_manager::PlanningInterfaceTestManager;
2930
std::shared_ptr<PlanningInterfaceTestManager> generateTestManager()
3031
{
3132
auto test_manager = std::make_shared<PlanningInterfaceTestManager>();
32-
test_manager->setRouteInputTopicName("freespace_planner/input/route");
33-
test_manager->setTrajectorySubscriber("freespace_planner/output/trajectory");
34-
test_manager->setOdometryTopicName("freespace_planner/input/odometry");
35-
test_manager->setInitialPoseTopicName("freespace_planner/input/odometry");
33+
test_manager->subscribeOutput<autoware_planning_msgs::msg::Trajectory>(
34+
"freespace_planner/output/trajectory");
3635
return test_manager;
3736
}
3837

@@ -55,10 +54,16 @@ void publishMandatoryTopics(
5554
rclcpp::Node::SharedPtr test_target_node)
5655
{
5756
// publish necessary topics from test_manager
58-
test_manager->publishTF(test_target_node, "/tf");
59-
test_manager->publishOdometry(test_target_node, "freespace_planner/input/odometry");
60-
test_manager->publishOccupancyGrid(test_target_node, "freespace_planner/input/occupancy_grid");
61-
test_manager->publishParkingScenario(test_target_node, "freespace_planner/input/scenario");
57+
test_manager->publishInput(
58+
test_target_node, "/tf", autoware::test_utils::makeTFMsg(test_target_node, "base_link", "map"));
59+
test_manager->publishInput(
60+
test_target_node, "freespace_planner/input/odometry", autoware::test_utils::makeOdometry());
61+
test_manager->publishInput(
62+
test_target_node, "freespace_planner/input/occupancy_grid",
63+
autoware::test_utils::makeCostMapMsg());
64+
test_manager->publishInput(
65+
test_target_node, "freespace_planner/input/scenario",
66+
autoware::test_utils::makeScenarioMsg(tier4_planning_msgs::msg::Scenario::PARKING));
6267
}
6368

6469
// the following tests are disable because they randomly fail
@@ -70,12 +75,16 @@ TEST(PlanningModuleInterfaceTest, DISABLED_testPlanningInterfaceWithVariousTraje
7075
auto test_target_node = generateNode();
7176
publishMandatoryTopics(test_manager, test_target_node);
7277

78+
const std::string input_route_topic = "freespace_planner/input/route";
79+
7380
// test with normal route
74-
ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node));
81+
ASSERT_NO_THROW_WITH_ERROR_MSG(
82+
test_manager->testWithBehaviorNormalRoute(test_target_node, input_route_topic));
7583
EXPECT_GE(test_manager->getReceivedTopicNum(), 1);
7684

7785
// test with empty route
78-
ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalRoute(test_target_node));
86+
ASSERT_NO_THROW_WITH_ERROR_MSG(
87+
test_manager->testWithAbnormalRoute(test_target_node, input_route_topic));
7988
rclcpp::shutdown();
8089
}
8190

@@ -87,11 +96,16 @@ TEST(PlanningModuleInterfaceTest, DISABLED_NodeTestWithOffTrackEgoPose)
8796

8897
publishMandatoryTopics(test_manager, test_target_node);
8998

99+
const std::string input_route_topic = "freespace_planner/input/route";
100+
const std::string input_odometry_topic = "freespace_planner/input/odometry";
101+
90102
// test for normal route
91-
ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node));
103+
ASSERT_NO_THROW_WITH_ERROR_MSG(
104+
test_manager->testWithBehaviorNormalRoute(test_target_node, input_route_topic));
92105
EXPECT_GE(test_manager->getReceivedTopicNum(), 1);
93106

94-
ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testRouteWithInvalidEgoPose(test_target_node));
107+
ASSERT_NO_THROW_WITH_ERROR_MSG(
108+
test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic));
95109

96110
rclcpp::shutdown();
97111
}

‎planning/autoware_obstacle_cruise_planner/test/test_obstacle_cruise_planner_node_interface.cpp

+25-13
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,7 @@
2121
#include <gtest/gtest.h>
2222

2323
#include <memory>
24+
#include <string>
2425
#include <vector>
2526

2627
using autoware::motion_planning::ObstacleCruisePlannerNode;
@@ -31,12 +32,8 @@ std::shared_ptr<PlanningInterfaceTestManager> generateTestManager()
3132
auto test_manager = std::make_shared<PlanningInterfaceTestManager>();
3233

3334
// set subscriber with topic name: obstacle_cruise_planner → test_node_
34-
test_manager->setTrajectorySubscriber("obstacle_cruise_planner/output/trajectory");
35-
36-
// set obstacle_cruise_planners input topic name(this topic is changed to test node):
37-
test_manager->setTrajectoryInputTopicName("obstacle_cruise_planner/input/trajectory");
38-
39-
test_manager->setOdometryTopicName("obstacle_cruise_planner/input/odometry");
35+
test_manager->subscribeOutput<autoware_planning_msgs::msg::Trajectory>(
36+
"obstacle_cruise_planner/output/trajectory");
4037

4138
return test_manager;
4239
}
@@ -63,9 +60,15 @@ void publishMandatoryTopics(
6360
std::shared_ptr<ObstacleCruisePlannerNode> test_target_node)
6461
{
6562
// publish necessary topics from test_manager
66-
test_manager->publishOdometry(test_target_node, "obstacle_cruise_planner/input/odometry");
67-
test_manager->publishPredictedObjects(test_target_node, "obstacle_cruise_planner/input/objects");
68-
test_manager->publishAcceleration(test_target_node, "obstacle_cruise_planner/input/acceleration");
63+
test_manager->publishInput(
64+
test_target_node, "obstacle_cruise_planner/input/odometry",
65+
autoware::test_utils::makeOdometry());
66+
test_manager->publishInput(
67+
test_target_node, "obstacle_cruise_planner/input/objects",
68+
autoware_perception_msgs::msg::PredictedObjects{});
69+
test_manager->publishInput(
70+
test_target_node, "obstacle_cruise_planner/input/acceleration",
71+
geometry_msgs::msg::AccelWithCovarianceStamped{});
6972
}
7073

7174
TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory)
@@ -76,12 +79,16 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory)
7679

7780
publishMandatoryTopics(test_manager, test_target_node);
7881

82+
const std::string input_trajectory_topic = "obstacle_cruise_planner/input/trajectory";
83+
7984
// test for normal trajectory
80-
ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node));
85+
ASSERT_NO_THROW_WITH_ERROR_MSG(
86+
test_manager->testWithNormalTrajectory(test_target_node, input_trajectory_topic));
8187
EXPECT_GE(test_manager->getReceivedTopicNum(), 1);
8288

8389
// test for trajectory with empty/one point/overlapping point
84-
ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalTrajectory(test_target_node));
90+
ASSERT_NO_THROW_WITH_ERROR_MSG(
91+
test_manager->testWithAbnormalTrajectory(test_target_node, input_trajectory_topic));
8592

8693
rclcpp::shutdown();
8794
}
@@ -94,12 +101,17 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose)
94101

95102
publishMandatoryTopics(test_manager, test_target_node);
96103

104+
const std::string input_trajectory_topic = "obstacle_cruise_planner/input/trajectory";
105+
const std::string input_odometry_topic = "obstacle_cruise_planner/input/odometry";
106+
97107
// test for normal trajectory
98-
ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node));
108+
ASSERT_NO_THROW_WITH_ERROR_MSG(
109+
test_manager->testWithNormalTrajectory(test_target_node, input_trajectory_topic));
99110
EXPECT_GE(test_manager->getReceivedTopicNum(), 1);
100111

101112
// test for trajectory with empty/one point/overlapping point
102-
ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testTrajectoryWithInvalidEgoPose(test_target_node));
113+
ASSERT_NO_THROW_WITH_ERROR_MSG(
114+
test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic));
103115

104116
rclcpp::shutdown();
105117
}

‎planning/autoware_obstacle_stop_planner/test/test_autoware_obstacle_stop_planner_node_interface.cpp

+30-24
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@
2323
#include <gtest/gtest.h>
2424

2525
#include <memory>
26+
#include <string>
2627
#include <vector>
2728

2829
using autoware::motion_planning::ObstacleStopPlannerNode;
@@ -32,9 +33,8 @@ using tier4_planning_msgs::msg::ExpandStopRange;
3233
std::shared_ptr<PlanningInterfaceTestManager> generateTestManager()
3334
{
3435
auto test_manager = std::make_shared<PlanningInterfaceTestManager>();
35-
test_manager->setTrajectorySubscriber("obstacle_stop_planner/output/trajectory");
36-
test_manager->setTrajectoryInputTopicName("obstacle_stop_planner/input/trajectory");
37-
test_manager->setOdometryTopicName("obstacle_stop_planner/input/odometry");
36+
test_manager->subscribeOutput<autoware_planning_msgs::msg::Trajectory>(
37+
"obstacle_stop_planner/output/trajectory");
3838
return test_manager;
3939
}
4040

@@ -65,21 +65,21 @@ void publishMandatoryTopics(
6565
std::shared_ptr<ObstacleStopPlannerNode> test_target_node)
6666
{
6767
// publish necessary topics from test_manager
68-
test_manager->publishOdometry(test_target_node, "obstacle_stop_planner/input/odometry");
69-
test_manager->publishPointCloud(test_target_node, "obstacle_stop_planner/input/pointcloud");
70-
test_manager->publishAcceleration(test_target_node, "obstacle_stop_planner/input/acceleration");
71-
test_manager->publishPredictedObjects(test_target_node, "obstacle_stop_planner/input/objects");
72-
}
73-
74-
void publishExpandStopRange(
75-
std::shared_ptr<PlanningInterfaceTestManager> test_manager,
76-
std::shared_ptr<ObstacleStopPlannerNode> test_target_node)
77-
{
78-
auto test_node = test_manager->getTestNode();
79-
const auto expand_stop_range = test_manager->getTestNode()->create_publisher<ExpandStopRange>(
80-
"obstacle_stop_planner/input/expand_stop_range", 1);
81-
expand_stop_range->publish(ExpandStopRange{});
82-
autoware::test_utils::spinSomeNodes(test_node, test_target_node, 3);
68+
test_manager->publishInput(
69+
test_target_node, "obstacle_stop_planner/input/odometry", autoware::test_utils::makeOdometry());
70+
test_manager->publishInput(
71+
test_target_node, "obstacle_stop_planner/input/pointcloud",
72+
sensor_msgs::msg::PointCloud2{}.set__header(
73+
std_msgs::msg::Header{}.set__frame_id("base_link")));
74+
test_manager->publishInput(
75+
test_target_node, "obstacle_stop_planner/input/acceleration",
76+
geometry_msgs::msg::AccelWithCovarianceStamped{});
77+
test_manager->publishInput(
78+
test_target_node, "obstacle_stop_planner/input/objects",
79+
autoware_perception_msgs::msg::PredictedObjects{});
80+
test_manager->publishInput(
81+
test_target_node, "obstacle_stop_planner/input/expand_stop_range",
82+
tier4_planning_msgs::msg::ExpandStopRange{});
8383
}
8484

8585
TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory)
@@ -90,15 +90,17 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory)
9090
auto test_target_node = generateNode();
9191

9292
publishMandatoryTopics(test_manager, test_target_node);
93-
publishExpandStopRange(test_manager, test_target_node);
93+
94+
const std::string input_trajectory_topic = "obstacle_stop_planner/input/trajectory";
9495

9596
// test for normal trajectory
96-
ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node));
97+
ASSERT_NO_THROW_WITH_ERROR_MSG(
98+
test_manager->testWithNormalTrajectory(test_target_node, input_trajectory_topic));
9799

98100
EXPECT_GE(test_manager->getReceivedTopicNum(), 1);
99101

100102
// test for trajectory with empty/one point/overlapping point
101-
test_manager->testWithAbnormalTrajectory(test_target_node);
103+
test_manager->testWithAbnormalTrajectory(test_target_node, input_trajectory_topic);
102104

103105
rclcpp::shutdown();
104106
}
@@ -110,14 +112,18 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose)
110112
auto test_target_node = generateNode();
111113

112114
publishMandatoryTopics(test_manager, test_target_node);
113-
publishExpandStopRange(test_manager, test_target_node);
115+
116+
const std::string input_trajectory_topic = "obstacle_stop_planner/input/trajectory";
117+
const std::string input_odometry_topic = "obstacle_stop_planner/input/odometry";
114118

115119
// test for normal trajectory
116-
ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node));
120+
ASSERT_NO_THROW_WITH_ERROR_MSG(
121+
test_manager->testWithNormalTrajectory(test_target_node, input_trajectory_topic));
117122
EXPECT_GE(test_manager->getReceivedTopicNum(), 1);
118123

119124
// test for trajectory with empty/one point/overlapping point
120-
ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testTrajectoryWithInvalidEgoPose(test_target_node));
125+
ASSERT_NO_THROW_WITH_ERROR_MSG(
126+
test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic));
121127

122128
rclcpp::shutdown();
123129
}

‎planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp

+10-6
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,7 @@
2121
#include <gtest/gtest.h>
2222

2323
#include <memory>
24+
#include <string>
2425
#include <vector>
2526

2627
TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory)
@@ -47,21 +48,24 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory)
4748
auto test_target_node = std::make_shared<autoware::path_optimizer::PathOptimizer>(node_options);
4849

4950
// publish necessary topics from test_manager
50-
test_manager->publishOdometry(test_target_node, "path_optimizer/input/odometry");
51+
test_manager->publishInput(
52+
test_target_node, "path_optimizer/input/odometry", autoware::test_utils::makeOdometry());
5153

5254
// set subscriber with topic name: path_optimizer → test_node_
53-
test_manager->setTrajectorySubscriber("path_optimizer/output/path");
55+
test_manager->subscribeOutput<autoware_planning_msgs::msg::Trajectory>(
56+
"path_optimizer/output/path");
5457

55-
// set path_optimizer's input topic name(this topic is changed to test node)
56-
test_manager->setPathInputTopicName("path_optimizer/input/path");
58+
const std::string input_trajectory_topic = "path_optimizer/input/path";
5759

5860
// test with normal trajectory
59-
ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPath(test_target_node));
61+
ASSERT_NO_THROW_WITH_ERROR_MSG(
62+
test_manager->testWithNormalPath(test_target_node, input_trajectory_topic));
6063

6164
EXPECT_GE(test_manager->getReceivedTopicNum(), 1);
6265

6366
// test with trajectory with empty/one point/overlapping point
64-
ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPath(test_target_node));
67+
ASSERT_NO_THROW_WITH_ERROR_MSG(
68+
test_manager->testWithAbnormalPath(test_target_node, input_trajectory_topic));
6569

6670
rclcpp::shutdown();
6771
}

‎planning/autoware_path_smoother/test/test_autoware_path_smoother_node_interface.cpp

+13-7
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,7 @@
2121
#include <gtest/gtest.h>
2222

2323
#include <memory>
24+
#include <string>
2425
#include <vector>
2526

2627
TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory)
@@ -46,22 +47,27 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory)
4647
std::make_shared<autoware::path_smoother::ElasticBandSmoother>(node_options);
4748

4849
// publish necessary topics from test_manager
49-
test_manager->publishOdometry(test_target_node, "autoware_path_smoother/input/odometry");
50+
test_manager->publishInput(
51+
test_target_node, "autoware_path_smoother/input/odometry",
52+
autoware::test_utils::makeOdometry());
5053

5154
// set subscriber with topic name
52-
test_manager->setTrajectorySubscriber("autoware_path_smoother/output/traj");
53-
test_manager->setPathSubscriber("autoware_path_smoother/output/path");
55+
test_manager->subscribeOutput<autoware_planning_msgs::msg::Trajectory>(
56+
"autoware_path_smoother/output/traj");
57+
test_manager->subscribeOutput<autoware_planning_msgs::msg::Path>(
58+
"autoware_path_smoother/output/path");
5459

55-
// set input topic name (this topic is changed to test node)
56-
test_manager->setPathInputTopicName("autoware_path_smoother/input/path");
60+
const std::string input_path_topic = "autoware_path_smoother/input/path";
5761

5862
// test with normal trajectory
59-
ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPath(test_target_node));
63+
ASSERT_NO_THROW_WITH_ERROR_MSG(
64+
test_manager->testWithNormalPath(test_target_node, input_path_topic));
6065

6166
EXPECT_GE(test_manager->getReceivedTopicNum(), 1);
6267

6368
// test with trajectory with empty/one point/overlapping point
64-
ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPath(test_target_node));
69+
ASSERT_NO_THROW_WITH_ERROR_MSG(
70+
test_manager->testWithAbnormalPath(test_target_node, input_path_topic));
6571

6672
rclcpp::shutdown();
6773
}

0 commit comments

Comments
 (0)