From 582d9f11b9be92cb5d82f7a407408e06a59c6828 Mon Sep 17 00:00:00 2001 From: Tiankui Xian <1041084556@qq.com> Date: Fri, 18 Oct 2024 14:16:30 +0900 Subject: [PATCH] test(autoware_control_evaluator): add test for autoware_control_evaluator. (#9114) * init Signed-off-by: xtk8532704 <1041084556@qq.com> * tmp save. Signed-off-by: xtk8532704 <1041084556@qq.com> * save, there is a bug Signed-off-by: xtk8532704 <1041084556@qq.com> * update package.xml Signed-off-by: xtk8532704 <1041084556@qq.com> * coverage rate 64.5 Signed-off-by: xtk8532704 <1041084556@qq.com> * remove comments. Signed-off-by: xtk8532704 <1041084556@qq.com> --------- Signed-off-by: xtk8532704 <1041084556@qq.com> --- .../autoware_control_evaluator/CMakeLists.txt | 9 + .../control_evaluator_node.hpp | 2 - .../autoware_control_evaluator/package.xml | 5 +- .../test/test_control_evaluator_node.cpp | 197 ++++++++++++++++++ 4 files changed, 210 insertions(+), 3 deletions(-) create mode 100644 evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp diff --git a/evaluator/autoware_control_evaluator/CMakeLists.txt b/evaluator/autoware_control_evaluator/CMakeLists.txt index 2e6f9851cafce..5d6474de88015 100644 --- a/evaluator/autoware_control_evaluator/CMakeLists.txt +++ b/evaluator/autoware_control_evaluator/CMakeLists.txt @@ -16,6 +16,15 @@ rclcpp_components_register_node(control_evaluator_node EXECUTABLE control_evaluator ) +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_control_evaluator + test/test_control_evaluator_node.cpp + ) + target_link_libraries(test_control_evaluator + control_evaluator_node + ) +endif() + ament_auto_package( INSTALL_TO_SHARE diff --git a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp index e2437e614d211..9641f4f75026a 100644 --- a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp +++ b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp @@ -56,8 +56,6 @@ class ControlEvaluatorNode : public rclcpp::Node const Trajectory & traj, const Point & ego_point); DiagnosticStatus generateYawDeviationDiagnosticStatus( const Trajectory & traj, const Pose & ego_pose); - std::optional generateStopDiagnosticStatus( - const DiagnosticArray & diag, const std::string & function_name); DiagnosticStatus generateAEBDiagnosticStatus(const DiagnosticStatus & diag); DiagnosticStatus generateLaneletDiagnosticStatus(const Pose & ego_pose) const; diff --git a/evaluator/autoware_control_evaluator/package.xml b/evaluator/autoware_control_evaluator/package.xml index 52f5a04092713..1de3c0ecacae6 100644 --- a/evaluator/autoware_control_evaluator/package.xml +++ b/evaluator/autoware_control_evaluator/package.xml @@ -20,12 +20,15 @@ autoware_motion_utils autoware_planning_msgs autoware_route_handler + autoware_test_utils autoware_universe_utils diagnostic_msgs + nav_msgs pluginlib rclcpp rclcpp_components - + tf2 + tf2_ros ament_cmake_ros ament_lint_auto diff --git a/evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp b/evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp new file mode 100644 index 0000000000000..ca51721fa1932 --- /dev/null +++ b/evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp @@ -0,0 +1,197 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ament_index_cpp/get_package_share_directory.hpp" +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/time.hpp" +#include "tf2_ros/transform_broadcaster.h" + +#include + +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "diagnostic_msgs/msg/diagnostic_array.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" + +#include "boost/lexical_cast.hpp" + +#include + +#include +#include +#include +#include + +using EvalNode = control_diagnostics::ControlEvaluatorNode; +using Trajectory = autoware_planning_msgs::msg::Trajectory; +using TrajectoryPoint = autoware_planning_msgs::msg::TrajectoryPoint; +using diagnostic_msgs::msg::DiagnosticArray; +using nav_msgs::msg::Odometry; + +constexpr double epsilon = 1e-6; + +class EvalTest : public ::testing::Test +{ +protected: + void SetUp() override + { + rclcpp::init(0, nullptr); + + rclcpp::NodeOptions options; + const auto share_dir = + ament_index_cpp::get_package_share_directory("autoware_control_evaluator"); + + dummy_node = std::make_shared("control_evaluator_test_node"); + eval_node = std::make_shared(options); + // Enable all logging in the node + auto ret = rcutils_logging_set_logger_level( + dummy_node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG); + if (ret != RCUTILS_RET_OK) { + std::cerr << "Failed to set logging severity to DEBUG\n"; + } + ret = rcutils_logging_set_logger_level( + eval_node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG); + if (ret != RCUTILS_RET_OK) { + std::cerr << "Failed to set logging severity to DEBUG\n"; + } + traj_pub_ = + rclcpp::create_publisher(dummy_node, "/control_evaluator/input/trajectory", 1); + odom_pub_ = + rclcpp::create_publisher(dummy_node, "/control_evaluator/input/odometry", 1); + tf_broadcaster_ = std::make_unique(dummy_node); + publishEgoPose(0.0, 0.0, 0.0); + } + + ~EvalTest() override { rclcpp::shutdown(); } + + void setTargetMetric(const std::string & metric_str) + { + const auto is_target_metric = [metric_str](const auto & status) { + return status.name == metric_str; + }; + metric_sub_ = rclcpp::create_subscription( + dummy_node, "/control_evaluator/metrics", 1, [=](const DiagnosticArray::ConstSharedPtr msg) { + const auto it = std::find_if(msg->status.begin(), msg->status.end(), is_target_metric); + if (it != msg->status.end()) { + metric_value_ = boost::lexical_cast(it->values[0].value); + metric_updated_ = true; + } + }); + } + + Trajectory makeTrajectory(const std::vector> & traj) + { + Trajectory t; + t.header.frame_id = "map"; + TrajectoryPoint p; + for (const std::pair & point : traj) { + p.pose.position.x = point.first; + p.pose.position.y = point.second; + t.points.push_back(p); + } + return t; + } + + void publishTrajectory(const Trajectory & traj) + { + traj_pub_->publish(traj); + rclcpp::spin_some(eval_node); + rclcpp::spin_some(dummy_node); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + + double publishTrajectoryAndGetMetric(const Trajectory & traj) + { + metric_updated_ = false; + traj_pub_->publish(traj); + while (!metric_updated_) { + rclcpp::spin_some(eval_node); + rclcpp::spin_some(dummy_node); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + return metric_value_; + } + + void publishEgoPose(const double x, const double y, const double yaw) + { + Odometry odom; + odom.header.frame_id = "map"; + odom.header.stamp = dummy_node->now(); + odom.pose.pose.position.x = x; + odom.pose.pose.position.y = y; + odom.pose.pose.position.z = 0.0; + tf2::Quaternion q; + q.setRPY(0.0, 0.0, yaw); + odom.pose.pose.orientation.x = q.x(); + odom.pose.pose.orientation.y = q.y(); + odom.pose.pose.orientation.z = q.z(); + odom.pose.pose.orientation.w = q.w(); + + odom_pub_->publish(odom); + rclcpp::spin_some(eval_node); + rclcpp::spin_some(dummy_node); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + + // Latest metric value + bool metric_updated_ = false; + double metric_value_; + // Node + rclcpp::Node::SharedPtr dummy_node; + EvalNode::SharedPtr eval_node; + // Trajectory publishers + rclcpp::Publisher::SharedPtr traj_pub_; + rclcpp::Publisher::SharedPtr odom_pub_; + rclcpp::Subscription::SharedPtr metric_sub_; + // TF broadcaster + std::unique_ptr tf_broadcaster_; +}; + +TEST_F(EvalTest, TestYawDeviation) +{ + auto setYaw = [](geometry_msgs::msg::Quaternion & msg, const double yaw_rad) { + tf2::Quaternion q; + q.setRPY(0.0, 0.0, yaw_rad); + msg.x = q.x(); + msg.y = q.y(); + msg.z = q.z(); + msg.w = q.w(); + }; + setTargetMetric("yaw_deviation"); + Trajectory t = makeTrajectory({{0.0, 0.0}, {1.0, 0.0}}); + for (auto & p : t.points) { + setYaw(p.pose.orientation, M_PI); + } + + publishEgoPose(0.0, 0.0, M_PI); + EXPECT_NEAR(publishTrajectoryAndGetMetric(t), 0.0, epsilon); + + publishEgoPose(0.0, 0.0, 0.0); + EXPECT_NEAR(publishTrajectoryAndGetMetric(t), M_PI, epsilon); + + publishEgoPose(0.0, 0.0, -M_PI); + EXPECT_NEAR(publishTrajectoryAndGetMetric(t), 0.0, epsilon); +} + +TEST_F(EvalTest, TestLateralDeviation) +{ + setTargetMetric("lateral_deviation"); + Trajectory t = makeTrajectory({{0.0, 0.0}, {1.0, 0.0}}); + + publishEgoPose(0.0, 0.0, 0.0); + EXPECT_NEAR(publishTrajectoryAndGetMetric(t), 0.0, epsilon); + + publishEgoPose(1.0, 1.0, 0.0); + EXPECT_NEAR(publishTrajectoryAndGetMetric(t), 1.0, epsilon); +}