From 4b166614d1dc6e372498f52e2fba14f4f79a03fa Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sun, 19 Jan 2025 17:40:26 +0900 Subject: [PATCH] feat(autoware_planning_test_manager): remove dependency of VirtualTrafficLightState and ExpandStopRange (#9953) * feat(autoware_planning_test_manager): remove dependency of virtual traffic light Signed-off-by: Takayuki Murooka * modify obstacle_stop test code Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- ...e_obstacle_stop_planner_node_interface.cpp | 18 +++++++++++++-- .../autoware_planning_test_manager.hpp | 13 +---------- .../package.xml | 1 - .../src/autoware_planning_test_manager.cpp | 20 ++++------------ .../test/test_node_interface.cpp | 23 +++++++++++++++---- 5 files changed, 41 insertions(+), 34 deletions(-) diff --git a/planning/autoware_obstacle_stop_planner/test/test_autoware_obstacle_stop_planner_node_interface.cpp b/planning/autoware_obstacle_stop_planner/test/test_autoware_obstacle_stop_planner_node_interface.cpp index 9f840586a1959..9da8409c6c2ae 100644 --- a/planning/autoware_obstacle_stop_planner/test/test_autoware_obstacle_stop_planner_node_interface.cpp +++ b/planning/autoware_obstacle_stop_planner/test/test_autoware_obstacle_stop_planner_node_interface.cpp @@ -18,6 +18,8 @@ #include #include +#include + #include #include @@ -25,6 +27,7 @@ using autoware::motion_planning::ObstacleStopPlannerNode; using autoware::planning_test_manager::PlanningInterfaceTestManager; +using tier4_planning_msgs::msg::ExpandStopRange; std::shared_ptr generateTestManager() { @@ -66,8 +69,17 @@ void publishMandatoryTopics( 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"); - test_manager->publishExpandStopRange( - test_target_node, "obstacle_stop_planner/input/expand_stop_range"); +} + +void publishExpandStopRange( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ + auto test_node = test_manager->getTestNode(); + const auto expand_stop_range = test_manager->getTestNode()->create_publisher( + "obstacle_stop_planner/input/expand_stop_range", 1); + expand_stop_range->publish(ExpandStopRange{}); + autoware::test_utils::spinSomeNodes(test_node, test_target_node, 3); } TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) @@ -78,6 +90,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) auto test_target_node = generateNode(); publishMandatoryTopics(test_manager, test_target_node); + publishExpandStopRange(test_manager, test_target_node); // test for normal trajectory ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node)); @@ -97,6 +110,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) auto test_target_node = generateNode(); publishMandatoryTopics(test_manager, test_target_node); + publishExpandStopRange(test_manager, test_target_node); // test for normal trajectory ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node)); diff --git a/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager.hpp b/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager.hpp index 2c9a935c2077b..aa7836bc66b88 100644 --- a/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager.hpp +++ b/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager.hpp @@ -52,14 +52,10 @@ #include #include #include -#include -#include -#include #include #include #include #include -#include #include #include @@ -87,14 +83,10 @@ using nav_msgs::msg::OccupancyGrid; using nav_msgs::msg::Odometry; using sensor_msgs::msg::PointCloud2; using tf2_msgs::msg::TFMessage; -using tier4_api_msgs::msg::CrosswalkStatus; -using tier4_api_msgs::msg::IntersectionStatus; -using tier4_planning_msgs::msg::ExpandStopRange; using tier4_planning_msgs::msg::LateralOffset; using tier4_planning_msgs::msg::PathWithLaneId; using tier4_planning_msgs::msg::Scenario; using tier4_planning_msgs::msg::VelocityLimit; -using tier4_v2x_msgs::msg::VirtualTrafficLightStateArray; enum class ModuleName { UNKNOWN = 0, @@ -117,7 +109,6 @@ class PlanningInterfaceTestManager void publishPointCloud(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishAcceleration(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishPredictedObjects(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishExpandStopRange(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishOccupancyGrid(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishCostMap(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishMap(rclcpp::Node::SharedPtr target_node, std::string topic_name); @@ -131,7 +122,6 @@ class PlanningInterfaceTestManager void publishLateralOffset(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishOperationModeState(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishTrafficSignals(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishVirtualTrafficLightState(rclcpp::Node::SharedPtr target_node, std::string topic_name); void setTrajectoryInputTopicName(std::string topic_name); void setParkingTrajectoryInputTopicName(std::string topic_name); @@ -178,6 +168,7 @@ class PlanningInterfaceTestManager void testOffTrackFromTrajectory(rclcpp::Node::SharedPtr target_node); int getReceivedTopicNum(); + rclcpp::Node::SharedPtr getTestNode() const; private: // Publisher (necessary for node running) @@ -187,7 +178,6 @@ class PlanningInterfaceTestManager rclcpp::Publisher::SharedPtr point_cloud_pub_; rclcpp::Publisher::SharedPtr acceleration_pub_; rclcpp::Publisher::SharedPtr predicted_objects_pub_; - rclcpp::Publisher::SharedPtr expand_stop_range_pub_; rclcpp::Publisher::SharedPtr occupancy_grid_pub_; rclcpp::Publisher::SharedPtr cost_map_pub_; rclcpp::Publisher::SharedPtr map_pub_; @@ -202,7 +192,6 @@ class PlanningInterfaceTestManager rclcpp::Publisher::SharedPtr lateral_offset_pub_; rclcpp::Publisher::SharedPtr operation_mode_state_pub_; rclcpp::Publisher::SharedPtr traffic_signals_pub_; - rclcpp::Publisher::SharedPtr virtual_traffic_light_states_pub_; // Subscriber rclcpp::Subscription::SharedPtr traj_sub_; diff --git a/planning/autoware_planning_test_manager/package.xml b/planning/autoware_planning_test_manager/package.xml index 80801a8102453..d3395a6c518f3 100644 --- a/planning/autoware_planning_test_manager/package.xml +++ b/planning/autoware_planning_test_manager/package.xml @@ -31,7 +31,6 @@ rclcpp tf2_msgs tf2_ros - tier4_api_msgs tier4_planning_msgs tier4_v2x_msgs unique_identifier_msgs diff --git a/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp b/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp index 62f89cab44e7b..9bc73f3455821 100644 --- a/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp +++ b/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp @@ -71,13 +71,6 @@ void PlanningInterfaceTestManager::publishPredictedObjects( test_node_, target_node, topic_name, predicted_objects_pub_, PredictedObjects{}); } -void PlanningInterfaceTestManager::publishExpandStopRange( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, expand_stop_range_pub_, ExpandStopRange{}); -} - void PlanningInterfaceTestManager::publishOccupancyGrid( rclcpp::Node::SharedPtr target_node, std::string topic_name) { @@ -181,14 +174,6 @@ void PlanningInterfaceTestManager::publishTrafficSignals( test_node_, target_node, topic_name, traffic_signals_pub_, TrafficLightGroupArray{}); } -void PlanningInterfaceTestManager::publishVirtualTrafficLightState( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, virtual_traffic_light_states_pub_, - VirtualTrafficLightStateArray{}); -} - void PlanningInterfaceTestManager::publishInitialPoseTF( rclcpp::Node::SharedPtr target_node, std::string topic_name) { @@ -471,4 +456,9 @@ int PlanningInterfaceTestManager::getReceivedTopicNum() return count_; } +rclcpp::Node::SharedPtr PlanningInterfaceTestManager::getTestNode() const +{ + return test_node_; +} + } // namespace autoware::planning_test_manager diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_node_interface.cpp index ab2fd5847c5be..0e22413c0c8ec 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_node_interface.cpp @@ -13,6 +13,9 @@ // limitations under the License. #include +#include + +#include #include @@ -23,6 +26,19 @@ namespace autoware::behavior_velocity_planner { +using tier4_v2x_msgs::msg::VirtualTrafficLightStateArray; + +void publishVirtualTrafficLightState( + std::shared_ptr test_manager, rclcpp::Node::SharedPtr target_node) +{ + auto test_node = test_manager->getTestNode(); + const auto pub_virtual_traffic_light = + test_manager->getTestNode()->create_publisher( + "behavior_velocity_planner_node/input/virtual_traffic_light_states", 1); + pub_virtual_traffic_light->publish(VirtualTrafficLightStateArray{}); + autoware::test_utils::spinSomeNodes(test_node, target_node, 3); +} + TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) { rclcpp::init(0, nullptr); @@ -35,8 +51,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); - test_manager->publishVirtualTrafficLightState( - test_target_node, "behavior_velocity_planner_node/input/virtual_traffic_light_states"); + publishVirtualTrafficLightState(test_manager, test_target_node); // test with nominal path_with_lane_id ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); @@ -57,9 +72,9 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); - test_manager->publishVirtualTrafficLightState( - test_target_node, "behavior_velocity_planner_node/input/virtual_traffic_light_states"); + publishVirtualTrafficLightState(test_manager, test_target_node); // test for normal trajectory ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node));