Go to the documentation of this file.00001 #include <ros/ros.h>
00002 #include <nav_msgs/Odometry.h>
00003
00004 #include <gtest/gtest.h>
00005 #include <iostream>
00006
00007 #include <tf/tf.h>
00008
00009 nav_msgs::Odometry filtered_;
00010
00011 void filterCallback(const nav_msgs::OdometryConstPtr &msg)
00012 {
00013 filtered_ = *msg;
00014 }
00015
00016 TEST (BagTest, PoseCheck)
00017 {
00018 ros::NodeHandle nh;
00019 ros::NodeHandle nhLocal("~");
00020
00021 double finalX;
00022 double finalY;
00023 double finalZ;
00024 double tolerance;
00025
00026 nhLocal.getParam("final_x", finalX);
00027 nhLocal.getParam("final_y", finalY);
00028 nhLocal.getParam("final_z", finalZ);
00029 nhLocal.getParam("tolerance", tolerance);
00030
00031 ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback);
00032
00033 while(ros::ok())
00034 {
00035 ros::spinOnce();
00036 ros::Duration(0.0333333).sleep();
00037 }
00038
00039 double xDiff = filtered_.pose.pose.position.x - finalX;
00040 double yDiff = filtered_.pose.pose.position.y - finalY;
00041 double zDiff = filtered_.pose.pose.position.z - finalZ;
00042
00043 EXPECT_LT(::sqrt(xDiff*xDiff + yDiff*yDiff + zDiff*zDiff), tolerance);
00044 }
00045
00046 int main(int argc, char **argv)
00047 {
00048 testing::InitGoogleTest(&argc, argv);
00049
00050 ros::init(argc, argv, "ekf_navigation_node-bag-pose-tester");
00051 ros::Time::init();
00052
00053
00054 ros::Duration(2.0).sleep();
00055
00056 return RUN_ALL_TESTS();
00057 }
00058