ackermann_steering_controller_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 
30 
31 #include "../common/include/test_common.h"
32 
33 // TEST CASES
35 {
36  // wait for ROS
37  while(!isControllerAlive())
38  {
39  ros::Duration(0.1).sleep();
40  }
41  // zero everything before test
42  geometry_msgs::Twist cmd_vel;
43  cmd_vel.linear.x = 0.0;
44  cmd_vel.angular.z = 0.0;
45  publish(cmd_vel);
46  ros::Duration(2.0).sleep();
47  // get initial odom
48  nav_msgs::Odometry old_odom = getLastOdom();
49  // send a big command
50  cmd_vel.linear.x = 10.0;
51  publish(cmd_vel);
52  // wait for a while
53  ros::Duration(0.5).sleep();
54 
55  nav_msgs::Odometry new_odom = getLastOdom();
56 
57  // check if the robot speed is now 0.37m.s-1
58  EXPECT_NEAR(new_odom.twist.twist.linear.x, 0.37, JERK_LINEAR_VELOCITY_TOLERANCE);
59  EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), EPS);
60 
61  cmd_vel.linear.x = 0.0;
62  publish(cmd_vel);
63 }
64 
65 TEST_F(AckermannSteeringControllerTest, testLinearAccelerationLimits)
66 {
67  // wait for ROS
68  while(!isControllerAlive())
69  {
70  ros::Duration(0.1).sleep();
71  }
72  // zero everything before test
73  geometry_msgs::Twist cmd_vel;
74  cmd_vel.linear.x = 0.0;
75  cmd_vel.angular.z = 0.0;
76  publish(cmd_vel);
77  ros::Duration(2.0).sleep();
78  // get initial odom
79  nav_msgs::Odometry old_odom = getLastOdom();
80  // send a big command
81  cmd_vel.linear.x = 10.0;
82  publish(cmd_vel);
83  // wait for a while
84  ros::Duration(0.5).sleep();
85 
86  nav_msgs::Odometry new_odom = getLastOdom();
87 
88  // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s
89  EXPECT_LT(fabs(new_odom.twist.twist.linear.x - old_odom.twist.twist.linear.x), 0.5 + VELOCITY_TOLERANCE);
90  EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), EPS);
91 
92  cmd_vel.linear.x = 0.0;
93  publish(cmd_vel);
94 }
95 
96 TEST_F(AckermannSteeringControllerTest, testLinearVelocityLimits)
97 {
98  // wait for ROS
99  while(!isControllerAlive())
100  {
101  ros::Duration(0.1).sleep();
102  }
103  // zero everything before test
104  geometry_msgs::Twist cmd_vel;
105  cmd_vel.linear.x = 0.0;
106  cmd_vel.angular.z = 0.0;
107  publish(cmd_vel);
108  ros::Duration(2.0).sleep();
109  // get initial odom
110  nav_msgs::Odometry old_odom = getLastOdom();
111  // send a big command
112  cmd_vel.linear.x = 10.0;
113  publish(cmd_vel);
114  // wait for a while
115  ros::Duration(5.0).sleep();
116 
117  nav_msgs::Odometry new_odom = getLastOdom();
118 
119  // check if the robot speed is now 1.0 m.s-1, the limit
120  EXPECT_LT(fabs(new_odom.twist.twist.linear.x - old_odom.twist.twist.linear.x), 1.0 + VELOCITY_TOLERANCE);
121  EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), EPS);
122 
123  cmd_vel.linear.x = 0.0;
124  publish(cmd_vel);
125 }
126 
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  // send linear command too
145  // because sending only angular command doesn't actuate wheels for steer drive mechanism
146  cmd_vel.linear.x = 0.1;
147  publish(cmd_vel);
148  // wait for a while
149  ros::Duration(0.5).sleep();
150 
151  nav_msgs::Odometry new_odom = getLastOdom();
152 
153  // check if the robot speed is now 0.18rad.s-1
154  EXPECT_NEAR(new_odom.twist.twist.angular.z, 0.18, JERK_ANGULAR_VELOCITY_TOLERANCE);
155  // check if the robot speed is now 0.1m.s-1
156  EXPECT_NEAR(new_odom.twist.twist.linear.x, 0.1, VELOCITY_TOLERANCE);
157 
158  cmd_vel.angular.z = 0.0;
159  publish(cmd_vel);
160 }
161 
162 TEST_F(AckermannSteeringControllerTest, testAngularAccelerationLimits)
163 {
164  // wait for ROS
165  while(!isControllerAlive())
166  {
167  ros::Duration(0.1).sleep();
168  }
169  // zero everything before test
170  geometry_msgs::Twist cmd_vel;
171  cmd_vel.linear.x = 0.0;
172  cmd_vel.angular.z = 0.0;
173  publish(cmd_vel);
174  ros::Duration(2.0).sleep();
175  // get initial odom
176  nav_msgs::Odometry old_odom = getLastOdom();
177  // send a big command
178  cmd_vel.angular.z = 10.0;
179  // send linear command too
180  // because sending only angular command doesn't actuate wheels for steer drive mechanism
181  cmd_vel.linear.x = 0.1;
182  publish(cmd_vel);
183  // wait for a while
184  ros::Duration(0.5).sleep();
185 
186  nav_msgs::Odometry new_odom = getLastOdom();
187 
188  // check if the robot speed is now 0.25rad.s-1, which is 0.5.s-2 * 0.5s
189  EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), 0.25 + VELOCITY_TOLERANCE);
190  // check if the robot speed is now 0.1m.s-1
191  EXPECT_LT(fabs(new_odom.twist.twist.linear.x - old_odom.twist.twist.linear.x), 0.1 + VELOCITY_TOLERANCE);
192 
193  cmd_vel.angular.z = 0.0;
194  publish(cmd_vel);
195 }
196 
197 TEST_F(AckermannSteeringControllerTest, testAngularVelocityLimits)
198 {
199  // wait for ROS
200  while(!isControllerAlive())
201  {
202  ros::Duration(0.1).sleep();
203  }
204  // zero everything before test
205  geometry_msgs::Twist cmd_vel;
206  cmd_vel.linear.x = 0.0;
207  cmd_vel.angular.z = 0.0;
208  publish(cmd_vel);
209  ros::Duration(2.0).sleep();
210  // get initial odom
211  nav_msgs::Odometry old_odom = getLastOdom();
212  cmd_vel.angular.z = 10.0;
213  // send linear command too
214  // because sending only angular command doesn't actuate wheels for steer drive mechanism
215  cmd_vel.linear.x = 0.1;
216  publish(cmd_vel);
217  // wait for a while
218  ros::Duration(5.0).sleep();
219 
220  nav_msgs::Odometry new_odom = getLastOdom();
221 
222  // check if the robot speed is now 0.5rad.s-1, the limit
223  EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), 0.5 + ANGULAR_VELOCITY_TOLERANCE);
224  // check if the robot speed is now 0.1m.s-1
225  EXPECT_LT(fabs(new_odom.twist.twist.linear.x - old_odom.twist.twist.linear.x), 0.1 + VELOCITY_TOLERANCE);
226 
227  cmd_vel.angular.z = 0.0;
228  publish(cmd_vel);
229 }
230 
231 int main(int argc, char** argv)
232 {
233  testing::InitGoogleTest(&argc, argv);
234  ros::init(argc, argv, "ackermann_steering_controller_limits_test");
235 
237  spinner.start();
238  //ros::Duration(0.5).sleep();
239  int ret = RUN_ALL_TESTS();
240  spinner.stop();
241  ros::shutdown();
242  return ret;
243 }
bool sleep() const
const double VELOCITY_TOLERANCE
Definition: test_common.h:46
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
TEST_F(AckermannSteeringControllerTest, testLinearJerkLimits)
const double EPS
Definition: test_common.h:44
void spinner()
const double JERK_ANGULAR_VELOCITY_TOLERANCE
Definition: test_common.h:49
int main(int argc, char **argv)
const double JERK_LINEAR_VELOCITY_TOLERANCE
Definition: test_common.h:48
const double ANGULAR_VELOCITY_TOLERANCE
Definition: test_common.h:47
ROSCPP_DECL void shutdown()


ackermann_steering_controller
Author(s): Masaru Morita
autogenerated on Thu Apr 11 2019 03:08:13