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 #include <tf/transform_listener.h>
00033
00034
00035 TEST_F(SteerDriveControllerTest, testForward)
00036 {
00037
00038 while(!isControllerAlive())
00039 {
00040 ros::Duration(0.1).sleep();
00041 }
00042
00043 geometry_msgs::Twist cmd_vel;
00044 cmd_vel.linear.x = 0.0;
00045 cmd_vel.angular.z = 0.0;
00046 publish(cmd_vel);
00047 ros::Duration(0.1).sleep();
00048
00049 nav_msgs::Odometry old_odom = getLastOdom();
00050
00051 cmd_vel.linear.x = 0.1;
00052 publish(cmd_vel);
00053
00054 ros::Duration(10.0).sleep();
00055
00056 nav_msgs::Odometry new_odom = getLastOdom();
00057
00058
00059 const double dx = new_odom.pose.pose.position.x - old_odom.pose.pose.position.x;
00060 const double dy = new_odom.pose.pose.position.y - old_odom.pose.pose.position.y;
00061 const double dz = new_odom.pose.pose.position.z - old_odom.pose.pose.position.z;
00062 EXPECT_NEAR(sqrt(dx*dx + dy*dy), 1.0, POSITION_TOLERANCE);
00063 EXPECT_LT(fabs(dz), EPS);
00064
00065
00066 double roll_old, pitch_old, yaw_old;
00067 double roll_new, pitch_new, yaw_new;
00068 tf::Matrix3x3(tfQuatFromGeomQuat(old_odom.pose.pose.orientation)).getRPY(roll_old, pitch_old, yaw_old);
00069 tf::Matrix3x3(tfQuatFromGeomQuat(new_odom.pose.pose.orientation)).getRPY(roll_new, pitch_new, yaw_new);
00070 EXPECT_LT(fabs(roll_new - roll_old), EPS);
00071 EXPECT_LT(fabs(pitch_new - pitch_old), EPS);
00072 EXPECT_LT(fabs(yaw_new - yaw_old), EPS);
00073 EXPECT_NEAR(fabs(new_odom.twist.twist.linear.x), cmd_vel.linear.x, EPS);
00074 EXPECT_LT(fabs(new_odom.twist.twist.linear.y), EPS);
00075 EXPECT_LT(fabs(new_odom.twist.twist.linear.z), EPS);
00076
00077 EXPECT_LT(fabs(new_odom.twist.twist.angular.x), EPS);
00078 EXPECT_LT(fabs(new_odom.twist.twist.angular.y), EPS);
00079 EXPECT_LT(fabs(new_odom.twist.twist.angular.z), EPS);
00080 }
00081
00082 TEST_F(SteerDriveControllerTest, testTurn)
00083 {
00084
00085 while(!isControllerAlive())
00086 {
00087 ros::Duration(0.1).sleep();
00088 }
00089
00090 geometry_msgs::Twist cmd_vel;
00091 cmd_vel.linear.x = 0.0;
00092 cmd_vel.angular.z = 0.0;
00093 publish(cmd_vel);
00094 ros::Duration(0.1).sleep();
00095
00096 nav_msgs::Odometry old_odom = getLastOdom();
00097
00098 cmd_vel.angular.z = M_PI/10.0;
00099
00100
00101 cmd_vel.linear.x = 0.1;
00102 publish(cmd_vel);
00103
00104 ros::Duration(10.0).sleep();
00105
00106 nav_msgs::Odometry new_odom = getLastOdom();
00107
00108
00109 double x_answer = 0.0;
00110 double y_answer = 2.0 * cmd_vel.linear.x / cmd_vel.angular.z;
00111 EXPECT_NEAR(fabs(new_odom.pose.pose.position.x - old_odom.pose.pose.position.x), x_answer, POSITION_TOLERANCE);
00112 EXPECT_NEAR(fabs(new_odom.pose.pose.position.y - old_odom.pose.pose.position.y), y_answer, POSITION_TOLERANCE);
00113 EXPECT_LT(fabs(new_odom.pose.pose.position.z - old_odom.pose.pose.position.z), EPS);
00114
00115
00116 double roll_old, pitch_old, yaw_old;
00117 double roll_new, pitch_new, yaw_new;
00118 tf::Matrix3x3(tfQuatFromGeomQuat(old_odom.pose.pose.orientation)).getRPY(roll_old, pitch_old, yaw_old);
00119 tf::Matrix3x3(tfQuatFromGeomQuat(new_odom.pose.pose.orientation)).getRPY(roll_new, pitch_new, yaw_new);
00120 EXPECT_LT(fabs(roll_new - roll_old), EPS);
00121 EXPECT_LT(fabs(pitch_new - pitch_old), EPS);
00122 EXPECT_NEAR(fabs(yaw_new - yaw_old), M_PI, ORIENTATION_TOLERANCE);
00123
00124 EXPECT_NEAR(new_odom.twist.twist.linear.x, 0.1, VELOCITY_TOLERANCE);
00125 EXPECT_LT(fabs(new_odom.twist.twist.linear.y), EPS);
00126 EXPECT_LT(fabs(new_odom.twist.twist.linear.z), EPS);
00127
00128 EXPECT_LT(fabs(new_odom.twist.twist.angular.x), EPS);
00129 EXPECT_LT(fabs(new_odom.twist.twist.angular.y), EPS);
00130 EXPECT_NEAR(fabs(new_odom.twist.twist.angular.z), M_PI/10.0, EPS);
00131 }
00132
00133 TEST_F(SteerDriveControllerTest, testOdomFrame)
00134 {
00135
00136 while(!isControllerAlive())
00137 {
00138 ros::Duration(0.1).sleep();
00139 }
00140
00141 tf::TransformListener listener;
00142 ros::Duration(2.0).sleep();
00143
00144 EXPECT_TRUE(listener.frameExists("odom"));
00145 }
00146
00147 int main(int argc, char** argv)
00148 {
00149 testing::InitGoogleTest(&argc, argv);
00150 ros::init(argc, argv, "steer_drive_controller_test");
00151
00152 ros::AsyncSpinner spinner(1);
00153 spinner.start();
00154
00155 int ret = RUN_ALL_TESTS();
00156 spinner.stop();
00157 ros::shutdown();
00158 return ret;
00159 }