test_robot_pose_ekf.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Wim Meeussen */
36 
37 #include <string>
38 #include <gtest/gtest.h>
39 #include "ros/ros.h"
40 #include "nav_msgs/Odometry.h"
41 #include "geometry_msgs/PoseWithCovarianceStamped.h"
42 
43 #include <boost/thread.hpp>
44 
45 using namespace ros;
46 
47 
48 static const double time_end = 1264198883.0;
49 static const double ekf_duration = 62.0;
50 static const double EPS_trans_x = 0.02;
51 static const double EPS_trans_y = 0.04;
52 static const double EPS_trans_z = 0.00001;
53 static const double EPS_rot_x = 0.005;
54 static const double EPS_rot_y = 0.005;
55 static const double EPS_rot_z = 0.005;
56 static const double EPS_rot_w = 0.005;
57 
58 
59 int g_argc;
60 char** g_argv;
61 
64 
65 class TestEKF : public testing::Test
66 {
67 public:
70  EkfConstPtr ekf_begin_, ekf_end_;
72  double ekf_counter_, odom_counter_;
73  Time ekf_time_begin_, odom_time_begin_;
74 
75 
76  void OdomCallback(const OdomConstPtr& odom)
77  {
78  // get initial time
79  if (odom_counter_ == 0)
80  odom_time_begin_ = odom->header.stamp;
81 
82  odom_end_ = odom;
83 
84  // count number of callbacks
85  odom_counter_++;
86  }
87 
88 
89  void EKFCallback(const EkfConstPtr& ekf)
90  {
91  // get initial time
92  if (ekf_counter_ == 0){
93  ekf_time_begin_ = ekf->header.stamp;
94  ekf_begin_ = ekf;
95  }
96  if (ekf->header.stamp.toSec() < time_end)
97  ekf_end_ = ekf;
98 
99  // count number of callbacks
100  ekf_counter_++;
101  }
102 
103 
104 protected:
107  {
108  ekf_counter_ = 0;
109  odom_counter_ = 0;
110 
111  }
112 
113 
116  {
117  }
118 
119  void SetUp()
120  {
121  ROS_INFO("Subscribing to robot_pose_ekf/odom_combined");
122  ekf_sub_ = node_.subscribe("/robot_pose_ekf/odom_combined", 10, &TestEKF::EKFCallback, (TestEKF*)this);
123 
124  ROS_INFO("Subscribing to base_odometry/odom");
125  odom_sub_ = node_.subscribe("base_odometry/odom", 10 , &TestEKF::OdomCallback, (TestEKF*)this);
126  }
127 
128  void TearDown()
129  {
130  odom_sub_.shutdown();
131  ekf_sub_.shutdown();
132  }
133 };
134 
135 
136 
137 
139 {
140  Duration d(0.01);
141  // wait while bag is played back
142  ROS_INFO("Waiting for bag to start playing");
143  while (odom_counter_ == 0)
144  d.sleep();
145  ROS_INFO("Detected that bag is playing");
146 
147  ROS_INFO("Waiting untile end time is reached");
148  while( odom_end_->header.stamp.toSec() < time_end){
149  d.sleep();
150  }
151  ROS_INFO("End time reached");
152  // give filter some time to catch up
153  WallDuration(2.0).sleep();
154 
155  // check if callback was called enough times
156  ROS_INFO("Number of ekf callbacks: %f", ekf_counter_);
157  EXPECT_GT(ekf_counter_, 500);
158 
159  // check if time interval is correct
160  ROS_INFO("Ekf duration: %f", ekf_duration);
161  EXPECT_LT(Duration(ekf_end_->header.stamp - ekf_time_begin_).toSec(), ekf_duration * 1.25);
162  EXPECT_GT(Duration(ekf_end_->header.stamp - ekf_time_begin_).toSec(), ekf_duration * 0.85);
163 
164  // check if ekf time is same as odom time
165  EXPECT_NEAR(ekf_time_begin_.toSec(), odom_time_begin_.toSec(), 1.0);
166  EXPECT_NEAR(ekf_end_->header.stamp.toSec(), time_end, 1.0);
167 
168  // check filter result
169  ROS_INFO("%f %f %f %f %f %f %f -- %f",ekf_end_->pose.pose.position.x, ekf_end_->pose.pose.position.y, ekf_end_->pose.pose.position.z,
170  ekf_end_->pose.pose.orientation.x, ekf_end_->pose.pose.orientation.y, ekf_end_->pose.pose.orientation.z, ekf_end_->pose.pose.orientation.w,
171  ekf_end_->header.stamp.toSec());
172  EXPECT_NEAR(ekf_end_->pose.pose.position.x, -0.0586126, EPS_trans_x);
173  EXPECT_NEAR(ekf_end_->pose.pose.position.y, 0.0124321, EPS_trans_y);
174  EXPECT_NEAR(ekf_end_->pose.pose.position.z, 0.0, EPS_trans_z);
175  EXPECT_NEAR(ekf_end_->pose.pose.orientation.x, 0.00419421, EPS_rot_x);
176  EXPECT_NEAR(ekf_end_->pose.pose.orientation.y, 0.00810739, EPS_rot_y);
177  EXPECT_NEAR(ekf_end_->pose.pose.orientation.z, -0.0440686, EPS_rot_z);
178  EXPECT_NEAR(ekf_end_->pose.pose.orientation.w, 0.998987, EPS_rot_w);
179 
180  SUCCEED();
181 }
182 
183 
184 
185 
186 int main(int argc, char** argv)
187 {
188  testing::InitGoogleTest(&argc, argv);
189  g_argc = argc;
190  g_argv = argv;
191 
192  init(g_argc, g_argv, "testEKF");
193 
194  boost::thread spinner(boost::bind(&ros::spin));
195 
196  int res = RUN_ALL_TESTS();
197  spinner.interrupt();
198  spinner.join();
199 
200  return res;
201 }
void OdomCallback(const OdomConstPtr &odom)
d
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
boost::shared_ptr< nav_msgs::Odometry const > OdomConstPtr
static const double ekf_duration
static const double EPS_trans_z
int main(int argc, char **argv)
bool sleep() const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
boost::shared_ptr< geometry_msgs::PoseWithCovarianceStamped const > EkfConstPtr
static const double EPS_trans_y
int g_argc
ros::Subscriber odom_sub_
NodeHandle node_
void spinner()
TEST_F(TestEKF, test)
ROSCPP_DECL void spin(Spinner &spinner)
TestEKF()
constructor
static const double EPS_rot_x
char ** g_argv
bool sleep() const
static const double EPS_rot_y
#define ROS_INFO(...)
OdomConstPtr odom_end_
int SUCCEED
static const double EPS_rot_z
static const double EPS_rot_w
static const double time_end
double odom_counter_
static const double EPS_trans_x
~TestEKF()
Destructor.
EkfConstPtr ekf_end_
void EKFCallback(const EkfConstPtr &ekf)


robot_pose_ekf
Author(s): Wim Meeussen, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:05:38