From e5bd5d1c28ce0805089ca3f5d1f72a39012d39d3 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Thu, 9 Jan 2025 17:33:36 +0900 Subject: [PATCH 01/33] abstract message-specific functions Signed-off-by: mitukou1109 --- .../autoware_planning_test_manager.hpp | 245 +++------- .../src/autoware_planning_test_manager.cpp | 435 +++--------------- 2 files changed, 107 insertions(+), 573 deletions(-) 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 cf45154893af7..87aaf2f6db7a3 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 @@ -32,30 +32,9 @@ #include #include +#include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - #include #include #include @@ -66,187 +45,71 @@ namespace autoware::planning_test_manager { -using autoware_adapi_v1_msgs::msg::OperationModeState; -using autoware_map_msgs::msg::LaneletMapBin; -using autoware_perception_msgs::msg::PredictedObjects; -using autoware_perception_msgs::msg::TrafficLightGroupArray; -using autoware_planning_msgs::msg::LaneletRoute; -using autoware_planning_msgs::msg::Path; -using autoware_planning_msgs::msg::Trajectory; -using geometry_msgs::msg::AccelWithCovarianceStamped; -using geometry_msgs::msg::Point; -using geometry_msgs::msg::PoseStamped; -using geometry_msgs::msg::Quaternion; -using geometry_msgs::msg::TransformStamped; -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::PathWithLaneId; -using tier4_planning_msgs::msg::Scenario; -using tier4_planning_msgs::msg::VelocityLimit; - -enum class ModuleName { - UNKNOWN = 0, - START_PLANNER, -}; - class PlanningInterfaceTestManager { public: PlanningInterfaceTestManager(); - void publishOdometry( - rclcpp::Node::SharedPtr target_node, std::string topic_name, const double shift = 0.0); - - void publishInitialPose( - rclcpp::Node::SharedPtr target_node, std::string topic_name, const double shift = 0.0, - ModuleName module_name = ModuleName::UNKNOWN); - - void publishMaxVelocity(rclcpp::Node::SharedPtr target_node, std::string topic_name); - 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 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); - void publishLaneDrivingScenario(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishParkingScenario(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishParkingState(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishTrajectory(rclcpp::Node::SharedPtr target_node, std::string topic_name); - 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 publishOperationModeState(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishTrafficSignals(rclcpp::Node::SharedPtr target_node, std::string topic_name); - - void setTrajectoryInputTopicName(std::string topic_name); - void setParkingTrajectoryInputTopicName(std::string topic_name); - void setLaneDrivingTrajectoryInputTopicName(std::string topic_name); - void setRouteInputTopicName(std::string topic_name); - void setPathInputTopicName(std::string topic_name); - void setPathWithLaneIdTopicName(std::string topic_name); - void setPathTopicName(std::string topic_name); - - void setTrajectorySubscriber(std::string topic_name); - void setScenarioSubscriber(std::string topic_name); - void setPathWithLaneIdSubscriber(std::string topic_name); - void setRouteSubscriber(std::string topic_name); - void setPathSubscriber(std::string topic_name); - - void setInitialPoseTopicName(std::string topic_name); - void setOdometryTopicName(std::string topic_name); - - void testWithNominalTrajectory(rclcpp::Node::SharedPtr target_node); - void testWithAbnormalTrajectory(rclcpp::Node::SharedPtr target_node); - - void testWithNominalRoute(rclcpp::Node::SharedPtr target_node); - void testWithAbnormalRoute(rclcpp::Node::SharedPtr target_node); - - void testWithBehaviorNominalRoute( - rclcpp::Node::SharedPtr target_node, ModuleName module_name = ModuleName::UNKNOWN); - - void testWithNominalPathWithLaneId(rclcpp::Node::SharedPtr target_node); - void testWithAbnormalPathWithLaneId(rclcpp::Node::SharedPtr target_node); - - void testWithNominalPath(rclcpp::Node::SharedPtr target_node); - void testWithAbnormalPath(rclcpp::Node::SharedPtr target_node); - - // for invalid ego poses, contains some tests inside. - void testRouteWithInvalidEgoPose(rclcpp::Node::SharedPtr target_node); - void testPathWithInvalidEgoPose(rclcpp::Node::SharedPtr target_node); - void testPathWithLaneIdWithInvalidEgoPose(rclcpp::Node::SharedPtr target_node); - void testTrajectoryWithInvalidEgoPose(rclcpp::Node::SharedPtr target_node); - - // ego vehicle is located far from trajectory - void testOffTrackFromRoute(rclcpp::Node::SharedPtr target_node); - void testOffTrackFromPath(rclcpp::Node::SharedPtr target_node); - void testOffTrackFromPathWithLaneId(rclcpp::Node::SharedPtr target_node); - void testOffTrackFromTrajectory(rclcpp::Node::SharedPtr target_node); - - int getReceivedTopicNum(); - rclcpp::Node::SharedPtr getTestNode() const; + template + void publishInput( + const rclcpp::Node::SharedPtr target_node, const std::string & topic_name, const InputT & input, + const int repeat_count = 3) const + { + autoware::test_utils::publishToTargetNode( + test_node_, target_node, topic_name, {}, input, repeat_count); + } -private: - // Publisher (necessary for node running) - rclcpp::Publisher::SharedPtr odom_pub_; - rclcpp::Publisher::SharedPtr initial_pose_pub_; - rclcpp::Publisher::SharedPtr max_velocity_pub_; - rclcpp::Publisher::SharedPtr point_cloud_pub_; - rclcpp::Publisher::SharedPtr acceleration_pub_; - rclcpp::Publisher::SharedPtr predicted_objects_pub_; - rclcpp::Publisher::SharedPtr occupancy_grid_pub_; - rclcpp::Publisher::SharedPtr cost_map_pub_; - rclcpp::Publisher::SharedPtr map_pub_; - rclcpp::Publisher::SharedPtr scenario_pub_; - rclcpp::Publisher::SharedPtr parking_scenario_pub_; - rclcpp::Publisher::SharedPtr lane_driving_scenario_pub_; - rclcpp::Publisher::SharedPtr parking_state_pub_; - rclcpp::Publisher::SharedPtr trajectory_pub_; - rclcpp::Publisher::SharedPtr route_pub_; - rclcpp::Publisher::SharedPtr TF_pub_; - rclcpp::Publisher::SharedPtr initial_pose_tf_pub_; - rclcpp::Publisher::SharedPtr operation_mode_state_pub_; - rclcpp::Publisher::SharedPtr traffic_signals_pub_; + template + void subscribeOutput(const std::string & topic_name, typename OutputT::ConstSharedPtr buffer = {}) + { + test_output_subs_.push_back(test_node_->create_subscription( + topic_name, 1, [&](const typename OutputT::ConstSharedPtr msg) { + if (buffer) { + buffer = msg; + } + received_topic_num_++; + })); + } + + void testWithNormalTrajectory( + rclcpp::Node::SharedPtr target_node, const std::string & topic_name); + void testWithAbnormalTrajectory( + rclcpp::Node::SharedPtr target_node, const std::string & topic_name); + + void testWithNormalRoute(rclcpp::Node::SharedPtr target_node, const std::string & topic_name); + void testWithAbnormalRoute(rclcpp::Node::SharedPtr target_node, const std::string & topic_name); + + void testWithBehaviorNormalRoute( + rclcpp::Node::SharedPtr target_node, const std::string & topic_name); + void testWithNormalPathWithLaneId( + rclcpp::Node::SharedPtr target_node, const std::string & topic_name); + void testWithAbnormalPathWithLaneId( + rclcpp::Node::SharedPtr target_node, const std::string & topic_name); + + void testWithNormalPath(rclcpp::Node::SharedPtr target_node, const std::string & topic_name); + void testWithAbnormalPath(rclcpp::Node::SharedPtr target_node, const std::string & topic_name); + + void testWithOffTrackInitialPoses( + rclcpp::Node::SharedPtr target_node, const std::string & topic_name); + + void testWithOffTrackOdometries( + rclcpp::Node::SharedPtr target_node, const std::string & topic_name); + + void resetReceivedTopicNum() { received_topic_num_ = 0; } + + size_t getReceivedTopicNum() const { return received_topic_num_; } + + rclcpp::Node::SharedPtr getTestNode() const { return test_node_; } + +private: // Subscriber - rclcpp::Subscription::SharedPtr traj_sub_; - rclcpp::Subscription::SharedPtr route_sub_; - rclcpp::Subscription::SharedPtr scenario_sub_; - rclcpp::Subscription::SharedPtr path_with_lane_id_sub_; - rclcpp::Subscription::SharedPtr path_sub_; - - // Publisher for testing(trajectory) - rclcpp::Publisher::SharedPtr normal_trajectory_pub_; - rclcpp::Publisher::SharedPtr abnormal_trajectory_pub_; - - // Publisher for testing(route) - rclcpp::Publisher::SharedPtr normal_route_pub_; - rclcpp::Publisher::SharedPtr abnormal_route_pub_; - - // Publisher for testing(route) - rclcpp::Publisher::SharedPtr behavior_normal_route_pub_; - - // Publisher for testing(PathWithLaneId) - rclcpp::Publisher::SharedPtr normal_path_with_lane_id_pub_; - rclcpp::Publisher::SharedPtr abnormal_path_with_lane_id_pub_; - - // Publisher for testing(Path) - rclcpp::Publisher::SharedPtr normal_path_pub_; - rclcpp::Publisher::SharedPtr abnormal_path_pub_; - - std::string input_trajectory_name_ = ""; - std::string input_parking_trajectory_name_ = ""; - std::string input_lane_driving_trajectory_name_ = ""; - std::string input_route_name_ = ""; - std::string input_path_name_ = ""; - std::string input_path_with_lane_id_name_ = ""; - std::string input_initial_pose_name_ = ""; // for the map based pose - std::string input_odometry_name_ = ""; + std::vector test_output_subs_; // Node rclcpp::Node::SharedPtr test_node_; - std::string map_frame_ = "map"; - size_t count_{0}; - std::shared_ptr tf_buffer_; - std::shared_ptr tf_listener_; - void publishNominalTrajectory(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishAbnormalTrajectory( - rclcpp::Node::SharedPtr target_node, const Trajectory & abnormal_trajectory); - - void publishNominalRoute(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishAbnormalRoute( - rclcpp::Node::SharedPtr target_node, const LaneletRoute & abnormal_route); - - void publishBehaviorNominalRoute( - rclcpp::Node::SharedPtr target_node, std::string topic_name, - ModuleName module_name = ModuleName::UNKNOWN); - void publishNominalPathWithLaneId(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishAbNominalPathWithLaneId(rclcpp::Node::SharedPtr target_node, std::string topic_name); - - void publishNominalPath(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishAbnormalPath(rclcpp::Node::SharedPtr target_node, std::string topic_name); + size_t received_topic_num_ = 0; }; // class PlanningInterfaceTestManager } // namespace autoware::planning_test_manager 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 f07efd0cda116..489c3a067afb9 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 @@ -12,12 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware_planning_test_manager/autoware_planning_test_manager.hpp" -#include -#include -#include -#include +#include + +#include +#include +#include +#include #include #include @@ -26,432 +28,101 @@ namespace autoware::planning_test_manager { +using autoware_planning_msgs::msg::LaneletRoute; +using autoware_planning_msgs::msg::Path; +using autoware_planning_msgs::msg::Trajectory; +using tier4_planning_msgs::msg::PathWithLaneId; PlanningInterfaceTestManager::PlanningInterfaceTestManager() { test_node_ = std::make_shared("planning_interface_test_node"); - tf_buffer_ = std::make_shared(test_node_->get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); -} - -void PlanningInterfaceTestManager::publishOdometry( - rclcpp::Node::SharedPtr target_node, std::string topic_name, const double shift) -{ - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, odom_pub_, autoware::test_utils::makeOdometry(shift)); -} - -void PlanningInterfaceTestManager::publishMaxVelocity( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, max_velocity_pub_, VelocityLimit{}); -} - -void PlanningInterfaceTestManager::publishPointCloud( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - PointCloud2 point_cloud_msg{}; - point_cloud_msg.header.frame_id = "base_link"; - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, point_cloud_pub_, point_cloud_msg); } -void PlanningInterfaceTestManager::publishAcceleration( - rclcpp::Node::SharedPtr target_node, std::string topic_name) +void PlanningInterfaceTestManager::testWithNormalTrajectory( + rclcpp::Node::SharedPtr target_node, const std::string & topic_name) { - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, acceleration_pub_, AccelWithCovarianceStamped{}); + publishInput( + target_node, topic_name, autoware::test_utils::generateTrajectory(10, 1.0), 5); } -void PlanningInterfaceTestManager::publishPredictedObjects( - rclcpp::Node::SharedPtr target_node, std::string topic_name) +void PlanningInterfaceTestManager::testWithAbnormalTrajectory( + rclcpp::Node::SharedPtr target_node, const std::string & topic_name) { - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, predicted_objects_pub_, PredictedObjects{}); + publishInput(target_node, topic_name, Trajectory{}, 5); + publishInput( + target_node, topic_name, autoware::test_utils::generateTrajectory(1, 0.0), 5); + publishInput( + target_node, topic_name, + autoware::test_utils::generateTrajectory(10, 0.0, 0.0, 0.0, 0.0, 1), 5); } -void PlanningInterfaceTestManager::publishOccupancyGrid( - rclcpp::Node::SharedPtr target_node, std::string topic_name) +void PlanningInterfaceTestManager::testWithNormalRoute( + rclcpp::Node::SharedPtr target_node, const std::string & topic_name) { - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, occupancy_grid_pub_, - autoware::test_utils::makeCostMapMsg()); + publishInput(target_node, topic_name, autoware::test_utils::makeNormalRoute(), 5); } -void PlanningInterfaceTestManager::publishCostMap( - rclcpp::Node::SharedPtr target_node, std::string topic_name) +void PlanningInterfaceTestManager::testWithAbnormalRoute( + rclcpp::Node::SharedPtr target_node, const std::string & topic_name) { - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, cost_map_pub_, autoware::test_utils::makeCostMapMsg()); + publishInput(target_node, topic_name, LaneletRoute{}, 5); } -void PlanningInterfaceTestManager::publishMap( - rclcpp::Node::SharedPtr target_node, std::string topic_name) +void PlanningInterfaceTestManager::testWithBehaviorNormalRoute( + rclcpp::Node::SharedPtr target_node, const std::string & topic_name) { - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, map_pub_, autoware::test_utils::makeMapBinMsg()); -} - -void PlanningInterfaceTestManager::publishLaneDrivingScenario( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, lane_driving_scenario_pub_, - autoware::test_utils::makeScenarioMsg(Scenario::LANEDRIVING)); -} - -void PlanningInterfaceTestManager::publishParkingScenario( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, parking_scenario_pub_, - autoware::test_utils::makeScenarioMsg(Scenario::PARKING)); -} - -void PlanningInterfaceTestManager::publishInitialPose( - rclcpp::Node::SharedPtr target_node, std::string topic_name, const double shift, - ModuleName module_name) -{ - if (module_name == ModuleName::START_PLANNER) { - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, initial_pose_pub_, - autoware_planning_test_manager::utils::makeInitialPoseFromLaneId(10291)); - } else { - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, initial_pose_pub_, - autoware::test_utils::makeInitialPose(shift)); - } + publishInput(target_node, topic_name, autoware::test_utils::makeBehaviorNormalRoute(), 5); } -void PlanningInterfaceTestManager::publishParkingState( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, parking_state_pub_, std_msgs::msg::Bool{}); -} - -void PlanningInterfaceTestManager::publishTrajectory( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, trajectory_pub_, Trajectory{}); -} - -void PlanningInterfaceTestManager::publishRoute( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, route_pub_, autoware::test_utils::makeNormalRoute()); -} - -void PlanningInterfaceTestManager::publishTF( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, TF_pub_, - autoware::test_utils::makeTFMsg(target_node, "base_link", "map")); -} - -void PlanningInterfaceTestManager::publishOperationModeState( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, operation_mode_state_pub_, OperationModeState{}); -} - -void PlanningInterfaceTestManager::publishTrafficSignals( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, traffic_signals_pub_, TrafficLightGroupArray{}); -} - -void PlanningInterfaceTestManager::publishInitialPoseTF( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, initial_pose_tf_pub_, - autoware::test_utils::makeTFMsg(target_node, "odom", "base_link")); -} - -void PlanningInterfaceTestManager::setTrajectoryInputTopicName(std::string topic_name) -{ - input_trajectory_name_ = topic_name; -} - -void PlanningInterfaceTestManager::setParkingTrajectoryInputTopicName(std::string topic_name) -{ - input_parking_trajectory_name_ = topic_name; -} - -void PlanningInterfaceTestManager::setLaneDrivingTrajectoryInputTopicName(std::string topic_name) -{ - input_lane_driving_trajectory_name_ = topic_name; -} - -void PlanningInterfaceTestManager::setRouteInputTopicName(std::string topic_name) -{ - input_route_name_ = topic_name; -} - -void PlanningInterfaceTestManager::setPathInputTopicName(std::string topic_name) -{ - input_path_name_ = topic_name; -} - -void PlanningInterfaceTestManager::setPathWithLaneIdTopicName(std::string topic_name) -{ - input_path_with_lane_id_name_ = topic_name; -} - -void PlanningInterfaceTestManager::setInitialPoseTopicName(std::string topic_name) -{ - input_initial_pose_name_ = topic_name; -} - -void PlanningInterfaceTestManager::setOdometryTopicName(std::string topic_name) -{ - input_odometry_name_ = topic_name; -} - -void PlanningInterfaceTestManager::publishNominalTrajectory( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, normal_trajectory_pub_, - autoware::test_utils::generateTrajectory(10, 1.0), 5); -} - -void PlanningInterfaceTestManager::publishAbnormalTrajectory( - rclcpp::Node::SharedPtr target_node, const Trajectory & abnormal_trajectory) -{ - autoware::test_utils::publishToTargetNode( - test_node_, target_node, input_trajectory_name_, abnormal_trajectory_pub_, abnormal_trajectory); -} - -void PlanningInterfaceTestManager::publishNominalRoute( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, normal_route_pub_, autoware::test_utils::makeNormalRoute(), - 5); -} - -void PlanningInterfaceTestManager::publishBehaviorNominalRoute( - rclcpp::Node::SharedPtr target_node, std::string topic_name, ModuleName module_name) -{ - if (module_name == ModuleName::START_PLANNER) { - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, behavior_normal_route_pub_, - autoware_planning_test_manager::utils::makeBehaviorRouteFromLaneId(10291, 10333), 5); - } else { - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, behavior_normal_route_pub_, - autoware::test_utils::makeBehaviorNormalRoute(), 5); - } -} - -void PlanningInterfaceTestManager::publishAbnormalRoute( - rclcpp::Node::SharedPtr target_node, const LaneletRoute & abnormal_route) -{ - autoware::test_utils::publishToTargetNode( - test_node_, target_node, input_route_name_, abnormal_route_pub_, abnormal_route, 5); -} - -void PlanningInterfaceTestManager::publishNominalPathWithLaneId( - rclcpp::Node::SharedPtr target_node, std::string topic_name) +void PlanningInterfaceTestManager::testWithNormalPathWithLaneId( + rclcpp::Node::SharedPtr target_node, const std::string & topic_name) { try { const auto path = autoware::test_utils::loadPathWithLaneIdInYaml(); - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, normal_path_with_lane_id_pub_, path, 5); + publishInput(target_node, topic_name, path, 5); } catch (const std::exception & e) { std::cerr << e.what() << '\n'; } } -void PlanningInterfaceTestManager::publishAbNominalPathWithLaneId( - rclcpp::Node::SharedPtr target_node, std::string topic_name) +void PlanningInterfaceTestManager::testWithAbnormalPathWithLaneId( + rclcpp::Node::SharedPtr target_node, const std::string & topic_name) { - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, abnormal_path_with_lane_id_pub_, PathWithLaneId{}, 5); + publishInput(target_node, topic_name, PathWithLaneId{}, 5); } -void PlanningInterfaceTestManager::publishNominalPath( - rclcpp::Node::SharedPtr target_node, std::string topic_name) +void PlanningInterfaceTestManager::testWithNormalPath( + rclcpp::Node::SharedPtr target_node, const std::string & topic_name) { try { const auto path = autoware::test_utils::loadPathWithLaneIdInYaml(); - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, normal_path_pub_, + publishInput( + target_node, topic_name, autoware::motion_utils::convertToPath(path), 5); } catch (const std::exception & e) { std::cerr << e.what() << '\n'; } } -void PlanningInterfaceTestManager::publishAbnormalPath( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, abnormal_path_pub_, Path{}, 5); -} - -void PlanningInterfaceTestManager::setTrajectorySubscriber(std::string topic_name) -{ - autoware::test_utils::setSubscriber(test_node_, topic_name, traj_sub_, count_); -} - -void PlanningInterfaceTestManager::setRouteSubscriber(std::string topic_name) -{ - autoware::test_utils::setSubscriber(test_node_, topic_name, route_sub_, count_); -} -void PlanningInterfaceTestManager::setScenarioSubscriber(std::string topic_name) -{ - autoware::test_utils::setSubscriber(test_node_, topic_name, scenario_sub_, count_); -} - -void PlanningInterfaceTestManager::setPathWithLaneIdSubscriber(std::string topic_name) -{ - autoware::test_utils::setSubscriber(test_node_, topic_name, path_with_lane_id_sub_, count_); -} - -void PlanningInterfaceTestManager::setPathSubscriber(std::string topic_name) -{ - autoware::test_utils::setSubscriber(test_node_, topic_name, path_sub_, count_); -} - -// test for normal working -void PlanningInterfaceTestManager::testWithNominalTrajectory(rclcpp::Node::SharedPtr target_node) -{ - publishNominalTrajectory(target_node, input_trajectory_name_); -} - -// check to see if target node is dead. -void PlanningInterfaceTestManager::testWithAbnormalTrajectory(rclcpp::Node::SharedPtr target_node) -{ - publishAbnormalTrajectory(target_node, Trajectory{}); - publishAbnormalTrajectory( - target_node, autoware::test_utils::generateTrajectory(1, 0.0)); - publishAbnormalTrajectory( - target_node, autoware::test_utils::generateTrajectory(10, 0.0, 0.0, 0.0, 0.0, 1)); -} - -// test for normal working -void PlanningInterfaceTestManager::testWithNominalRoute(rclcpp::Node::SharedPtr target_node) -{ - publishNominalRoute(target_node, input_route_name_); -} - -// test for normal working -void PlanningInterfaceTestManager::testWithBehaviorNominalRoute( - rclcpp::Node::SharedPtr target_node, ModuleName module_name) -{ - publishBehaviorNominalRoute(target_node, input_route_name_, module_name); -} - -// check to see if target node is dead. -void PlanningInterfaceTestManager::testWithAbnormalRoute(rclcpp::Node::SharedPtr target_node) -{ - publishAbnormalRoute(target_node, LaneletRoute{}); -} - -// test for normal working -void PlanningInterfaceTestManager::testWithNominalPathWithLaneId( - rclcpp::Node::SharedPtr target_node) -{ - publishNominalPathWithLaneId(target_node, input_path_with_lane_id_name_); -} - -// check to see if target node is dead. -void PlanningInterfaceTestManager::testWithAbnormalPathWithLaneId( - rclcpp::Node::SharedPtr target_node) -{ - publishAbNominalPathWithLaneId(target_node, input_path_with_lane_id_name_); -} - -// put all abnormal ego pose related tests in this functions to run all tests with one line code -void PlanningInterfaceTestManager::testRouteWithInvalidEgoPose(rclcpp::Node::SharedPtr target_node) -{ - testOffTrackFromRoute(target_node); -} - -// put all abnormal ego pose related tests in this functions to run all tests with one line code -void PlanningInterfaceTestManager::testPathWithInvalidEgoPose(rclcpp::Node::SharedPtr target_node) -{ - testOffTrackFromPath(target_node); -} - -// put all abnormal ego pose related tests in this functions to run all tests with one line code -void PlanningInterfaceTestManager::testPathWithLaneIdWithInvalidEgoPose( - rclcpp::Node::SharedPtr target_node) -{ - testOffTrackFromPathWithLaneId(target_node); -} - -// put all abnormal ego pose related tests in this functions to run all tests with one line code -void PlanningInterfaceTestManager::testTrajectoryWithInvalidEgoPose( - rclcpp::Node::SharedPtr target_node) -{ - testOffTrackFromTrajectory(target_node); -} - -void PlanningInterfaceTestManager::testOffTrackFromRoute(rclcpp::Node::SharedPtr target_node) -{ - publishBehaviorNominalRoute(target_node, input_route_name_); - - const std::vector deviation_from_route = {0.0, 1.0, 10.0, 100.0}; - for (const auto & deviation : deviation_from_route) { - publishInitialPose(target_node, input_initial_pose_name_, deviation); - } -} - -void PlanningInterfaceTestManager::testOffTrackFromPath(rclcpp::Node::SharedPtr target_node) +void PlanningInterfaceTestManager::testWithAbnormalPath( + rclcpp::Node::SharedPtr target_node, const std::string & topic_name) { - // write me - (void)target_node; + publishInput(target_node, topic_name, Path{}, 5); } -void PlanningInterfaceTestManager::testOffTrackFromPathWithLaneId( - rclcpp::Node::SharedPtr target_node) +void PlanningInterfaceTestManager::testWithOffTrackInitialPoses( + rclcpp::Node::SharedPtr target_node, const std::string & topic_name) { - publishNominalPathWithLaneId(target_node, input_path_with_lane_id_name_); - - const std::vector deviation_from_path = {0.0, 1.0, 10.0, 100.0}; - for (const auto & deviation : deviation_from_path) { - publishOdometry(target_node, input_odometry_name_, deviation); + for (const auto & deviation : {0.0, 1.0, 10.0, 100.0}) { + publishInput(target_node, topic_name, autoware::test_utils::makeInitialPose(deviation), 5); } } -void PlanningInterfaceTestManager::testOffTrackFromTrajectory(rclcpp::Node::SharedPtr target_node) +void PlanningInterfaceTestManager::testWithOffTrackOdometries( + rclcpp::Node::SharedPtr target_node, const std::string & topic_name) { - publishNominalTrajectory(target_node, input_trajectory_name_); - - const std::vector deviation_from_traj = {0.0, 1.0, 10.0, 100.0}; - for (const auto & deviation : deviation_from_traj) { - publishOdometry(target_node, input_odometry_name_, deviation); + for (const auto & deviation : {0.0, 1.0, 10.0, 100.0}) { + publishInput(target_node, topic_name, autoware::test_utils::makeOdometry(deviation), 5); } } - -void PlanningInterfaceTestManager::testWithNominalPath(rclcpp::Node::SharedPtr target_node) -{ - publishNominalPath(target_node, input_path_name_); -} - -void PlanningInterfaceTestManager::testWithAbnormalPath(rclcpp::Node::SharedPtr target_node) -{ - publishAbnormalPath(target_node, input_path_name_); -} - -int PlanningInterfaceTestManager::getReceivedTopicNum() -{ - return count_; -} - -rclcpp::Node::SharedPtr PlanningInterfaceTestManager::getTestNode() const -{ - return test_node_; -} - } // namespace autoware::planning_test_manager From dd14d5f7623a7bd07c6d4a07f4d1971810a8b95a Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Thu, 9 Jan 2025 19:00:56 +0900 Subject: [PATCH 02/33] include necessary header Signed-off-by: mitukou1109 --- .../autoware_planning_test_manager.hpp | 1 + .../src/autoware_planning_test_manager.cpp | 3 --- 2 files changed, 1 insertion(+), 3 deletions(-) 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 87aaf2f6db7a3..edafbb55461d7 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 @@ -42,6 +42,7 @@ #include #include #include +#include namespace autoware::planning_test_manager { 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 489c3a067afb9..3add1f8c56f5d 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 @@ -22,9 +22,6 @@ #include #include -#include -#include -#include namespace autoware::planning_test_manager { From c8f6f1498bb172b89e2ab813324f426d1459cb84 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Thu, 9 Jan 2025 19:13:47 +0900 Subject: [PATCH 03/33] adapt velocity_smoother to new test manager Signed-off-by: mitukou1109 --- .../test_velocity_smoother_node_interface.cpp | 36 ++++++++++++------- 1 file changed, 23 insertions(+), 13 deletions(-) diff --git a/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp b/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp index b590af1b3ee0a..5511daa1931e5 100644 --- a/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp +++ b/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp @@ -29,9 +29,8 @@ using autoware::velocity_smoother::VelocitySmootherNode; std::shared_ptr generateTestManager() { auto test_manager = std::make_shared(); - test_manager->setTrajectorySubscriber("velocity_smoother/output/trajectory"); - test_manager->setTrajectoryInputTopicName("velocity_smoother/input/trajectory"); - test_manager->setOdometryTopicName("/localization/kinematic_state"); + test_manager->subscribeOutput( + "velocity_smoother/output/trajectory"); return test_manager; } @@ -60,12 +59,17 @@ void publishMandatoryTopics( rclcpp::Node::SharedPtr test_target_node) { // publish necessary topics from test_manager - test_manager->publishOdometry(test_target_node, "/localization/kinematic_state"); - test_manager->publishMaxVelocity( - test_target_node, "velocity_smoother/input/external_velocity_limit_mps"); - test_manager->publishOperationModeState( - test_target_node, "velocity_smoother/input/operation_mode_state"); - test_manager->publishAcceleration(test_target_node, "velocity_smoother/input/acceleration"); + test_manager->publishInput( + test_target_node, "/localization/kinematic_state", autoware::test_utils::makeOdometry()); + test_manager->publishInput( + test_target_node, "velocity_smoother/input/external_velocity_limit_mps", + tier4_planning_msgs::msg::VelocityLimit{}); + test_manager->publishInput( + test_target_node, "velocity_smoother/input/operation_mode_state", + autoware_adapi_v1_msgs::msg::OperationModeState{}); + test_manager->publishInput( + test_target_node, "velocity_smoother/input/acceleration", + geometry_msgs::msg::AccelWithCovarianceStamped{}); } TEST(PlanningModuleInterfaceTest, testPlanningInterfaceWithVariousTrajectoryInput) @@ -76,12 +80,15 @@ TEST(PlanningModuleInterfaceTest, testPlanningInterfaceWithVariousTrajectoryInpu publishMandatoryTopics(test_manager, test_target_node); + const std::string input_trajectory_topic = "velocity_smoother/output/trajectory"; + // test for normal trajectory - ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); + ASSERT_NO_THROW(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(test_manager->testWithAbnormalTrajectory(test_target_node)); + ASSERT_NO_THROW( + test_manager->testWithAbnormalTrajectory(test_target_node, input_trajectory_topic)); rclcpp::shutdown(); } @@ -94,11 +101,14 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) publishMandatoryTopics(test_manager, test_target_node); + const std::string input_trajectory_topic = "velocity_smoother/output/trajectory"; + const std::string input_odometry_topic = "/localization/kinematic_state"; + // test for normal trajectory - ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); + ASSERT_NO_THROW(test_manager->testWithNormalTrajectory(test_target_node, input_trajectory_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - ASSERT_NO_THROW(test_manager->testTrajectoryWithInvalidEgoPose(test_target_node)); + ASSERT_NO_THROW(test_manager->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } From 7c576217e9ba264d3dfe179ff85ddb42e1ac2f22 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Thu, 9 Jan 2025 19:14:06 +0900 Subject: [PATCH 04/33] adapt behavior_velocity_planner to new test manager Signed-off-by: mitukou1109 --- .../src/test_utils.cpp | 56 +++++++++++-------- .../test/test_node_interface.cpp | 19 +++++-- 2 files changed, 47 insertions(+), 28 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/test_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/test_utils.cpp index 8b9f39e97d22b..45ce88c8e92d4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/test_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/test_utils.cpp @@ -29,14 +29,8 @@ std::shared_ptr generateTestManager() auto test_manager = std::make_shared(); // set subscriber with topic name: behavior_velocity_planner → test_node_ - test_manager->setPathSubscriber("behavior_velocity_planner_node/output/path"); - - // set behavior_velocity_planner node's input topic name(this topic is changed to test node) - test_manager->setPathWithLaneIdTopicName( - "behavior_velocity_planner_node/input/path_with_lane_id"); - - test_manager->setInitialPoseTopicName("behavior_velocity_planner_node/input/vehicle_odometry"); - test_manager->setOdometryTopicName("behavior_velocity_planner_node/input/vehicle_odometry"); + test_manager->subscribeOutput( + "behavior_velocity_planner_node/output/path"); return test_manager; } @@ -99,21 +93,35 @@ void publishMandatoryTopics( std::shared_ptr test_target_node) { // publish necessary topics from test_manager - test_manager->publishTF(test_target_node, "/tf"); - test_manager->publishAcceleration(test_target_node, "behavior_velocity_planner_node/input/accel"); - test_manager->publishPredictedObjects( - test_target_node, "behavior_velocity_planner_node/input/dynamic_objects"); - test_manager->publishPointCloud( - test_target_node, "behavior_velocity_planner_node/input/no_ground_pointcloud"); - test_manager->publishOdometry( - test_target_node, "behavior_velocity_planner_node/input/vehicle_odometry"); - test_manager->publishAcceleration(test_target_node, "behavior_velocity_planner_node/input/accel"); - test_manager->publishMap(test_target_node, "behavior_velocity_planner_node/input/vector_map"); - test_manager->publishTrafficSignals( - test_target_node, "behavior_velocity_planner_node/input/traffic_signals"); - test_manager->publishMaxVelocity( - test_target_node, "behavior_velocity_planner_node/input/external_velocity_limit_mps"); - test_manager->publishOccupancyGrid( - test_target_node, "behavior_velocity_planner_node/input/occupancy_grid"); + test_manager->publishInput( + test_target_node, "/tf", autoware::test_utils::makeTFMsg(test_target_node, "base_link", "map")); + test_manager->publishInput( + test_target_node, "behavior_velocity_planner_node/input/accel", + geometry_msgs::msg::AccelWithCovarianceStamped{}); + test_manager->publishInput( + test_target_node, "behavior_velocity_planner_node/input/dynamic_objects", + autoware_perception_msgs::msg::PredictedObjects{}); + test_manager->publishInput( + test_target_node, "behavior_velocity_planner_node/input/no_ground_pointcloud", + sensor_msgs::msg::PointCloud2{}.set__header( + std_msgs::msg::Header{}.set__frame_id("base_link"))); + test_manager->publishInput( + test_target_node, "behavior_velocity_planner_node/input/vehicle_odometry", + autoware::test_utils::makeOdometry()); + test_manager->publishInput( + test_target_node, "behavior_velocity_planner_node/input/accel", + geometry_msgs::msg::AccelWithCovarianceStamped{}); + test_manager->publishInput( + test_target_node, "behavior_velocity_planner_node/input/vector_map", + autoware::test_utils::makeMapBinMsg()); + test_manager->publishInput( + test_target_node, "behavior_velocity_planner_node/input/traffic_signals", + autoware_perception_msgs::msg::TrafficLightGroupArray{}); + test_manager->publishInput( + test_target_node, "behavior_velocity_planner_node/input/external_velocity_limit_mps", + tier4_planning_msgs::msg::VelocityLimit{}); + test_manager->publishInput( + test_target_node, "behavior_velocity_planner_node/input/occupancy_grid", + autoware::test_utils::makeCostMapMsg()); } } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/test_node_interface.cpp index 6e232318c1711..d2273205c1d61 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/test_node_interface.cpp @@ -31,12 +31,17 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + // test with nominal path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test with empty path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithAbnormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); rclcpp::shutdown(); } @@ -48,13 +53,19 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + const std::string input_odometry_topic = "behavior_velocity_planner_node/input/vehicle_odometry"; + // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); // make sure behavior_path_planner is running EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } From a3eaf515c6f5a089b86b85a9638dec49ca8aa886 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Fri, 17 Jan 2025 11:09:25 +0900 Subject: [PATCH 05/33] adapt path_optimizer to new test manager Signed-off-by: mitukou1109 --- .../test/test_path_optimizer_node_interface.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp b/planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp index e310fecf15d8c..97e5ed2e1a6be 100644 --- a/planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp +++ b/planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp @@ -47,21 +47,24 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) auto test_target_node = std::make_shared(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( + "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(); } From 0a89d6703cb4674cbfce5eaed1cf1d88cf1f327d Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Fri, 17 Jan 2025 15:56:10 +0900 Subject: [PATCH 06/33] fix output subscription Signed-off-by: mitukou1109 --- .../autoware_planning_test_manager.hpp | 18 +++++++++++------- .../src/autoware_planning_test_manager.cpp | 1 - 2 files changed, 11 insertions(+), 8 deletions(-) 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 edafbb55461d7..63e11e98be818 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 @@ -35,6 +35,8 @@ #include #include +#include + #include #include #include @@ -61,15 +63,17 @@ class PlanningInterfaceTestManager } template - void subscribeOutput(const std::string & topic_name, typename OutputT::ConstSharedPtr buffer = {}) + void subscribeOutput(const std::string & topic_name) { + const auto qos = []() { + if constexpr (std::is_same_v) { + return rclcpp::QoS{1}; + } + return rclcpp::QoS{10}; + }(); + test_output_subs_.push_back(test_node_->create_subscription( - topic_name, 1, [&](const typename OutputT::ConstSharedPtr msg) { - if (buffer) { - buffer = msg; - } - received_topic_num_++; - })); + topic_name, qos, [this](const typename OutputT::ConstSharedPtr) { received_topic_num_++; })); } void testWithNormalTrajectory( 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 3add1f8c56f5d..99a1ff927dd29 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 @@ -18,7 +18,6 @@ #include #include -#include #include #include From 50f34c8117350753babf9d3b917acfd6145ceb91 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Fri, 17 Jan 2025 15:56:39 +0900 Subject: [PATCH 07/33] adapt behavior_path_planner to new test manager Signed-off-by: mitukou1109 --- .../src/test_utils.cpp | 62 +++++++++---------- ...e_behavior_path_planner_node_interface.cpp | 17 +++-- 2 files changed, 42 insertions(+), 37 deletions(-) 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 4930ead6d355d..d3bc4a25e97ca 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 @@ -26,33 +26,13 @@ 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(); // set subscriber with topic name: behavior_path_planner → test_node_ - test_manager->setPathWithLaneIdSubscriber("behavior_path_planner/output/path"); - - // set behavior_path_planner's input topic name(this topic is changed to test node) - test_manager->setRouteInputTopicName("behavior_path_planner/input/route"); - - test_manager->setInitialPoseTopicName("behavior_path_planner/input/odometry"); + test_manager->subscribeOutput( + "behavior_path_planner/output/path"); return test_manager; } @@ -103,16 +83,32 @@ void publishMandatoryTopics( std::shared_ptr test_target_node) { // publish necessary topics from test_manager - test_manager->publishInitialPose(test_target_node, "behavior_path_planner/input/odometry"); - test_manager->publishAcceleration(test_target_node, "behavior_path_planner/input/accel"); - test_manager->publishPredictedObjects(test_target_node, "behavior_path_planner/input/perception"); - test_manager->publishOccupancyGrid( - test_target_node, "behavior_path_planner/input/occupancy_grid_map"); - test_manager->publishLaneDrivingScenario( - test_target_node, "behavior_path_planner/input/scenario"); - 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"); - publishLateralOffset(test_manager, test_target_node); + test_manager->publishInput( + test_target_node, "behavior_path_planner/input/odometry", + autoware::test_utils::makeInitialPose()); + test_manager->publishInput( + test_target_node, "behavior_path_planner/input/accel", + geometry_msgs::msg::AccelWithCovarianceStamped{}); + test_manager->publishInput( + test_target_node, "behavior_path_planner/input/perception", + autoware_perception_msgs::msg::PredictedObjects{}); + test_manager->publishInput( + test_target_node, "behavior_path_planner/input/occupancy_grid_map", + autoware::test_utils::makeCostMapMsg()); + test_manager->publishInput( + test_target_node, "behavior_path_planner/input/scenario", + autoware::test_utils::makeScenarioMsg(tier4_planning_msgs::msg::Scenario::LANEDRIVING)); + test_manager->publishInput( + test_target_node, "behavior_path_planner/input/vector_map", + autoware::test_utils::makeMapBinMsg()); + test_manager->publishInput( + test_target_node, "behavior_path_planner/input/costmap", + autoware::test_utils::makeCostMapMsg()); + test_manager->publishInput( + test_target_node, "system/operation_mode/state", + autoware_adapi_v1_msgs::msg::OperationModeState{}); + test_manager->publishInput( + test_target_node, "behavior_path_planner/input/lateral_offset", + tier4_planning_msgs::msg::LateralOffset{}); } } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_autoware_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_autoware_behavior_path_planner_node_interface.cpp index 0c933345e7647..d4c1c0b56fb7f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_autoware_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_autoware_behavior_path_planner_node_interface.cpp @@ -34,12 +34,16 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) publishMandatoryTopics(test_manager, test_target_node); + const std::string input_route_topic = "behavior_path_planner/input/route"; + // test for normal trajectory - 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(); } @@ -51,13 +55,18 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) auto test_target_node = generateNode({}, {}); publishMandatoryTopics(test_manager, test_target_node); + const std::string input_route_topic = "behavior_path_planner/input/route"; + const std::string input_odometry_topic = "behavior_path_planner/input/odometry"; + // test for normal trajectory - 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)); // make sure behavior_path_planner is running 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->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } From 2d32133bfe5cd67192cc32a7cff4b220c1429e37 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Fri, 17 Jan 2025 16:44:44 +0900 Subject: [PATCH 08/33] adapt scenario_selector to new test manager Signed-off-by: mitukou1109 --- ...oware_scenario_selector_node_interface.cpp | 46 ++++++++++--------- 1 file changed, 25 insertions(+), 21 deletions(-) diff --git a/planning/autoware_scenario_selector/test/test_autoware_scenario_selector_node_interface.cpp b/planning/autoware_scenario_selector/test/test_autoware_scenario_selector_node_interface.cpp index ef9af48967a11..de56bfee2660f 100644 --- a/planning/autoware_scenario_selector/test/test_autoware_scenario_selector_node_interface.cpp +++ b/planning/autoware_scenario_selector/test/test_autoware_scenario_selector_node_interface.cpp @@ -32,9 +32,7 @@ std::shared_ptr generateTestManager() auto test_manager = std::make_shared(); // set subscriber with topic name: scenario_selector → test_node_ - test_manager->setScenarioSubscriber("output/scenario"); - - test_manager->setOdometryTopicName("input/odometry"); + test_manager->subscribeOutput("output/scenario"); return test_manager; } @@ -58,12 +56,18 @@ void publishMandatoryTopics( std::shared_ptr test_target_node) { // publish necessary topics from test_manager - test_manager->publishOdometry(test_target_node, "input/odometry"); - test_manager->publishParkingState(test_target_node, "is_parking_completed"); - test_manager->publishTrajectory(test_target_node, "input/parking/trajectory"); - test_manager->publishMap(test_target_node, "input/lanelet_map"); - test_manager->publishRoute(test_target_node, "input/route"); - test_manager->publishOperationModeState(test_target_node, "input/operation_mode_state"); + test_manager->publishInput( + test_target_node, "input/odometry", autoware::test_utils::makeOdometry()); + test_manager->publishInput(test_target_node, "is_parking_completed", std_msgs::msg::Bool{}); + test_manager->publishInput( + test_target_node, "input/parking/trajectory", autoware_planning_msgs::msg::Trajectory{}); + test_manager->publishInput( + test_target_node, "input/lanelet_map", autoware::test_utils::makeMapBinMsg()); + test_manager->publishInput( + test_target_node, "input/route", autoware::test_utils::makeNormalRoute()); + test_manager->publishInput( + test_target_node, "input/operation_mode_state", + autoware_adapi_v1_msgs::msg::OperationModeState{}); } TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectoryLaneDrivingMode) @@ -74,15 +78,15 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectoryLaneDrivingMode publishMandatoryTopics(test_manager, test_target_node); - // set scenario_selector's input topic name(this topic is changed to test node) - test_manager->setTrajectoryInputTopicName("input/lane_driving/trajectory"); + const std::string input_trajectory_topic = "input/lane_driving/trajectory"; // test for normal trajectory - ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); + ASSERT_NO_THROW(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(test_manager->testWithAbnormalTrajectory(test_target_node)); + ASSERT_NO_THROW( + test_manager->testWithAbnormalTrajectory(test_target_node, input_trajectory_topic)); rclcpp::shutdown(); } @@ -95,15 +99,15 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectoryParkingMode) publishMandatoryTopics(test_manager, test_target_node); - // set scenario_selector's input topic name(this topic is changed to test node) - test_manager->setTrajectoryInputTopicName("input/parking/trajectory"); + const std::string input_trajectory_topic = "input/parking/trajectory"; // test for normal trajectory - ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); + ASSERT_NO_THROW(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(test_manager->testWithAbnormalTrajectory(test_target_node)); + ASSERT_NO_THROW( + test_manager->testWithAbnormalTrajectory(test_target_node, input_trajectory_topic)); rclcpp::shutdown(); } @@ -115,14 +119,14 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) publishMandatoryTopics(test_manager, test_target_node); - // set scenario_selector's input topic name(this topic is changed to test node) - test_manager->setTrajectoryInputTopicName("input/lane_driving/trajectory"); + const std::string input_trajectory_topic = "input/lane_driving/trajectory"; + const std::string input_odometry_topic = "input/odometry"; // test for normal trajectory - ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); + ASSERT_NO_THROW(test_manager->testWithNormalTrajectory(test_target_node, input_trajectory_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - ASSERT_NO_THROW(test_manager->testTrajectoryWithInvalidEgoPose(test_target_node)); + ASSERT_NO_THROW(test_manager->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } } // namespace autoware::scenario_selector From d2f3e7c962165ff5de479d73c1944b43e74c0210 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Fri, 17 Jan 2025 17:05:18 +0900 Subject: [PATCH 09/33] adapt freespace_planner to new test manager Signed-off-by: mitukou1109 --- .../test_freespace_planner_node_interface.cpp | 41 ++++++++++++------- 1 file changed, 27 insertions(+), 14 deletions(-) diff --git a/planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp b/planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp index 8bf30757aafb7..bb8fc9b405031 100644 --- a/planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp +++ b/planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp @@ -29,10 +29,8 @@ using autoware::planning_test_manager::PlanningInterfaceTestManager; std::shared_ptr generateTestManager() { auto test_manager = std::make_shared(); - 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( + "freespace_planner/output/trajectory"); return test_manager; } @@ -55,14 +53,20 @@ 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 -TEST(PlanningModuleInterfaceTest, DISABLED_testPlanningInterfaceWithVariousTrajectoryInput) +TEST(PlanningModuleInterfaceTest, testPlanningInterfaceWithVariousTrajectoryInput) { rclcpp::init(0, nullptr); @@ -70,16 +74,20 @@ 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(); } -TEST(PlanningModuleInterfaceTest, DISABLED_NodeTestWithOffTrackEgoPose) +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) { rclcpp::init(0, nullptr); auto test_manager = generateTestManager(); @@ -87,11 +95,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->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } From 03a2c7930dd5633685938b2ff614e79653716917 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Fri, 17 Jan 2025 17:17:50 +0900 Subject: [PATCH 10/33] adapt planning_validator to new test manager Signed-off-by: mitukou1109 --- ...test_planning_validator_node_interface.cpp | 25 +++++++++++-------- 1 file changed, 14 insertions(+), 11 deletions(-) diff --git a/planning/autoware_planning_validator/test/src/test_planning_validator_node_interface.cpp b/planning/autoware_planning_validator/test/src/test_planning_validator_node_interface.cpp index 2cf03e04311f2..34a4ce3a0be77 100644 --- a/planning/autoware_planning_validator/test/src/test_planning_validator_node_interface.cpp +++ b/planning/autoware_planning_validator/test/src/test_planning_validator_node_interface.cpp @@ -31,12 +31,8 @@ std::shared_ptr generateTestManager() auto test_manager = std::make_shared(); // set subscriber with topic name: planning_validator → test_node_ - test_manager->setTrajectorySubscriber("planning_validator/output/trajectory"); - - // set planning_validator's input topic name(this topic is changed to test node) - test_manager->setTrajectoryInputTopicName("planning_validator/input/trajectory"); - - test_manager->setOdometryTopicName("planning_validator/input/kinematics"); + test_manager->subscribeOutput( + "planning_validator/output/trajectory"); return test_manager; } @@ -60,7 +56,8 @@ void publishMandatoryTopics( std::shared_ptr test_target_node) { // publish necessary topics from test_manager - test_manager->publishOdometry(test_target_node, "planning_validator/input/kinematics"); + test_manager->publishInput( + test_target_node, "planning_validator/input/kinematics", autoware::test_utils::makeOdometry()); } TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) @@ -70,12 +67,15 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) publishMandatoryTopics(test_manager, test_target_node); + const std::string input_trajectory_topic = "planning_validator/input/trajectory"; + // test for normal trajectory - ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); + ASSERT_NO_THROW(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(test_manager->testWithAbnormalTrajectory(test_target_node)); + ASSERT_NO_THROW( + test_manager->testWithAbnormalTrajectory(test_target_node, input_trajectory_topic)); } TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) @@ -85,10 +85,13 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) publishMandatoryTopics(test_manager, test_target_node); + const std::string input_trajectory_topic = "planning_validator/input/trajectory"; + const std::string input_odometry_topic = "planning_validator/input/kinematics"; + // test for normal trajectory - ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); + ASSERT_NO_THROW(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(test_manager->testTrajectoryWithInvalidEgoPose(test_target_node)); + ASSERT_NO_THROW(test_manager->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); } From 6e73706c2f71ce13b4a8d257c8f4cee0d4ff2f0d Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Fri, 17 Jan 2025 17:37:45 +0900 Subject: [PATCH 11/33] adapt obstacle_stop_planner to new test manager Signed-off-by: mitukou1109 --- ...e_obstacle_stop_planner_node_interface.cpp | 53 ++++++++++--------- 1 file changed, 29 insertions(+), 24 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 9da8409c6c2ae..040fb4acd488d 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 @@ -32,9 +32,8 @@ using tier4_planning_msgs::msg::ExpandStopRange; std::shared_ptr generateTestManager() { auto test_manager = std::make_shared(); - 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( + "obstacle_stop_planner/output/trajectory"); return test_manager; } @@ -65,21 +64,21 @@ void publishMandatoryTopics( std::shared_ptr 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 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_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) @@ -90,15 +89,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(); } @@ -110,14 +111,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->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } From 9253af005ee1dd2f257abe2ee5e5093aa0fa9818 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Fri, 17 Jan 2025 17:49:50 +0900 Subject: [PATCH 12/33] adapt obstacle_cruise_planner to new test manager Signed-off-by: mitukou1109 --- ...obstacle_cruise_planner_node_interface.cpp | 37 ++++++++++++------- 1 file changed, 24 insertions(+), 13 deletions(-) diff --git a/planning/autoware_obstacle_cruise_planner/test/test_obstacle_cruise_planner_node_interface.cpp b/planning/autoware_obstacle_cruise_planner/test/test_obstacle_cruise_planner_node_interface.cpp index 2235704a3a070..fccfca9707cbb 100644 --- a/planning/autoware_obstacle_cruise_planner/test/test_obstacle_cruise_planner_node_interface.cpp +++ b/planning/autoware_obstacle_cruise_planner/test/test_obstacle_cruise_planner_node_interface.cpp @@ -31,12 +31,8 @@ std::shared_ptr generateTestManager() auto test_manager = std::make_shared(); // 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( + "obstacle_cruise_planner/output/trajectory"); return test_manager; } @@ -63,9 +59,15 @@ void publishMandatoryTopics( std::shared_ptr 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) @@ -76,12 +78,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(); } @@ -94,12 +100,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->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } From f04061572190806b2c7d4c7abc125302d00f69ec Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Mon, 27 Jan 2025 13:08:49 +0900 Subject: [PATCH 13/33] disable test for freespace_planner Signed-off-by: mitukou1109 --- .../test/test_freespace_planner_node_interface.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp b/planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp index bb8fc9b405031..1078713f5e4ca 100644 --- a/planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp +++ b/planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp @@ -66,7 +66,7 @@ void publishMandatoryTopics( } // the following tests are disable because they randomly fail -TEST(PlanningModuleInterfaceTest, testPlanningInterfaceWithVariousTrajectoryInput) +TEST(PlanningModuleInterfaceTest, DISABLED_testPlanningInterfaceWithVariousTrajectoryInput) { rclcpp::init(0, nullptr); @@ -87,7 +87,7 @@ TEST(PlanningModuleInterfaceTest, testPlanningInterfaceWithVariousTrajectoryInpu rclcpp::shutdown(); } -TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +TEST(PlanningModuleInterfaceTest, DISABLED_NodeTestWithOffTrackEgoPose) { rclcpp::init(0, nullptr); auto test_manager = generateTestManager(); From 431ab38044652722dd6b4436cfb389065c15e11b Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Mon, 27 Jan 2025 13:48:11 +0900 Subject: [PATCH 14/33] adapt behavior_velocity_crosswalk_module to new test manager Signed-off-by: mitukou1109 --- .../test/test_node_interface.cpp | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/test/test_node_interface.cpp index 7a875327d4dde..69532e5fcb467 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/test/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/test/test_node_interface.cpp @@ -31,12 +31,17 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + // test with nominal path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test with empty path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithAbnormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); rclcpp::shutdown(); } @@ -51,13 +56,19 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + const std::string input_odometry_topic = "behavior_velocity_planner_node/input/vehicle_odometry"; + // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); // make sure behavior_path_planner is running EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } From d9a822f223e81133665fa9559d9396a3c3bf9666 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Mon, 27 Jan 2025 13:59:32 +0900 Subject: [PATCH 15/33] adapt behavior_path_lane_change_module to new test manager Signed-off-by: mitukou1109 --- ...est_behavior_path_planner_node_interface.cpp | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp index f6b3e9eeaf715..116194840c877 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -36,12 +36,16 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) publishMandatoryTopics(test_manager, test_target_node); + const std::string input_route_topic = "behavior_path_planner/input/route"; + // test for normal trajectory - 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(); } @@ -55,13 +59,18 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) "autoware::behavior_path_planner::LaneChangeLeftModuleManager"}); publishMandatoryTopics(test_manager, test_target_node); + const std::string input_route_topic = "behavior_path_planner/input/route"; + const std::string input_odometry_topic = "behavior_path_planner/input/odometry"; + // test for normal trajectory - 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)); // make sure behavior_path_planner is running 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->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } From 3b95bca296a58b25b89c35312c92af3e4e246f61 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Mon, 27 Jan 2025 14:27:47 +0900 Subject: [PATCH 16/33] adapt behavior_path_avoidance_by_lane_change_module to new test manager Signed-off-by: mitukou1109 --- ...est_behavior_path_planner_node_interface.cpp | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp index 349d92377d091..a04d1c41cdbaa 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -35,12 +35,16 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) {"autoware::behavior_path_planner::AvoidanceByLaneChangeModuleManager"}); publishMandatoryTopics(test_manager, test_target_node); + const std::string input_route_topic = "behavior_path_planner/input/route"; + // test for normal trajectory - 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(); } @@ -54,13 +58,18 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) {"autoware::behavior_path_planner::AvoidanceByLaneChangeModuleManager"}); publishMandatoryTopics(test_manager, test_target_node); + const std::string input_route_topic = "behavior_path_planner/input/route"; + const std::string input_odometry_topic = "behavior_path_planner/input/odometry"; + // test for normal trajectory - 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)); // make sure behavior_path_planner is running 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->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } From 7baa97eb332d03bd90d745036aa3547e779bbecc Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Mon, 27 Jan 2025 14:28:09 +0900 Subject: [PATCH 17/33] adapt behavior_path_dynamic_obstacle_avoidance_module to new test manager Signed-off-by: mitukou1109 --- ...est_behavior_path_planner_node_interface.cpp | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp index 488475079613f..aef0520ba0b7f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp @@ -32,12 +32,16 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) publishMandatoryTopics(test_manager, test_target_node); + const std::string input_route_topic = "behavior_path_planner/input/route"; + // test for normal trajectory - 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(); } @@ -51,13 +55,18 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) {"autoware::behavior_path_planner::DynamicObstacleAvoidanceModuleManager"}); publishMandatoryTopics(test_manager, test_target_node); + const std::string input_route_topic = "behavior_path_planner/input/route"; + const std::string input_odometry_topic = "behavior_path_planner/input/odometry"; + // test for normal trajectory - 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)); // make sure behavior_path_planner is running 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->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } From a8384f91d33cc3332d4bfaea78001a9d46471a20 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Mon, 27 Jan 2025 14:28:22 +0900 Subject: [PATCH 18/33] adapt behavior_path_external_request_lane_change_module to new test manager Signed-off-by: mitukou1109 --- ...est_behavior_path_planner_node_interface.cpp | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp index 506e43a94de1d..5b73d947daa65 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -36,12 +36,16 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) "autoware::behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager"}); publishMandatoryTopics(test_manager, test_target_node); + const std::string input_route_topic = "behavior_path_planner/input/route"; + // test for normal trajectory - 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(); } @@ -56,13 +60,18 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) "autoware::behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager"}); publishMandatoryTopics(test_manager, test_target_node); + const std::string input_route_topic = "behavior_path_planner/input/route"; + const std::string input_odometry_topic = "behavior_path_planner/input/odometry"; + // test for normal trajectory - 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)); // make sure behavior_path_planner is running 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->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } From 2817507dd856ac7e51a867187c2e9293231b74e9 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Mon, 27 Jan 2025 14:28:37 +0900 Subject: [PATCH 19/33] adapt behavior_path_side_shift_module to new test manager Signed-off-by: mitukou1109 --- ...est_behavior_path_planner_node_interface.cpp | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp index c248a40eb151a..01c712d86e743 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp @@ -34,12 +34,16 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) publishMandatoryTopics(test_manager, test_target_node); + const std::string input_route_topic = "behavior_path_planner/input/route"; + // test for normal trajectory - 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(); } @@ -52,13 +56,18 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) generateNode({"side_shift"}, {"autoware::behavior_path_planner::SideShiftModuleManager"}); publishMandatoryTopics(test_manager, test_target_node); + const std::string input_route_topic = "behavior_path_planner/input/route"; + const std::string input_odometry_topic = "behavior_path_planner/input/odometry"; + // test for normal trajectory - 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)); // make sure behavior_path_planner is running 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->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } From a20a0aaa7bf5472117be056fb4d4004ea5986551 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Mon, 27 Jan 2025 14:28:54 +0900 Subject: [PATCH 20/33] adapt behavior_path_static_obstacle_avoidance_module to new test manager Signed-off-by: mitukou1109 --- ...est_behavior_path_planner_node_interface.cpp | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp index c2531e5f2bb9f..674a2803aabd5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp @@ -32,12 +32,16 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) {"autoware::behavior_path_planner::StaticObstacleAvoidanceModuleManager"}); publishMandatoryTopics(test_manager, test_target_node); + const std::string input_route_topic = "behavior_path_planner/input/route"; + // test for normal trajectory - 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(); } @@ -51,13 +55,18 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) {"autoware::behavior_path_planner::StaticObstacleAvoidanceModuleManager"}); publishMandatoryTopics(test_manager, test_target_node); + const std::string input_route_topic = "behavior_path_planner/input/route"; + const std::string input_odometry_topic = "behavior_path_planner/input/odometry"; + // test for normal trajectory - 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)); // make sure behavior_path_planner is running 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->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } From 3731a377ddd1d43e85bacd1676f54ac938d601f4 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Mon, 27 Jan 2025 14:40:05 +0900 Subject: [PATCH 21/33] adapt path_smoother to new test manager Signed-off-by: mitukou1109 --- ..._autoware_path_smoother_node_interface.cpp | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) diff --git a/planning/autoware_path_smoother/test/test_autoware_path_smoother_node_interface.cpp b/planning/autoware_path_smoother/test/test_autoware_path_smoother_node_interface.cpp index 7402541c727ff..8ae9885827932 100644 --- a/planning/autoware_path_smoother/test/test_autoware_path_smoother_node_interface.cpp +++ b/planning/autoware_path_smoother/test/test_autoware_path_smoother_node_interface.cpp @@ -46,22 +46,27 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) std::make_shared(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_path_smoother/output/traj"); + test_manager->subscribeOutput( + "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(); } From 88464f4d30de383b3a894a3dee4e5fc77b1e5b5d Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Mon, 27 Jan 2025 14:54:23 +0900 Subject: [PATCH 22/33] adapt behavior_velocity_blind_spot_module to new test manager Signed-off-by: mitukou1109 --- .../test/test_node_interface.cpp | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_node_interface.cpp index e0ceca4e12951..c49fb0568c8ec 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_node_interface.cpp @@ -31,12 +31,17 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + // test with nominal path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test with empty path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithAbnormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); rclcpp::shutdown(); } @@ -51,13 +56,19 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + const std::string input_odometry_topic = "behavior_velocity_planner_node/input/vehicle_odometry"; + // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); // make sure behavior_path_planner is running EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } From 770ee888a78e9c09f228cb1cebfdd5655ad0dd27 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Mon, 27 Jan 2025 14:54:39 +0900 Subject: [PATCH 23/33] adapt behavior_velocity_detection_area_module to new test manager Signed-off-by: mitukou1109 --- .../test/test_node_interface.cpp | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/test/test_node_interface.cpp index f84d22debea8e..0c0e0d537abba 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/test/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/test/test_node_interface.cpp @@ -31,12 +31,17 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + // test with nominal path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test with empty path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithAbnormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); rclcpp::shutdown(); } @@ -51,13 +56,19 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + const std::string input_odometry_topic = "behavior_velocity_planner_node/input/vehicle_odometry"; + // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); // make sure behavior_path_planner is running EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } From 0b8c864f5e7e5e7f083bca0e0bbdad3cb679fa9b Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Mon, 27 Jan 2025 14:54:50 +0900 Subject: [PATCH 24/33] adapt behavior_velocity_intersection_module to new test manager Signed-off-by: mitukou1109 --- .../test/test_node_interface.cpp | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/test/test_node_interface.cpp index b515107e0ae8e..04ae9a5d2f972 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/test/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/test/test_node_interface.cpp @@ -31,12 +31,17 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + // test with nominal path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test with empty path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithAbnormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); rclcpp::shutdown(); } @@ -51,13 +56,19 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + const std::string input_odometry_topic = "behavior_velocity_planner_node/input/vehicle_odometry"; + // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); // make sure behavior_path_planner is running EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } From bf79bc052fa2a1d112d1137cb1127fd5ee30b656 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Mon, 27 Jan 2025 14:55:05 +0900 Subject: [PATCH 25/33] adapt behavior_velocity_no_stopping_area_module to new test manager Signed-off-by: mitukou1109 --- .../test/test_node_interface.cpp | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_node_interface.cpp index 875e757cbfcec..c08fed51ea5a4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_node_interface.cpp @@ -31,12 +31,17 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + // test with nominal path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test with empty path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithAbnormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); rclcpp::shutdown(); } @@ -51,13 +56,19 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + const std::string input_odometry_topic = "behavior_velocity_planner_node/input/vehicle_odometry"; + // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); // make sure behavior_path_planner is running EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } From ee0517390be566eced4cb3b868a65701c3b6faec Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Mon, 27 Jan 2025 14:55:14 +0900 Subject: [PATCH 26/33] adapt behavior_velocity_run_out_module to new test manager Signed-off-by: mitukou1109 --- .../test/test_node_interface.cpp | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_node_interface.cpp index 75bf59751ed44..69ddca56e54b0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_node_interface.cpp @@ -31,12 +31,17 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + // test with nominal path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test with empty path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithAbnormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); rclcpp::shutdown(); } @@ -51,13 +56,19 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + const std::string input_odometry_topic = "behavior_velocity_planner_node/input/vehicle_odometry"; + // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); // make sure behavior_path_planner is running EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } From 936a5bb09a8a6e3c354d5fb83dfd20702556915c Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Mon, 27 Jan 2025 14:55:24 +0900 Subject: [PATCH 27/33] adapt behavior_velocity_stop_line_module to new test manager Signed-off-by: mitukou1109 --- .../test/test_node_interface.cpp | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_node_interface.cpp index c6d6ff638cfbb..e22f47cba9078 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_node_interface.cpp @@ -31,12 +31,17 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + // test with nominal path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test with empty path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithAbnormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); rclcpp::shutdown(); } @@ -51,13 +56,19 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + const std::string input_odometry_topic = "behavior_velocity_planner_node/input/vehicle_odometry"; + // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); // make sure behavior_path_planner is running EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } From a9015c05c71f98d96e62cdcd1c4df84c61b41907 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Mon, 27 Jan 2025 14:55:34 +0900 Subject: [PATCH 28/33] adapt behavior_velocity_traffic_light_module to new test manager Signed-off-by: mitukou1109 --- .../test/test_node_interface.cpp | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/test/test_node_interface.cpp index e24c2dab5dab9..a8583c52b74ff 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/test/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/test/test_node_interface.cpp @@ -31,12 +31,17 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + // test with nominal path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test with empty path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithAbnormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); rclcpp::shutdown(); } @@ -51,13 +56,19 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + const std::string input_odometry_topic = "behavior_velocity_planner_node/input/vehicle_odometry"; + // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); // make sure behavior_path_planner is running EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } From 4674ffe6b1530b3a6196f864d788bdf3441678f3 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Mon, 27 Jan 2025 14:55:46 +0900 Subject: [PATCH 29/33] adapt behavior_velocity_virtual_traffic_light_module to new test manager Signed-off-by: mitukou1109 --- .../test/test_node_interface.cpp | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) 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 0e22413c0c8ec..6709f001c9a35 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 @@ -53,12 +53,17 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); publishVirtualTrafficLightState(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + // test with nominal path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test with empty path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithAbnormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); rclcpp::shutdown(); } @@ -76,13 +81,19 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); publishVirtualTrafficLightState(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + const std::string input_odometry_topic = "behavior_velocity_planner_node/input/vehicle_odometry"; + // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); // make sure behavior_path_planner is running EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } From 91cafb504d3cc4dada75f29536efa970c6e334d9 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Mon, 27 Jan 2025 14:55:57 +0900 Subject: [PATCH 30/33] adapt behavior_velocity_walkway_module to new test manager Signed-off-by: mitukou1109 --- .../test/test_node_interface.cpp | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/test/test_node_interface.cpp index e0b7fa5c31965..a123975273c4f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/test/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/test/test_node_interface.cpp @@ -31,12 +31,17 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + // test with nominal path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test with empty path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithAbnormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); rclcpp::shutdown(); } @@ -51,13 +56,19 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + const std::string input_odometry_topic = "behavior_velocity_planner_node/input/vehicle_odometry"; + // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); // make sure behavior_path_planner is running EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } From ca7773f6e21a64978b1e793c928ceae616916b3b Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Mon, 27 Jan 2025 15:29:05 +0900 Subject: [PATCH 31/33] adapt motion_velocity_planner_node_universe to new test manager Signed-off-by: mitukou1109 --- .../test/src/test_node_interface.cpp | 64 +++++++++++-------- 1 file changed, 39 insertions(+), 25 deletions(-) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/test/src/test_node_interface.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/test/src/test_node_interface.cpp index b4a95dc020ffd..8bb5a78c05b3c 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/test/src/test_node_interface.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/test/src/test_node_interface.cpp @@ -32,13 +32,8 @@ std::shared_ptr generateTestManager() auto test_manager = std::make_shared(); // set subscriber with topic name: motion_velocity_planner → test_node_ - test_manager->setTrajectorySubscriber("motion_velocity_planner_node/output/trajectory"); - - // set motion_velocity_planner node's input topic name(this topic is changed to test node) - test_manager->setTrajectoryInputTopicName("motion_velocity_planner_node/input/trajectory"); - - test_manager->setInitialPoseTopicName("motion_velocity_planner_node/input/vehicle_odometry"); - test_manager->setOdometryTopicName("motion_velocity_planner_node/input/vehicle_odometry"); + test_manager->subscribeOutput( + "motion_velocity_planner_node/output/trajectory"); return test_manager; } @@ -89,20 +84,30 @@ void publishMandatoryTopics( std::shared_ptr test_target_node) { // publish necessary topics from test_manager - test_manager->publishTF(test_target_node, "/tf"); - test_manager->publishAcceleration(test_target_node, "motion_velocity_planner_node/input/accel"); - test_manager->publishPredictedObjects( - test_target_node, "motion_velocity_planner_node/input/dynamic_objects"); - test_manager->publishPointCloud( - test_target_node, "motion_velocity_planner_node/input/no_ground_pointcloud"); - test_manager->publishOdometry( - test_target_node, "motion_velocity_planner_node/input/vehicle_odometry"); - test_manager->publishAcceleration(test_target_node, "motion_velocity_planner_node/input/accel"); - test_manager->publishMap(test_target_node, "motion_velocity_planner_node/input/vector_map"); - test_manager->publishTrafficSignals( - test_target_node, "motion_velocity_planner_node/input/traffic_signals"); - test_manager->publishOccupancyGrid( - test_target_node, "motion_velocity_planner_node/input/occupancy_grid"); + test_manager->publishInput( + test_target_node, "/tf", autoware::test_utils::makeTFMsg(test_target_node, "base_link", "map")); + test_manager->publishInput( + test_target_node, "motion_velocity_planner_node/input/accel", + geometry_msgs::msg::AccelWithCovarianceStamped{}); + test_manager->publishInput( + test_target_node, "motion_velocity_planner_node/input/dynamic_objects", + autoware_perception_msgs::msg::PredictedObjects{}); + test_manager->publishInput( + test_target_node, "motion_velocity_planner_node/input/no_ground_pointcloud", + sensor_msgs::msg::PointCloud2{}.set__header( + std_msgs::msg::Header{}.set__frame_id("base_link"))); + test_manager->publishInput( + test_target_node, "motion_velocity_planner_node/input/vehicle_odometry", + autoware::test_utils::makeOdometry()); + test_manager->publishInput( + test_target_node, "motion_velocity_planner_node/input/vector_map", + autoware::test_utils::makeMapBinMsg()); + test_manager->publishInput( + test_target_node, "motion_velocity_planner_node/input/traffic_signals", + autoware_perception_msgs::msg::TrafficLightGroupArray{}); + test_manager->publishInput( + test_target_node, "motion_velocity_planner_node/input/occupancy_grid", + autoware::test_utils::makeCostMapMsg()); } TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) @@ -113,12 +118,16 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) publishMandatoryTopics(test_manager, test_target_node); + const std::string input_trajectory_topic = "motion_velocity_planner_node/input/trajectory"; + // test with nominal 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 with empty trajectory - 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(); } @@ -130,13 +139,18 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) auto test_target_node = generateNode(); publishMandatoryTopics(test_manager, test_target_node); + const std::string input_trajectory_topic = "motion_velocity_planner_node/input/trajectory"; + const std::string input_odometry_topic = "motion_velocity_planner_node/input/vehicle_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)); // make sure motion_velocity_planner is running EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromTrajectory(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } From 007905ee35d85e623b82b18a40e61de004bbde99 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Mon, 27 Jan 2025 18:34:48 +0900 Subject: [PATCH 32/33] include necessary headers Signed-off-by: mitukou1109 --- .../test/test_freespace_planner_node_interface.cpp | 1 + .../test/test_obstacle_cruise_planner_node_interface.cpp | 1 + .../test/test_autoware_obstacle_stop_planner_node_interface.cpp | 1 + .../test/test_path_optimizer_node_interface.cpp | 1 + .../test/test_autoware_path_smoother_node_interface.cpp | 1 + .../src/autoware_planning_test_manager.cpp | 2 ++ .../test/src/test_planning_validator_node_interface.cpp | 1 + .../test/test_autoware_scenario_selector_node_interface.cpp | 2 ++ .../test/test_velocity_smoother_node_interface.cpp | 1 + 9 files changed, 11 insertions(+) diff --git a/planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp b/planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp index 1078713f5e4ca..0bef16546b4d0 100644 --- a/planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp +++ b/planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp @@ -21,6 +21,7 @@ #include #include +#include #include using autoware::freespace_planner::FreespacePlannerNode; diff --git a/planning/autoware_obstacle_cruise_planner/test/test_obstacle_cruise_planner_node_interface.cpp b/planning/autoware_obstacle_cruise_planner/test/test_obstacle_cruise_planner_node_interface.cpp index fccfca9707cbb..c6dff036e7619 100644 --- a/planning/autoware_obstacle_cruise_planner/test/test_obstacle_cruise_planner_node_interface.cpp +++ b/planning/autoware_obstacle_cruise_planner/test/test_obstacle_cruise_planner_node_interface.cpp @@ -21,6 +21,7 @@ #include #include +#include #include using autoware::motion_planning::ObstacleCruisePlannerNode; 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 040fb4acd488d..6adf3f26f6d22 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 @@ -23,6 +23,7 @@ #include #include +#include #include using autoware::motion_planning::ObstacleStopPlannerNode; diff --git a/planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp b/planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp index 97e5ed2e1a6be..164da3173b655 100644 --- a/planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp +++ b/planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp @@ -21,6 +21,7 @@ #include #include +#include #include TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) diff --git a/planning/autoware_path_smoother/test/test_autoware_path_smoother_node_interface.cpp b/planning/autoware_path_smoother/test/test_autoware_path_smoother_node_interface.cpp index 8ae9885827932..edf03556f2c21 100644 --- a/planning/autoware_path_smoother/test/test_autoware_path_smoother_node_interface.cpp +++ b/planning/autoware_path_smoother/test/test_autoware_path_smoother_node_interface.cpp @@ -21,6 +21,7 @@ #include #include +#include #include TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) 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 99a1ff927dd29..17db388a1b36c 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 @@ -21,6 +21,8 @@ #include #include +#include +#include namespace autoware::planning_test_manager { diff --git a/planning/autoware_planning_validator/test/src/test_planning_validator_node_interface.cpp b/planning/autoware_planning_validator/test/src/test_planning_validator_node_interface.cpp index 34a4ce3a0be77..20677e3cd5fa5 100644 --- a/planning/autoware_planning_validator/test/src/test_planning_validator_node_interface.cpp +++ b/planning/autoware_planning_validator/test/src/test_planning_validator_node_interface.cpp @@ -21,6 +21,7 @@ #include #include +#include #include using autoware::planning_test_manager::PlanningInterfaceTestManager; diff --git a/planning/autoware_scenario_selector/test/test_autoware_scenario_selector_node_interface.cpp b/planning/autoware_scenario_selector/test/test_autoware_scenario_selector_node_interface.cpp index de56bfee2660f..4885c6d55dcfa 100644 --- a/planning/autoware_scenario_selector/test/test_autoware_scenario_selector_node_interface.cpp +++ b/planning/autoware_scenario_selector/test/test_autoware_scenario_selector_node_interface.cpp @@ -22,7 +22,9 @@ #include #include +#include #include + namespace autoware::scenario_selector { using autoware::planning_test_manager::PlanningInterfaceTestManager; diff --git a/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp b/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp index 5511daa1931e5..e7d8f1748b6e7 100644 --- a/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp +++ b/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp @@ -21,6 +21,7 @@ #include #include +#include #include using autoware::planning_test_manager::PlanningInterfaceTestManager; From e0fbe51d5441323ea0eadc138f41b496c0c8ae73 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 30 Jan 2025 09:46:26 +0900 Subject: [PATCH 33/33] Odometries -> Odometry Signed-off-by: Takayuki Murooka --- .../test/test_freespace_planner_node_interface.cpp | 2 +- .../test/test_obstacle_cruise_planner_node_interface.cpp | 2 +- .../test/test_autoware_obstacle_stop_planner_node_interface.cpp | 2 +- .../autoware_planning_test_manager.hpp | 2 +- .../src/autoware_planning_test_manager.cpp | 2 +- .../test/src/test_planning_validator_node_interface.cpp | 2 +- .../test/test_autoware_scenario_selector_node_interface.cpp | 2 +- .../test/test_velocity_smoother_node_interface.cpp | 2 +- .../test/test_behavior_path_planner_node_interface.cpp | 2 +- .../test/test_behavior_path_planner_node_interface.cpp | 2 +- .../test/test_behavior_path_planner_node_interface.cpp | 2 +- .../test/test_behavior_path_planner_node_interface.cpp | 2 +- .../test/test_autoware_behavior_path_planner_node_interface.cpp | 2 +- .../test/test_behavior_path_planner_node_interface.cpp | 2 +- .../test/test_behavior_path_planner_node_interface.cpp | 2 +- .../test/test_node_interface.cpp | 2 +- .../test/test_node_interface.cpp | 2 +- .../test/test_node_interface.cpp | 2 +- .../test/test_node_interface.cpp | 2 +- .../test/test_node_interface.cpp | 2 +- .../test/test_node_interface.cpp | 2 +- .../test/test_node_interface.cpp | 2 +- .../test/test_node_interface.cpp | 2 +- .../test/test_node_interface.cpp | 2 +- .../test/test_node_interface.cpp | 2 +- .../test/test_node_interface.cpp | 2 +- .../test/src/test_node_interface.cpp | 2 +- 27 files changed, 27 insertions(+), 27 deletions(-) diff --git a/planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp b/planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp index 0bef16546b4d0..020a184224a02 100644 --- a/planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp +++ b/planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp @@ -105,7 +105,7 @@ TEST(PlanningModuleInterfaceTest, DISABLED_NodeTestWithOffTrackEgoPose) EXPECT_GE(test_manager->getReceivedTopicNum(), 1); ASSERT_NO_THROW_WITH_ERROR_MSG( - test_manager->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } diff --git a/planning/autoware_obstacle_cruise_planner/test/test_obstacle_cruise_planner_node_interface.cpp b/planning/autoware_obstacle_cruise_planner/test/test_obstacle_cruise_planner_node_interface.cpp index c6dff036e7619..db9fef1cb7598 100644 --- a/planning/autoware_obstacle_cruise_planner/test/test_obstacle_cruise_planner_node_interface.cpp +++ b/planning/autoware_obstacle_cruise_planner/test/test_obstacle_cruise_planner_node_interface.cpp @@ -111,7 +111,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) // test for trajectory with empty/one point/overlapping point ASSERT_NO_THROW_WITH_ERROR_MSG( - test_manager->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } 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 6adf3f26f6d22..2654be0937d35 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 @@ -123,7 +123,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) // test for trajectory with empty/one point/overlapping point ASSERT_NO_THROW_WITH_ERROR_MSG( - test_manager->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } 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 63e11e98be818..127cb11d8e101 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 @@ -98,7 +98,7 @@ class PlanningInterfaceTestManager void testWithOffTrackInitialPoses( rclcpp::Node::SharedPtr target_node, const std::string & topic_name); - void testWithOffTrackOdometries( + void testWithOffTrackOdometry( rclcpp::Node::SharedPtr target_node, const std::string & topic_name); void resetReceivedTopicNum() { received_topic_num_ = 0; } 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 17db388a1b36c..1d5dbd607cfb5 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 @@ -116,7 +116,7 @@ void PlanningInterfaceTestManager::testWithOffTrackInitialPoses( } } -void PlanningInterfaceTestManager::testWithOffTrackOdometries( +void PlanningInterfaceTestManager::testWithOffTrackOdometry( rclcpp::Node::SharedPtr target_node, const std::string & topic_name) { for (const auto & deviation : {0.0, 1.0, 10.0, 100.0}) { diff --git a/planning/autoware_planning_validator/test/src/test_planning_validator_node_interface.cpp b/planning/autoware_planning_validator/test/src/test_planning_validator_node_interface.cpp index 20677e3cd5fa5..e52b74571282d 100644 --- a/planning/autoware_planning_validator/test/src/test_planning_validator_node_interface.cpp +++ b/planning/autoware_planning_validator/test/src/test_planning_validator_node_interface.cpp @@ -94,5 +94,5 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test for trajectory with empty/one point/overlapping point - ASSERT_NO_THROW(test_manager->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); + ASSERT_NO_THROW(test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); } diff --git a/planning/autoware_scenario_selector/test/test_autoware_scenario_selector_node_interface.cpp b/planning/autoware_scenario_selector/test/test_autoware_scenario_selector_node_interface.cpp index 4885c6d55dcfa..0256e7463b5a7 100644 --- a/planning/autoware_scenario_selector/test/test_autoware_scenario_selector_node_interface.cpp +++ b/planning/autoware_scenario_selector/test/test_autoware_scenario_selector_node_interface.cpp @@ -128,7 +128,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) ASSERT_NO_THROW(test_manager->testWithNormalTrajectory(test_target_node, input_trajectory_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - ASSERT_NO_THROW(test_manager->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); + ASSERT_NO_THROW(test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } } // namespace autoware::scenario_selector diff --git a/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp b/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp index e7d8f1748b6e7..d8a424095fcc8 100644 --- a/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp +++ b/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp @@ -109,7 +109,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) ASSERT_NO_THROW(test_manager->testWithNormalTrajectory(test_target_node, input_trajectory_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - ASSERT_NO_THROW(test_manager->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); + ASSERT_NO_THROW(test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp index a04d1c41cdbaa..c917171c2eb19 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -69,7 +69,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) EXPECT_GE(test_manager->getReceivedTopicNum(), 1); ASSERT_NO_THROW_WITH_ERROR_MSG( - test_manager->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp index aef0520ba0b7f..1b2593288ca34 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp @@ -66,7 +66,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) EXPECT_GE(test_manager->getReceivedTopicNum(), 1); ASSERT_NO_THROW_WITH_ERROR_MSG( - test_manager->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp index 5b73d947daa65..0f1ab11acf3c3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -71,7 +71,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) EXPECT_GE(test_manager->getReceivedTopicNum(), 1); ASSERT_NO_THROW_WITH_ERROR_MSG( - test_manager->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp index 116194840c877..393ca57bc3a67 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -70,7 +70,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) EXPECT_GE(test_manager->getReceivedTopicNum(), 1); ASSERT_NO_THROW_WITH_ERROR_MSG( - test_manager->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_autoware_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_autoware_behavior_path_planner_node_interface.cpp index d4c1c0b56fb7f..4526f8a642408 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_autoware_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_autoware_behavior_path_planner_node_interface.cpp @@ -66,7 +66,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) EXPECT_GE(test_manager->getReceivedTopicNum(), 1); ASSERT_NO_THROW_WITH_ERROR_MSG( - test_manager->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp index 01c712d86e743..1603f0c225ce2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp @@ -67,7 +67,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) EXPECT_GE(test_manager->getReceivedTopicNum(), 1); ASSERT_NO_THROW_WITH_ERROR_MSG( - test_manager->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp index 674a2803aabd5..4c36245e5b7d0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp @@ -66,7 +66,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) EXPECT_GE(test_manager->getReceivedTopicNum(), 1); ASSERT_NO_THROW_WITH_ERROR_MSG( - test_manager->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_node_interface.cpp index c49fb0568c8ec..ab8876a012654 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_node_interface.cpp @@ -68,7 +68,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) EXPECT_GE(test_manager->getReceivedTopicNum(), 1); ASSERT_NO_THROW_WITH_ERROR_MSG( - test_manager->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/test/test_node_interface.cpp index 69532e5fcb467..6246adb0facea 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/test/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/test/test_node_interface.cpp @@ -68,7 +68,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) EXPECT_GE(test_manager->getReceivedTopicNum(), 1); ASSERT_NO_THROW_WITH_ERROR_MSG( - test_manager->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/test/test_node_interface.cpp index 0c0e0d537abba..a359a3a64e416 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/test/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/test/test_node_interface.cpp @@ -68,7 +68,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) EXPECT_GE(test_manager->getReceivedTopicNum(), 1); ASSERT_NO_THROW_WITH_ERROR_MSG( - test_manager->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/test/test_node_interface.cpp index 04ae9a5d2f972..5ab57735af505 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/test/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/test/test_node_interface.cpp @@ -68,7 +68,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) EXPECT_GE(test_manager->getReceivedTopicNum(), 1); ASSERT_NO_THROW_WITH_ERROR_MSG( - test_manager->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_node_interface.cpp index c08fed51ea5a4..714dbc102cc05 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_node_interface.cpp @@ -68,7 +68,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) EXPECT_GE(test_manager->getReceivedTopicNum(), 1); ASSERT_NO_THROW_WITH_ERROR_MSG( - test_manager->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/test_node_interface.cpp index d2273205c1d61..d62710f4daa24 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/test_node_interface.cpp @@ -65,7 +65,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) EXPECT_GE(test_manager->getReceivedTopicNum(), 1); ASSERT_NO_THROW_WITH_ERROR_MSG( - test_manager->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_node_interface.cpp index 69ddca56e54b0..9e906c816b63c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_node_interface.cpp @@ -68,7 +68,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) EXPECT_GE(test_manager->getReceivedTopicNum(), 1); ASSERT_NO_THROW_WITH_ERROR_MSG( - test_manager->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_node_interface.cpp index e22f47cba9078..ac158a0e59084 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_node_interface.cpp @@ -68,7 +68,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) EXPECT_GE(test_manager->getReceivedTopicNum(), 1); ASSERT_NO_THROW_WITH_ERROR_MSG( - test_manager->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/test/test_node_interface.cpp index a8583c52b74ff..7c261486c1270 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/test/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/test/test_node_interface.cpp @@ -68,7 +68,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) EXPECT_GE(test_manager->getReceivedTopicNum(), 1); ASSERT_NO_THROW_WITH_ERROR_MSG( - test_manager->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } 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 6709f001c9a35..74c61696a0d58 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 @@ -93,7 +93,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) EXPECT_GE(test_manager->getReceivedTopicNum(), 1); ASSERT_NO_THROW_WITH_ERROR_MSG( - test_manager->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/test/test_node_interface.cpp index a123975273c4f..fb8286874fe03 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/test/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/test/test_node_interface.cpp @@ -68,7 +68,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) EXPECT_GE(test_manager->getReceivedTopicNum(), 1); ASSERT_NO_THROW_WITH_ERROR_MSG( - test_manager->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/test/src/test_node_interface.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/test/src/test_node_interface.cpp index 8bb5a78c05b3c..6ce5e3e506262 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/test/src/test_node_interface.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/test/src/test_node_interface.cpp @@ -150,7 +150,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) EXPECT_GE(test_manager->getReceivedTopicNum(), 1); ASSERT_NO_THROW_WITH_ERROR_MSG( - test_manager->testWithOffTrackOdometries(test_target_node, input_odometry_topic)); + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); }