Skip to content

Commit

Permalink
feat(planning_test_manager): abstract message-specific functions (aut…
Browse files Browse the repository at this point in the history 
…owarefoundation#9882)

* abstract message-specific functions

Signed-off-by: mitukou1109 <mitukou1109@gmail.com>

* include necessary header

Signed-off-by: mitukou1109 <mitukou1109@gmail.com>

* adapt velocity_smoother to new test manager

Signed-off-by: mitukou1109 <mitukou1109@gmail.com>

* adapt behavior_velocity_planner to new test manager

Signed-off-by: mitukou1109 <mitukou1109@gmail.com>

* adapt path_optimizer to new test manager

Signed-off-by: mitukou1109 <mitukou1109@gmail.com>

* fix output subscription

Signed-off-by: mitukou1109 <mitukou1109@gmail.com>

* adapt behavior_path_planner to new test manager

Signed-off-by: mitukou1109 <mitukou1109@gmail.com>

* adapt scenario_selector to new test manager

Signed-off-by: mitukou1109 <mitukou1109@gmail.com>

* adapt freespace_planner to new test manager

Signed-off-by: mitukou1109 <mitukou1109@gmail.com>

* adapt planning_validator to new test manager

Signed-off-by: mitukou1109 <mitukou1109@gmail.com>

* adapt obstacle_stop_planner to new test manager

Signed-off-by: mitukou1109 <mitukou1109@gmail.com>

* adapt obstacle_cruise_planner to new test manager

Signed-off-by: mitukou1109 <mitukou1109@gmail.com>

* disable test for freespace_planner

Signed-off-by: mitukou1109 <mitukou1109@gmail.com>

* adapt behavior_velocity_crosswalk_module to new test manager

Signed-off-by: mitukou1109 <mitukou1109@gmail.com>

* adapt behavior_path_lane_change_module to new test manager

Signed-off-by: mitukou1109 <mitukou1109@gmail.com>

* adapt behavior_path_avoidance_by_lane_change_module to new test manager

Signed-off-by: mitukou1109 <mitukou1109@gmail.com>

* adapt behavior_path_dynamic_obstacle_avoidance_module to new test manager

Signed-off-by: mitukou1109 <mitukou1109@gmail.com>

* adapt behavior_path_external_request_lane_change_module to new test manager

Signed-off-by: mitukou1109 <mitukou1109@gmail.com>

* adapt behavior_path_side_shift_module to new test manager

Signed-off-by: mitukou1109 <mitukou1109@gmail.com>

* adapt behavior_path_static_obstacle_avoidance_module to new test manager

Signed-off-by: mitukou1109 <mitukou1109@gmail.com>

* adapt path_smoother to new test manager

Signed-off-by: mitukou1109 <mitukou1109@gmail.com>

* adapt behavior_velocity_blind_spot_module to new test manager

Signed-off-by: mitukou1109 <mitukou1109@gmail.com>

* adapt behavior_velocity_detection_area_module to new test manager

Signed-off-by: mitukou1109 <mitukou1109@gmail.com>

* adapt behavior_velocity_intersection_module to new test manager

Signed-off-by: mitukou1109 <mitukou1109@gmail.com>

* adapt behavior_velocity_no_stopping_area_module to new test manager

Signed-off-by: mitukou1109 <mitukou1109@gmail.com>

* adapt behavior_velocity_run_out_module to new test manager

Signed-off-by: mitukou1109 <mitukou1109@gmail.com>

* adapt behavior_velocity_stop_line_module to new test manager

Signed-off-by: mitukou1109 <mitukou1109@gmail.com>

* adapt behavior_velocity_traffic_light_module to new test manager

Signed-off-by: mitukou1109 <mitukou1109@gmail.com>

* adapt behavior_velocity_virtual_traffic_light_module to new test manager

Signed-off-by: mitukou1109 <mitukou1109@gmail.com>

* adapt behavior_velocity_walkway_module to new test manager

Signed-off-by: mitukou1109 <mitukou1109@gmail.com>

* adapt motion_velocity_planner_node_universe to new test manager

Signed-off-by: mitukou1109 <mitukou1109@gmail.com>

* include necessary headers

Signed-off-by: mitukou1109 <mitukou1109@gmail.com>

* Odometries -> Odometry

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

---------

Signed-off-by: mitukou1109 <mitukou1109@gmail.com>
Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com>
  • Loading branch information
@mitukou1109 @takayuki5168 @SakodaShintaro
2 people authored and SakodaShintaro committed Feb 7, 2025
1 parent 0875dcd commit 6e20d23
Show file tree
Hide file tree
Showing 31 changed files with 635 additions and 833 deletions.
38 changes: 26 additions & 12 deletions planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@
#include <gtest/gtest.h>

#include <memory>
#include <string>
#include <vector>

using autoware::freespace_planner::FreespacePlannerNode;
Expand All @@ -29,10 +30,8 @@ using autoware::planning_test_manager::PlanningInterfaceTestManager;
std::shared_ptr<PlanningInterfaceTestManager> generateTestManager()
{
auto test_manager = std::make_shared<PlanningInterfaceTestManager>();
test_manager->setRouteInputTopicName("freespace_planner/input/route");
test_manager->setTrajectorySubscriber("freespace_planner/output/trajectory");
test_manager->setOdometryTopicName("freespace_planner/input/odometry");
test_manager->setInitialPoseTopicName("freespace_planner/input/odometry");
test_manager->subscribeOutput<autoware_planning_msgs::msg::Trajectory>(
"freespace_planner/output/trajectory");
return test_manager;
}

Expand All @@ -55,10 +54,16 @@ void publishMandatoryTopics(
rclcpp::Node::SharedPtr test_target_node)
{
// publish necessary topics from test_manager
test_manager->publishTF(test_target_node, "/tf");
test_manager->publishOdometry(test_target_node, "freespace_planner/input/odometry");
test_manager->publishOccupancyGrid(test_target_node, "freespace_planner/input/occupancy_grid");
test_manager->publishParkingScenario(test_target_node, "freespace_planner/input/scenario");
test_manager->publishInput(
test_target_node, "/tf", autoware::test_utils::makeTFMsg(test_target_node, "base_link", "map"));
test_manager->publishInput(
test_target_node, "freespace_planner/input/odometry", autoware::test_utils::makeOdometry());
test_manager->publishInput(
test_target_node, "freespace_planner/input/occupancy_grid",
autoware::test_utils::makeCostMapMsg());
test_manager->publishInput(
test_target_node, "freespace_planner/input/scenario",
autoware::test_utils::makeScenarioMsg(tier4_planning_msgs::msg::Scenario::PARKING));
}

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

const std::string input_route_topic = "freespace_planner/input/route";

// test with normal route
ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node));
ASSERT_NO_THROW_WITH_ERROR_MSG(
test_manager->testWithBehaviorNormalRoute(test_target_node, input_route_topic));
EXPECT_GE(test_manager->getReceivedTopicNum(), 1);

