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  if (ros::Time::now() > start + ros::Duration(10.0))
51  {
52  FAIL()
53  << "Timeout" << std::endl
54  << "Pos " << getPos() << std::endl
55  << "Yaw " << getYaw() << std::endl
56  << "Status " << std::endl
57  << status_ << std::endl;
58  }
59 
60  publishTransform();
61  rate.sleep();
62  ros::spinOnce();
63  if (status_->status == trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL)
64  break;
65  }
66  for (int j = 0; j < 5; ++j)
67  {
68  for (int i = 0; i < 5; ++i)
69  {
70  publishTransform();
71  rate.sleep();
72  ros::spinOnce();
73  }
74 
75  // Check multiple times to assert overshoot.
76  ASSERT_NEAR(getYaw(), 0.0, error_ang_)
77  << "[overshoot after goal (" << j << ")] ";
78  ASSERT_NEAR(getPos()[0], 0.5, error_lin_)
79  << "[overshoot after goal (" << j << ")] ";
80  ASSERT_NEAR(getPos()[1], 0.0, error_lin_)
81  << "[overshoot after goal (" << j << ")] ";
82  }
83  ASSERT_EQ(last_path_header_.stamp, status_->path_header.stamp);
84 }
85 
86 TEST_F(TrajectoryTrackerTest, StraightStopOvershoot)
87 {
88  const double resolutions[] =
89  {
90  0.1,
91  0.001, // default epsilon
92  0.0001,
93  };
94  for (const double resolution : resolutions)
95  {
96  const std::string info_message = "resolution: " + std::to_string(resolution);
97 
98  initState(Eigen::Vector2d(1, 0), 0);
99 
100  std::vector<Eigen::Vector3d> poses;
101  for (double x = 0.0; x < 0.5 - resolution; x += 0.1)
102  poses.push_back(Eigen::Vector3d(x, 0, 0));
103  poses.push_back(Eigen::Vector3d(0.5 - resolution, 0, 0));
104  poses.push_back(Eigen::Vector3d(0.5, 0, 0));
105  waitUntilStart(std::bind(&TrajectoryTrackerTest::publishPath, this, poses));
106 
107  ros::Rate rate(50);
108  const ros::Time start = ros::Time::now();
109  while (ros::ok())
110  {
111  if (ros::Time::now() > start + ros::Duration(10.0))
112  {
113  FAIL()
114  << "Timeout" << std::endl
115  << "Pos " << getPos() << std::endl
116  << "Yaw " << getYaw() << std::endl
117  << "Status " << std::endl
118  << status_ << std::endl
119  << info_message;
120  }
121 
122  publishTransform();
123  rate.sleep();
124  ros::spinOnce();
125  if (status_->status == trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL)
126  break;
127  }
128  for (int j = 0; j < 5; ++j)
129  {
130  for (int i = 0; i < 5; ++i)
131  {
132  publishTransform();
133  rate.sleep();
134  ros::spinOnce();
135  }
136 
137  // Check multiple times to assert overshoot.
138  ASSERT_NEAR(getYaw(), 0.0, error_ang_)
139  << "[overshoot after goal (" << j << ")] "
140  << info_message;
141  EXPECT_NEAR(getPos()[0], 0.5, error_lin_)
142  << "[overshoot after goal (" << j << ")] "
143  << info_message;
144  ASSERT_NEAR(getPos()[1], 0.0, error_lin_)
145  << "[overshoot after goal (" << j << ")] "
146  << info_message;
147  }
148  ASSERT_EQ(last_path_header_.stamp, status_->path_header.stamp) << info_message;
149  }
150 }
151 
152 TEST_F(TrajectoryTrackerTest, StraightStopConvergence)
153 {
154  const double vels[] = {0.02, 0.05, 0.1, 0.2, 0.5, 1.0};
155  const double path_length = 2.0;
156  for (const double vel : vels)
157  {
158  const std::string info_message = "linear vel: " + std::to_string(vel);
159 
160  initState(Eigen::Vector2d(0, 0.01), 0);
161 
162  std::vector<Eigen::Vector4d> poses;
163  for (double x = 0.0; x < path_length; x += 0.01)
164  poses.push_back(Eigen::Vector4d(x, 0.0, 0.0, vel));
165  poses.push_back(Eigen::Vector4d(path_length, 0.0, 0.0, vel));
166  waitUntilStart(std::bind(&TrajectoryTrackerTest::publishPathVelocity, this, poses));
167 
168  ros::Rate rate(50);
169  const ros::Time start = ros::Time::now();
170  while (ros::ok())
171  {
172  if (ros::Time::now() > start + ros::Duration(5.0 + path_length / vel))
173  {
174  FAIL()
175  << "Timeout" << std::endl
176  << "Pos " << getPos() << std::endl
177  << "Yaw " << getYaw() << std::endl
178  << "Status " << std::endl
179  << status_ << std::endl
180  << info_message;
181  }
182 
183  publishTransform();
184  rate.sleep();
185  ros::spinOnce();
186  if (status_->status == trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL)
187  break;
188  }
189  for (int j = 0; j < 5; ++j)
190  {
191  for (int i = 0; i < 5; ++i)
192  {
193  publishTransform();
194  rate.sleep();
195  ros::spinOnce();
196  }
197 
198  // Check multiple times to assert overshoot.
199  EXPECT_NEAR(getYaw(), 0.0, error_ang_)
200  << "[overshoot after goal (" << j << ")] "
201  << info_message;
202  EXPECT_NEAR(getPos()[0], path_length, error_lin_)
203  << "[overshoot after goal (" << j << ")] "
204  << info_message;
205  EXPECT_NEAR(getPos()[1], 0.0, error_lin_)
206  << "[overshoot after goal (" << j << ")] "
207  << info_message;
208  }
209  ASSERT_EQ(last_path_header_.stamp, status_->path_header.stamp);
210  }
211 }
212 
213 TEST_F(TrajectoryTrackerTest, StraightVelocityChange)
214 {
215  initState(Eigen::Vector2d(0, 0), 0);
216 
217  std::vector<Eigen::Vector4d> poses;
218  for (double x = 0.0; x < 0.6; x += 0.01)
219  poses.push_back(Eigen::Vector4d(x, 0.0, 0.0, 0.3));
220  for (double x = 0.6; x < 1.5; x += 0.01)
221  poses.push_back(Eigen::Vector4d(x, 0.0, 0.0, 0.5));
222  poses.push_back(Eigen::Vector4d(1.5, 0.0, 0.0, 0.5));
223  waitUntilStart(std::bind(&TrajectoryTrackerTest::publishPathVelocity, this, poses));
224 
225  ros::Rate rate(50);
226  const ros::Time start = ros::Time::now();
227  while (ros::ok())
228  {
229  if (ros::Time::now() > start + ros::Duration(10.0))
230  {
231  FAIL()
232  << "Timeout" << std::endl
233  << "Pos " << getPos() << std::endl
234  << "Yaw " << getYaw() << std::endl
235  << "Status " << std::endl
236  << status_ << std::endl;
237  }
238 
239  publishTransform();
240  rate.sleep();
241  ros::spinOnce();
242 
243  if (0.3 < getPos()[0] && getPos()[0] < 0.35)
244  {
245  ASSERT_NEAR(cmd_vel_->linear.x, 0.3, error_lin_);
246  }
247  else if (0.95 < getPos()[0] && getPos()[0] < 1.0)
248  {
249  ASSERT_NEAR(cmd_vel_->linear.x, 0.5, error_lin_);
250  }
251 
252  if (status_->status == trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL)
253  break;
254  }
255  for (int j = 0; j < 5; ++j)
256  {
257  for (int i = 0; i < 5; ++i)
258  {
259  publishTransform();
260  rate.sleep();
261  ros::spinOnce();
262  }
263 
264  // Check multiple times to assert overshoot.
265  ASSERT_NEAR(getYaw(), 0.0, error_ang_)
266  << "[overshoot after goal (" << j << ")] ";
267  ASSERT_NEAR(getPos()[0], 1.5, error_lin_)
268  << "[overshoot after goal (" << j << ")] ";
269  ASSERT_NEAR(getPos()[1], 0.0, error_lin_)
270  << "[overshoot after goal (" << j << ")] ";
271  }
272  ASSERT_EQ(last_path_header_.stamp, status_->path_header.stamp);
273 }
274 
275 TEST_F(TrajectoryTrackerTest, CurveFollow)
276 {
277  initState(Eigen::Vector2d(0, 0), 0);
278 
279  std::vector<Eigen::Vector3d> poses;
280  Eigen::Vector3d p(0.0, 0.0, 0.0);
281  for (double t = 0.0; t < 1.0; t += 0.01)
282  {
283  p += Eigen::Vector3d(std::cos(p[2]) * 0.05, std::sin(p[2]) * 0.05, 0.005);
284  poses.push_back(p);
285  }
286  for (double t = 0.0; t < 1.0; t += 0.01)
287  {
288  p += Eigen::Vector3d(std::cos(p[2]) * 0.05, std::sin(p[2]) * 0.05, 0.0);
289  poses.push_back(p);
290  }
291  waitUntilStart(std::bind(&TrajectoryTrackerTest::publishPath, this, poses));
292 
293  ros::Rate rate(50);
294  const ros::Time start = ros::Time::now();
295  while (ros::ok())
296  {
297  if (ros::Time::now() > start + ros::Duration(20.0))
298  {
299  FAIL()
300  << "Timeout" << std::endl
301  << "Pos " << getPos() << std::endl
302  << "Yaw " << getYaw() << std::endl
303  << "Status " << std::endl
304  << status_ << std::endl;
305  }
306 
307  publishTransform();
308  rate.sleep();
309  ros::spinOnce();
310  if (status_->status == trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL)
311  break;
312  }
313  for (int j = 0; j < 5; ++j)
314  {
315  for (int i = 0; i < 5; ++i)
316  {
317  publishTransform();
318  rate.sleep();
319  ros::spinOnce();
320  }
321 
322  // Check multiple times to assert overshoot.
323  ASSERT_NEAR(getYaw(), p[2], error_ang_)
324  << "[overshoot after goal (" << j << ")] ";
325  ASSERT_NEAR(getPos()[0], p[0], error_large_lin_)
326  << "[overshoot after goal (" << j << ")] ";
327  ASSERT_NEAR(getPos()[1], p[1], error_large_lin_)
328  << "[overshoot after goal (" << j << ")] ";
329  }
330  ASSERT_EQ(last_path_header_.stamp, status_->path_header.stamp);
331 }
332 
333 TEST_F(TrajectoryTrackerTest, InPlaceTurn)
334 {
335  const float init_yaw_array[] = {0.0, 3.0};
336  for (const float init_yaw : init_yaw_array)
337  {
338  const std::vector<float> target_angle_array[] =
339  {
340  {0.5},
341  {-0.5},
342  {0.1, 0.2, 0.3, 0.4, 0.5},
343  {-0.1, -0.2, -0.3, -0.4, -0.5},
344  };
345  for (const auto& angles : target_angle_array)
346  {
347  for (const bool& has_short_path : {false, true})
348  {
349  std::stringstream condition_name;
350  condition_name
351  << "init_yaw: " << init_yaw
352  << ", angles: " << angles.front() << "-" << angles.back()
353  << ", has_short_path: " << has_short_path;
354 
355  initState(Eigen::Vector2d(0, 0), init_yaw);
356 
357  std::vector<Eigen::Vector3d> poses;
358  if (has_short_path)
359  {
360  poses.push_back(Eigen::Vector3d(-std::cos(init_yaw) * 0.01, std::sin(init_yaw) * 0.01, init_yaw));
361  }
362  for (float ang : angles)
363  {
364  poses.push_back(Eigen::Vector3d(0.0, 0.0, init_yaw + ang));
365  }
366  waitUntilStart(std::bind(&TrajectoryTrackerTest::publishPath, this, poses));
367 
368  ros::Rate rate(50);
369  const ros::Time start = ros::Time::now();
370  for (int i = 0; ros::ok(); ++i)
371  {
372  if (ros::Time::now() > start + ros::Duration(10.0))
373  {
374  FAIL()
375  << condition_name.str()
376  << "Timeout" << std::endl
377  << "Pos " << getPos() << std::endl
378  << "Yaw " << getYaw() << std::endl
379  << "Status " << std::endl
380  << status_ << std::endl;
381  }
382 
383  publishTransform();
384  rate.sleep();
385  ros::spinOnce();
386 
387  if (cmd_vel_ && i > 5)
388  {
389  ASSERT_GT(cmd_vel_->angular.z * std::copysign(1.0, angles.back()), -error_ang_)
390  << "[overshoot detected] "
391  << condition_name.str();
392  }
393  if (status_ && i > 5)
394  {
395  ASSERT_LT(status_->angle_remains * std::copysign(1.0, angles.back()), error_ang_)
396  << "[overshoot detected] "
397  << condition_name.str();
398  }
399 
400  if (status_->status == trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL)
401  break;
402  }
403  ASSERT_TRUE(static_cast<bool>(cmd_vel_)) << condition_name.str();
404  for (int j = 0; j < 5; ++j)
405  {
406  for (int i = 0; i < 5; ++i)
407  {
408  publishTransform();
409  rate.sleep();
410  ros::spinOnce();
411  }
412 
413  // Check multiple times to assert overshoot.
414  const double angle_diff = pose_.getRotation().angleShortestPath(
415  tf2::Quaternion(tf2::Vector3(0, 0, 1), init_yaw + angles.back()));
416  ASSERT_LT(angle_diff, error_ang_)
417  << "[overshoot after goal (" << j << ")] "
418  << condition_name.str();
419  }
420  ASSERT_EQ(last_path_header_.stamp, status_->path_header.stamp);
421  }
422  }
423  }
424 }
425 
426 TEST_F(TrajectoryTrackerTest, SwitchBack)
427 {
428  initState(Eigen::Vector2d(0, 0), 0);
429 
430  std::vector<Eigen::Vector3d> poses;
431  Eigen::Vector3d p(0.0, 0.0, 0.0);
432  for (double t = 0.0; t < 0.5; t += 0.01)
433  {
434  p -= Eigen::Vector3d(std::cos(p[2]) * 0.05, std::sin(p[2]) * 0.05, -0.01);
435  poses.push_back(p);
436  }
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  waitUntilStart(std::bind(&TrajectoryTrackerTest::publishPath, this, poses));
443 
444  ros::Rate rate(50);
445  const ros::Time start = ros::Time::now();
446  while (ros::ok())
447  {
448  if (ros::Time::now() > start + ros::Duration(10.0))
449  {
450  FAIL()
451  << "Timeout" << std::endl
452  << "Pos " << getPos() << std::endl
453  << "Yaw " << getYaw() << std::endl
454  << "Status " << std::endl
455  << status_ << std::endl;
456  }
457 
458  publishTransform();
459  rate.sleep();
460  ros::spinOnce();
461  if (status_->status == trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL)
462  break;
463  }
464  for (int j = 0; j < 5; ++j)
465  {
466  for (int i = 0; i < 5; ++i)
467  {
468  publishTransform();
469  rate.sleep();
470  ros::spinOnce();
471  }
472 
473  // Check multiple times to assert overshoot.
474  ASSERT_NEAR(getYaw(), p[2], error_ang_)
475  << "[overshoot after goal (" << j << ")] ";
476  ASSERT_NEAR(getPos()[0], p[0], error_large_lin_)
477  << "[overshoot after goal (" << j << ")] ";
478  ASSERT_NEAR(getPos()[1], p[1], error_large_lin_)
479  << "[overshoot after goal (" << j << ")] ";
480  }
481  ASSERT_EQ(last_path_header_.stamp, status_->path_header.stamp);
482 }
483 
484 TEST_F(TrajectoryTrackerTest, SwitchBackWithPathUpdate)
485 {
486  initState(Eigen::Vector2d(0, 0), 0);
487 
488  std::vector<Eigen::Vector3d> poses;
489  std::vector<Eigen::Vector3d> poses_second_half;
490  Eigen::Vector3d p(0.0, 0.0, 0.0);
491  for (double t = 0.0; t < 0.5; t += 0.01)
492  {
493  p -= Eigen::Vector3d(std::cos(p[2]) * 0.05, std::sin(p[2]) * 0.05, -0.01);
494  poses.push_back(p);
495  }
496  const Eigen::Vector2d pos_local_goal = p.head<2>();
497  for (double t = 0.0; t < 1.0; t += 0.01)
498  {
499  p += Eigen::Vector3d(std::cos(p[2]) * 0.05, std::sin(p[2]) * 0.05, 0.01);
500  poses.push_back(p);
501  poses_second_half.push_back(p);
502  }
503  waitUntilStart(std::bind(&TrajectoryTrackerTest::publishPath, this, poses));
504 
505  int cnt_arrive_local_goal(0);
506  ros::Rate rate(50);
507  const ros::Time start = ros::Time::now();
508  for (int i = 0; ros::ok(); i++)
509  {
510  if (ros::Time::now() > start + ros::Duration(15.0))
511  {
512  FAIL()
513  << "Timeout" << std::endl
514  << "Pos " << getPos() << std::endl
515  << "Yaw " << getYaw() << std::endl
516  << "Status " << std::endl
517  << status_ << std::endl;
518  }
519 
520  publishTransform();
521  rate.sleep();
522  ros::spinOnce();
523  if (status_->status == trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL)
524  break;
525 
526  if ((pos_local_goal - getPos()).norm() < 0.1)
527  cnt_arrive_local_goal++;
528 
529  if (i % 5)
530  {
531  // Republish path in 10Hz
532  if (cnt_arrive_local_goal > 25)
533  {
534  publishPath(poses_second_half);
535  }
536  else
537  {
538  publishPath(poses);
539  }
540  }
541  }
542  ASSERT_GT(cnt_arrive_local_goal, 25)
543  << "failed to update path";
544  for (int j = 0; j < 5; ++j)
545  {
546  for (int i = 0; i < 5; ++i)
547  {
548  publishTransform();
549  rate.sleep();
550  ros::spinOnce();
551  }
552 
553  // Check multiple times to assert overshoot.
554  ASSERT_NEAR(getYaw(), p[2], error_ang_)
555  << "[overshoot after goal (" << j << ")] ";
556  ASSERT_NEAR(getPos()[0], p[0], error_large_lin_)
557  << "[overshoot after goal (" << j << ")] ";
558  ASSERT_NEAR(getPos()[1], p[1], error_large_lin_)
559  << "[overshoot after goal (" << j << ")] ";
560  }
561  ASSERT_EQ(last_path_header_.stamp, status_->path_header.stamp);
562 }
563 
564 TEST_F(TrajectoryTrackerTest, FarAray)
565 {
566  const double y_pos = 500.0;
567 
568  initState(tf2::Transform(tf2::Quaternion(tf2::Vector3(1.0, 0.0, 0.0), 0.1), tf2::Vector3(0.0, y_pos, 0.0)));
569  std::vector<Eigen::Vector3d> poses;
570  for (double x = 0.0; x < 0.5; x += 0.01)
571  poses.push_back(Eigen::Vector3d(x, y_pos, 0.0));
572  poses.push_back(Eigen::Vector3d(0.5, y_pos, 0.0));
573  waitUntilStart(std::bind(&TrajectoryTrackerTest::publishPath, this, poses));
574 
575  ros::Rate rate(50);
576  const ros::Time start = ros::Time::now();
577  while (ros::ok())
578  {
579  if (ros::Time::now() > start + ros::Duration(10.0))
580  {
581  FAIL()
582  << "Timeout" << std::endl
583  << "Pos " << getPos() << std::endl
584  << "Yaw " << getYaw() << std::endl
585  << "Status " << std::endl
586  << status_ << std::endl;
587  }
588 
589  publishTransform();
590  rate.sleep();
591  ros::spinOnce();
592  if (status_->status == trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL)
593  break;
594  }
595  for (int j = 0; j < 5; ++j)
596  {
597  for (int i = 0; i < 5; ++i)
598  {
599  publishTransform();
600  rate.sleep();
601  ros::spinOnce();
602  }
603 
604  // Check multiple times to assert overshoot.
605  ASSERT_NEAR(getYaw(), 0.0, error_ang_)
606  << "[overshoot after goal (" << j << ")] ";
607  ASSERT_NEAR(getPos()[0], 0.5, error_lin_)
608  << "[overshoot after goal (" << j << ")] ";
609  ASSERT_NEAR(getPos()[1], y_pos, error_lin_)
610  << "[overshoot after goal (" << j << ")] ";
611  }
612  ASSERT_EQ(last_path_header_.stamp, status_->path_header.stamp);
613 }
614 
615 void timeSource()
616 {
617  ros::NodeHandle nh("/");
618  bool use_sim_time;
619  nh.param("/use_sim_time", use_sim_time, false);
620  if (!use_sim_time)
621  return;
622 
623  ros::Publisher pub = nh.advertise<rosgraph_msgs::Clock>("clock", 1);
624 
625  ros::WallRate rate(400.0); // 400% speed
626  ros::WallTime time = ros::WallTime::now();
627  while (ros::ok())
628  {
629  rosgraph_msgs::Clock clock;
630  clock.clock.fromNSec(time.toNSec());
631  pub.publish(clock);
632  rate.sleep();
633  time += ros::WallDuration(0.01);
634  }
635 }
636 
637 int main(int argc, char** argv)
638 {
639  testing::InitGoogleTest(&argc, argv);
640  ros::init(argc, argv, "test_trajectory_tracker");
641 
642  boost::thread time_thread(timeSource);
643 
644  return RUN_ALL_TESTS();
645 }
main
int main(int argc, char **argv)
Definition: test_trajectory_tracker.cpp:637
ros::Publisher
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
TrajectoryTrackerTest::publishPathVelocity
void publishPathVelocity(const std::vector< Eigen::Vector4d > &poses)
Definition: trajectory_tracker_test.h:230
ros::spinOnce
ROSCPP_DECL void spinOnce()
trajectory_tracker_test.h
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
ros::ok
ROSCPP_DECL bool ok()
TEST_F
TEST_F(TrajectoryTrackerTest, StraightStop)
Definition: test_trajectory_tracker.cpp:36
ros::WallTime::now
static WallTime now()
ros::WallRate
tf2::Transform
ros::WallTime
ros::Rate::sleep
bool sleep()
start
ROSCPP_DECL void start()
getYaw
double getYaw(const tf2::Quaternion &q)
ros::Time
TimeBase< WallTime, WallDuration >::toNSec
uint64_t toNSec() const
ros::NodeHandle::param
T param(const std::string &param_name, const T &default_val) const
tf2::Quaternion
ros::Rate
ros::WallDuration
TrajectoryTrackerTest
Definition: trajectory_tracker_test.h:62
ros::Duration
t
geometry_msgs::TransformStamped t
timeSource
void timeSource()
Definition: test_trajectory_tracker.cpp:615
ros::NodeHandle
TrajectoryTrackerTest::publishPath
void publishPath(const std::vector< Eigen::Vector3d > &poses)
Definition: trajectory_tracker_test.h:206
ros::Time::now
static Time now()

trajectory_tracker
Author(s): Atsushi Watanabe
autogenerated on Wed Mar 19 2025 02:14:15