From 67e3793d1f632298ddd70cf9ccb51eac74227e18 Mon Sep 17 00:00:00 2001 From: Mitsuhiro Sakamoto <50359861+mitukou1109@users.noreply.github.com> Date: Thu, 9 Jan 2025 18:12:53 +0900 Subject: [PATCH] test(simple_pure_pursuit): add unit tests (#1) * add unit tests Signed-off-by: mitukou1109 * add test for calc_longitudinal_control Signed-off-by: mitukou1109 * rename calc_steering_angle to calc_lateral_control Signed-off-by: mitukou1109 --------- Signed-off-by: mitukou1109 --- .../CMakeLists.txt | 9 ++ .../autoware_simple_pure_pursuit/package.xml | 5 + .../src/simple_pure_pursuit.cpp | 4 +- .../src/simple_pure_pursuit.hpp | 5 +- .../test/test_simple_pure_pursuit.cpp | 153 ++++++++++++++++++ 5 files changed, 173 insertions(+), 3 deletions(-) create mode 100644 control/autoware_simple_pure_pursuit/test/test_simple_pure_pursuit.cpp diff --git a/control/autoware_simple_pure_pursuit/CMakeLists.txt b/control/autoware_simple_pure_pursuit/CMakeLists.txt index c5a4b0e0..72f8f602 100644 --- a/control/autoware_simple_pure_pursuit/CMakeLists.txt +++ b/control/autoware_simple_pure_pursuit/CMakeLists.txt @@ -13,6 +13,15 @@ rclcpp_components_register_node(${PROJECT_NAME}_lib EXECUTABLE ${PROJECT_NAME}_exe ) +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(${PROJECT_NAME}_test + test/test_simple_pure_pursuit.cpp + ) + target_link_libraries(${PROJECT_NAME}_test + ${PROJECT_NAME}_lib + ) +endif() + ament_auto_package( INSTALL_TO_SHARE launch diff --git a/control/autoware_simple_pure_pursuit/package.xml b/control/autoware_simple_pure_pursuit/package.xml index 9243c25d..59457836 100644 --- a/control/autoware_simple_pure_pursuit/package.xml +++ b/control/autoware_simple_pure_pursuit/package.xml @@ -16,10 +16,15 @@ autoware_control_msgs autoware_motion_utils autoware_planning_msgs + autoware_test_utils autoware_vehicle_info_utils rclcpp rclcpp_components + ament_cmake_ros + ament_lint_auto + autoware_lint_common + ament_cmake diff --git a/control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.cpp b/control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.cpp index 7d3ddff0..8645cdfc 100644 --- a/control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.cpp +++ b/control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.cpp @@ -88,7 +88,7 @@ autoware_control_msgs::msg::Control SimplePurePursuitNode::create_control_comman autoware_control_msgs::msg::Control control_command; control_command.longitudinal = calc_longitudinal_control(odom, target_longitudinal_vel); control_command.lateral = - calc_steering_angle(odom, traj, target_longitudinal_vel, closest_traj_point_idx); + calc_lateral_control(odom, traj, target_longitudinal_vel, closest_traj_point_idx); return control_command; } @@ -106,7 +106,7 @@ autoware_control_msgs::msg::Longitudinal SimplePurePursuitNode::calc_longitudina return longitudinal_control_command; } -autoware_control_msgs::msg::Lateral SimplePurePursuitNode::calc_steering_angle( +autoware_control_msgs::msg::Lateral SimplePurePursuitNode::calc_lateral_control( const Odometry & odom, const Trajectory & traj, const double target_longitudinal_vel, const size_t closest_traj_point_idx) const { diff --git a/control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.hpp b/control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.hpp index 1f6b797a..86390e26 100644 --- a/control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.hpp +++ b/control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.hpp @@ -64,9 +64,12 @@ class SimplePurePursuitNode : public rclcpp::Node const Odometry & odom, const Trajectory & traj); autoware_control_msgs::msg::Longitudinal calc_longitudinal_control( const Odometry & odom, const double target_longitudinal_vel) const; - autoware_control_msgs::msg::Lateral calc_steering_angle( + autoware_control_msgs::msg::Lateral calc_lateral_control( const Odometry & odom, const Trajectory & traj, const double target_longitudinal_vel, const size_t closest_traj_point_idx) const; + +public: + friend class SimplePurePursuitNodeTest; }; } // namespace autoware::control::simple_pure_pursuit diff --git a/control/autoware_simple_pure_pursuit/test/test_simple_pure_pursuit.cpp b/control/autoware_simple_pure_pursuit/test/test_simple_pure_pursuit.cpp new file mode 100644 index 00000000..9bdbe283 --- /dev/null +++ b/control/autoware_simple_pure_pursuit/test/test_simple_pure_pursuit.cpp @@ -0,0 +1,153 @@ +// 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 "../src/simple_pure_pursuit.hpp" + +#include +#include + +#include + +namespace autoware::control::simple_pure_pursuit +{ +Odometry makeOdometry(const double x, const double y, const double yaw) +{ + Odometry odom; + odom.pose.pose.position.x = x; + odom.pose.pose.position.y = y; + odom.pose.pose.orientation.z = std::sin(yaw / 2); + odom.pose.pose.orientation.w = std::cos(yaw / 2); + return odom; +} + +class SimplePurePursuitNodeTest : public ::testing::Test +{ +protected: + void SetUp() override + { + rclcpp::init(0, nullptr); + + const auto autoware_test_utils_dir = + ament_index_cpp::get_package_share_directory("autoware_test_utils"); + const auto autoware_simple_pure_pursuit_dir = + ament_index_cpp::get_package_share_directory("autoware_simple_pure_pursuit"); + + auto node_options = rclcpp::NodeOptions{}; + autoware::test_utils::updateNodeOptions( + node_options, {autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml", + autoware_simple_pure_pursuit_dir + "/config/simple_pure_pursuit.param.yaml"}); + + node_ = std::make_shared(node_options); + } + + void TearDown() override { rclcpp::shutdown(); } + + autoware_control_msgs::msg::Control create_control_command( + const Odometry & odom, const Trajectory & traj) const + { + return node_->create_control_command(odom, traj); + } + + autoware_control_msgs::msg::Longitudinal calc_longitudinal_control( + const Odometry & odom, const double target_longitudinal_vel) const + { + return node_->calc_longitudinal_control(odom, target_longitudinal_vel); + } + + autoware_control_msgs::msg::Lateral calc_lateral_control( + const Odometry & odom, const Trajectory & traj, const double target_longitudinal_vel, + const size_t closest_traj_point_idx) const + { + return node_->calc_lateral_control(odom, traj, target_longitudinal_vel, closest_traj_point_idx); + } + + double speed_proportional_gain() const { return node_->speed_proportional_gain_; } + +private: + std::shared_ptr node_; +}; + +TEST_F(SimplePurePursuitNodeTest, create_control_command) +{ + { // normal case + const auto odom = makeOdometry(0.0, 0.0, 0.0); + const auto traj = autoware::test_utils::generateTrajectory(10, 1.0, 1.0); + + const auto result = create_control_command(odom, traj); + + EXPECT_DOUBLE_EQ(result.longitudinal.velocity, 1.0); + EXPECT_DOUBLE_EQ(result.longitudinal.acceleration, speed_proportional_gain() * 1.0); + } + + { // ego reached goal + const auto odom = makeOdometry(10.0, 0.0, 0.0); + const auto traj = autoware::test_utils::generateTrajectory(10, 1.0, 1.0); + + const auto result = create_control_command(odom, traj); + + EXPECT_DOUBLE_EQ(result.longitudinal.velocity, 0.0); + EXPECT_DOUBLE_EQ(result.longitudinal.acceleration, -10.0); + } + + { // reference trajectory is too short + const auto odom = makeOdometry(0.0, 0.0, 0.0); + const auto traj = autoware::test_utils::generateTrajectory(5, 1.0, 1.0); + + const auto result = create_control_command(odom, traj); + + EXPECT_DOUBLE_EQ(result.longitudinal.velocity, 0.0); + EXPECT_DOUBLE_EQ(result.longitudinal.acceleration, -10.0); + } +} + +TEST_F(SimplePurePursuitNodeTest, calc_longitudinal_control) +{ + { // normal case + const auto odom = makeOdometry(0.0, 0.0, 0.0); + const auto target_longitudinal_vel = 1.0; + + const auto result = calc_longitudinal_control(odom, target_longitudinal_vel); + + EXPECT_DOUBLE_EQ(result.velocity, 1.0); + EXPECT_DOUBLE_EQ(result.acceleration, speed_proportional_gain() * 1.0); + } +} + +TEST_F(SimplePurePursuitNodeTest, calc_lateral_control) +{ + const auto traj = autoware::test_utils::generateTrajectory(10, 1.0); + + { // normal case + const auto odom = makeOdometry(0.0, 0.0, 0.0); + const auto target_longitudinal_vel = 1.0; + const size_t closest_traj_point_idx = 0; + + const auto result = + calc_lateral_control(odom, traj, target_longitudinal_vel, closest_traj_point_idx); + + EXPECT_DOUBLE_EQ(result.steering_tire_angle, 0.0f); + } + + { // lookahead distance exceeds remaining trajectory length + const auto odom = makeOdometry(0.0, 0.0, 0.0); + const auto target_longitudinal_vel = 2.0; + const size_t closest_traj_point_idx = 8; + + const auto result = + calc_lateral_control(odom, traj, target_longitudinal_vel, closest_traj_point_idx); + + EXPECT_DOUBLE_EQ(result.steering_tire_angle, 0.0f); + } +} +} // namespace autoware::control::simple_pure_pursuit \ No newline at end of file