// test with empty route
ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalRoute(test_target_node));
ASSERT_NO_THROW_WITH_ERROR_MSG(
test_manager->testWithAbnormalRoute(test_target_node, input_route_topic));
rclcpp::shutdown();
}

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

publishMandatoryTopics(test_manager, test_target_node);

const std::string input_route_topic = "freespace_planner/input/route";
const std::string input_odometry_topic = "freespace_planner/input/odometry";

// test for normal route
ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node));
ASSERT_NO_THROW_WITH_ERROR_MSG(
test_manager->testWithBehaviorNormalRoute(test_target_node, input_route_topic));
EXPECT_GE(test_manager->getReceivedTopicNum(), 1);

ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testRouteWithInvalidEgoPose(test_target_node));
ASSERT_NO_THROW_WITH_ERROR_MSG(
test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic));

rclcpp::shutdown();
}
38 changes: 25 additions & 13 deletions ...ing/autoware_obstacle_cruise_planner/test/test_obstacle_cruise_planner_node_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@
#include <gtest/gtest.h>

#include <memory>
#include <string>
#include <vector>

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

// set subscriber with topic name: obstacle_cruise_planner → test_node_
test_manager->setTrajectorySubscriber("obstacle_cruise_planner/output/trajectory");

// set obstacle_cruise_planners input topic name(this topic is changed to test node):
test_manager->setTrajectoryInputTopicName("obstacle_cruise_planner/input/trajectory");

test_manager->setOdometryTopicName("obstacle_cruise_planner/input/odometry");
test_manager->subscribeOutput<autoware_planning_msgs::msg::Trajectory>(
"obstacle_cruise_planner/output/trajectory");

