Go to the documentation of this file.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
00027
00030
00031 #include "../common/include/test_common.h"
00032
00033
00034 #include <limits>
00035
00036
00037 TEST_F(SteerDriveControllerTest, testNaN)
00038 {
00039
00040 while(!isControllerAlive())
00041 {
00042 ros::Duration(0.1).sleep();
00043 }
00044
00045 geometry_msgs::Twist cmd_vel;
00046 cmd_vel.linear.x = 0.0;
00047 cmd_vel.angular.z = 0.0;
00048 publish(cmd_vel);
00049 ros::Duration(2.0).sleep();
00050
00051
00052 cmd_vel.linear.x = 0.1;
00053 ros::Duration(2.0).sleep();
00054
00055
00056 stop();
00057 ros::Duration(2.0).sleep();
00058
00059 nav_msgs::Odometry odom = getLastOdom();
00060
00061 EXPECT_NE(std::isnan(odom.twist.twist.linear.x), true);
00062 EXPECT_NE(std::isnan(odom.twist.twist.angular.z), true);
00063 EXPECT_NE(std::isnan(odom.pose.pose.position.x), true);
00064 EXPECT_NE(std::isnan(odom.pose.pose.position.y), true);
00065 EXPECT_NE(std::isnan(odom.pose.pose.orientation.z), true);
00066 EXPECT_NE(std::isnan(odom.pose.pose.orientation.w), true);
00067
00068
00069 start();
00070 ros::Duration(2.0).sleep();
00071
00072 odom = getLastOdom();
00073
00074 EXPECT_NE(std::isnan(odom.twist.twist.linear.x), true);
00075 EXPECT_NE(std::isnan(odom.twist.twist.angular.z), true);
00076 EXPECT_NE(std::isnan(odom.pose.pose.position.x), true);
00077 EXPECT_NE(std::isnan(odom.pose.pose.position.y), true);
00078 EXPECT_NE(std::isnan(odom.pose.pose.orientation.z), true);
00079 EXPECT_NE(std::isnan(odom.pose.pose.orientation.w), true);
00080 }
00081
00082 int main(int argc, char** argv)
00083 {
00084 testing::InitGoogleTest(&argc, argv);
00085 ros::init(argc, argv, "steer_drive_controller_nan_test");
00086
00087 ros::AsyncSpinner spinner(1);
00088 spinner.start();
00089 int ret = RUN_ALL_TESTS();
00090 spinner.stop();
00091 ros::shutdown();
00092 return ret;
00093 }