test_robot_pose_ekf.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Wim Meeussen */
00036 
00037 #include <string>
00038 #include <gtest/gtest.h>
00039 #include "ros/ros.h"
00040 #include "nav_msgs/Odometry.h"
00041 #include "geometry_msgs/PoseWithCovarianceStamped.h"
00042 
00043 #include <boost/thread.hpp>
00044 
00045 using namespace ros;
00046 
00047 
00048 static const double time_end     = 1264198883.0;
00049 static const double ekf_duration = 62.0;
00050 static const double EPS_trans_x    = 0.02;
00051 static const double EPS_trans_y    = 0.04;
00052 static const double EPS_trans_z    = 0.00001;
00053 static const double EPS_rot_x      = 0.005;
00054 static const double EPS_rot_y      = 0.005;
00055 static const double EPS_rot_z      = 0.005;
00056 static const double EPS_rot_w      = 0.005;
00057 
00058 
00059 int g_argc;
00060 char** g_argv;
00061 
00062 typedef boost::shared_ptr<nav_msgs::Odometry const> OdomConstPtr;
00063 typedef boost::shared_ptr<geometry_msgs::PoseWithCovarianceStamped const> EkfConstPtr;
00064 
00065 class TestEKF : public testing::Test
00066 {
00067 public:
00068   NodeHandle node_;
00069   ros::Subscriber odom_sub_, ekf_sub_;
00070   EkfConstPtr ekf_begin_, ekf_end_;
00071   OdomConstPtr odom_end_;
00072   double ekf_counter_, odom_counter_;
00073   Time ekf_time_begin_, odom_time_begin_;
00074 
00075 
00076   void OdomCallback(const OdomConstPtr& odom)
00077   {
00078     // get initial time
00079     if (odom_counter_ == 0)
00080       odom_time_begin_ = odom->header.stamp;
00081 
00082     odom_end_ = odom;
00083 
00084     // count number of callbacks
00085     odom_counter_++;
00086   }
00087 
00088 
00089   void EKFCallback(const EkfConstPtr& ekf)
00090   {
00091     // get initial time
00092     if (ekf_counter_ == 0){
00093       ekf_time_begin_ = ekf->header.stamp;
00094       ekf_begin_ = ekf;
00095     }
00096     if (ekf->header.stamp.toSec() < time_end)
00097       ekf_end_ = ekf;
00098 
00099     // count number of callbacks
00100     ekf_counter_++;
00101   }
00102 
00103 
00104 protected:
00106   TestEKF()
00107   {
00108     ekf_counter_ = 0;
00109     odom_counter_ = 0;
00110 
00111   }
00112 
00113 
00115   ~TestEKF()
00116   {
00117   }
00118 
00119   void SetUp()
00120   {
00121     ROS_INFO("Subscribing to robot_pose_ekf/odom_combined");
00122     ekf_sub_ = node_.subscribe("/robot_pose_ekf/odom_combined", 10, &TestEKF::EKFCallback, (TestEKF*)this);
00123 
00124     ROS_INFO("Subscribing to base_odometry/odom");
00125     odom_sub_ = node_.subscribe("base_odometry/odom", 10 , &TestEKF::OdomCallback, (TestEKF*)this);
00126   }
00127 
00128   void TearDown()
00129   {
00130     odom_sub_.shutdown();
00131     ekf_sub_.shutdown();
00132   }
00133 };
00134 
00135 
00136 
00137 
00138 TEST_F(TestEKF, test)
00139 {
00140   Duration d(0.01);
00141   // wait while bag is played back
00142   ROS_INFO("Waiting for bag to start playing");
00143   while (odom_counter_ == 0)
00144     d.sleep();
00145   ROS_INFO("Detected that bag is playing");
00146 
00147   ROS_INFO("Waiting untile end time is reached");
00148   while( odom_end_->header.stamp.toSec() < time_end){
00149     d.sleep();
00150   }
00151   ROS_INFO("End time reached");
00152   // give filter some time to catch up
00153   WallDuration(2.0).sleep();
00154 
00155   // check if callback was called enough times
00156   ROS_INFO("Number of ekf callbacks: %f", ekf_counter_);
00157   EXPECT_GT(ekf_counter_, 500);
00158 
00159   // check if time interval is correct
00160   ROS_INFO("Ekf duration: %f", ekf_duration);
00161   EXPECT_LT(Duration(ekf_end_->header.stamp - ekf_time_begin_).toSec(), ekf_duration * 1.25);
00162   EXPECT_GT(Duration(ekf_end_->header.stamp - ekf_time_begin_).toSec(), ekf_duration * 0.85);
00163 
00164   // check if ekf time is same as odom time
00165   EXPECT_NEAR(ekf_time_begin_.toSec(),  odom_time_begin_.toSec(), 1.0);
00166   EXPECT_NEAR(ekf_end_->header.stamp.toSec(), time_end, 1.0);
00167 
00168   // check filter result
00169   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,
00170             ekf_end_->pose.pose.orientation.x, ekf_end_->pose.pose.orientation.y, ekf_end_->pose.pose.orientation.z, ekf_end_->pose.pose.orientation.w,
00171             ekf_end_->header.stamp.toSec());
00172   EXPECT_NEAR(ekf_end_->pose.pose.position.x, -0.0586126, EPS_trans_x);
00173   EXPECT_NEAR(ekf_end_->pose.pose.position.y, 0.0124321, EPS_trans_y);
00174   EXPECT_NEAR(ekf_end_->pose.pose.position.z, 0.0, EPS_trans_z);
00175   EXPECT_NEAR(ekf_end_->pose.pose.orientation.x, 0.00419421,  EPS_rot_x);
00176   EXPECT_NEAR(ekf_end_->pose.pose.orientation.y,  0.00810739, EPS_rot_y);
00177   EXPECT_NEAR(ekf_end_->pose.pose.orientation.z, -0.0440686,  EPS_rot_z);
00178   EXPECT_NEAR(ekf_end_->pose.pose.orientation.w, 0.998987,  EPS_rot_w);
00179 
00180   SUCCEED();
00181 }
00182 
00183 
00184 
00185 
00186 int main(int argc, char** argv)
00187 {
00188   testing::InitGoogleTest(&argc, argv);
00189   g_argc = argc;
00190   g_argv = argv;
00191 
00192   init(g_argc, g_argv, "testEKF");
00193 
00194   boost::thread spinner(boost::bind(&ros::spin));
00195 
00196   int res = RUN_ALL_TESTS();
00197   spinner.interrupt();
00198   spinner.join();
00199 
00200   return res;
00201 }


robot_pose_ekf
Author(s): Wim Meeussen, contradict@gmail.com
autogenerated on Sun Mar 3 2019 03:46:08