return test_manager;
}
Expand All @@ -63,9 +60,15 @@ void publishMandatoryTopics(
std::shared_ptr<ObstacleCruisePlannerNode> test_target_node)
{
// publish necessary topics from test_manager
test_manager->publishOdometry(test_target_node, "obstacle_cruise_planner/input/odometry");
test_manager->publishPredictedObjects(test_target_node, "obstacle_cruise_planner/input/objects");
test_manager->publishAcceleration(test_target_node, "obstacle_cruise_planner/input/acceleration");
test_manager->publishInput(
test_target_node, "obstacle_cruise_planner/input/odometry",
autoware::test_utils::makeOdometry());
test_manager->publishInput(
test_target_node, "obstacle_cruise_planner/input/objects",
autoware_perception_msgs::msg::PredictedObjects{});
test_manager->publishInput(
test_target_node, "obstacle_cruise_planner/input/acceleration",
geometry_msgs::msg::AccelWithCovarianceStamped{});
}

TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory)
Expand All @@ -76,12 +79,16 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory)

publishMandatoryTopics(test_manager, test_target_node);

const std::string input_trajectory_topic = "obstacle_cruise_planner/input/trajectory";

// test for normal trajectory
ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node));
ASSERT_NO_THROW_WITH_ERROR_MSG(
test_manager->testWithNormalTrajectory(test_target_node, input_trajectory_topic));
EXPECT_GE(test_manager->getReceivedTopicNum(), 1);

// test for trajectory with empty/one point/overlapping point
ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalTrajectory(test_target_node));
ASSERT_NO_THROW_WITH_ERROR_MSG(
test_manager->testWithAbnormalTrajectory(test_target_node, input_trajectory_topic));

rclcpp::shutdown();
}
Expand All @@ -94,12 +101,17 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose)

publishMandatoryTopics(test_manager, test_target_node);

const std::string input_trajectory_topic = "obstacle_cruise_planner/input/trajectory";
const std::string input_odometry_topic = "obstacle_cruise_planner/input/odometry";

// test for normal trajectory
ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node));
ASSERT_NO_THROW_WITH_ERROR_MSG(
test_manager->testWithNormalTrajectory(test_target_node, input_trajectory_topic));
EXPECT_GE(test_manager->getReceivedTopicNum(), 1);

// test for trajectory with empty/one point/overlapping point
ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testTrajectoryWithInvalidEgoPose(test_target_node));
ASSERT_NO_THROW_WITH_ERROR_MSG(
test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic));

rclcpp::shutdown();
}
54 changes: 30 additions & 24 deletions ...utoware_obstacle_stop_planner/test/test_autoware_obstacle_stop_planner_node_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@
#include <gtest/gtest.h>

#include <memory>
#include <string>
#include <vector>

using autoware::motion_planning::ObstacleStopPlannerNode;
Expand All @@ -32,9 +33,8 @@ using tier4_planning_msgs::msg::ExpandStopRange;
std::shared_ptr<PlanningInterfaceTestManager> generateTestManager()
{
auto test_manager = std::make_shared<PlanningInterfaceTestManager>();
test_manager->setTrajectorySubscriber("obstacle_stop_planner/output/trajectory");
test_manager->setTrajectoryInputTopicName("obstacle_stop_planner/input/trajectory");
test_manager->setOdometryTopicName("obstacle_stop_planner/input/odometry");
test_manager->subscribeOutput<autoware_planning_msgs::msg::Trajectory>(
"obstacle_stop_planner/output/trajectory");
return test_manager;
}

Expand Down Expand Up @@ -65,21 +65,21 @@ void publishMandatoryTopics(
std::shared_ptr<ObstacleStopPlannerNode> test_target_node)
{
// publish necessary topics from test_manager
test_manager->publishOdometry(test_target_node, "obstacle_stop_planner/input/odometry");
test_manager->publishPointCloud(test_target_node, "obstacle_stop_planner/input/pointcloud");
test_manager->publishAcceleration(test_target_node, "obstacle_stop_planner/input/acceleration");
test_manager->publishPredictedObjects(test_target_node, "obstacle_stop_planner/input/objects");
}

void publishExpandStopRange(
std::shared_ptr<PlanningInterfaceTestManager> test_manager,
std::shared_ptr<ObstacleStopPlannerNode> test_target_node)
{
auto test_node = test_manager->getTestNode();
const auto expand_stop_range = test_manager->getTestNode()->create_publisher<ExpandStopRange>(
"obstacle_stop_planner/input/expand_stop_range", 1);
expand_stop_range->publish(ExpandStopRange{});
autoware::test_utils::spinSomeNodes(test_node, test_target_node, 3);
test_manager->publishInput(
test_target_node, "obstacle_stop_planner/input/odometry", autoware::test_utils::makeOdometry());
test_manager->publishInput(
test_target_node, "obstacle_stop_planner/input/pointcloud",
sensor_msgs::msg::PointCloud2{}.set__header(
std_msgs::msg::Header{}.set__frame_id("base_link")));
test_manager->publishInput(
test_target_node, "obstacle_stop_planner/input/acceleration",
geometry_msgs::msg::AccelWithCovarianceStamped{});
test_manager->publishInput(
test_target_node, "obstacle_stop_planner/input/objects",
autoware_perception_msgs::msg::PredictedObjects{});
test_manager->publishInput(
test_target_node, "obstacle_stop_planner/input/expand_stop_range",
tier4_planning_msgs::msg::ExpandStopRange{});
}

TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory)
Expand All @@ -90,15 +90,17 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory)
auto test_target_node = generateNode();

publishMandatoryTopics(test_manager, test_target_node);
publishExpandStopRange(test_manager, test_target_node);

const std::string input_trajectory_topic = "obstacle_stop_planner/input/trajectory";

// test for normal trajectory
ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node));
ASSERT_NO_THROW_WITH_ERROR_MSG(
test_manager->testWithNormalTrajectory(test_target_node, input_trajectory_topic));

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

// test for trajectory with empty/one point/overlapping point
test_manager->testWithAbnormalTrajectory(test_target_node);
test_manager->testWithAbnormalTrajectory(test_target_node, input_trajectory_topic);

rclcpp::shutdown();
}
Expand All @@ -110,14 +112,18 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose)
auto test_target_node = generateNode();

publishMandatoryTopics(test_manager, test_target_node);
publishExpandStopRange(test_manager, test_target_node);

const std::string input_trajectory_topic = "obstacle_stop_planner/input/trajectory";
const std::string input_odometry_topic = "obstacle_stop_planner/input/odometry";

// test for normal trajectory
ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node));
ASSERT_NO_THROW_WITH_ERROR_MSG(
test_manager->testWithNormalTrajectory(test_target_node, input_trajectory_topic));
EXPECT_GE(test_manager->getReceivedTopicNum(), 1);

// test for trajectory with empty/one point/overlapping point
ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testTrajectoryWithInvalidEgoPose(test_target_node));
ASSERT_NO_THROW_WITH_ERROR_MSG(
test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic));

rclcpp::shutdown();
}
16 changes: 10 additions & 6 deletions planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@
#include <gtest/gtest.h>

#include <memory>
#include <string>
#include <vector>

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

// publish necessary topics from test_manager
test_manager->publishOdometry(test_target_node, "path_optimizer/input/odometry");
test_manager->publishInput(
test_target_node, "path_optimizer/input/odometry", autoware::test_utils::makeOdometry());

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

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

// test with normal trajectory
ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPath(test_target_node));
ASSERT_NO_THROW_WITH_ERROR_MSG(
test_manager->testWithNormalPath(test_target_node, input_trajectory_topic));

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

// test with trajectory with empty/one point/overlapping point
ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPath(test_target_node));
ASSERT_NO_THROW_WITH_ERROR_MSG(
test_manager->testWithAbnormalPath(test_target_node, input_trajectory_topic));

rclcpp::shutdown();
}
20 changes: 13 additions & 7 deletions planning/autoware_path_smoother/test/test_autoware_path_smoother_node_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@
#include <gtest/gtest.h>

#include <memory>
#include <string>
#include <vector>

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

// publish necessary topics from test_manager
test_manager->publishOdometry(test_target_node, "autoware_path_smoother/input/odometry");
test_manager->publishInput(
test_target_node, "autoware_path_smoother/input/odometry",
autoware::test_utils::makeOdometry());

// set subscriber with topic name
test_manager->setTrajectorySubscriber("autoware_path_smoother/output/traj");
test_manager->setPathSubscriber("autoware_path_smoother/output/path");
test_manager->subscribeOutput<autoware_planning_msgs::msg::Trajectory>(
"autoware_path_smoother/output/traj");
test_manager->subscribeOutput<autoware_planning_msgs::msg::Path>(
"autoware_path_smoother/output/path");

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

// test with normal trajectory
ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPath(test_target_node));
ASSERT_NO_THROW_WITH_ERROR_MSG(
test_manager->testWithNormalPath(test_target_node, input_path_topic));

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

// test with trajectory with empty/one point/overlapping point
ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPath(test_target_node));
ASSERT_NO_THROW_WITH_ERROR_MSG(
test_manager->testWithAbnormalPath(test_target_node, input_path_topic));

rclcpp::shutdown();
}
Loading

0 comments on commit 6e20d23

Please sign in to comment.