test_safety_limiter.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2018-2019, the neonavigation authors
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the copyright holder nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <algorithm>
31 #include <cmath>
32 #include <string>
33 
34 #include <ros/ros.h>
35 
36 #include <diagnostic_msgs/DiagnosticStatus.h>
37 #include <geometry_msgs/Twist.h>
38 #include <sensor_msgs/PointCloud2.h>
39 
40 #include <test_safety_limiter_base.h>
41 
42 #include <gtest/gtest.h>
43 
44 TEST_F(SafetyLimiterTest, Timeouts)
45 {
46  ros::Rate wait(10.0);
47 
48  for (int with_cloud = 0; with_cloud < 3; ++with_cloud)
49  {
50  for (int with_watchdog_reset = 0; with_watchdog_reset < 2; ++with_watchdog_reset)
51  {
52  const std::string test_condition =
53  "with_watchdog_reset: " + std::to_string(with_watchdog_reset) +
54  ", with_cloud: " + std::to_string(with_cloud);
55  ros::Duration(0.3).sleep();
56 
57  cmd_vel_.reset();
58  for (int i = 0; i < 20; ++i)
59  {
60  ASSERT_TRUE(ros::ok());
61 
62  if (with_watchdog_reset > 0)
63  publishWatchdogReset();
64 
65  if (with_cloud > 0)
66  {
67  // cloud must have timestamp, otherwise the robot stops
68  if (with_cloud > 1)
69  publishSinglePointPointcloud2(1000, 1000, 0, "base_link", ros::Time::now());
70  else
71  publishSinglePointPointcloud2(1000, 1000, 0, "base_link", ros::Time());
72  }
73 
74  const float vel = 0.1;
75  const float ang_vel = 0.2;
76  publishTwist(vel, ang_vel);
77  broadcastTF("odom", "base_link", 0.0, 0.0);
78 
79  wait.sleep();
80  ros::spinOnce();
81 
82  if (i > 5 && cmd_vel_)
83  {
84  if (with_watchdog_reset > 0 && with_cloud > 1)
85  {
86  ASSERT_EQ(cmd_vel_->linear.x, vel) << test_condition;
87  ASSERT_EQ(cmd_vel_->linear.y, 0.0) << test_condition;
88  ASSERT_EQ(cmd_vel_->linear.z, 0.0) << test_condition;
89  ASSERT_EQ(cmd_vel_->angular.x, 0.0) << test_condition;
90  ASSERT_EQ(cmd_vel_->angular.y, 0.0) << test_condition;
91  ASSERT_EQ(cmd_vel_->angular.z, ang_vel) << test_condition;
92  }
93  else
94  {
95  ASSERT_EQ(cmd_vel_->linear.x, 0.0) << test_condition;
96  ASSERT_EQ(cmd_vel_->linear.y, 0.0) << test_condition;
97  ASSERT_EQ(cmd_vel_->linear.z, 0.0) << test_condition;
98  ASSERT_EQ(cmd_vel_->angular.x, 0.0) << test_condition;
99  ASSERT_EQ(cmd_vel_->angular.y, 0.0) << test_condition;
100  ASSERT_EQ(cmd_vel_->angular.z, 0.0) << test_condition;
101  }
102  }
103  }
104 
105  ASSERT_TRUE(hasStatus()) << test_condition;
106  EXPECT_EQ(with_cloud > 1, status_->is_cloud_available) << test_condition;
107  EXPECT_EQ(status_->stuck_started_since, ros::Time(0)) << test_condition;
108 
109  if (with_watchdog_reset > 0 && with_cloud > 1)
110  {
111  ASSERT_TRUE(hasDiag()) << test_condition;
112  EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::OK, diag_->status[0].level)
113  << test_condition << ", "
114  << "message: " << diag_->status[0].message;
115 
116  EXPECT_FALSE(status_->has_watchdog_timed_out) << test_condition;
117  }
118  else
119  {
120  ASSERT_TRUE(hasDiag()) << test_condition;
121  EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::ERROR, diag_->status[0].level)
122  << test_condition << ", "
123  << "message: " << diag_->status[0].message;
124  if (with_watchdog_reset == 0)
125  {
126  EXPECT_TRUE(status_->has_watchdog_timed_out) << test_condition;
127  }
128  }
129  }
130  }
131 }
132 
133 TEST_F(SafetyLimiterTest, CloudBuffering)
134 {
135  ros::Rate wait(60.0);
136 
137  // Skip initial state
138  for (int i = 0; i < 30 && ros::ok(); ++i)
139  {
140  publishSinglePointPointcloud2(0.5, 0, 0, "base_link", ros::Time::now());
141  publishWatchdogReset();
142 
143  wait.sleep();
144  ros::spinOnce();
145  }
146 
147  bool received = false;
148  bool en = false;
149  // 1.0 m/ss, obstacle at 0.5 m: limited to 1.0 m/s (t_margin: 0)
150 
151  for (int i = 0; i < 60 * 6 && ros::ok(); ++i)
152  {
153  // enable check after two cycles of safety_limiter
154  if (i > 8)
155  en = true;
156  // safety_limiter: 15 hz, cloud publish: 60 hz
157  // safety_limiter must check 4 buffered clouds
158  // 3/4 of pointclouds have collision point
159  if ((i % 4) == 0)
160  publishSinglePointPointcloud2(10, 0, 0, "base_link", ros::Time::now());
161  else
162  publishSinglePointPointcloud2(0.5, 0, 0, "base_link", ros::Time::now());
163 
164  publishWatchdogReset();
165  publishTwist(2.0, 0);
166  broadcastTF("odom", "base_link", 0.0, 0.0);
167 
168  wait.sleep();
169  ros::spinOnce();
170  if (en && cmd_vel_)
171  {
172  received = true;
173  ASSERT_NEAR(cmd_vel_->linear.x, 1.0, 1e-1);
174  }
175  }
176  ASSERT_TRUE(received);
177 }
178 
179 TEST_F(SafetyLimiterTest, SafetyLimitLinear)
180 {
181  ros::Rate wait(20.0);
182 
183  // Skip initial state
184  for (int i = 0; i < 10 && ros::ok(); ++i)
185  {
186  publishSinglePointPointcloud2(0.5, 0, 0, "base_link", ros::Time::now());
187  publishWatchdogReset();
188 
189  wait.sleep();
190  ros::spinOnce();
191  }
192 
193  for (float vel = 0.0; vel < 2.0; vel += 0.4)
194  {
195  // 1.0 m/ss, obstacle at 0.5 m: limited to 1.0 m/s
196  bool received = false;
197  bool en = false;
198 
199  for (int i = 0; i < 10 && ros::ok(); ++i)
200  {
201  if (i > 5)
202  en = true;
203  publishSinglePointPointcloud2(0.5, 0, 0, "base_link", ros::Time::now());
204  publishWatchdogReset();
205  publishTwist(vel, ((i % 5) - 2.0) * 0.01);
206  broadcastTF("odom", "base_link", 0.0, 0.0);
207 
208  wait.sleep();
209  ros::spinOnce();
210  if (en && cmd_vel_)
211  {
212  received = true;
213  const float expected_vel = std::min<float>(vel, 1.0);
214  ASSERT_NEAR(cmd_vel_->linear.x, expected_vel, 1e-1);
215  }
216  }
217  ASSERT_TRUE(hasDiag());
218  EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::OK, diag_->status[0].level)
219  << "message: " << diag_->status[0].message;
220 
221  ASSERT_TRUE(hasStatus());
222  EXPECT_TRUE(status_->is_cloud_available);
223  EXPECT_FALSE(status_->has_watchdog_timed_out);
224  EXPECT_EQ(status_->stuck_started_since, ros::Time(0));
225 
226  ASSERT_TRUE(received);
227  }
228 }
229 
230 TEST_F(SafetyLimiterTest, SafetyLimitLinearBackward)
231 {
232  ros::Rate wait(20.0);
233 
234  // Skip initial state
235  for (int i = 0; i < 10 && ros::ok(); ++i)
236  {
237  publishSinglePointPointcloud2(-2.5, 0, 0, "base_link", ros::Time::now());
238  publishWatchdogReset();
239 
240  wait.sleep();
241  ros::spinOnce();
242  }
243 
244  for (float vel = 0.0; vel > -2.0; vel -= 0.4)
245  {
246  // 1.0 m/ss, obstacle at -2.5 m: limited to -1.0 m/s
247  bool received = false;
248  bool en = false;
249 
250  for (int i = 0; i < 10 && ros::ok(); ++i)
251  {
252  if (i > 5)
253  en = true;
254  publishSinglePointPointcloud2(-2.5, 0, 0, "base_link", ros::Time::now());
255  publishWatchdogReset();
256  publishTwist(vel, ((i % 5) - 2.0) * 0.01);
257  broadcastTF("odom", "base_link", 0.0, 0.0);
258 
259  wait.sleep();
260  ros::spinOnce();
261  if (en && cmd_vel_)
262  {
263  received = true;
264  const float expected_vel = std::max<float>(vel, -1.0);
265  ASSERT_NEAR(cmd_vel_->linear.x, expected_vel, 1e-1);
266  }
267  }
268  ASSERT_TRUE(hasDiag());
269  EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::OK, diag_->status[0].level)
270  << "message: " << diag_->status[0].message;
271 
272  ASSERT_TRUE(hasStatus());
273  EXPECT_TRUE(status_->is_cloud_available);
274  EXPECT_FALSE(status_->has_watchdog_timed_out);
275  EXPECT_EQ(status_->stuck_started_since, ros::Time(0));
276 
277  ASSERT_TRUE(received);
278  }
279 }
280 
281 TEST_F(SafetyLimiterTest, SafetyLimitLinearEscape)
282 {
283  ros::Rate wait(20.0);
284 
285  // Skip initial state
286  for (int i = 0; i < 10 && ros::ok(); ++i)
287  {
288  publishSinglePointPointcloud2(-0.05, 0, 0, "base_link", ros::Time::now());
289  publishWatchdogReset();
290 
291  wait.sleep();
292  ros::spinOnce();
293  }
294 
295  const float vel_ref[] = { -0.2, -0.4, 0.2, 0.4 };
296  for (const float vel : vel_ref)
297  {
298  // 1.0 m/ss, obstacle at -0.05 m (already in collision): escape motion must be allowed
299  bool received = false;
300  bool en = false;
301 
302  for (int i = 0; i < 10 && ros::ok(); ++i)
303  {
304  if (i > 5)
305  en = true;
306  publishSinglePointPointcloud2(-0.05, 0, 0, "base_link", ros::Time::now());
307  publishWatchdogReset();
308  publishTwist(vel, 0.0);
309  broadcastTF("odom", "base_link", 0.0, 0.0);
310 
311  wait.sleep();
312  ros::spinOnce();
313  if (en && cmd_vel_)
314  {
315  received = true;
316  if (vel < 0)
317  {
318  // escaping from collision must be allowed
319  ASSERT_NEAR(cmd_vel_->linear.x, vel, 1e-1);
320  }
321  else
322  {
323  // colliding motion must be limited
324  ASSERT_NEAR(cmd_vel_->linear.x, 0.0, 1e-1);
325  }
326  }
327  }
328  if (vel < 0)
329  {
330  ASSERT_TRUE(hasDiag());
331  EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::OK, diag_->status[0].level)
332  << "message: " << diag_->status[0].message;
333  }
334  else
335  {
336  ASSERT_TRUE(hasDiag());
337  EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::WARN, diag_->status[0].level)
338  << "message: " << diag_->status[0].message;
339  }
340 
341  ASSERT_TRUE(hasStatus());
342  EXPECT_TRUE(status_->is_cloud_available);
343  EXPECT_FALSE(status_->has_watchdog_timed_out);
344  EXPECT_NE(status_->stuck_started_since, ros::Time(0));
345 
346  ASSERT_TRUE(received);
347  }
348 }
349 
350 TEST_F(SafetyLimiterTest, SafetyLimitAngular)
351 {
352  ros::Rate wait(20.0);
353 
354  // Skip initial state
355  for (int i = 0; i < 10 && ros::ok(); ++i)
356  {
357  publishSinglePointPointcloud2(-1, -1, 0, "base_link", ros::Time::now());
358  publishWatchdogReset();
359 
360  wait.sleep();
361  ros::spinOnce();
362  }
363 
364  for (float vel = 0.0; vel < M_PI; vel += M_PI / 10)
365  {
366  // pi/2 rad/ss, obstacle at pi/4 rad: limited to pi/2 rad/s
367  bool received = false;
368  bool en = false;
369 
370  for (int i = 0; i < 10 && ros::ok(); ++i)
371  {
372  if (i > 5)
373  en = true;
374  publishSinglePointPointcloud2(-1, -1.1, 0, "base_link", ros::Time::now());
375  publishWatchdogReset();
376  publishTwist((i % 3) * 0.01, vel);
377  broadcastTF("odom", "base_link", 0.0, 0.0);
378 
379  wait.sleep();
380  ros::spinOnce();
381  if (en && cmd_vel_)
382  {
383  received = true;
384  const float expected_vel = std::min<float>(vel, M_PI / 2);
385  ASSERT_NEAR(cmd_vel_->angular.z, expected_vel, M_PI / 20);
386  }
387  }
388  ASSERT_TRUE(hasDiag());
389  EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::OK, diag_->status[0].level)
390  << "message: " << diag_->status[0].message;
391 
392  ASSERT_TRUE(hasStatus());
393  EXPECT_TRUE(status_->is_cloud_available);
394  EXPECT_FALSE(status_->has_watchdog_timed_out);
395  EXPECT_EQ(status_->stuck_started_since, ros::Time(0));
396 
397  ASSERT_TRUE(received);
398  }
399 }
400 
401 TEST_F(SafetyLimiterTest, SafetyLimitAngularEscape)
402 {
403  ros::Rate wait(20.0);
404 
405  // Skip initial state
406  for (int i = 0; i < 10 && ros::ok(); ++i)
407  {
408  publishSinglePointPointcloud2(-1, -0.09, 0, "base_link", ros::Time::now());
409  publishWatchdogReset();
410 
411  wait.sleep();
412  ros::spinOnce();
413  }
414 
415  const float vel_ref[] = { -0.2, -0.4, 0.2, 0.4 };
416  for (const float vel : vel_ref)
417  {
418  // already colliding at rear-right side: only positive rotation must be allowed
419  bool received = false;
420  bool en = false;
421 
422  for (int i = 0; i < 10 && ros::ok(); ++i)
423  {
424  if (i > 5)
425  en = true;
426  publishSinglePointPointcloud2(-1, -0.09, 0, "base_link", ros::Time::now());
427  publishWatchdogReset();
428  publishTwist(0.0, vel);
429  broadcastTF("odom", "base_link", 0.0, 0.0);
430 
431  wait.sleep();
432  ros::spinOnce();
433  if (en && cmd_vel_)
434  {
435  received = true;
436  if (vel < 0)
437  {
438  // escaping from collision must be allowed
439  ASSERT_NEAR(cmd_vel_->angular.z, vel, 1e-1);
440  }
441  else
442  {
443  // colliding motion must be limited
444  ASSERT_NEAR(cmd_vel_->angular.z, 0.0, 1e-1);
445  }
446  }
447  }
448  if (vel < 0)
449  {
450  ASSERT_TRUE(hasDiag());
451  EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::OK, diag_->status[0].level)
452  << "message: " << diag_->status[0].message;
453  }
454  else
455  {
456  ASSERT_TRUE(hasDiag());
457  EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::WARN, diag_->status[0].level)
458  << "message: " << diag_->status[0].message;
459  }
460 
461  ASSERT_TRUE(hasStatus());
462  EXPECT_TRUE(status_->is_cloud_available);
463  EXPECT_FALSE(status_->has_watchdog_timed_out);
464  EXPECT_NE(status_->stuck_started_since, ros::Time(0));
465 
466  ASSERT_TRUE(received);
467  }
468 }
469 
470 TEST_F(SafetyLimiterTest, NoCollision)
471 {
472  ros::Rate wait(20.0);
473 
474  for (float vel = 0.0; vel < 1.0; vel += 0.2)
475  {
476  for (float ang_vel = 0.0; ang_vel < 1.0; ang_vel += 0.2)
477  {
478  bool received = false;
479  bool en = false;
480 
481  for (int i = 0; i < 10 && ros::ok(); ++i)
482  {
483  if (i > 5)
484  en = true;
485  publishSinglePointPointcloud2(1000, 1000, 0, "base_link", ros::Time::now());
486  publishWatchdogReset();
487  publishTwist(vel, ang_vel);
488  broadcastTF("odom", "base_link", 0.0, 0.0);
489 
490  wait.sleep();
491  ros::spinOnce();
492  if (en && cmd_vel_)
493  {
494  received = true;
495  ASSERT_NEAR(cmd_vel_->linear.x, vel, 1e-3);
496  ASSERT_NEAR(cmd_vel_->angular.z, ang_vel, 1e-3);
497  }
498  }
499  ASSERT_TRUE(received);
500 
501  ASSERT_TRUE(hasDiag());
502  EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::OK, diag_->status[0].level)
503  << "message: " << diag_->status[0].message;
504 
505  ASSERT_TRUE(hasStatus());
506  EXPECT_EQ(1.0, status_->limit_ratio);
507  EXPECT_TRUE(status_->is_cloud_available);
508  EXPECT_FALSE(status_->has_watchdog_timed_out);
509  EXPECT_EQ(status_->stuck_started_since, ros::Time(0));
510  }
511  }
512 }
513 
514 TEST_F(SafetyLimiterTest, SafetyLimitLinearSimpleSimulation)
515 {
516  const float dt = 0.02;
517  ros::Rate wait(1.0 / dt);
518 
519  const float velocities[] =
520  {
521  -0.8, -0.5, 0.5, 0.8
522  };
523  for (const float vel : velocities)
524  {
525  float x = 0;
526  bool stop = false;
527 
528  for (int i = 0; i < std::lround(10.0 / dt) && ros::ok() && !stop; ++i)
529  {
530  if (vel > 0)
531  publishSinglePointPointcloud2(1.0 - x, 0, 0, "base_link", ros::Time::now());
532  else
533  publishSinglePointPointcloud2(-3.0 - x, 0, 0, "base_link", ros::Time::now());
534 
535  publishWatchdogReset();
536  publishTwist(vel, 0.0);
537  broadcastTF("odom", "base_link", x, 0.0);
538 
539  wait.sleep();
540  ros::spinOnce();
541  if (cmd_vel_)
542  {
543  if (std::abs(cmd_vel_->linear.x) < 1e-4 && x > 0.5)
544  stop = true;
545 
546  x += dt * cmd_vel_->linear.x;
547  }
548  }
549  if (vel > 0)
550  {
551  EXPECT_GT(1.01, x);
552  EXPECT_LT(0.95, x);
553  }
554  else
555  {
556  EXPECT_LT(-1.01, x);
557  EXPECT_GT(-0.95, x);
558  }
559  }
560 }
561 
562 TEST_F(SafetyLimiterTest, SafetyLimitMaxVelocitiesValues)
563 {
564  ros::Rate wait(20);
565 
566  const float linear_velocities[] =
567  {-1.7, -1.5, 0.0, 1.5, 1.7 };
568  const float angular_velocities[] =
569  {-2.7, -2.5, 0.0, 2.5, 2.7 };
570  const float expected_linear_velocities[] =
571  {-1.5, -1.5, 0.0, 1.5, 1.5 };
572  const float expected_angular_velocities[] =
573  {-2.5, -2.5, 0.0, 2.5, 2.5 };
574 
575  for (int linear_index = 0; linear_index < 5; linear_index++)
576  {
577  for (int angular_index = 0; angular_index < 5; angular_index++)
578  {
579  bool received = false;
580  bool en = false;
581 
582  for (int i = 0; i < 10 && ros::ok(); ++i)
583  {
584  if (i > 5) en = true;
585  publishSinglePointPointcloud2(1000, 1000, 0, "base_link", ros::Time::now());
586  publishWatchdogReset();
587  publishTwist(linear_velocities[linear_index], angular_velocities[angular_index]);
588  broadcastTF("odom", "base_link", 0.0, 0.0);
589 
590  wait.sleep();
591  ros::spinOnce();
592  if (en && cmd_vel_)
593  {
594  received = true;
595  ASSERT_NEAR(expected_linear_velocities[linear_index], cmd_vel_->linear.x, 1e-3);
596  ASSERT_NEAR(expected_angular_velocities[angular_index], cmd_vel_->angular.z, 1e-3);
597  }
598  }
599  ASSERT_TRUE(received);
600 
601  ASSERT_TRUE(hasDiag());
602  EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::OK, diag_->status[0].level)
603  << "message: " << diag_->status[0].message;
604 
605  ASSERT_TRUE(hasStatus());
606  EXPECT_EQ(1.0, status_->limit_ratio);
607  EXPECT_TRUE(status_->is_cloud_available);
608  EXPECT_FALSE(status_->has_watchdog_timed_out);
609  EXPECT_EQ(status_->stuck_started_since, ros::Time(0));
610  }
611  }
612 }
613 
614 
615 int main(int argc, char** argv)
616 {
617  testing::InitGoogleTest(&argc, argv);
618  ros::init(argc, argv, "test_safety_limiter");
619 
620  return RUN_ALL_TESTS();
621 }
ros::Duration::sleep
bool sleep() const
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
main
int main(int argc, char **argv)
Definition: test_safety_limiter.cpp:615
test_safety_limiter_base.h
ros::ok
ROSCPP_DECL bool ok()
x
TFSIMD_FORCE_INLINE const tfScalar & x() const
ros::Rate::sleep
bool sleep()
wait
void wait(int seconds)
TEST_F
TEST_F(SafetyLimiterTest, Timeouts)
Definition: test_safety_limiter.cpp:44
ros::Time::now
static Time now()
ros::spinOnce
ROSCPP_DECL void spinOnce()

safety_limiter
Author(s): Atsushi Watanabe
autogenerated on Wed May 12 2021 02:20:36