test_trajectory_tracker.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2018, the neonavigation authors
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the copyright holder nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <algorithm>
31 #include <string>
32 #include <vector>
33 
34 #include <trajectory_tracker_test.h>
35 
36 TEST_F(TrajectoryTrackerTest, StraightStop)
37 {
38  initState(Eigen::Vector2d(0, 0), 0);
39 
40  std::vector<Eigen::Vector3d> poses;
41  for (double x = 0.0; x < 0.5; x += 0.01)
42  poses.push_back(Eigen::Vector3d(x, 0.0, 0.0));
43  poses.push_back(Eigen::Vector3d(0.5, 0.0, 0.0));
44  waitUntilStart(std::bind(&TrajectoryTrackerTest::publishPath, this, poses));
45 
46  ros::Rate rate(50);
47  const ros::Time start = ros::Time::now();
48  while (ros::ok())
49  {
50  ASSERT_LT(ros::Time::now() - start, ros::Duration(10.0));
51 
52  publishTransform();
53  rate.sleep();
54  ros::spinOnce();
55  if (status_->status == trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL)
56  break;
57  }
58  for (int j = 0; j < 5; ++j)
59  {
60  for (int i = 0; i < 5; ++i)
61  {
62  publishTransform();
63  rate.sleep();
64  ros::spinOnce();
65  }
66 
67  // Check multiple times to assert overshoot.
68  ASSERT_NEAR(yaw_, 0.0, error_ang_)
69  << "[overshoot after goal (" << j << ")] ";
70  ASSERT_NEAR(pos_[0], 0.5, error_lin_)
71  << "[overshoot after goal (" << j << ")] ";
72  ASSERT_NEAR(pos_[1], 0.0, error_lin_)
73  << "[overshoot after goal (" << j << ")] ";
74  }
75  ASSERT_EQ(last_path_header_.stamp, status_->path_header.stamp);
76 }
77 
78 TEST_F(TrajectoryTrackerTest, StraightStopOvershoot)
79 {
80  const double resolutions[] =
81  {
82  0.1,
83  0.001, // default epsilon
84  0.0001,
85  };
86  for (const double resolution : resolutions)
87  {
88  const std::string info_message = "resolution: " + std::to_string(resolution);
89 
90  initState(Eigen::Vector2d(1, 0), 0);
91 
92  std::vector<Eigen::Vector3d> poses;
93  for (double x = 0.0; x < 0.5 - resolution; x += 0.1)
94  poses.push_back(Eigen::Vector3d(x, 0, 0));
95  poses.push_back(Eigen::Vector3d(0.5 - resolution, 0, 0));
96  poses.push_back(Eigen::Vector3d(0.5, 0, 0));
97  waitUntilStart(std::bind(&TrajectoryTrackerTest::publishPath, this, poses));
98 
99  ros::Rate rate(50);
100  const ros::Time start = ros::Time::now();
101  while (ros::ok())
102  {
103  ASSERT_LT(ros::Time::now() - start, ros::Duration(10.0)) << info_message;
104 
105  publishTransform();
106  rate.sleep();
107  ros::spinOnce();
108  if (status_->status == trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL)
109  break;
110  }
111  for (int j = 0; j < 5; ++j)
112  {
113  for (int i = 0; i < 5; ++i)
114  {
115  publishTransform();
116  rate.sleep();
117  ros::spinOnce();
118  }
119 
120  // Check multiple times to assert overshoot.
121  ASSERT_NEAR(yaw_, 0.0, error_ang_)
122  << "[overshoot after goal (" << j << ")] "
123  << info_message;
124  EXPECT_NEAR(pos_[0], 0.5, error_lin_)
125  << "[overshoot after goal (" << j << ")] "
126  << info_message;
127  ASSERT_NEAR(pos_[1], 0.0, error_lin_)
128  << "[overshoot after goal (" << j << ")] "
129  << info_message;
130  }
131  ASSERT_EQ(last_path_header_.stamp, status_->path_header.stamp) << info_message;
132  }
133 }
134 
135 TEST_F(TrajectoryTrackerTest, StraightStopConvergence)
136 {
137  const double vels[] =
138  {
139  0.02, 0.05, 0.1, 0.2, 0.5, 1.0
140  };
141  const double path_length = 2.0;
142  for (const double vel : vels)
143  {
144  const std::string info_message = "linear vel: " + std::to_string(vel);
145 
146  initState(Eigen::Vector2d(0, 0.01), 0);
147 
148  std::vector<Eigen::Vector4d> poses;
149  for (double x = 0.0; x < path_length; x += 0.01)
150  poses.push_back(Eigen::Vector4d(x, 0.0, 0.0, vel));
151  poses.push_back(Eigen::Vector4d(path_length, 0.0, 0.0, vel));
152  waitUntilStart(std::bind(&TrajectoryTrackerTest::publishPathVelocity, this, poses));
153 
154  ros::Rate rate(50);
155  const ros::Time start = ros::Time::now();
156  while (ros::ok())
157  {
158  ASSERT_LT(ros::Time::now() - start, ros::Duration(5.0 + path_length / vel)) << info_message;
159 
160  publishTransform();
161  rate.sleep();
162  ros::spinOnce();
163  if (status_->status == trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL)
164  break;
165  }
166  for (int j = 0; j < 5; ++j)
167  {
168  for (int i = 0; i < 5; ++i)
169  {
170  publishTransform();
171  rate.sleep();
172  ros::spinOnce();
173  }
174 
175  // Check multiple times to assert overshoot.
176  EXPECT_NEAR(yaw_, 0.0, error_ang_)
177  << "[overshoot after goal (" << j << ")] "
178  << info_message;
179  EXPECT_NEAR(pos_[0], path_length, error_lin_)
180  << "[overshoot after goal (" << j << ")] "
181  << info_message;
182  EXPECT_NEAR(pos_[1], 0.0, error_lin_)
183  << "[overshoot after goal (" << j << ")] "
184  << info_message;
185  }
186  ASSERT_EQ(last_path_header_.stamp, status_->path_header.stamp);
187  }
188 }
189 
190 TEST_F(TrajectoryTrackerTest, StraightVelocityChange)
191 {
192  initState(Eigen::Vector2d(0, 0), 0);
193 
194  std::vector<Eigen::Vector4d> poses;
195  for (double x = 0.0; x < 0.6; x += 0.01)
196  poses.push_back(Eigen::Vector4d(x, 0.0, 0.0, 0.3));
197  for (double x = 0.6; x < 1.5; x += 0.01)
198  poses.push_back(Eigen::Vector4d(x, 0.0, 0.0, 0.5));
199  poses.push_back(Eigen::Vector4d(1.5, 0.0, 0.0, 0.5));
200  waitUntilStart(std::bind(&TrajectoryTrackerTest::publishPathVelocity, this, poses));
201 
202  ros::Rate rate(50);
203  const ros::Time start = ros::Time::now();
204  while (ros::ok())
205  {
206  ASSERT_LT(ros::Time::now(), start + ros::Duration(10.0));
207 
208  publishTransform();
209  rate.sleep();
210  ros::spinOnce();
211 
212  if (0.3 < pos_[0] && pos_[0] < 0.35)
213  {
214  ASSERT_NEAR(cmd_vel_->linear.x, 0.3, error_lin_);
215  }
216  else if (0.95 < pos_[0] && pos_[0] < 1.0)
217  {
218  ASSERT_NEAR(cmd_vel_->linear.x, 0.5, error_lin_);
219  }
220 
221  if (status_->status == trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL)
222  break;
223  }
224  for (int j = 0; j < 5; ++j)
225  {
226  for (int i = 0; i < 5; ++i)
227  {
228  publishTransform();
229  rate.sleep();
230  ros::spinOnce();
231  }
232 
233  // Check multiple times to assert overshoot.
234  ASSERT_NEAR(yaw_, 0.0, error_ang_)
235  << "[overshoot after goal (" << j << ")] ";
236  ASSERT_NEAR(pos_[0], 1.5, error_lin_)
237  << "[overshoot after goal (" << j << ")] ";
238  ASSERT_NEAR(pos_[1], 0.0, error_lin_)
239  << "[overshoot after goal (" << j << ")] ";
240  }
241  ASSERT_EQ(last_path_header_.stamp, status_->path_header.stamp);
242 }
243 
244 TEST_F(TrajectoryTrackerTest, CurveFollow)
245 {
246  initState(Eigen::Vector2d(0, 0), 0);
247 
248  std::vector<Eigen::Vector3d> poses;
249  Eigen::Vector3d p(0.0, 0.0, 0.0);
250  for (double t = 0.0; t < 1.0; t += 0.01)
251  {
252  p += Eigen::Vector3d(std::cos(p[2]) * 0.05, std::sin(p[2]) * 0.05, 0.005);
253  poses.push_back(p);
254  }
255  for (double t = 0.0; t < 1.0; t += 0.01)
256  {
257  p += Eigen::Vector3d(std::cos(p[2]) * 0.05, std::sin(p[2]) * 0.05, 0.0);
258  poses.push_back(p);
259  }
260  waitUntilStart(std::bind(&TrajectoryTrackerTest::publishPath, this, poses));
261 
262  ros::Rate rate(50);
263  const ros::Time start = ros::Time::now();
264  while (ros::ok())
265  {
266  ASSERT_LT(ros::Time::now() - start, ros::Duration(20.0));
267 
268  publishTransform();
269  rate.sleep();
270  ros::spinOnce();
271  if (status_->status == trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL)
272  break;
273  }
274  for (int j = 0; j < 5; ++j)
275  {
276  for (int i = 0; i < 5; ++i)
277  {
278  publishTransform();
279  rate.sleep();
280  ros::spinOnce();
281  }
282 
283  // Check multiple times to assert overshoot.
284  ASSERT_NEAR(yaw_, p[2], error_ang_)
285  << "[overshoot after goal (" << j << ")] ";
286  ASSERT_NEAR(pos_[0], p[0], error_large_lin_)
287  << "[overshoot after goal (" << j << ")] ";
288  ASSERT_NEAR(pos_[1], p[1], error_large_lin_)
289  << "[overshoot after goal (" << j << ")] ";
290  }
291  ASSERT_EQ(last_path_header_.stamp, status_->path_header.stamp);
292 }
293 
294 TEST_F(TrajectoryTrackerTest, InPlaceTurn)
295 {
296  const float init_yaw_array[] =
297  {
298  0.0,
299  3.0
300  };
301  for (const float init_yaw : init_yaw_array)
302  {
303  const std::vector<float> target_angle_array[] =
304  {
305  { 0.5 },
306  { -0.5 },
307  { 0.1, 0.2, 0.3, 0.4, 0.5 },
308  { -0.1, -0.2, -0.3, -0.4, -0.5 },
309  };
310  for (const auto& angles : target_angle_array)
311  {
312  for (const bool& has_short_path : { false, true })
313  {
314  std::stringstream condition_name;
315  condition_name
316  << "init_yaw: " << init_yaw
317  << ", angles: " << angles.front() << "-" << angles.back()
318  << ", has_short_path: " << has_short_path;
319 
320  initState(Eigen::Vector2d(0, 0), init_yaw);
321 
322  std::vector<Eigen::Vector3d> poses;
323  if (has_short_path)
324  {
325  poses.push_back(Eigen::Vector3d(-std::cos(init_yaw) * 0.01, std::sin(init_yaw) * 0.01, init_yaw));
326  }
327  for (float ang : angles)
328  {
329  poses.push_back(Eigen::Vector3d(0.0, 0.0, init_yaw + ang));
330  }
331  waitUntilStart(std::bind(&TrajectoryTrackerTest::publishPath, this, poses));
332 
333  ros::Rate rate(50);
334  const ros::Time start = ros::Time::now();
335  for (int i = 0; ros::ok(); ++i)
336  {
337  ASSERT_LT(ros::Time::now() - start, ros::Duration(10.0)) << condition_name.str();
338 
339  publishTransform();
340  rate.sleep();
341  ros::spinOnce();
342 
343  if (cmd_vel_ && i > 5)
344  {
345  ASSERT_GT(cmd_vel_->angular.z * std::copysign(1.0, angles.back()), -error_ang_)
346  << "[overshoot detected] "
347  << condition_name.str();
348  }
349  if (status_ && i > 5)
350  {
351  ASSERT_LT(status_->angle_remains * std::copysign(1.0, angles.back()), error_ang_)
352  << "[overshoot detected] "
353  << condition_name.str();
354  }
355 
356  if (status_->status == trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL)
357  break;
358  }
359  ASSERT_TRUE(static_cast<bool>(cmd_vel_)) << condition_name.str();
360  for (int j = 0; j < 5; ++j)
361  {
362  for (int i = 0; i < 5; ++i)
363  {
364  publishTransform();
365  rate.sleep();
366  ros::spinOnce();
367  }
368 
369  // Check multiple times to assert overshoot.
370  ASSERT_NEAR(yaw_, init_yaw + angles.back(), error_ang_)
371  << "[overshoot after goal (" << j << ")] "
372  << condition_name.str();
373  }
374  ASSERT_EQ(last_path_header_.stamp, status_->path_header.stamp);
375  }
376  }
377  }
378 }
379 
380 TEST_F(TrajectoryTrackerTest, SwitchBack)
381 {
382  initState(Eigen::Vector2d(0, 0), 0);
383 
384  std::vector<Eigen::Vector3d> poses;
385  Eigen::Vector3d p(0.0, 0.0, 0.0);
386  for (double t = 0.0; t < 0.5; t += 0.01)
387  {
388  p -= Eigen::Vector3d(std::cos(p[2]) * 0.05, std::sin(p[2]) * 0.05, -0.01);
389  poses.push_back(p);
390  }
391  for (double t = 0.0; t < 0.5; t += 0.01)
392  {
393  p += Eigen::Vector3d(std::cos(p[2]) * 0.05, std::sin(p[2]) * 0.05, 0.01);
394  poses.push_back(p);
395  }
396  waitUntilStart(std::bind(&TrajectoryTrackerTest::publishPath, this, poses));
397 
398  ros::Rate rate(50);
399  const ros::Time start = ros::Time::now();
400  while (ros::ok())
401  {
402  ASSERT_LT(ros::Time::now() - start, ros::Duration(10.0));
403 
404  publishTransform();
405  rate.sleep();
406  ros::spinOnce();
407  if (status_->status == trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL)
408  break;
409  }
410  for (int j = 0; j < 5; ++j)
411  {
412  for (int i = 0; i < 5; ++i)
413  {
414  publishTransform();
415  rate.sleep();
416  ros::spinOnce();
417  }
418 
419  // Check multiple times to assert overshoot.
420  ASSERT_NEAR(yaw_, p[2], error_ang_)
421  << "[overshoot after goal (" << j << ")] ";
422  ASSERT_NEAR(pos_[0], p[0], error_large_lin_)
423  << "[overshoot after goal (" << j << ")] ";
424  ASSERT_NEAR(pos_[1], p[1], error_large_lin_)
425  << "[overshoot after goal (" << j << ")] ";
426  }
427  ASSERT_EQ(last_path_header_.stamp, status_->path_header.stamp);
428 }
429 
430 TEST_F(TrajectoryTrackerTest, SwitchBackWithPathUpdate)
431 {
432  initState(Eigen::Vector2d(0, 0), 0);
433 
434  std::vector<Eigen::Vector3d> poses;
435  std::vector<Eigen::Vector3d> poses_second_half;
436  Eigen::Vector3d p(0.0, 0.0, 0.0);
437  for (double t = 0.0; t < 0.5; t += 0.01)
438  {
439  p -= Eigen::Vector3d(std::cos(p[2]) * 0.05, std::sin(p[2]) * 0.05, -0.01);
440  poses.push_back(p);
441  }
442  const Eigen::Vector2d pos_local_goal = p.head<2>();
443  for (double t = 0.0; t < 1.0; t += 0.01)
444  {
445  p += Eigen::Vector3d(std::cos(p[2]) * 0.05, std::sin(p[2]) * 0.05, 0.01);
446  poses.push_back(p);
447  poses_second_half.push_back(p);
448  }
449  waitUntilStart(std::bind(&TrajectoryTrackerTest::publishPath, this, poses));
450 
451  int cnt_arrive_local_goal(0);
452  ros::Rate rate(50);
453  const ros::Time start = ros::Time::now();
454  while (ros::ok())
455  {
456  ASSERT_LT(ros::Time::now() - start, ros::Duration(10.0));
457 
458  publishTransform();
459  rate.sleep();
460  ros::spinOnce();
461  if (status_->status == trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL)
462  break;
463 
464  if ((pos_local_goal - pos_).norm() < 0.1)
465  cnt_arrive_local_goal++;
466 
467  if (cnt_arrive_local_goal > 25)
468  publishPath(poses_second_half);
469  else
470  publishPath(poses);
471  }
472  for (int j = 0; j < 5; ++j)
473  {
474  for (int i = 0; i < 5; ++i)
475  {
476  publishTransform();
477  rate.sleep();
478  ros::spinOnce();
479  }
480 
481  // Check multiple times to assert overshoot.
482  ASSERT_NEAR(yaw_, p[2], error_ang_)
483  << "[overshoot after goal (" << j << ")] ";
484  ASSERT_NEAR(pos_[0], p[0], error_large_lin_)
485  << "[overshoot after goal (" << j << ")] ";
486  ASSERT_NEAR(pos_[1], p[1], error_large_lin_)
487  << "[overshoot after goal (" << j << ")] ";
488  }
489  ASSERT_EQ(last_path_header_.stamp, status_->path_header.stamp);
490 }
491 
492 void timeSource()
493 {
494  ros::NodeHandle nh("/");
495  bool use_sim_time;
496  nh.param("/use_sim_time", use_sim_time, false);
497  if (!use_sim_time)
498  return;
499 
500  ros::Publisher pub = nh.advertise<rosgraph_msgs::Clock>("clock", 1);
501 
502  ros::WallRate rate(500.0); // 500% speed
503  ros::WallTime time = ros::WallTime::now();
504  while (ros::ok())
505  {
506  rosgraph_msgs::Clock clock;
507  clock.clock.fromNSec(time.toNSec());
508  pub.publish(clock);
509  rate.sleep();
510  time += ros::WallDuration(0.01);
511  }
512 }
513 
514 int main(int argc, char** argv)
515 {
516  testing::InitGoogleTest(&argc, argv);
517  ros::init(argc, argv, "test_trajectory_tracker");
518 
519  boost::thread time_thread(timeSource);
520 
521  return RUN_ALL_TESTS();
522 }
ros::NodeHandle
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
TrajectoryTrackerTest::publishPath
void publishPath(const std::vector< Eigen::Vector3d > &poses)
Definition: trajectory_tracker_test.h:184
timeSource
void timeSource()
Definition: test_trajectory_tracker.cpp:492
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
t
geometry_msgs::TransformStamped t
main
int main(int argc, char **argv)
Definition: test_trajectory_tracker.cpp:514
ros::NodeHandle::param
bool param(const std::string &param_name, T &param_val, const T &default_val) const
ros::ok
ROSCPP_DECL bool ok()
TEST_F
TEST_F(TrajectoryTrackerTest, StraightStop)
Definition: test_trajectory_tracker.cpp:36
x
TFSIMD_FORCE_INLINE const tfScalar & x() const
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::Rate::sleep
bool sleep()
TrajectoryTrackerTest::publishPathVelocity
void publishPathVelocity(const std::vector< Eigen::Vector4d > &poses)
Definition: trajectory_tracker_test.h:208
trajectory_tracker_test.h
ros::WallTime::now
static WallTime now()
ros::Time::now
static Time now()
ros::spinOnce
ROSCPP_DECL void spinOnce()

trajectory_tracker
Author(s): Atsushi Watanabe
autogenerated on Wed May 12 2021 02:20:40