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

track_odometry
Author(s): Atsushi Watanabe
autogenerated on Tue Jul 9 2019 05:00:05