test_trajectory_tracker_with_odom.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2020, 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 <vector>
31 
33 
35 {
36  initState(Eigen::Vector2d(0, 0), 0);
37 
38  std::vector<Eigen::Vector3d> poses;
39  for (double x = 0.0; x < 0.5; x += 0.01)
40  poses.push_back(Eigen::Vector3d(x, 0.0, 0.0));
41  poses.push_back(Eigen::Vector3d(0.5, 0.0, 0.0));
42  waitUntilStart(std::bind(&TrajectoryTrackerTest::publishPath, this, poses));
43 
44  ros::Rate rate(50);
45  const ros::Time start = ros::Time::now();
46  while (ros::ok())
47  {
48  ASSERT_LT(ros::Time::now() - start, ros::Duration(10.0));
49 
50  publishTransform();
51  rate.sleep();
52  ros::spinOnce();
53  if (status_->status == trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL)
54  break;
55  }
56  const double frame_rate = getCmdVelFrameRate();
57  for (int i = 0; i < 25; ++i)
58  {
59  publishTransform();
60  rate.sleep();
61  ros::spinOnce();
62  }
63  ASSERT_NEAR(yaw_, 0.0, 1e-2);
64  ASSERT_NEAR(pos_[0], 0.5, 1e-2);
65  ASSERT_NEAR(pos_[1], 0.0, 1e-2);
66  ASSERT_EQ(last_path_header_.stamp, status_->path_header.stamp);
67  // Parameter "hz" is set to 30.0, but the actual frame rate is around 50.0 as it is syncronized with the odometry.
68  ASSERT_NEAR(50.0, frame_rate, 10.0);
69 }
70 
72 {
73  initState(Eigen::Vector2d(0, 0), 0);
74 
75  std::vector<Eigen::Vector3d> poses;
76  for (double x = 0.0; x < 2.0; x += 0.01)
77  poses.push_back(Eigen::Vector3d(x, 0.0, 0.0));
78  poses.push_back(Eigen::Vector3d(2.0, 0.0, 0.0));
79  waitUntilStart(std::bind(&TrajectoryTrackerTest::publishPath, this, poses));
80 
81  ros::Rate rate(50);
82  for (int i = 0; i < 50; ++i)
83  {
84  publishTransform();
85  rate.sleep();
86  ros::spinOnce();
87  }
88  // Wait until odometry timeout
89  ros::Duration(0.2).sleep();
90  ros::spinOnce();
91 
92  ASSERT_FLOAT_EQ(cmd_vel_->linear.x, 0.0);
93  ASSERT_FLOAT_EQ(cmd_vel_->angular.z, 0.0);
94  ASSERT_GT(pos_[0], 0.0);
95  ASSERT_LT(pos_[0], 2.0);
96  ASSERT_EQ(status_->status, trajectory_tracker_msgs::TrajectoryTrackerStatus::NO_PATH);
97 
98  while (ros::ok())
99  {
100  publishTransform();
101  rate.sleep();
102  ros::spinOnce();
103  if (status_->status == trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL)
104  break;
105  }
106  for (int i = 0; i < 25; ++i)
107  {
108  publishTransform();
109  rate.sleep();
110  ros::spinOnce();
111  }
112  ASSERT_NEAR(yaw_, 0.0, 1e-2);
113  ASSERT_NEAR(pos_[0], 2.0, 1e-2);
114  ASSERT_NEAR(pos_[1], 0.0, 1e-2);
115  ASSERT_EQ(last_path_header_.stamp, status_->path_header.stamp);
116 }
117 
118 int main(int argc, char** argv)
119 {
120  testing::InitGoogleTest(&argc, argv);
121  ros::init(argc, argv, "test_trajectory_tracker_frame_rate");
122 
123  return RUN_ALL_TESTS();
124 }
void publishPath(const std::vector< Eigen::Vector3d > &poses)
bool sleep() const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ROSCPP_DECL bool ok()
TFSIMD_FORCE_INLINE const tfScalar & x() const
bool sleep()
int main(int argc, char **argv)
static Time now()
ROSCPP_DECL void spinOnce()
TEST_F(TrajectoryTrackerTest, FrameRate)


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