four_wheel_steering_twist_cmd_test.cpp
Go to the documentation of this file.
00001 #include "test_common.h"
00002 #include <tf/transform_listener.h>
00003 
00004 // TEST CASES
00005 TEST_F(FourWheelSteeringControllerTest, testForward)
00006 {
00007   // wait for ROS
00008   while(!isControllerAlive())
00009   {
00010     ros::Duration(0.1).sleep();
00011   }
00012   // zero everything before test
00013   geometry_msgs::Twist cmd_vel;
00014   cmd_vel.linear.x = 0.0;
00015   cmd_vel.angular.z = 0.0;
00016   publish(cmd_vel);
00017   ros::Duration(0.1).sleep();
00018   // get initial odom
00019   nav_msgs::Odometry old_odom = getLastOdom();
00020   // send a velocity command of 0.1 m/s
00021   cmd_vel.linear.x = 0.1;
00022   publish(cmd_vel);
00023   // wait for 10s
00024   ros::Duration(10.0).sleep();
00025 
00026   nav_msgs::Odometry new_odom = getLastOdom();
00027 
00028   // check if the robot traveled 1 meter in XY plane, changes in z should be ~~0
00029   const double dx = new_odom.pose.pose.position.x - old_odom.pose.pose.position.x;
00030   const double dy = new_odom.pose.pose.position.y - old_odom.pose.pose.position.y;
00031   const double dz = new_odom.pose.pose.position.z - old_odom.pose.pose.position.z;
00032   EXPECT_NEAR(sqrt(dx*dx + dy*dy), 1.0, POSITION_TOLERANCE);
00033   EXPECT_LT(fabs(dz), EPS);
00034 
00035   // convert to rpy and test that way
00036   double roll_old, pitch_old, yaw_old;
00037   double roll_new, pitch_new, yaw_new;
00038   tf::Matrix3x3(tfQuatFromGeomQuat(old_odom.pose.pose.orientation)).getRPY(roll_old, pitch_old, yaw_old);
00039   tf::Matrix3x3(tfQuatFromGeomQuat(new_odom.pose.pose.orientation)).getRPY(roll_new, pitch_new, yaw_new);
00040   EXPECT_LT(fabs(roll_new - roll_old), EPS);
00041   EXPECT_LT(fabs(pitch_new - pitch_old), EPS);
00042   EXPECT_LT(fabs(yaw_new - yaw_old), EPS);
00043   EXPECT_NEAR(fabs(new_odom.twist.twist.linear.x), cmd_vel.linear.x, EPS);
00044   EXPECT_LT(fabs(new_odom.twist.twist.linear.y), EPS);
00045   EXPECT_LT(fabs(new_odom.twist.twist.linear.z), EPS);
00046 
00047   EXPECT_LT(fabs(new_odom.twist.twist.angular.x), EPS);
00048   EXPECT_LT(fabs(new_odom.twist.twist.angular.y), EPS);
00049   EXPECT_LT(fabs(new_odom.twist.twist.angular.z), EPS);
00050 }
00051 
00052 TEST_F(FourWheelSteeringControllerTest, testTurn)
00053 {
00054   // wait for ROS
00055   while(!isControllerAlive())
00056   {
00057     ros::Duration(0.1).sleep();
00058   }
00059   // zero everything before test
00060   geometry_msgs::Twist cmd_vel;
00061   cmd_vel.linear.x = 0.0;
00062   cmd_vel.angular.z = 0.0;
00063   publish(cmd_vel);
00064   ros::Duration(0.1).sleep();
00065   // get initial odom
00066   nav_msgs::Odometry old_odom = getLastOdom();
00067   // send a velocity command
00068   cmd_vel.linear.x = M_PI/2.0;
00069   cmd_vel.angular.z = M_PI/10.0;
00070   publish(cmd_vel);
00071   // wait for 10s to make a half turn
00072   ros::Duration(10.0).sleep();
00073 
00074   nav_msgs::Odometry new_odom = getLastOdom();
00075 
00076   // check if the robot traveled 20 meter in XY plane, changes in z should be ~~0
00077   const double dx = new_odom.pose.pose.position.x - old_odom.pose.pose.position.x;
00078   const double dy = new_odom.pose.pose.position.y - old_odom.pose.pose.position.y;
00079   const double dz = new_odom.pose.pose.position.z - old_odom.pose.pose.position.z;
00080   EXPECT_NEAR(sqrt(dx*dx + dy*dy), 2*cmd_vel.linear.x/cmd_vel.angular.z, POSITION_TOLERANCE);
00081   EXPECT_LT(fabs(dz), EPS);
00082 
00083   // convert to rpy and test that way
00084   double roll_old, pitch_old, yaw_old;
00085   double roll_new, pitch_new, yaw_new;
00086   tf::Matrix3x3(tfQuatFromGeomQuat(old_odom.pose.pose.orientation)).getRPY(roll_old, pitch_old, yaw_old);
00087   tf::Matrix3x3(tfQuatFromGeomQuat(new_odom.pose.pose.orientation)).getRPY(roll_new, pitch_new, yaw_new);
00088   EXPECT_LT(fabs(roll_new - roll_old), EPS);
00089   EXPECT_LT(fabs(pitch_new - pitch_old), EPS);
00090   EXPECT_NEAR(fabs(yaw_new - yaw_old), M_PI, ORIENTATION_TOLERANCE);
00091 
00092   EXPECT_NEAR(fabs(new_odom.twist.twist.linear.x), cmd_vel.linear.x, EPS);
00093   EXPECT_LT(fabs(new_odom.twist.twist.linear.y), EPS);
00094   EXPECT_LT(fabs(new_odom.twist.twist.linear.z), EPS);
00095 
00096   EXPECT_LT(fabs(new_odom.twist.twist.angular.x), EPS);
00097   EXPECT_LT(fabs(new_odom.twist.twist.angular.y), EPS);
00098   EXPECT_NEAR(fabs(new_odom.twist.twist.angular.z), cmd_vel.angular.z, EPS);
00099 }
00100 
00101 TEST_F(FourWheelSteeringControllerTest, testOdomFrame)
00102 {
00103   // wait for ROS
00104   while(!isControllerAlive())
00105   {
00106     ros::Duration(0.1).sleep();
00107   }
00108   // set up tf listener
00109   tf::TransformListener listener;
00110   ros::Duration(2.0).sleep();
00111   // check the odom frame exist
00112   EXPECT_TRUE(listener.frameExists("odom"));
00113 }
00114 
00115 int main(int argc, char** argv)
00116 {
00117   testing::InitGoogleTest(&argc, argv);
00118   ros::init(argc, argv, "four_wheel_steering_twist_cmd_test");
00119 
00120   ros::AsyncSpinner spinner(1);
00121   spinner.start();
00122   //ros::Duration(0.5).sleep();
00123   int ret = RUN_ALL_TESTS();
00124   spinner.stop();
00125   ros::shutdown();
00126   return ret;
00127 }


four_wheel_steering_controller
Author(s): Vincent Rousseau
autogenerated on Sat Jun 8 2019 20:06:24