1+ // Copyright 2024 Tier IV, Inc.
2+ //
3+ // Licensed under the Apache License, Version 2.0 (the "License");
4+ // you may not use this file except in compliance with the License.
5+ // You may obtain a copy of the License at
6+ //
7+ // http://www.apache.org/licenses/LICENSE-2.0
8+ //
9+ // Unless required by applicable law or agreed to in writing, software
10+ // distributed under the License is distributed on an "AS IS" BASIS,
11+ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+ // See the License for the specific language governing permissions and
13+ // limitations under the License.
14+
15+ #include " ../src/simple_pure_pursuit.hpp"
16+
17+ #include < ament_index_cpp/get_package_share_directory.hpp>
18+ #include < autoware_test_utils/autoware_test_utils.hpp>
19+
20+ #include < gtest/gtest.h>
21+
22+ namespace autoware ::control::simple_pure_pursuit
23+ {
24+ Odometry makeOdometry (const double x, const double y, const double yaw)
25+ {
26+ Odometry odom;
27+ odom.pose .pose .position .x = x;
28+ odom.pose .pose .position .y = y;
29+ odom.pose .pose .orientation .z = std::sin (yaw / 2 );
30+ odom.pose .pose .orientation .w = std::cos (yaw / 2 );
31+ return odom;
32+ }
33+
34+ class SimplePurePursuitNodeTest : public ::testing::Test
35+ {
36+ protected:
37+ void SetUp () override
38+ {
39+ rclcpp::init (0 , nullptr );
40+
41+ const auto autoware_test_utils_dir =
42+ ament_index_cpp::get_package_share_directory (" autoware_test_utils"
43+ const auto autoware_simple_pure_pursuit_dir =
44+ ament_index_cpp::get_package_share_directory (" autoware_simple_pure_pursuit"
45+
46+ auto node_options = rclcpp::NodeOptions{};
47+ autoware::test_utils::updateNodeOptions (
48+ node_options, {autoware_test_utils_dir + " /config/test_vehicle_info.param.yaml"
49+ autoware_simple_pure_pursuit_dir + " /config/simple_pure_pursuit.param.yaml"
50+
51+ node_ = std::make_shared<SimplePurePursuitNode>(node_options);
52+ }
53+
54+ void TearDown () override { rclcpp::shutdown (); }
55+
56+ autoware_control_msgs::msg::Control create_control_command (
57+ const Odometry & odom, const Trajectory & traj) const
58+ {
59+ return node_->create_control_command (odom, traj);
60+ }
61+
62+ autoware_control_msgs::msg::Longitudinal calc_longitudinal_control (
63+ const Odometry & odom, const double target_longitudinal_vel) const
64+ {
65+ return node_->calc_longitudinal_control (odom, target_longitudinal_vel);
66+ }
67+
68+ autoware_control_msgs::msg::Lateral calc_lateral_control (
69+ const Odometry & odom, const Trajectory & traj, const double target_longitudinal_vel,
70+ const size_t closest_traj_point_idx) const
71+ {
72+ return node_->calc_lateral_control (odom, traj, target_longitudinal_vel, closest_traj_point_idx);
73+ }
74+
75+ double speed_proportional_gain () const { return node_->speed_proportional_gain_ ; }
76+
77+ private:
78+ std::shared_ptr<SimplePurePursuitNode> node_;
79+ };
80+
81+ TEST_F (SimplePurePursuitNodeTest, create_control_command)
82+ {
83+ { // normal case
84+ const auto odom = makeOdometry (0.0 , 0.0 , 0.0 );
85+ const auto traj = autoware::test_utils::generateTrajectory<Trajectory>(10 , 1.0 , 1.0 );
86+
87+ const auto result = create_control_command (odom, traj);
88+
89+ EXPECT_DOUBLE_EQ (result.longitudinal .velocity , 1.0 );
90+ EXPECT_DOUBLE_EQ (result.longitudinal .acceleration , speed_proportional_gain () * 1.0 );
91+ }
92+
93+ { // ego reached goal
94+ const auto odom = makeOdometry (10.0 , 0.0 , 0.0 );
95+ const auto traj = autoware::test_utils::generateTrajectory<Trajectory>(10 , 1.0 , 1.0 );
96+
97+ const auto result = create_control_command (odom, traj);
98+
99+ EXPECT_DOUBLE_EQ (result.longitudinal .velocity , 0.0 );
100+ EXPECT_DOUBLE_EQ (result.longitudinal .acceleration , -10.0 );
101+ }
102+
103+ { // reference trajectory is too short
104+ const auto odom = makeOdometry (0.0 , 0.0 , 0.0 );
105+ const auto traj = autoware::test_utils::generateTrajectory<Trajectory>(5 , 1.0 , 1.0 );
106+
107+ const auto result = create_control_command (odom, traj);
108+
109+ EXPECT_DOUBLE_EQ (result.longitudinal .velocity , 0.0 );
110+ EXPECT_DOUBLE_EQ (result.longitudinal .acceleration , -10.0 );
111+ }
112+ }
113+
114+ TEST_F (SimplePurePursuitNodeTest, calc_longitudinal_control)
115+ {
116+ { // normal case
117+ const auto odom = makeOdometry (0.0 , 0.0 , 0.0 );
118+ const auto target_longitudinal_vel = 1.0 ;
119+
120+ const auto result = calc_longitudinal_control (odom, target_longitudinal_vel);
121+
122+ EXPECT_DOUBLE_EQ (result.velocity , 1.0 );
123+ EXPECT_DOUBLE_EQ (result.acceleration , speed_proportional_gain () * 1.0 );
124+ }
125+ }
126+
127+ TEST_F (SimplePurePursuitNodeTest, calc_lateral_control)
128+ {
129+ const auto traj = autoware::test_utils::generateTrajectory<Trajectory>(10 , 1.0 );
130+
131+ { // normal case
132+ const auto odom = makeOdometry (0.0 , 0.0 , 0.0 );
133+ const auto target_longitudinal_vel = 1.0 ;
134+ const size_t closest_traj_point_idx = 0 ;
135+
136+ const auto result =
137+ calc_lateral_control (odom, traj, target_longitudinal_vel, closest_traj_point_idx);
138+
139+ EXPECT_DOUBLE_EQ (result.steering_tire_angle , 0 .0f );
140+ }
141+
142+ { // lookahead distance exceeds remaining trajectory length
143+ const auto odom = makeOdometry (0.0 , 0.0 , 0.0 );
144+ const auto target_longitudinal_vel = 2.0 ;
145+ const size_t closest_traj_point_idx = 8 ;
146+
147+ const auto result =
148+ calc_lateral_control (odom, traj, target_longitudinal_vel, closest_traj_point_idx);
149+
150+ EXPECT_DOUBLE_EQ (result.steering_tire_angle , 0 .0f );
151+ }
152+ }
153+ } // namespace autoware::control::simple_pure_pursuit
0 commit comments