test(simple_pure_pursuit): add unit tests (#1)

* add unit tests Signed-off-by: mitukou1109 <[email protected]> * add test for calc_longitudinal_control Signed-off-by: mitukou1109 <[email protected]> * rename calc_steering_angle to calc_lateral_control Signed-off-by: mitukou1109 <[email protected]> --------- Signed-off-by: mitukou1109 <[email protected]>
autowarefoundation · Jan 9, 2025 · 67e3793 · 67e3793
1 parent 3c7e31f
commit 67e3793
Show file tree

Hide file tree

Showing 5 changed files with 173 additions and 3 deletions.
diff --git 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
@@ -16,10 +16,15 @@
   <depend>autoware_control_msgs</depend>
   <depend>autoware_motion_utils</depend>
   <depend>autoware_planning_msgs</depend>
+  <depend>autoware_test_utils</depend>
   <depend>autoware_vehicle_info_utils</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
 
+  <test_depend>ament_cmake_ros</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>

diff --git 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
@@ -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
@@ -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 <ament_index_cpp/get_package_share_directory.hpp>
+#include <autoware_test_utils/autoware_test_utils.hpp>
+
+#include <gtest/gtest.h>
+
+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<SimplePurePursuitNode>(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<SimplePurePursuitNode> 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<Trajectory>(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<Trajectory>(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<Trajectory>(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<Trajectory>(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