00001 00002 // Copyright (C) 2013, PAL Robotics S.L. 00003 // 00004 // Redistribution and use in source and binary forms, with or without 00005 // modification, are permitted provided that the following conditions are met: 00006 // * Redistributions of source code must retain the above copyright notice, 00007 // this list of conditions and the following disclaimer. 00008 // * Redistributions in binary form must reproduce the above copyright 00009 // notice, this list of conditions and the following disclaimer in the 00010 // documentation and/or other materials provided with the distribution. 00011 // * Neither the name of PAL Robotics, Inc. nor the names of its 00012 // contributors may be used to endorse or promote products derived from 00013 // this software without specific prior written permission. 00014 // 00015 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00016 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00017 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00018 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00019 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00020 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00021 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00022 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00023 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00024 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00025 // POSSIBILITY OF SUCH DAMAGE. 00027 00030 00031 #include "../common/include/test_common.h" 00032 00033 // TEST CASES 00034 TEST_F(SteerDriveControllerTest, testTimeout) 00035 { 00036 // wait for ROS 00037 while(!isControllerAlive()) 00038 { 00039 ros::Duration(0.1).sleep(); 00040 } 00041 // zero everything before test 00042 geometry_msgs::Twist cmd_vel; 00043 cmd_vel.linear.x = 0.0; 00044 cmd_vel.angular.z = 0.0; 00045 publish(cmd_vel); 00046 // give some time to the controller to react to the command 00047 ros::Duration(0.1).sleep(); 00048 // get initial odom 00049 nav_msgs::Odometry old_odom = getLastOdom(); 00050 // send a velocity command of 1 m/s 00051 cmd_vel.linear.x = 1.0; 00052 publish(cmd_vel); 00053 // wait a bit 00054 ros::Duration(3.0).sleep(); 00055 00056 nav_msgs::Odometry new_odom = getLastOdom(); 00057 00058 // check if the robot has stopped after 0.5s, thus covering less than 0.5s*1.0m.s-1 + some (big) tolerance 00059 EXPECT_LT(fabs(new_odom.pose.pose.position.x - old_odom.pose.pose.position.x), 0.8); 00060 } 00061 00062 int main(int argc, char** argv) 00063 { 00064 testing::InitGoogleTest(&argc, argv); 00065 ros::init(argc, argv, "steer_drive_controller_timeout_test"); 00066 00067 ros::AsyncSpinner spinner(1); 00068 spinner.start(); 00069 int ret = RUN_ALL_TESTS(); 00070 spinner.stop(); 00071 ros::shutdown(); 00072 return ret; 00073 }