00001 #include "test_common.h"
00002 #include <tf/transform_listener.h>
00003
00004
00005 TEST_F(FourWheelSteeringControllerTest, testForward)
00006 {
00007
00008 while(!isControllerAlive())
00009 {
00010 ros::Duration(0.1).sleep();
00011 }
00012
00013 four_wheel_steering_msgs::FourWheelSteering cmd_vel;
00014 cmd_vel.speed = 0.0;
00015 cmd_vel.front_steering_angle = 0.0;
00016 cmd_vel.rear_steering_angle = 0.0;
00017 publish_4ws(cmd_vel);
00018 ros::Duration(0.1).sleep();
00019
00020 nav_msgs::Odometry old_odom = getLastOdom();
00021
00022 cmd_vel.speed = 0.1;
00023 publish_4ws(cmd_vel);
00024
00025 ros::Duration(10.0).sleep();
00026
00027 nav_msgs::Odometry new_odom = getLastOdom();
00028
00029
00030 const double dx = new_odom.pose.pose.position.x - old_odom.pose.pose.position.x;
00031 const double dy = new_odom.pose.pose.position.y - old_odom.pose.pose.position.y;
00032 const double dz = new_odom.pose.pose.position.z - old_odom.pose.pose.position.z;
00033 EXPECT_NEAR(sqrt(dx*dx + dy*dy), 1.0, POSITION_TOLERANCE);
00034 EXPECT_LT(fabs(dz), EPS);
00035
00036
00037 double roll_old, pitch_old, yaw_old;
00038 double roll_new, pitch_new, yaw_new;
00039 tf::Matrix3x3(tfQuatFromGeomQuat(old_odom.pose.pose.orientation)).getRPY(roll_old, pitch_old, yaw_old);
00040 tf::Matrix3x3(tfQuatFromGeomQuat(new_odom.pose.pose.orientation)).getRPY(roll_new, pitch_new, yaw_new);
00041 EXPECT_LT(fabs(roll_new - roll_old), EPS);
00042 EXPECT_LT(fabs(pitch_new - pitch_old), EPS);
00043 EXPECT_LT(fabs(yaw_new - yaw_old), EPS);
00044 EXPECT_NEAR(fabs(new_odom.twist.twist.linear.x), cmd_vel.speed, EPS);
00045 EXPECT_LT(fabs(new_odom.twist.twist.linear.y), EPS);
00046 EXPECT_LT(fabs(new_odom.twist.twist.linear.z), EPS);
00047
00048 EXPECT_LT(fabs(new_odom.twist.twist.angular.x), EPS);
00049 EXPECT_LT(fabs(new_odom.twist.twist.angular.y), EPS);
00050 EXPECT_LT(fabs(new_odom.twist.twist.angular.z), EPS);
00051 }
00052
00053 TEST_F(FourWheelSteeringControllerTest, testCrab)
00054 {
00055
00056 while(!isControllerAlive())
00057 {
00058 ros::Duration(0.1).sleep();
00059 }
00060
00061 four_wheel_steering_msgs::FourWheelSteering cmd_vel;
00062 cmd_vel.speed = 0.0;
00063 cmd_vel.front_steering_angle = 0.0;
00064 cmd_vel.rear_steering_angle = 0.0;
00065 publish_4ws(cmd_vel);
00066 ros::Duration(0.1).sleep();
00067
00068 nav_msgs::Odometry old_odom = getLastOdom();
00069
00070 cmd_vel.speed = 0.2;
00071 cmd_vel.front_steering_angle = M_PI/8.0;
00072 cmd_vel.rear_steering_angle = cmd_vel.front_steering_angle;
00073 publish_4ws(cmd_vel);
00074
00075 double travel_time = 8.0;
00076 ros::Duration(travel_time).sleep();
00077
00078 nav_msgs::Odometry new_odom = getLastOdom();
00079
00080
00081 const double dx = new_odom.pose.pose.position.x - old_odom.pose.pose.position.x;
00082 const double dy = new_odom.pose.pose.position.y - old_odom.pose.pose.position.y;
00083 const double dz = new_odom.pose.pose.position.z - old_odom.pose.pose.position.z;
00084 EXPECT_NEAR(sqrt(dx*dx + dy*dy), cmd_vel.speed*travel_time, POSITION_TOLERANCE);
00085 EXPECT_NEAR(dx, cmd_vel.speed*travel_time*cos(cmd_vel.front_steering_angle), POSITION_TOLERANCE);
00086 EXPECT_NEAR(dy, cmd_vel.speed*travel_time*sin(cmd_vel.front_steering_angle), POSITION_TOLERANCE);
00087 EXPECT_LT(fabs(dz), EPS);
00088
00089
00090 double roll_old, pitch_old, yaw_old;
00091 double roll_new, pitch_new, yaw_new;
00092 tf::Matrix3x3(tfQuatFromGeomQuat(old_odom.pose.pose.orientation)).getRPY(roll_old, pitch_old, yaw_old);
00093 tf::Matrix3x3(tfQuatFromGeomQuat(new_odom.pose.pose.orientation)).getRPY(roll_new, pitch_new, yaw_new);
00094 EXPECT_LT(fabs(roll_new - roll_old), EPS);
00095 EXPECT_LT(fabs(pitch_new - pitch_old), EPS);
00096 EXPECT_LT(fabs(yaw_new - yaw_old), EPS);
00097 EXPECT_NEAR(fabs(new_odom.twist.twist.linear.x), cmd_vel.speed*cos(cmd_vel.front_steering_angle), EPS);
00098 EXPECT_NEAR(fabs(new_odom.twist.twist.linear.y), cmd_vel.speed*sin(cmd_vel.front_steering_angle), EPS);
00099 EXPECT_LT(fabs(new_odom.twist.twist.linear.z), EPS);
00100
00101 EXPECT_LT(fabs(new_odom.twist.twist.angular.x), EPS);
00102 EXPECT_LT(fabs(new_odom.twist.twist.angular.y), EPS);
00103 EXPECT_LT(fabs(new_odom.twist.twist.angular.z), EPS);
00104 }
00105
00106 TEST_F(FourWheelSteeringControllerTest, testSymmetricTurn)
00107 {
00108
00109 while(!isControllerAlive())
00110 {
00111 ros::Duration(0.1).sleep();
00112 }
00113
00114 four_wheel_steering_msgs::FourWheelSteering cmd_vel;
00115 cmd_vel.speed = 0.0;
00116 cmd_vel.front_steering_angle = 0.0;
00117 cmd_vel.rear_steering_angle = 0.0;
00118 publish_4ws(cmd_vel);
00119 ros::Duration(0.1).sleep();
00120
00121 nav_msgs::Odometry old_odom = getLastOdom();
00122
00123 cmd_vel.speed = M_PI/2.0;
00124
00125 double cmd_angular = M_PI/10.0;
00127 cmd_vel.front_steering_angle = 0.18776;
00128 cmd_vel.rear_steering_angle = -0.18776;
00129 publish_4ws(cmd_vel);
00130
00131 ros::Duration(10.0).sleep();
00132
00133 nav_msgs::Odometry new_odom = getLastOdom();
00134
00135
00136 const double dx = new_odom.pose.pose.position.x - old_odom.pose.pose.position.x;
00137 const double dy = new_odom.pose.pose.position.y - old_odom.pose.pose.position.y;
00138 const double dz = new_odom.pose.pose.position.z - old_odom.pose.pose.position.z;
00139 EXPECT_NEAR(sqrt(dx*dx + dy*dy), fabs(2*cmd_vel.speed/(cmd_angular)), POSITION_TOLERANCE);
00140 EXPECT_LT(fabs(dz), EPS);
00141
00142
00143 double roll_old, pitch_old, yaw_old;
00144 double roll_new, pitch_new, yaw_new;
00145 tf::Matrix3x3(tfQuatFromGeomQuat(old_odom.pose.pose.orientation)).getRPY(roll_old, pitch_old, yaw_old);
00146 tf::Matrix3x3(tfQuatFromGeomQuat(new_odom.pose.pose.orientation)).getRPY(roll_new, pitch_new, yaw_new);
00147 EXPECT_LT(fabs(roll_new - roll_old), EPS);
00148 EXPECT_LT(fabs(pitch_new - pitch_old), EPS);
00149 EXPECT_NEAR(fabs(yaw_new - yaw_old), M_PI, ORIENTATION_TOLERANCE);
00150
00151 EXPECT_NEAR(fabs(new_odom.twist.twist.linear.x), cmd_vel.speed, EPS);
00152 EXPECT_LT(fabs(new_odom.twist.twist.linear.y), EPS);
00153 EXPECT_LT(fabs(new_odom.twist.twist.linear.z), EPS);
00154
00155 EXPECT_LT(fabs(new_odom.twist.twist.angular.x), EPS);
00156 EXPECT_LT(fabs(new_odom.twist.twist.angular.y), EPS);
00157 EXPECT_NEAR(new_odom.twist.twist.angular.z, cmd_angular, EPS);
00158 }
00159
00160 TEST_F(FourWheelSteeringControllerTest, testNonSymmetricTurn)
00161 {
00162
00163 while(!isControllerAlive())
00164 {
00165 ros::Duration(0.1).sleep();
00166 }
00167
00168 four_wheel_steering_msgs::FourWheelSteering cmd_vel;
00169 cmd_vel.speed = 0.0;
00170 cmd_vel.front_steering_angle = 0.0;
00171 cmd_vel.rear_steering_angle = 0.0;
00172 publish_4ws(cmd_vel);
00173 ros::Duration(0.1).sleep();
00174
00175 nav_msgs::Odometry old_odom = getLastOdom();
00176
00177 cmd_vel.speed = M_PI/2.0;
00178
00179 double cmd_angular = -M_PI/10.0;
00182 cmd_vel.front_steering_angle = -0.21655;
00183 cmd_vel.rear_steering_angle = 0.15866;
00184 publish_4ws(cmd_vel);
00185
00186 ros::Duration(10.0).sleep();
00187
00188 nav_msgs::Odometry new_odom = getLastOdom();
00189
00190
00191 const double dx = new_odom.pose.pose.position.x - old_odom.pose.pose.position.x;
00192 const double dy = new_odom.pose.pose.position.y - old_odom.pose.pose.position.y;
00193 const double dz = new_odom.pose.pose.position.z - old_odom.pose.pose.position.z;
00194 EXPECT_NEAR(sqrt(dx*dx + dy*dy), fabs(2*cmd_vel.speed/(cmd_angular)), POSITION_TOLERANCE);
00195 EXPECT_LT(fabs(dz), EPS);
00196
00197
00198 double roll_old, pitch_old, yaw_old;
00199 double roll_new, pitch_new, yaw_new;
00200 tf::Matrix3x3(tfQuatFromGeomQuat(old_odom.pose.pose.orientation)).getRPY(roll_old, pitch_old, yaw_old);
00201 tf::Matrix3x3(tfQuatFromGeomQuat(new_odom.pose.pose.orientation)).getRPY(roll_new, pitch_new, yaw_new);
00202 EXPECT_LT(fabs(roll_new - roll_old), EPS);
00203 EXPECT_LT(fabs(pitch_new - pitch_old), EPS);
00204 EXPECT_NEAR(fabs(yaw_new - yaw_old), M_PI, ORIENTATION_TOLERANCE);
00205
00206 EXPECT_NEAR(sqrt(pow(new_odom.twist.twist.linear.x,2)+pow(new_odom.twist.twist.linear.y,2)),
00207 fabs(cmd_vel.speed), EPS);
00208 EXPECT_LT(fabs(new_odom.twist.twist.linear.z), EPS);
00209
00210 EXPECT_LT(fabs(new_odom.twist.twist.angular.x), EPS);
00211 EXPECT_LT(fabs(new_odom.twist.twist.angular.y), EPS);
00212 EXPECT_NEAR(new_odom.twist.twist.angular.z, cmd_angular, EPS);
00213 }
00214
00215 TEST_F(FourWheelSteeringControllerTest, testOdomFrame)
00216 {
00217
00218 while(!isControllerAlive())
00219 {
00220 ros::Duration(0.1).sleep();
00221 }
00222
00223 tf::TransformListener listener;
00224 ros::Duration(2.0).sleep();
00225
00226 EXPECT_TRUE(listener.frameExists("odom"));
00227 }
00228
00229 int main(int argc, char** argv)
00230 {
00231 testing::InitGoogleTest(&argc, argv);
00232 ros::init(argc, argv, "four_wheel_steering_4ws_cmd_test");
00233
00234 ros::AsyncSpinner spinner(1);
00235 spinner.start();
00236
00237 int ret = RUN_ALL_TESTS();
00238 spinner.stop();
00239 ros::shutdown();
00240 return ret;
00241 }