test_collision.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2018, Ubiquity Robotics
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  * 1. Redistributions of source code must retain the above copyright notice,
9  * this list of conditions and the following disclaimer.
10  * 2. Redistributions in binary form must reproduce the above copyright notice,
11  * this list of conditions and the following disclaimer in the documentation
12  * and/or other materials provided with the distribution.
13  *
14  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
15  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
16  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
17  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
18  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
19  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
20  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
21  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
22  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
23  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
24  * POSSIBILITY OF SUCH DAMAGE.
25  *
26  * The views and conclusions contained in the software and documentation are
27  * those of the authors and should not be interpreted as representing official
28  * policies, either expressed or implied, of the FreeBSD Project.
29  *
30  */
31 
32 #include <gtest/gtest.h>
33 
34 #include <ros/ros.h>
35 #include <move_basic/collision_checker.h>
36 #include <tf2_ros/buffer.h>
37 #include <tf2_ros/transform_listener.h>
38 
39 #include <string>
40 
41 class CollisionCheckerTests : public ::testing::Test {
42 protected:
43  virtual void SetUp() {
44  listener = new tf2_ros::TransformListener(tf_buffer);
45 
46  obstacle_points = new ObstaclePoints(nh, tf_buffer);
47  collision_checker = new CollisionChecker(nh, tf_buffer, *obstacle_points);
48  }
49 
50  virtual void TearDown() {
51  delete listener;
52  delete collision_checker;
53  delete obstacle_points;
54  }
55 
56  CollisionChecker* collision_checker;
57  ObstaclePoints* obstacle_points;
58 
59  ros::NodeHandle nh;
60  tf2_ros::Buffer tf_buffer;
61  tf2_ros::TransformListener *listener;
62 };
63 
64 
65 TEST_F(CollisionCheckerTests, noObstacles) {
66  tf2::Vector3 fl;
67  tf2::Vector3 fr;
68  float left;
69  float right;
70  float forward_dist = collision_checker->obstacle_dist(true, left, right, fl,fr);
71  float backward_dist = collision_checker->obstacle_dist(false, left, right, fl,fr);
72  ASSERT_FLOAT_EQ(forward_dist, 10.0 - 0.09);
73  ASSERT_FLOAT_EQ(backward_dist, 10.0 - 0.19);
74 }
75 
76 TEST_F(CollisionCheckerTests, forward) {
77  tf2::Vector3 fl;
78  tf2::Vector3 fr;
79  float left;
80  float right;
81  obstacle_points->add_test_point(tf2::Vector3(1, 0, 0));
82  float forward_dist = collision_checker->obstacle_dist(true, left, right, fl,fr);
83  float backward_dist = collision_checker->obstacle_dist(false, left, right, fl,fr);
84  ASSERT_FLOAT_EQ(forward_dist, 1.0 - 0.09);
85  ASSERT_FLOAT_EQ(backward_dist, 10.0 - 0.19);
86 }
87 
88 TEST_F(CollisionCheckerTests, backward) {
89  tf2::Vector3 fl;
90  tf2::Vector3 fr;
91  float left;
92  float right;
93  obstacle_points->clear_test_points();
94  obstacle_points->add_test_point(tf2::Vector3(-1, 0, 0));
95  float forward_dist = collision_checker->obstacle_dist(true, left, right, fl,fr);
96  float backward_dist = collision_checker->obstacle_dist(false, left, right, fl,fr);
97  ASSERT_FLOAT_EQ(forward_dist, 10.0 - 0.09);
98  ASSERT_FLOAT_EQ(backward_dist, 1.0 - 0.19);
99 }
100 
101 TEST_F(CollisionCheckerTests, noObstaclesRot) {
102  float left_angle = collision_checker->obstacle_angle(true);
103  float right_angle = collision_checker->obstacle_angle(false);
104  ASSERT_FLOAT_EQ(left_angle, M_PI);
105  ASSERT_FLOAT_EQ(right_angle, M_PI);
106 }
107 
108 TEST_F(CollisionCheckerTests, obstaclesRotLeft) {
109  obstacle_points->clear_test_points();
110  obstacle_points->add_test_point(tf2::Vector3(0, .15, 0));
111  float left_angle = collision_checker->obstacle_angle(true);
112  float right_angle = collision_checker->obstacle_angle(false);
113  ASSERT_FLOAT_EQ(left_angle, M_PI);
114  ASSERT_FLOAT_EQ(right_angle, 1.0082601);
115 }
116 
117 TEST_F(CollisionCheckerTests, obstaclesRotRight) {
118  obstacle_points->clear_test_points();
119  obstacle_points->add_test_point(tf2::Vector3(0, -.15, 0));
120  float left_angle = collision_checker->obstacle_angle(true);
121  float right_angle = collision_checker->obstacle_angle(false);
122  ASSERT_FLOAT_EQ(left_angle, 1.0082601);
123  ASSERT_FLOAT_EQ(right_angle, M_PI);
124 }
125 
126 TEST_F(CollisionCheckerTests, obstaclesRotLeftBack) {
127  obstacle_points->clear_test_points();
128  obstacle_points->add_test_point(tf2::Vector3(-0.05, .15, 0));
129  float left_angle = collision_checker->obstacle_angle(true);
130  float right_angle = collision_checker->obstacle_angle(false);
131  ASSERT_FLOAT_EQ(left_angle, M_PI);
132  ASSERT_FLOAT_EQ(right_angle, 0.71854615);
133 }
134 
135 TEST_F(CollisionCheckerTests, obstaclesRotRightBack) {
136  obstacle_points->clear_test_points();
137  obstacle_points->add_test_point(tf2::Vector3(-0.05, -.15, 0));
138  float left_angle = collision_checker->obstacle_angle(true);
139  float right_angle = collision_checker->obstacle_angle(false);
140  ASSERT_FLOAT_EQ(left_angle, 0.71854615);
141  ASSERT_FLOAT_EQ(right_angle, M_PI);
142 }
143 
144 TEST_F(CollisionCheckerTests, obstaclesRotFrontCenter) {
145  obstacle_points->clear_test_points();
146  obstacle_points->add_test_point(tf2::Vector3(0.1, 0.0, 0));
147  float left_angle = collision_checker->obstacle_angle(true);
148  float right_angle = collision_checker->obstacle_angle(false);
149  ASSERT_FLOAT_EQ(left_angle, 0.45102686);
150  ASSERT_FLOAT_EQ(right_angle, 0.45102686);
151 }
152 
153 TEST_F(CollisionCheckerTests, obstaclesRotFrontLeft) {
154  obstacle_points->clear_test_points();
155  obstacle_points->add_test_point(tf2::Vector3(0.1, 0.01, 0));
156  float left_angle = collision_checker->obstacle_angle(true);
157  float right_angle = collision_checker->obstacle_angle(false);
158  ASSERT_FLOAT_EQ(left_angle, 0.56083643);
159  ASSERT_FLOAT_EQ(right_angle, 0.36149913);
160 }
161 
162 TEST_F(CollisionCheckerTests, obstaclesRotFrontRight) {
163  obstacle_points->clear_test_points();
164  obstacle_points->add_test_point(tf2::Vector3(0.1, -0.01, 0));
165  float left_angle = collision_checker->obstacle_angle(true);
166  float right_angle = collision_checker->obstacle_angle(false);
167  ASSERT_FLOAT_EQ(left_angle, 0.36149913);
168  ASSERT_FLOAT_EQ(right_angle, 0.56083643);
169 }
170 
171 TEST_F(CollisionCheckerTests, obstaclesRotBackCenter) {
172  obstacle_points->clear_test_points();
173  obstacle_points->add_test_point(tf2::Vector3(-0.145, 0.0, 0));
174  float left_angle = collision_checker->obstacle_angle(true);
175  float right_angle = collision_checker->obstacle_angle(false);
176  ASSERT_FLOAT_EQ(left_angle, 0.58442998);
177  ASSERT_FLOAT_EQ(right_angle, 0.58443004);
178 }
179 
180 TEST_F(CollisionCheckerTests, obstaclesRotBackLeft) {
181  obstacle_points->clear_test_points();
182  obstacle_points->add_test_point(tf2::Vector3(-0.145, 0.01, 0));
183  float left_angle = collision_checker->obstacle_angle(true);
184  float right_angle = collision_checker->obstacle_angle(false);
185  ASSERT_FLOAT_EQ(left_angle, 0.51400685);
186  ASSERT_FLOAT_EQ(right_angle, 0.65171939);
187 }
188 
189 TEST_F(CollisionCheckerTests, obstaclesRotBackRight) {
190  obstacle_points->clear_test_points();
191  obstacle_points->add_test_point(tf2::Vector3(-0.145, -0.01, 0));
192  float left_angle = collision_checker->obstacle_angle(true);
193  float right_angle = collision_checker->obstacle_angle(false);
194  ASSERT_FLOAT_EQ(left_angle, 0.65171939);
195  ASSERT_FLOAT_EQ(right_angle, 0.51400685);
196 }
197 
198 TEST_F(CollisionCheckerTests, arcNoObstacles) {
199  obstacle_points->clear_test_points();
200  float t = collision_checker->obstacle_arc_angle(0.0,0.0);
201  EXPECT_FLOAT_EQ(t, M_PI);
202  t = collision_checker->obstacle_arc_angle(0.0,1.0);
203  EXPECT_FLOAT_EQ(t, M_PI);
204 }
205 
206 TEST_F(CollisionCheckerTests, arcForward) {
207  obstacle_points->clear_test_points();
208  obstacle_points->add_test_point(tf2::Vector3(0.1,0,0));
209  float left = collision_checker->obstacle_arc_angle(1.0,1.0);
210  float right = collision_checker->obstacle_arc_angle(1.0,-1.0);
211  EXPECT_FLOAT_EQ(left, -1.471128);
212  EXPECT_FLOAT_EQ(right, 1.471128);
213 
214  obstacle_points->clear_test_points();
215  obstacle_points->add_test_point(tf2::Vector3(1,1,0));
216  obstacle_points->add_test_point(tf2::Vector3(1,-1,0));
217  left = collision_checker->obstacle_arc_angle(1.0,1.0);
218  right = collision_checker->obstacle_arc_angle(1.0,-1.0);
219  EXPECT_FLOAT_EQ(left, 0.0);
220  EXPECT_FLOAT_EQ(right, 0.0);
221 
222  obstacle_points->clear_test_points();
223  obstacle_points->add_test_point(tf2::Vector3(0,0,0));
224  left = collision_checker->obstacle_arc_angle(1.0,1.0);
225  right = collision_checker->obstacle_arc_angle(1.0,-1.0);
226  EXPECT_FLOAT_EQ(left, -M_PI/2);
227  EXPECT_FLOAT_EQ(right, M_PI/2);
228 
229  obstacle_points->clear_test_points();
230  obstacle_points->add_test_point(tf2::Vector3(-1,0,0));
231  left = collision_checker->obstacle_arc_angle(1.0,1.0);
232  right = collision_checker->obstacle_arc_angle(1.0,-1.0);
233  EXPECT_FLOAT_EQ(left, M_PI);
234  EXPECT_FLOAT_EQ(right, M_PI);
235 
236  obstacle_points->clear_test_points();
237  obstacle_points->add_test_point(tf2::Vector3(1,-1,0));
238  left = collision_checker->obstacle_arc_angle(1.0,1.0);
239  right = collision_checker->obstacle_arc_angle(1.0,-1.0);
240  EXPECT_FLOAT_EQ(left, M_PI);
241  EXPECT_FLOAT_EQ(right, 0.0);
242 }
243 
244 int main(int argc, char **argv) {
245  ros::init(argc, argv, "collision_checker_tests");
246  testing::InitGoogleTest(&argc, argv);
247  return RUN_ALL_TESTS();
248 }
transform_listener.h
CollisionCheckerTests::TearDown
virtual void TearDown()
Definition: test_collision.cpp:50
ros::NodeHandle
CollisionCheckerTests::collision_checker
CollisionChecker * collision_checker
Definition: test_collision.cpp:56
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
CollisionChecker::obstacle_arc_angle
float obstacle_arc_angle(double linear, double angular)
Definition: collision_checker.cpp:467
tf2_ros::Buffer
t
geometry_msgs::TransformStamped t
CollisionChecker::obstacle_angle
float obstacle_angle(bool left)
Definition: collision_checker.cpp:357
main
int main(int argc, char **argv)
Definition: test_collision.cpp:244
CollisionChecker::obstacle_dist
float obstacle_dist(bool forward, float &left_dist, float &right_dist, tf2::Vector3 &fl, tf2::Vector3 &fr)
Definition: collision_checker.cpp:153
ObstaclePoints::add_test_point
void add_test_point(tf2::Vector3 p)
Definition: obstacle_points.cpp:230
tf2_ros::TransformListener
collision_checker.h
TEST_F
TEST_F(CollisionCheckerTests, noObstacles)
Definition: test_collision.cpp:65
CollisionCheckerTests::tf_buffer
tf2_ros::Buffer tf_buffer
Definition: test_collision.cpp:60
CollisionCheckerTests::obstacle_points
ObstaclePoints * obstacle_points
Definition: test_collision.cpp:57
CollisionCheckerTests::listener
tf2_ros::TransformListener * listener
Definition: test_collision.cpp:61

move_basic
Author(s): Jim Vaughan
autogenerated on Fri Mar 26 2021 02:46:58