diff_drive_limits_test.cpp
Go to the documentation of this file.
1 // Copyright (C) 2013, PAL Robotics S.L.
3 //
4 // Redistribution and use in source and binary forms, with or without
5 // modification, are permitted provided that the following conditions are met:
6 // * Redistributions of source code must retain the above copyright notice,
7 // this list of conditions and the following disclaimer.
8 // * Redistributions in binary form must reproduce the above copyright
9 // notice, this list of conditions and the following disclaimer in the
10 // documentation and/or other materials provided with the distribution.
11 // * Neither the name of PAL Robotics, Inc. nor the names of its
12 // contributors may be used to endorse or promote products derived from
13 // this software without specific prior written permission.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
16 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
19 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
20 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
21 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
22 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
23 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
24 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
25 // POSSIBILITY OF SUCH DAMAGE.
27 
29 
30 #include "test_common.h"
31 
32 // TEST CASES
33 TEST_F(DiffDriveControllerTest, testLinearJerkLimits)
34 {
35  // wait for ROS
36  while(!isControllerAlive())
37  {
38  ros::Duration(0.1).sleep();
39  }
40  // zero everything before test
41  geometry_msgs::Twist cmd_vel;
42  cmd_vel.linear.x = 0.0;
43  cmd_vel.angular.z = 0.0;
44  publish(cmd_vel);
45  ros::Duration(2.0).sleep();
46  // get initial odom
47  nav_msgs::Odometry old_odom = getLastOdom();
48  // send a big command
49  cmd_vel.linear.x = 10.0;
50  publish(cmd_vel);
51  // wait for a while
52  ros::Duration(0.5).sleep();
53 
54  nav_msgs::Odometry new_odom = getLastOdom();
55 
56  // check if the robot speed is now 0.37m.s-1
57  EXPECT_NEAR(new_odom.twist.twist.linear.x, 0.37, JERK_LINEAR_VELOCITY_TOLERANCE);
58  EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), EPS);
59 
60  cmd_vel.linear.x = 0.0;
61  publish(cmd_vel);
62 }
63 
64 TEST_F(DiffDriveControllerTest, testLinearAccelerationLimits)
65 {
66  // wait for ROS
67  while(!isControllerAlive())
68  {
69  ros::Duration(0.1).sleep();
70  }
71  // zero everything before test
72  geometry_msgs::Twist cmd_vel;
73  cmd_vel.linear.x = 0.0;
74  cmd_vel.angular.z = 0.0;
75  publish(cmd_vel);
76  ros::Duration(2.0).sleep();
77  // get initial odom
78  nav_msgs::Odometry old_odom = getLastOdom();
79  // send a big command
80  cmd_vel.linear.x = 10.0;
81  publish(cmd_vel);
82  // wait for a while
83  ros::Duration(0.5).sleep();
84 
85  nav_msgs::Odometry new_odom = getLastOdom();
86 
87  // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s
88  EXPECT_LT(fabs(new_odom.twist.twist.linear.x - old_odom.twist.twist.linear.x), 0.5 + VELOCITY_TOLERANCE);
89  EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), EPS);
90 
91  cmd_vel.linear.x = 0.0;
92  publish(cmd_vel);
93 }
94 
95 TEST_F(DiffDriveControllerTest, testLinearVelocityLimits)
96 {
97  // wait for ROS
98  while(!isControllerAlive())
99  {
100  ros::Duration(0.1).sleep();
101  }
102  // zero everything before test
103  geometry_msgs::Twist cmd_vel;
104  cmd_vel.linear.x = 0.0;
105  cmd_vel.angular.z = 0.0;
106  publish(cmd_vel);
107  ros::Duration(2.0).sleep();
108  // get initial odom
109  nav_msgs::Odometry old_odom = getLastOdom();
110  // send a big command
111  cmd_vel.linear.x = 10.0;
112  publish(cmd_vel);
113  // wait for a while
114  ros::Duration(5.0).sleep();
115 
116  nav_msgs::Odometry new_odom = getLastOdom();
117 
118  // check if the robot speed is now 1.0 m.s-1, the limit
119  EXPECT_LT(fabs(new_odom.twist.twist.linear.x - old_odom.twist.twist.linear.x), 1.0 + VELOCITY_TOLERANCE);
120  EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), EPS);
121 
122  cmd_vel.linear.x = 0.0;
123  publish(cmd_vel);
124 }
125 
126 /* This test has been failing on Travis for a long time due to timing issues however it works well when ran manually
127 TEST_F(DiffDriveControllerTest, testAngularJerkLimits)
128 {
129  // wait for ROS
130  while(!isControllerAlive())
131  {
132  ros::Duration(0.1).sleep();
133  }
134  // zero everything before test
135  geometry_msgs::Twist cmd_vel;
136  cmd_vel.linear.x = 0.0;
137  cmd_vel.angular.z = 0.0;
138  publish(cmd_vel);
139  ros::Duration(2.0).sleep();
140  // get initial odom
141  nav_msgs::Odometry old_odom = getLastOdom();
142  // send a big command
143  cmd_vel.angular.z = 10.0;
144  publish(cmd_vel);
145  // wait for a while
146  ros::Duration(0.5).sleep();
147 
148  nav_msgs::Odometry new_odom = getLastOdom();
149 
150  // check if the robot speed is now 0.7rad.s-1
151  EXPECT_NEAR(new_odom.twist.twist.angular.z, 0.7, JERK_ANGULAR_VELOCITY_TOLERANCE);
152  EXPECT_LT(fabs(new_odom.twist.twist.linear.x - old_odom.twist.twist.linear.x), EPS);
153 
154  cmd_vel.angular.z = 0.0;
155  publish(cmd_vel);
156 }
157 */
158 
159 TEST_F(DiffDriveControllerTest, testAngularAccelerationLimits)
160 {
161  // wait for ROS
162  while(!isControllerAlive())
163  {
164  ros::Duration(0.1).sleep();
165  }
166  // zero everything before test
167  geometry_msgs::Twist cmd_vel;
168  cmd_vel.linear.x = 0.0;
169  cmd_vel.angular.z = 0.0;
170  publish(cmd_vel);
171  ros::Duration(2.0).sleep();
172  // get initial odom
173  nav_msgs::Odometry old_odom = getLastOdom();
174  // send a big command
175  cmd_vel.angular.z = 10.0;
176  publish(cmd_vel);
177  // wait for a while
178  ros::Duration(0.5).sleep();
179 
180  nav_msgs::Odometry new_odom = getLastOdom();
181 
182  // check if the robot speed is now 1.0rad.s-1, which is 2.0rad.s-2 * 0.5s
183  EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), 1.0 + VELOCITY_TOLERANCE);
184  EXPECT_LT(fabs(new_odom.twist.twist.linear.x - old_odom.twist.twist.linear.x), EPS);
185 
186  cmd_vel.angular.z = 0.0;
187  publish(cmd_vel);
188 }
189 
190 TEST_F(DiffDriveControllerTest, testAngularVelocityLimits)
191 {
192  // wait for ROS
193  while(!isControllerAlive())
194  {
195  ros::Duration(0.1).sleep();
196  }
197  // zero everything before test
198  geometry_msgs::Twist cmd_vel;
199  cmd_vel.linear.x = 0.0;
200  cmd_vel.angular.z = 0.0;
201  publish(cmd_vel);
202  ros::Duration(2.0).sleep();
203  // get initial odom
204  nav_msgs::Odometry old_odom = getLastOdom();
205  // send a big command
206  cmd_vel.angular.z = 10.0;
207  publish(cmd_vel);
208  // wait for a while
209  ros::Duration(5.0).sleep();
210 
211  nav_msgs::Odometry new_odom = getLastOdom();
212 
213  // check if the robot speed is now 2.0rad.s-1, the limit
214  EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), 2.0 + VELOCITY_TOLERANCE);
215  EXPECT_LT(fabs(new_odom.twist.twist.linear.x - old_odom.twist.twist.linear.x), EPS);
216 
217  cmd_vel.angular.z = 0.0;
218  publish(cmd_vel);
219 }
220 
221 int main(int argc, char** argv)
222 {
223  testing::InitGoogleTest(&argc, argv);
224  ros::init(argc, argv, "diff_drive_limits_test");
225 
227  spinner.start();
228  //ros::Duration(0.5).sleep();
229  int ret = RUN_ALL_TESTS();
230  spinner.stop();
231  ros::shutdown();
232  return ret;
233 }
bool sleep() const
const double VELOCITY_TOLERANCE
Definition: test_common.h:45
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
const double EPS
Definition: test_common.h:43
TEST_F(DiffDriveControllerTest, testLinearJerkLimits)
void spinner()
const double JERK_LINEAR_VELOCITY_TOLERANCE
Definition: test_common.h:46
int main(int argc, char **argv)
ROSCPP_DECL void shutdown()


diff_drive_controller
Author(s): Bence Magyar
autogenerated on Sat Apr 18 2020 03:58:05