diff --git a/common/autoware_test_utils/package.xml b/common/autoware_test_utils/package.xml index 740e7f840141d..f67b366480b00 100644 --- a/common/autoware_test_utils/package.xml +++ b/common/autoware_test_utils/package.xml @@ -33,9 +33,7 @@ std_srvs tf2_msgs tf2_ros - tier4_api_msgs tier4_planning_msgs - tier4_v2x_msgs unique_identifier_msgs yaml_cpp_vendor 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 aa7836bc66b88..bb808484e10be 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,7 +52,6 @@ #include #include #include -#include #include #include #include @@ -83,7 +82,6 @@ using nav_msgs::msg::OccupancyGrid; using nav_msgs::msg::Odometry; using sensor_msgs::msg::PointCloud2; using tf2_msgs::msg::TFMessage; -using tier4_planning_msgs::msg::LateralOffset; using tier4_planning_msgs::msg::PathWithLaneId; using tier4_planning_msgs::msg::Scenario; using tier4_planning_msgs::msg::VelocityLimit; @@ -119,7 +117,6 @@ class PlanningInterfaceTestManager void publishRoute(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishTF(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishInitialPoseTF(rclcpp::Node::SharedPtr target_node, std::string topic_name); - 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); @@ -189,7 +186,6 @@ class PlanningInterfaceTestManager rclcpp::Publisher::SharedPtr route_pub_; rclcpp::Publisher::SharedPtr TF_pub_; rclcpp::Publisher::SharedPtr initial_pose_tf_pub_; - rclcpp::Publisher::SharedPtr lateral_offset_pub_; rclcpp::Publisher::SharedPtr operation_mode_state_pub_; rclcpp::Publisher::SharedPtr traffic_signals_pub_; diff --git a/planning/autoware_planning_test_manager/package.xml b/planning/autoware_planning_test_manager/package.xml index d3395a6c518f3..d5f8383913274 100644 --- a/planning/autoware_planning_test_manager/package.xml +++ b/planning/autoware_planning_test_manager/package.xml @@ -32,7 +32,6 @@ tf2_msgs tf2_ros tier4_planning_msgs - tier4_v2x_msgs unique_identifier_msgs yaml_cpp_vendor 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 9bc73f3455821..f07efd0cda116 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 @@ -153,13 +153,6 @@ void PlanningInterfaceTestManager::publishTF( autoware::test_utils::makeTFMsg(target_node, "base_link", "map")); } -void PlanningInterfaceTestManager::publishLateralOffset( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, lateral_offset_pub_, LateralOffset{}); -} - void PlanningInterfaceTestManager::publishOperationModeState( rclcpp::Node::SharedPtr target_node, std::string topic_name) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/test_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/test_utils.cpp index 2ce6f5bee9a50..4930ead6d355d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/test_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/test_utils.cpp @@ -18,12 +18,30 @@ #include #include +#include + #include #include #include namespace autoware::behavior_path_planner { +namespace +{ +using tier4_planning_msgs::msg::LateralOffset; + +void publishLateralOffset( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ + auto test_node = test_manager->getTestNode(); + const auto pub_lateral_offset = test_manager->getTestNode()->create_publisher( + "behavior_path_planner/input/lateral_offset", 1); + pub_lateral_offset->publish(LateralOffset{}); + autoware::test_utils::spinSomeNodes(test_node, test_target_node, 3); +} +} // namespace + std::shared_ptr generateTestManager() { auto test_manager = std::make_shared(); @@ -95,7 +113,6 @@ void publishMandatoryTopics( test_manager->publishMap(test_target_node, "behavior_path_planner/input/vector_map"); test_manager->publishCostMap(test_target_node, "behavior_path_planner/input/costmap"); test_manager->publishOperationModeState(test_target_node, "system/operation_mode/state"); - test_manager->publishLateralOffset( - test_target_node, "behavior_path_planner/input/lateral_offset"); + publishLateralOffset(test_manager, test_target_node); } } // namespace autoware::behavior_path_planner