00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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
00079 if (odom_counter_ == 0)
00080 odom_time_begin_ = odom->header.stamp;
00081
00082 odom_end_ = odom;
00083
00084
00085 odom_counter_++;
00086 }
00087
00088
00089 void EKFCallback(const EkfConstPtr& ekf)
00090 {
00091
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
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
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
00153 WallDuration(2.0).sleep();
00154
00155
00156 ROS_INFO("Number of ekf callbacks: %f", ekf_counter_);
00157 EXPECT_GT(ekf_counter_, 500);
00158
00159
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
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
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 }