test_track_odometry.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2018-2019, 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 <string>
31 #include <vector>
32 
33 #include <ros/ros.h>
34 #include <nav_msgs/Odometry.h>
35 #include <sensor_msgs/Imu.h>
36 #include <tf2/utils.h>
37 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
38 
39 #include <gtest/gtest.h>
40 
41 class TrackOdometryTest : public ::testing::TestWithParam<const char*>
42 {
43 protected:
44  std::vector<nav_msgs::Odometry> odom_msg_buffer_;
45 
46 public:
47  void initializeNode(const std::string& ns)
48  {
49  ros::NodeHandle nh(ns);
50  pub_odom_ = nh.advertise<nav_msgs::Odometry>("odom_raw", 10);
51  pub_imu_ = nh.advertise<sensor_msgs::Imu>("imu/data", 10);
52  sub_odom_ = nh.subscribe("odom", 10, &TrackOdometryTest::cbOdom, this);
53  }
54  bool initializeTrackOdometry(
55  nav_msgs::Odometry& odom_raw,
56  sensor_msgs::Imu& imu)
57  {
58  ros::Duration(0.1).sleep();
59  ros::Rate rate(100);
60  odom_ = nullptr;
61  for (int i = 0; i < 1000 && ros::ok(); ++i)
62  {
63  odom_raw.header.stamp = ros::Time::now();
64  imu.header.stamp = odom_raw.header.stamp + ros::Duration(0.0001);
65  pub_odom_.publish(odom_raw);
66  pub_imu_.publish(imu);
67  rate.sleep();
68  odom_.reset();
69  ros::spinOnce();
70  if (odom_ && i > 50)
71  break;
72  }
73  return static_cast<bool>(odom_);
74  }
75  bool run(
76  nav_msgs::Odometry& odom_raw,
77  sensor_msgs::Imu& imu,
78  const double dt, const int steps)
79  {
80  ros::Rate rate(1.0 / dt);
81  int cnt(0);
82 
83  while (ros::ok())
84  {
85  tf2::Quaternion quat_odom;
86  tf2::fromMsg(odom_raw.pose.pose.orientation, quat_odom);
87  odom_raw.pose.pose.orientation =
88  tf2::toMsg(
89  quat_odom *
90  tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), dt * odom_raw.twist.twist.angular.z));
91  tf2::Quaternion quat_imu;
92  tf2::fromMsg(imu.orientation, quat_imu);
93  imu.orientation =
94  tf2::toMsg(
95  quat_imu *
96  tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), dt * imu.angular_velocity.z));
97 
98  const double yaw = tf2::getYaw(odom_raw.pose.pose.orientation);
99  odom_raw.pose.pose.position.x += cos(yaw) * dt * odom_raw.twist.twist.linear.x;
100  odom_raw.pose.pose.position.y += sin(yaw) * dt * odom_raw.twist.twist.linear.x;
101 
102  stepAndPublish(odom_raw, imu, dt);
103 
104  rate.sleep();
105  ros::spinOnce();
106  if (++cnt >= steps)
107  break;
108  }
109  flushOdomMsgs();
110  return ros::ok();
111  }
112  void stepAndPublish(
113  nav_msgs::Odometry& odom_raw,
114  sensor_msgs::Imu& imu,
115  const double dt)
116  {
117  odom_raw.header.stamp += ros::Duration(dt);
118  imu.header.stamp += ros::Duration(dt);
119  pub_imu_.publish(imu);
120 
121  // Buffer odom message to add delay and jitter.
122  // Send odometry in half rate of IMU.
123  ++odom_cnt_;
124  if (odom_cnt_ % 2 == 0)
125  odom_msg_buffer_.push_back(odom_raw);
126  if (odom_msg_buffer_.size() > 10)
127  {
128  flushOdomMsgs();
129  }
130  }
131  void flushOdomMsgs()
132  {
133  for (nav_msgs::Odometry& o : odom_msg_buffer_)
134  {
135  pub_odom_.publish(o);
136  }
137  odom_msg_buffer_.clear();
138  }
139  void waitAndSpinOnce()
140  {
141  nav_msgs::Odometry::ConstPtr odom_prev = odom_;
142  while (true)
143  {
144  ros::Duration(0.1).sleep();
145  ros::spinOnce();
146  if (odom_prev == odom_)
147  {
148  // no more new messages
149  return;
150  }
151  odom_prev = odom_;
152  }
153  }
154 
155 protected:
156  ros::Publisher pub_odom_;
157  ros::Publisher pub_imu_;
158  ros::Subscriber sub_odom_;
159  nav_msgs::Odometry::ConstPtr odom_;
160  size_t odom_cnt_;
161 
162  void cbOdom(const nav_msgs::Odometry::ConstPtr& msg)
163  {
164  odom_ = msg;
165  };
166 };
167 
168 TEST_F(TrackOdometryTest, OdomImuFusion)
169 {
170  initializeNode("");
171 
172  const double dt = 0.01;
173  const int steps = 200;
174 
175  nav_msgs::Odometry odom_raw;
176  odom_raw.header.frame_id = "odom";
177  odom_raw.pose.pose.orientation.w = 1;
178 
179  sensor_msgs::Imu imu;
180  imu.header.frame_id = "base_link";
181  imu.orientation.w = 1;
182  imu.linear_acceleration.z = 9.8;
183 
184  ASSERT_TRUE(initializeTrackOdometry(odom_raw, imu));
185 
186  // Go forward for 1m
187  odom_raw.twist.twist.linear.x = 0.5;
188  ASSERT_TRUE(run(odom_raw, imu, dt, steps));
189 
190  odom_raw.twist.twist.linear.x = 0.0;
191  stepAndPublish(odom_raw, imu, dt);
192  flushOdomMsgs();
193 
194  waitAndSpinOnce();
195  ASSERT_NEAR(odom_->pose.pose.position.x, 1.0, 1e-3);
196  ASSERT_NEAR(odom_->pose.pose.position.y, 0.0, 1e-3);
197  ASSERT_NEAR(odom_->pose.pose.position.z, 0.0, 1e-3);
198  ASSERT_NEAR(tf2::getYaw(odom_->pose.pose.orientation), 0.0, 1e-3);
199 
200  // Turn 90 degrees with 10% of odometry errors
201  imu.angular_velocity.z = M_PI * 0.25;
202  odom_raw.twist.twist.angular.z = imu.angular_velocity.z * 1.1; // Odometry with error
203  ASSERT_TRUE(run(odom_raw, imu, dt, steps));
204 
205  imu.angular_velocity.z = 0;
206  odom_raw.twist.twist.angular.z = 0;
207  imu.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), M_PI / 2));
208  stepAndPublish(odom_raw, imu, dt);
209  flushOdomMsgs();
210 
211  waitAndSpinOnce();
212  ASSERT_NEAR(odom_->pose.pose.position.x, 1.0, 1e-2);
213  ASSERT_NEAR(odom_->pose.pose.position.y, 0.0, 1e-2);
214  ASSERT_NEAR(odom_->pose.pose.position.z, 0.0, 1e-2);
215  ASSERT_NEAR(tf2::getYaw(odom_->pose.pose.orientation), M_PI / 2, 1e-2);
216 
217  // Go forward for 1m
218  odom_raw.twist.twist.linear.x = 0.5;
219  ASSERT_TRUE(run(odom_raw, imu, dt, steps));
220 
221  odom_raw.twist.twist.linear.x = 0.0;
222  stepAndPublish(odom_raw, imu, dt);
223  flushOdomMsgs();
224 
225  waitAndSpinOnce();
226  ASSERT_NEAR(odom_->pose.pose.position.x, 1.0, 5e-2);
227  ASSERT_NEAR(odom_->pose.pose.position.y, 1.0, 5e-2);
228  ASSERT_NEAR(odom_->pose.pose.position.z, 0.0, 5e-2);
229  ASSERT_NEAR(tf2::getYaw(odom_->pose.pose.orientation), M_PI / 2, 1e-2);
230 }
231 
232 TEST_P(TrackOdometryTest, ZFilterOff)
233 {
234  const std::string ns_postfix(GetParam());
235  initializeNode("no_z_filter" + ns_postfix);
236 
237  const double dt = 0.01;
238  const int steps = 200;
239 
240  nav_msgs::Odometry odom_raw;
241  odom_raw.header.frame_id = "odom";
242  odom_raw.pose.pose.orientation.w = 1;
243 
244  sensor_msgs::Imu imu;
245  imu.header.frame_id = "base_link";
246  imu.orientation.y = sin(-M_PI / 4);
247  imu.orientation.w = cos(-M_PI / 4);
248  imu.linear_acceleration.x = -9.8;
249 
250  ASSERT_TRUE(initializeTrackOdometry(odom_raw, imu));
251 
252  // Go forward for 1m
253  odom_raw.twist.twist.linear.x = 0.5;
254  ASSERT_TRUE(run(odom_raw, imu, dt, steps));
255 
256  odom_raw.twist.twist.linear.x = 0.0;
257  stepAndPublish(odom_raw, imu, dt);
258  flushOdomMsgs();
259 
260  waitAndSpinOnce();
261  ASSERT_NEAR(odom_->pose.pose.position.x, 0.0, 1e-3);
262  ASSERT_NEAR(odom_->pose.pose.position.y, 0.0, 1e-3);
263  ASSERT_NEAR(odom_->pose.pose.position.z, 1.0, 1e-3);
264 }
265 
266 TEST_P(TrackOdometryTest, ZFilterOn)
267 {
268  const std::string ns_postfix(GetParam());
269  initializeNode("z_filter" + ns_postfix);
270 
271  const double dt = 0.01;
272  const int steps = 200;
273 
274  nav_msgs::Odometry odom_raw;
275  odom_raw.header.frame_id = "odom";
276  odom_raw.pose.pose.orientation.w = 1;
277 
278  sensor_msgs::Imu imu;
279  imu.header.frame_id = "base_link";
280  imu.orientation.y = sin(-M_PI / 4);
281  imu.orientation.w = cos(-M_PI / 4);
282  imu.linear_acceleration.x = -9.8;
283 
284  ASSERT_TRUE(initializeTrackOdometry(odom_raw, imu));
285 
286  // Go forward for 1m
287  odom_raw.twist.twist.linear.x = 0.5;
288  ASSERT_TRUE(run(odom_raw, imu, dt, steps));
289 
290  odom_raw.twist.twist.linear.x = 0.0;
291  stepAndPublish(odom_raw, imu, dt);
292  flushOdomMsgs();
293 
294  waitAndSpinOnce();
295  ASSERT_NEAR(odom_->pose.pose.position.z, 1.0 - 1.0 / M_E, 5e-2);
296 }
297 
298 INSTANTIATE_TEST_CASE_P(
299  TrackOdometryTestInstance, TrackOdometryTest,
300  ::testing::Values("", "_old_param"));
301 
302 int main(int argc, char** argv)
303 {
304  testing::InitGoogleTest(&argc, argv);
305  ros::init(argc, argv, "test_track_odometry");
306 
307  return RUN_ALL_TESTS();
308 }
TrackOdometryTest::initializeTrackOdometry
bool initializeTrackOdometry(nav_msgs::Odometry &odom_raw, sensor_msgs::Imu &imu)
Definition: test_track_odometry.cpp:54
TrackOdometryTest::odom_
nav_msgs::Odometry::ConstPtr odom_
Definition: test_track_odometry.cpp:159
ros::NodeHandle
TEST_P
TEST_P(TrackOdometryTest, ZFilterOff)
Definition: test_track_odometry.cpp:232
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ros::Duration::sleep
bool sleep() const
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
TrackOdometryTest::sub_odom_
ros::Subscriber sub_odom_
Definition: test_track_odometry.cpp:158
main
int main(int argc, char **argv)
Definition: test_track_odometry.cpp:302
TrackOdometryTest::stepAndPublish
void stepAndPublish(nav_msgs::Odometry &odom_raw, sensor_msgs::Imu &imu, const double dt)
Definition: test_track_odometry.cpp:112
TrackOdometryTest::run
bool run(nav_msgs::Odometry &odom_raw, sensor_msgs::Imu &imu, const double dt, const int steps)
Definition: test_track_odometry.cpp:75
TrackOdometryTest::odom_msg_buffer_
std::vector< nav_msgs::Odometry > odom_msg_buffer_
Definition: test_track_odometry.cpp:44
TEST_F
TEST_F(TrackOdometryTest, OdomImuFusion)
Definition: test_track_odometry.cpp:168
tf2::fromMsg
void fromMsg(const A &, B &b)
ros::ok
ROSCPP_DECL bool ok()
tf2_geometry_msgs.h
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::Rate::sleep
bool sleep()
tf2::getYaw
double getYaw(const A &a)
tf2::toMsg
B toMsg(const A &a)
ros::Time::now
static Time now()
INSTANTIATE_TEST_CASE_P
INSTANTIATE_TEST_CASE_P(TrackOdometryTestInstance, TrackOdometryTest,::testing::Values("","_old_param"))
cos
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
ros::spinOnce
ROSCPP_DECL void spinOnce()
TrackOdometryTest::cbOdom
void cbOdom(const nav_msgs::Odometry::ConstPtr &msg)
Definition: test_track_odometry.cpp:162
sin
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
TrackOdometryTest::initializeNode
void initializeNode(const std::string &ns)
Definition: test_track_odometry.cpp:47

track_odometry
Author(s): Atsushi Watanabe
autogenerated on Wed May 12 2021 02:20:38