test_ekf_localization_node_interfaces.cpp
Go to the documentation of this file.
00001 #include "robot_localization/SetPose.h"
00002 
00003 #include <ros/ros.h>
00004 #include <nav_msgs/Odometry.h>
00005 #include <geometry_msgs/PoseWithCovarianceStamped.h>
00006 #include <geometry_msgs/TwistWithCovarianceStamped.h>
00007 #include <sensor_msgs/Imu.h>
00008 
00009 #include <gtest/gtest.h>
00010 #include <iostream>
00011 
00012 #include <tf/tf.h>
00013 
00014 nav_msgs::Odometry filtered_;
00015 
00016 bool stateUpdated_;
00017 
00018 void resetFilter()
00019 {
00020   ros::NodeHandle nh;
00021   ros::ServiceClient client = nh.serviceClient<robot_localization::SetPose>("/set_pose");
00022 
00023   robot_localization::SetPose setPose;
00024   setPose.request.pose.pose.pose.orientation.w = 1;
00025   setPose.request.pose.header.frame_id = "odom";
00026   for(size_t ind = 0; ind < 36; ind+=7)
00027   {
00028     setPose.request.pose.pose.covariance[ind] = 1e-6;
00029   }
00030 
00031   setPose.request.pose.header.stamp = ros::Time::now();
00032   client.call(setPose);
00033   setPose.request.pose.header.seq++;
00034   ros::spinOnce();
00035   ros::Duration(0.01).sleep();
00036   stateUpdated_ = false;
00037 
00038   double deltaX = 0.0;
00039   double deltaY = 0.0;
00040   double deltaZ = 0.0;
00041 
00042   while(!stateUpdated_ || ::sqrt(deltaX * deltaX + deltaY * deltaY + deltaZ * deltaZ) > 0.1)
00043   {
00044     ros::spinOnce();
00045     ros::Duration(0.01).sleep();
00046 
00047     deltaX = filtered_.pose.pose.position.x - setPose.request.pose.pose.pose.position.x;
00048     deltaY = filtered_.pose.pose.position.y - setPose.request.pose.pose.pose.position.y;
00049     deltaZ = filtered_.pose.pose.position.z - setPose.request.pose.pose.pose.position.z;
00050   }
00051 
00052   EXPECT_LT(::sqrt(deltaX * deltaX + deltaY * deltaY + deltaZ * deltaZ), 0.1);
00053 }
00054 
00055 void filterCallback(const nav_msgs::OdometryConstPtr &msg)
00056 {
00057   filtered_ = *msg;
00058   stateUpdated_ = true;
00059 }
00060 
00061 TEST (InterfacesTest, OdomPoseBasicIO)
00062 {
00063   stateUpdated_ = false;
00064 
00065   ros::NodeHandle nh;
00066   ros::Publisher odomPub = nh.advertise<nav_msgs::Odometry>("/odom_input0", 5);
00067   ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback);
00068 
00069   nav_msgs::Odometry odom;
00070   odom.pose.pose.position.x = 20.0;
00071   odom.pose.pose.position.y = 10.0;
00072   odom.pose.pose.position.z = -40.0;
00073   
00074   odom.pose.covariance[0] = 2.0;
00075   odom.pose.covariance[7] = 2.0;
00076   odom.pose.covariance[14] = 2.0;
00077 
00078   odom.header.frame_id = "odom";
00079   odom.child_frame_id = "base_link";
00080 
00081   ros::Rate loopRate(50);
00082   for(size_t i = 0; i < 50; ++i)
00083   {
00084     odom.header.stamp = ros::Time::now();
00085     odomPub.publish(odom);
00086     ros::spinOnce();
00087 
00088     loopRate.sleep();
00089 
00090     odom.header.seq++;
00091   }
00092 
00093   // Now check the values from the callback
00094   EXPECT_LT(::fabs(filtered_.pose.pose.position.x - odom.pose.pose.position.x), 0.01);
00095   EXPECT_LT(::fabs(filtered_.pose.pose.position.y), 0.01); // Configuration for this variable for this sensor is false
00096   EXPECT_LT(::fabs(filtered_.pose.pose.position.z - odom.pose.pose.position.z), 0.01);
00097 
00098   EXPECT_LT(filtered_.pose.covariance[0], 0.5);
00099   EXPECT_LT(filtered_.pose.covariance[7], 0.25); // Configuration for this variable for this sensor is false
00100   EXPECT_LT(filtered_.pose.covariance[14], 0.5);
00101 
00102   resetFilter();
00103 }
00104 
00105 TEST (InterfacesTest, OdomTwistBasicIO)
00106 {
00107   ros::NodeHandle nh;
00108   ros::Publisher odomPub = nh.advertise<nav_msgs::Odometry>("/odom_input2", 5);
00109   ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback);
00110 
00111   nav_msgs::Odometry odom;
00112   odom.twist.twist.linear.x = 5.0;
00113   odom.twist.twist.linear.y = 0.0;
00114   odom.twist.twist.linear.z = 0.0;
00115   odom.twist.twist.angular.x = 0.0;
00116   odom.twist.twist.angular.y = 0.0;
00117   odom.twist.twist.angular.z = 0.0;
00118 
00119   for(size_t ind = 0; ind < 36; ind+=7)
00120   {
00121     odom.twist.covariance[ind] = 1e-6;
00122   }
00123 
00124   odom.header.frame_id = "odom";
00125   odom.child_frame_id = "base_link";
00126 
00127   ros::Rate loopRate(20);
00128   for(size_t i = 0; i < 400; ++i)
00129   {
00130     odom.header.stamp = ros::Time::now();
00131     odomPub.publish(odom);
00132     ros::spinOnce();
00133 
00134     loopRate.sleep();
00135 
00136     odom.header.seq++;
00137   }
00138   ros::spinOnce();
00139 
00140   EXPECT_LT(::fabs(filtered_.twist.twist.linear.x - odom.twist.twist.linear.x), 0.1);
00141   EXPECT_LT(::fabs(filtered_.pose.pose.position.x - 100.0), 2.0);
00142 
00143   resetFilter();
00144 
00145   odom.twist.twist.linear.x = 0.0;
00146   odom.twist.twist.linear.y = 5.0;
00147 
00148   loopRate = ros::Rate(20);
00149   for(size_t i = 0; i < 200; ++i)
00150   {
00151     odom.header.stamp = ros::Time::now();
00152     odomPub.publish(odom);
00153     ros::spinOnce();
00154 
00155     loopRate.sleep();
00156 
00157     odom.header.seq++;
00158   }
00159   ros::spinOnce();
00160 
00161   EXPECT_LT(::fabs(filtered_.twist.twist.linear.y - odom.twist.twist.linear.y), 0.1);
00162   EXPECT_LT(::fabs(filtered_.pose.pose.position.y - 50.0), 1.0);
00163 
00164   resetFilter();
00165 
00166   odom.twist.twist.linear.y = 0.0;
00167   odom.twist.twist.linear.z = 5.0;
00168 
00169   loopRate = ros::Rate(20);
00170   for(size_t i = 0; i < 100; ++i)
00171   {
00172     odom.header.stamp = ros::Time::now();
00173     odomPub.publish(odom);
00174     ros::spinOnce();
00175 
00176     loopRate.sleep();
00177 
00178     odom.header.seq++;
00179   }
00180   ros::spinOnce();
00181 
00182   EXPECT_LT(::fabs(filtered_.twist.twist.linear.z - odom.twist.twist.linear.z), 0.1);
00183   EXPECT_LT(::fabs(filtered_.pose.pose.position.z - 25.0), 1.0);
00184 
00185   resetFilter();
00186 
00187   odom.twist.twist.linear.z = 0.0;
00188   odom.twist.twist.linear.x = 1.0;
00189   odom.twist.twist.angular.z = (M_PI/2) / (100.0 * 0.05);
00190 
00191   loopRate = ros::Rate(20);
00192   for(size_t i = 0; i < 100; ++i)
00193   {
00194     odom.header.stamp = ros::Time::now();
00195     odomPub.publish(odom);
00196     ros::spinOnce();
00197 
00198     loopRate.sleep();
00199 
00200     odom.header.seq++;
00201   }
00202   ros::spinOnce();
00203 
00204   EXPECT_LT(::fabs(filtered_.twist.twist.linear.x - odom.twist.twist.linear.x), 0.1);
00205   EXPECT_LT(::fabs(filtered_.twist.twist.angular.z - odom.twist.twist.angular.z), 0.1);
00206   EXPECT_LT(::fabs(filtered_.pose.pose.position.x - filtered_.pose.pose.position.y), 0.5);
00207 
00208   resetFilter();
00209 
00210   odom.twist.twist.linear.x = 0.0;
00211   odom.twist.twist.angular.z = 0.0;
00212   odom.twist.twist.angular.x = -(M_PI/2) / (100.0 * 0.05);
00213 
00214   // First, roll the vehicle on its side
00215   loopRate = ros::Rate(20);
00216   for(size_t i = 0; i < 100; ++i)
00217   {
00218     odom.header.stamp = ros::Time::now();
00219     odomPub.publish(odom);
00220     ros::spinOnce();
00221 
00222     loopRate.sleep();
00223 
00224     odom.header.seq++;
00225   }
00226   ros::spinOnce();
00227 
00228   odom.twist.twist.angular.x = 0.0;
00229   odom.twist.twist.angular.y = (M_PI/2) / (100.0 * 0.05);
00230 
00231   // Now, pitch it down (positive pitch velocity in vehicle frame)
00232   loopRate = ros::Rate(20);
00233   for(size_t i = 0; i < 100; ++i)
00234   {
00235     odom.header.stamp = ros::Time::now();
00236     odomPub.publish(odom);
00237     ros::spinOnce();
00238 
00239     loopRate.sleep();
00240 
00241     odom.header.seq++;
00242   }
00243   ros::spinOnce();
00244 
00245   odom.twist.twist.angular.y = 0.0;
00246   odom.twist.twist.linear.x = 3.0;
00247 
00248   // We should now be on our side and facing -Y. Move forward in
00249   // the vehicle frame X direction, and make sure Y decreases in
00250   // the world frame.
00251   loopRate = ros::Rate(20);
00252   for(size_t i = 0; i < 100; ++i)
00253   {
00254     odom.header.stamp = ros::Time::now();
00255     odomPub.publish(odom);
00256     ros::spinOnce();
00257 
00258     loopRate.sleep();
00259 
00260     odom.header.seq++;
00261   }
00262   ros::spinOnce();
00263 
00264   EXPECT_LT(::fabs(filtered_.twist.twist.linear.x - odom.twist.twist.linear.x), 0.1);
00265   EXPECT_LT(::fabs(filtered_.pose.pose.position.y + 15), 1.0);
00266 
00267   resetFilter();
00268 }
00269 
00270 TEST (InterfacesTest, PoseBasicIO)
00271 {
00272   ros::NodeHandle nh;
00273   ros::Publisher posePub = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("/pose_input0", 5);
00274   ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback);
00275 
00276   geometry_msgs::PoseWithCovarianceStamped pose;
00277   pose.pose.pose.position.x = 20.0;
00278   pose.pose.pose.position.y = 10.0;
00279   pose.pose.pose.position.z = -40.0;
00280   pose.pose.pose.orientation.x = 0;
00281   pose.pose.pose.orientation.y = 0;
00282   pose.pose.pose.orientation.z = 0;
00283   pose.pose.pose.orientation.w = 1;
00284 
00285   for(size_t ind = 0; ind < 36; ind+=7)
00286   {
00287     pose.pose.covariance[ind] = 1e-6;
00288   }
00289 
00290   pose.header.frame_id = "odom";
00291 
00292   ros::Rate loopRate = ros::Rate(50);
00293   for(size_t i = 0; i < 50; ++i)
00294   {
00295     pose.header.stamp = ros::Time::now();
00296     posePub.publish(pose);
00297     ros::spinOnce();
00298 
00299     loopRate.sleep();
00300 
00301     pose.header.seq++;
00302   }
00303 
00304   // Now check the values from the callback
00305   EXPECT_LT(::fabs(filtered_.pose.pose.position.x - pose.pose.pose.position.x), 0.1);
00306   EXPECT_LT(::fabs(filtered_.pose.pose.position.y), 0.1); // Configuration for this variable for this sensor is false
00307   EXPECT_LT(::fabs(filtered_.pose.pose.position.z - pose.pose.pose.position.z), 0.1);
00308 
00309   EXPECT_LT(filtered_.pose.covariance[0], 0.5);
00310   EXPECT_LT(filtered_.pose.covariance[7], 0.25); // Configuration for this variable for this sensor is false
00311   EXPECT_LT(filtered_.pose.covariance[14], 0.5);
00312 
00313   resetFilter();
00314 }
00315 
00316 TEST (InterfacesTest, TwistBasicIO)
00317 {
00318   ros::NodeHandle nh;
00319   ros::Publisher twistPub = nh.advertise<geometry_msgs::TwistWithCovarianceStamped>("/twist_input0", 5);
00320   ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback);
00321 
00322   geometry_msgs::TwistWithCovarianceStamped twist;
00323   twist.twist.twist.linear.x = 5.0;
00324   twist.twist.twist.linear.y = 0;
00325   twist.twist.twist.linear.z = 0;
00326   twist.twist.twist.angular.x = 0;
00327   twist.twist.twist.angular.y = 0;
00328   twist.twist.twist.angular.z = 0;
00329 
00330   for(size_t ind = 0; ind < 36; ind+=7)
00331   {
00332     twist.twist.covariance[ind] = 1e-6;
00333   }
00334 
00335   twist.header.frame_id = "base_link";
00336 
00337   ros::Rate loopRate = ros::Rate(20);
00338   for(size_t i = 0; i < 400; ++i)
00339   {
00340     twist.header.stamp = ros::Time::now();
00341     twistPub.publish(twist);
00342     ros::spinOnce();
00343 
00344     loopRate.sleep();
00345 
00346     twist.header.seq++;
00347   }
00348   ros::spinOnce();
00349 
00350   EXPECT_LT(::fabs(filtered_.twist.twist.linear.x - twist.twist.twist.linear.x), 0.1);
00351   EXPECT_LT(::fabs(filtered_.pose.pose.position.x - 100.0), 2.0);
00352 
00353   resetFilter();
00354 
00355   twist.twist.twist.linear.x = 0.0;
00356   twist.twist.twist.linear.y = 5.0;
00357 
00358   loopRate = ros::Rate(20);
00359   for(size_t i = 0; i < 200; ++i)
00360   {
00361     twist.header.stamp = ros::Time::now();
00362     twistPub.publish(twist);
00363     ros::spinOnce();
00364 
00365     loopRate.sleep();
00366 
00367     twist.header.seq++;
00368   }
00369   ros::spinOnce();
00370 
00371   EXPECT_LT(::fabs(filtered_.twist.twist.linear.y - twist.twist.twist.linear.y), 0.1);
00372   EXPECT_LT(::fabs(filtered_.pose.pose.position.y - 50.0), 1.0);
00373 
00374   resetFilter();
00375 
00376   twist.twist.twist.linear.y = 0.0;
00377   twist.twist.twist.linear.z = 5.0;
00378 
00379   loopRate = ros::Rate(20);
00380   for(size_t i = 0; i < 100; ++i)
00381   {
00382     twist.header.stamp = ros::Time::now();
00383     twistPub.publish(twist);
00384     ros::spinOnce();
00385 
00386     loopRate.sleep();
00387 
00388     twist.header.seq++;
00389   }
00390   ros::spinOnce();
00391 
00392   EXPECT_LT(::fabs(filtered_.twist.twist.linear.z - twist.twist.twist.linear.z), 0.1);
00393   EXPECT_LT(::fabs(filtered_.pose.pose.position.z - 25.0), 1.0);
00394 
00395   resetFilter();
00396 
00397   twist.twist.twist.linear.z = 0.0;
00398   twist.twist.twist.linear.x = 1.0;
00399   twist.twist.twist.angular.z = (M_PI/2) / (100.0 * 0.05);
00400 
00401   loopRate = ros::Rate(20);
00402   for(size_t i = 0; i < 100; ++i)
00403   {
00404     twist.header.stamp = ros::Time::now();
00405     twistPub.publish(twist);
00406     ros::spinOnce();
00407 
00408     loopRate.sleep();
00409 
00410     twist.header.seq++;
00411   }
00412   ros::spinOnce();
00413 
00414   EXPECT_LT(::fabs(filtered_.twist.twist.linear.x - twist.twist.twist.linear.x), 0.1);
00415   EXPECT_LT(::fabs(filtered_.twist.twist.angular.z - twist.twist.twist.angular.z), 0.1);
00416   EXPECT_LT(::fabs(filtered_.pose.pose.position.x - filtered_.pose.pose.position.y), 0.5);
00417 
00418   resetFilter();
00419 
00420   twist.twist.twist.linear.x = 0.0;
00421   twist.twist.twist.angular.z = 0.0;
00422   twist.twist.twist.angular.x = -(M_PI/2) / (100.0 * 0.05);
00423 
00424   // First, roll the vehicle on its side
00425   loopRate = ros::Rate(20);
00426   for(size_t i = 0; i < 100; ++i)
00427   {
00428     twist.header.stamp = ros::Time::now();
00429     twistPub.publish(twist);
00430     ros::spinOnce();
00431 
00432     loopRate.sleep();
00433 
00434     twist.header.seq++;
00435   }
00436   ros::spinOnce();
00437 
00438   twist.twist.twist.angular.x = 0.0;
00439   twist.twist.twist.angular.y = (M_PI/2) / (100.0 * 0.05);
00440 
00441   // Now, pitch it down (positive pitch velocity in vehicle frame)
00442   loopRate = ros::Rate(20);
00443   for(size_t i = 0; i < 100; ++i)
00444   {
00445     twist.header.stamp = ros::Time::now();
00446     twistPub.publish(twist);
00447     ros::spinOnce();
00448 
00449     loopRate.sleep();
00450 
00451     twist.header.seq++;
00452   }
00453   ros::spinOnce();
00454 
00455   twist.twist.twist.angular.y = 0.0;
00456   twist.twist.twist.linear.x = 3.0;
00457 
00458   // We should now be on our side and facing -Y. Move forward in
00459   // the vehicle frame X direction, and make sure Y decreases in
00460   // the world frame.
00461   loopRate = ros::Rate(20);
00462   for(size_t i = 0; i < 100; ++i)
00463   {
00464     twist.header.stamp = ros::Time::now();
00465     twistPub.publish(twist);
00466     ros::spinOnce();
00467 
00468     loopRate.sleep();
00469 
00470     twist.header.seq++;
00471   }
00472   ros::spinOnce();
00473 
00474   EXPECT_LT(::fabs(filtered_.twist.twist.linear.x - twist.twist.twist.linear.x), 0.1);
00475   EXPECT_LT(::fabs(filtered_.pose.pose.position.y + 15), 1.0);
00476 
00477   resetFilter();
00478 }
00479 
00480 TEST (InterfacesTest, ImuPoseBasicIO)
00481 {
00482   ros::NodeHandle nh;
00483   ros::Publisher imuPub = nh.advertise<sensor_msgs::Imu>("/imu_input0", 5);
00484   ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback);
00485 
00486   sensor_msgs::Imu imu;
00487   tf::Quaternion quat;
00488   quat.setRPY(M_PI/4, -M_PI/4, M_PI/2);
00489   tf::quaternionTFToMsg(quat, imu.orientation);
00490 
00491   for(size_t ind = 0; ind < 9; ind+=4)
00492   {
00493     imu.orientation_covariance[ind] = 1e-6;
00494   }
00495 
00496   imu.header.frame_id = "base_link";
00497 
00498   // Make sure the pose reset worked. Test will timeout
00499   // if this fails.
00500   ros::Rate loopRate(50);
00501   for(size_t i = 0; i < 50; ++i)
00502   {
00503     imu.header.stamp = ros::Time::now();
00504     imuPub.publish(imu);
00505     ros::spinOnce();
00506 
00507     loopRate.sleep();
00508 
00509     imu.header.seq++;
00510   }
00511 
00512   // Now check the values from the callback
00513   tf::quaternionMsgToTF(filtered_.pose.pose.orientation, quat);
00514   tf::Matrix3x3 mat(quat);
00515   double r, p, y;
00516   mat.getRPY(r, p, y);
00517   EXPECT_LT(::fabs(r - M_PI/4), 0.1);
00518   EXPECT_LT(::fabs(p + M_PI/4), 0.1);
00519   EXPECT_LT(::fabs(y - M_PI/2), 0.1);
00520 
00521   EXPECT_LT(filtered_.pose.covariance[21], 0.5);
00522   EXPECT_LT(filtered_.pose.covariance[28], 0.25);
00523   EXPECT_LT(filtered_.pose.covariance[35], 0.5);
00524 
00525   resetFilter();
00526 }
00527 
00528 TEST (InterfacesTest, ImuTwistBasicIO)
00529 {
00530   ros::NodeHandle nh;
00531   ros::Publisher imuPub = nh.advertise<sensor_msgs::Imu>("/imu_input1", 5);
00532   ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback);
00533 
00534   sensor_msgs::Imu imu;
00535   tf::Quaternion quat;
00536   imu.angular_velocity.x = (M_PI / 2.0);
00537 
00538   for(size_t ind = 0; ind < 9; ind+=4)
00539   {
00540     imu.angular_velocity_covariance[ind] = 1e-6;
00541   }
00542 
00543   imu.header.frame_id = "base_link";
00544 
00545   ros::Rate loopRate(50);
00546   for(size_t i = 0; i < 50; ++i)
00547   {
00548     imu.header.stamp = ros::Time::now();
00549     imuPub.publish(imu);
00550     loopRate.sleep();
00551     ros::spinOnce();
00552 
00553     imu.header.seq++;
00554   }
00555 
00556   // Now check the values from the callback
00557   tf::quaternionMsgToTF(filtered_.pose.pose.orientation, quat);
00558   tf::Matrix3x3 mat(quat);
00559   double r, p, y;
00560   mat.getRPY(r, p, y);
00561 
00562   // Tolerances may seem loose, but the initial state covariances
00563   // are tiny, so the filter is sluggish to accept velocity data
00564   EXPECT_LT(::fabs(r - M_PI / 2.0), 0.7);
00565   EXPECT_LT(::fabs(p), 0.1);
00566   EXPECT_LT(::fabs(y), 0.1);
00567 
00568   EXPECT_LT(filtered_.twist.covariance[21], 1e-3);
00569   EXPECT_LT(filtered_.pose.covariance[21], 0.1);
00570 
00571   resetFilter();
00572 
00573   imu.angular_velocity.x = 0.0;
00574   imu.angular_velocity.y = -(M_PI / 2.0);
00575 
00576   // Make sure the pose reset worked. Test will timeout
00577   // if this fails.
00578   loopRate = ros::Rate(50);
00579   for(size_t i = 0; i < 50; ++i)
00580   {
00581     imu.header.stamp = ros::Time::now();
00582     imuPub.publish(imu);
00583     loopRate.sleep();
00584     ros::spinOnce();
00585 
00586     imu.header.seq++;
00587   }
00588 
00589   // Now check the values from the callback
00590   tf::quaternionMsgToTF(filtered_.pose.pose.orientation, quat);
00591   mat.setRotation(quat);
00592   mat.getRPY(r, p, y);
00593   EXPECT_LT(::fabs(r), 0.1);
00594   EXPECT_LT(::fabs(p + M_PI / 2.0), 0.7);
00595   EXPECT_LT(::fabs(y), 0.1);
00596 
00597   EXPECT_LT(filtered_.twist.covariance[28], 1e-3);
00598   EXPECT_LT(filtered_.pose.covariance[28], 0.1);
00599 
00600   resetFilter();
00601 
00602   imu.angular_velocity.y = 0;
00603   imu.angular_velocity.z = M_PI / 4.0;
00604 
00605   // Make sure the pose reset worked. Test will timeout
00606   // if this fails.
00607   loopRate = ros::Rate(50);
00608   for(size_t i = 0; i < 50; ++i)
00609   {
00610     imu.header.stamp = ros::Time::now();
00611     imuPub.publish(imu);
00612     loopRate.sleep();
00613     ros::spinOnce();
00614 
00615     imu.header.seq++;
00616   }
00617 
00618   // Now check the values from the callback
00619   tf::quaternionMsgToTF(filtered_.pose.pose.orientation, quat);
00620   mat.setRotation(quat);
00621   mat.getRPY(r, p, y);
00622   EXPECT_LT(::fabs(r), 0.1);
00623   EXPECT_LT(::fabs(p), 0.1);
00624   EXPECT_LT(::fabs(y - M_PI / 4.0), 0.7);
00625 
00626   EXPECT_LT(filtered_.twist.covariance[35], 1e-3);
00627   EXPECT_LT(filtered_.pose.covariance[35], 0.1);
00628 
00629   resetFilter();
00630 }
00631 
00632 TEST (InterfacesTest, ImuAccBasicIO)
00633 {
00634   ros::NodeHandle nh;
00635   ros::Publisher imuPub = nh.advertise<sensor_msgs::Imu>("/imu_input2", 5);
00636   ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback);
00637 
00638   sensor_msgs::Imu imu;
00639   imu.header.frame_id = "base_link";
00640   imu.linear_acceleration_covariance[0] = 1e-6;
00641   imu.linear_acceleration_covariance[4] = 1e-6;
00642   imu.linear_acceleration_covariance[8] = 1e-6;
00643 
00644   imu.linear_acceleration.x = 1;
00645   imu.linear_acceleration.y = -1;
00646   imu.linear_acceleration.z = 1;
00647 
00648   // Move with constant acceleration for 1 second,
00649   // then check our velocity.
00650   ros::Rate loopRate(50);
00651   for(size_t i = 0; i < 50; ++i)
00652   {
00653     imu.header.stamp = ros::Time::now();
00654     imuPub.publish(imu);
00655     loopRate.sleep();
00656     ros::spinOnce();
00657 
00658     imu.header.seq++;
00659   }
00660 
00661   EXPECT_LT(::fabs(filtered_.twist.twist.linear.x - 1.0), 0.4);
00662   EXPECT_LT(::fabs(filtered_.twist.twist.linear.y + 1.0), 0.4);
00663   EXPECT_LT(::fabs(filtered_.twist.twist.linear.z - 1.0), 0.4);
00664 
00665   imu.linear_acceleration.x = 0.0;
00666   imu.linear_acceleration.y = 0.0;
00667   imu.linear_acceleration.z = 0.0;
00668 
00669   // Now move for another second, and see where we
00670   // end up
00671   loopRate = ros::Rate(50);
00672   for(size_t i = 0; i < 50; ++i)
00673   {
00674     imu.header.stamp = ros::Time::now();
00675     imuPub.publish(imu);
00676     loopRate.sleep();
00677     ros::spinOnce();
00678 
00679     imu.header.seq++;
00680   }
00681 
00682   EXPECT_LT(::fabs(filtered_.pose.pose.position.x - 1.2), 0.4);
00683   EXPECT_LT(::fabs(filtered_.pose.pose.position.y + 1.2), 0.4);
00684   EXPECT_LT(::fabs(filtered_.pose.pose.position.z - 1.2), 0.4);
00685 
00686   resetFilter();
00687 }
00688 
00689 TEST (InterfacesTest, OdomDifferentialIO)
00690 {
00691   ros::NodeHandle nh;
00692   ros::Publisher odomPub = nh.advertise<nav_msgs::Odometry>("/odom_input1", 5);
00693   ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback);
00694 
00695   nav_msgs::Odometry odom;
00696   odom.pose.pose.position.x = 20.0;
00697   odom.pose.pose.position.y = 10.0;
00698   odom.pose.pose.position.z = -40.0;
00699 
00700   odom.pose.pose.orientation.w = 1;
00701 
00702   odom.pose.covariance[0] = 2.0;
00703   odom.pose.covariance[7] = 2.0;
00704   odom.pose.covariance[14] = 2.0;
00705   odom.pose.covariance[21] = 0.2;
00706   odom.pose.covariance[28] = 0.2;
00707   odom.pose.covariance[35] = 0.2;
00708 
00709   odom.header.frame_id = "odom";
00710   odom.child_frame_id = "base_link";
00711 
00712   // No guaranteeing that the zero state
00713   // we're expecting to see here isn't just
00714   // a result of zeroing it out previously,
00715   // so check 10 times in succession.
00716   size_t zeroCount = 0;
00717   while(zeroCount++ < 10)
00718   {
00719     odom.header.stamp = ros::Time::now();
00720     odomPub.publish(odom);
00721     ros::spinOnce();
00722 
00723     EXPECT_LT(::fabs(filtered_.pose.pose.position.x), 0.01);
00724     EXPECT_LT(::fabs(filtered_.pose.pose.position.y), 0.01);
00725     EXPECT_LT(::fabs(filtered_.pose.pose.position.z), 0.01);
00726     EXPECT_LT(::fabs(filtered_.pose.pose.orientation.x), 0.01);
00727     EXPECT_LT(::fabs(filtered_.pose.pose.orientation.y), 0.01);
00728     EXPECT_LT(::fabs(filtered_.pose.pose.orientation.z), 0.01);
00729     EXPECT_LT(::fabs(filtered_.pose.pose.orientation.w - 1), 0.01);
00730 
00731     ros::Duration(0.1).sleep();
00732 
00733     odom.header.seq++;
00734   }
00735 
00736   for(size_t ind = 0; ind < 36; ind+=7)
00737   {
00738     odom.pose.covariance[ind] = 1e-6;
00739   }
00740 
00741   // Slowly move the position, and hope that the
00742   // differential position keeps up
00743   ros::Rate loopRate(20);
00744   for(size_t i = 0; i < 100; ++i)
00745   {
00746     odom.pose.pose.position.x += 0.01;
00747     odom.pose.pose.position.y += 0.02;
00748     odom.pose.pose.position.z -= 0.03;
00749 
00750     odom.header.stamp = ros::Time::now();
00751     odomPub.publish(odom);
00752     ros::spinOnce();
00753 
00754     loopRate.sleep();
00755 
00756     odom.header.seq++;
00757   }
00758   ros::spinOnce();
00759 
00760   EXPECT_LT(::fabs(filtered_.pose.pose.position.x - 1), 0.2);
00761   EXPECT_LT(::fabs(filtered_.pose.pose.position.y - 2), 0.4);
00762   EXPECT_LT(::fabs(filtered_.pose.pose.position.z + 3), 0.6);
00763 
00764   resetFilter();
00765 }
00766 
00767 TEST (InterfacesTest, PoseDifferentialIO)
00768 {
00769   ros::NodeHandle nh;
00770   ros::Publisher posePub = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("/pose_input1", 5);
00771   ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback);
00772 
00773   geometry_msgs::PoseWithCovarianceStamped pose;
00774   pose.pose.pose.position.x = 20.0;
00775   pose.pose.pose.position.y = 10.0;
00776   pose.pose.pose.position.z = -40.0;
00777 
00778   pose.pose.pose.orientation.w = 1;
00779 
00780   pose.pose.covariance[0] = 2.0;
00781   pose.pose.covariance[7] = 2.0;
00782   pose.pose.covariance[14] = 2.0;
00783   pose.pose.covariance[21] = 0.2;
00784   pose.pose.covariance[28] = 0.2;
00785   pose.pose.covariance[35] = 0.2;
00786 
00787   pose.header.frame_id = "odom";
00788 
00789   // No guaranteeing that the zero state
00790   // we're expecting to see here isn't just
00791   // a result of zeroing it out previously,
00792   // so check 10 times in succession.
00793   size_t zeroCount = 0;
00794   while(zeroCount++ < 10)
00795   {
00796     pose.header.stamp = ros::Time::now();
00797     posePub.publish(pose);
00798     ros::spinOnce();
00799 
00800     EXPECT_LT(::fabs(filtered_.pose.pose.position.x), 0.01);
00801     EXPECT_LT(::fabs(filtered_.pose.pose.position.y), 0.01);
00802     EXPECT_LT(::fabs(filtered_.pose.pose.position.z), 0.01);
00803     EXPECT_LT(::fabs(filtered_.pose.pose.orientation.x), 0.01);
00804     EXPECT_LT(::fabs(filtered_.pose.pose.orientation.y), 0.01);
00805     EXPECT_LT(::fabs(filtered_.pose.pose.orientation.z), 0.01);
00806     EXPECT_LT(::fabs(filtered_.pose.pose.orientation.w - 1), 0.01);
00807 
00808     ros::Duration(0.1).sleep();
00809 
00810     pose.header.seq++;
00811   }
00812 
00813   // ...but only if we give the measurement a tiny covariance
00814   for(size_t ind = 0; ind < 36; ind+=7)
00815   {
00816     pose.pose.covariance[ind] = 1e-6;
00817   }
00818 
00819   // Issue this location repeatedly, and see if we get
00820   // a final reported position of (1, 2, -3)
00821   ros::Rate loopRate(20);
00822   for(size_t i = 0; i < 100; ++i)
00823   {
00824     pose.pose.pose.position.x += 0.01;
00825     pose.pose.pose.position.y += 0.02;
00826     pose.pose.pose.position.z -= 0.03;
00827 
00828     pose.header.stamp = ros::Time::now();
00829     posePub.publish(pose);
00830     ros::spinOnce();
00831 
00832     loopRate.sleep();
00833 
00834     pose.header.seq++;
00835   }
00836   ros::spinOnce();
00837 
00838   EXPECT_LT(::fabs(filtered_.pose.pose.position.x - 1), 0.2);
00839   EXPECT_LT(::fabs(filtered_.pose.pose.position.y - 2), 0.4);
00840   EXPECT_LT(::fabs(filtered_.pose.pose.position.z + 3), 0.6);
00841 
00842   resetFilter();
00843 }
00844 
00845 TEST (InterfacesTest, ImuDifferentialIO)
00846 {
00847   ros::NodeHandle nh;
00848   ros::Publisher imu0Pub = nh.advertise<sensor_msgs::Imu>("/imu_input0", 5);
00849   ros::Publisher imu1Pub = nh.advertise<sensor_msgs::Imu>("/imu_input1", 5);
00850   ros::Publisher imuPub = nh.advertise<sensor_msgs::Imu>("/imu_input3", 5);
00851   ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback);
00852 
00853   sensor_msgs::Imu imu;
00854   imu.header.frame_id = "base_link";
00855   tf::Quaternion quat;
00856   const double roll = M_PI/2.0;
00857   const double pitch = -M_PI;
00858   const double yaw = -M_PI/4.0;
00859   quat.setRPY(roll, pitch, yaw);
00860   tf::quaternionTFToMsg(quat, imu.orientation);
00861 
00862   imu.orientation_covariance[0] = 0.02;
00863   imu.orientation_covariance[4] = 0.02;
00864   imu.orientation_covariance[8] = 0.02;
00865 
00866   imu.angular_velocity_covariance[0] = 0.02;
00867   imu.angular_velocity_covariance[4] = 0.02;
00868   imu.angular_velocity_covariance[8] = 0.02;
00869 
00870   size_t setCount = 0;
00871   while(setCount++ < 10)
00872   {
00873     imu.header.stamp = ros::Time::now();
00874     imu0Pub.publish(imu); // Use this to move the absolute orientation
00875     imu1Pub.publish(imu); // Use this to keep velocities at 0
00876     ros::spinOnce();
00877 
00878     ros::Duration(0.1).sleep();
00879 
00880     imu.header.seq++;
00881   }
00882 
00883   size_t zeroCount = 0;
00884   while(zeroCount++ < 10)
00885   {
00886     imu.header.stamp = ros::Time::now();
00887     imuPub.publish(imu);
00888     ros::spinOnce();
00889 
00890     ros::Duration(0.1).sleep();
00891 
00892     imu.header.seq++;
00893   }
00894 
00895   double rollFinal = roll;
00896   double pitchFinal = pitch;
00897   double yawFinal = yaw;
00898 
00899   // Move the orientation slowly, and see if we
00900   // can push it to 0
00901   ros::Rate loopRate(20);
00902   for(size_t i = 0; i < 100; ++i)
00903   {
00904     yawFinal -= 0.01 * (3.0 * M_PI/4.0);
00905 
00906     quat.setRPY(rollFinal, pitchFinal, yawFinal);
00907 
00908     tf::quaternionTFToMsg(quat, imu.orientation);
00909     imu.header.stamp = ros::Time::now();
00910     imuPub.publish(imu);
00911     ros::spinOnce();
00912 
00913     loopRate.sleep();
00914 
00915     imu.header.seq++;
00916   }
00917   ros::spinOnce();
00918 
00919   // Move the orientation slowly, and see if we
00920   // can push it to 0
00921   loopRate = ros::Rate(20);
00922   for(size_t i = 0; i < 100; ++i)
00923   {
00924     rollFinal += 0.01 * (M_PI/2.0);
00925 
00926     quat.setRPY(rollFinal, pitchFinal, yawFinal);
00927 
00928     tf::quaternionTFToMsg(quat, imu.orientation);
00929     imu.header.stamp = ros::Time::now();
00930     imuPub.publish(imu);
00931     ros::spinOnce();
00932 
00933     loopRate.sleep();
00934 
00935     imu.header.seq++;
00936   }
00937   ros::spinOnce();
00938 
00939   tf::quaternionMsgToTF(filtered_.pose.pose.orientation, quat);
00940   tf::Matrix3x3 mat(quat);
00941   mat.getRPY(rollFinal, pitchFinal, yawFinal);
00942   EXPECT_LT(::fabs(rollFinal), 0.2);
00943   EXPECT_LT(::fabs(pitchFinal), 0.2);
00944   EXPECT_LT(::fabs(yawFinal), 0.2);
00945 
00946   resetFilter();
00947 }
00948 
00949 int main(int argc, char **argv)
00950 {
00951   testing::InitGoogleTest(&argc, argv);
00952 
00953   ros::init(argc, argv, "ekf_navigation_node-test-interfaces");
00954   ros::Time::init();
00955 
00956   // Give ekf_localization_node time to initialize
00957   ros::Duration(2.0).sleep();
00958 
00959   return RUN_ALL_TESTS();
00960 }


robot_localization
Author(s): Tom Moore
autogenerated on Fri Aug 28 2015 12:26:20