laser_joint_processor_unittest.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 #include <gtest/gtest.h>
36 #include <laser_joint_processor/laser_joint_processor.h>
37 
38 using namespace std;
39 using namespace laser_joint_processor;
40 
41 static const float eps = 1e-6;
42 
43 class LaserJointProcessor_EasyTests : public ::testing::Test
44 {
45 protected:
46  virtual void SetUp()
47  {
48  bool success;
49 
50  // Set up joints of interest
51  vector<string> names;
52  names.push_back("JointA");
53  names.push_back("JointB");
54  processor_.setJointNames(names);
55  processor_.setCacheSize(1000);
56 
57  // Populate the cache
58  for (unsigned int i=0; i<20; i++)
59  {
60  sensor_msgs::JointStatePtr joint_state(new sensor_msgs::JointState);
61  joint_state->header.stamp = ros::Time(100+i, 0);
62  joint_state->name.push_back("JointA");
63  joint_state->name.push_back("JointB");
64  joint_state->name.push_back("JointC");
65  joint_state->position.push_back(i*1.0);
66  joint_state->position.push_back(i*10.0);
67  joint_state->position.push_back(i*20.0);
68  joint_state->velocity.resize(3);
69  joint_state->effort.resize(3);
70 
71  success = processor_.addJointState(joint_state);
72  ASSERT_TRUE(success);
73  }
74 
75  // Build The Snapshot
76  snapshot_.angle_min = 10.0;
77  snapshot_.angle_increment = 1.0;
78  snapshot_.time_increment = .5;
79  snapshot_.readings_per_scan = 4;
80  snapshot_.num_scans = 3;
81  snapshot_.ranges.resize(12);
82  for (unsigned int i=0; i<snapshot_.ranges.size(); i++)
83  snapshot_.ranges[i] = i*100;
84  snapshot_.scan_start.resize(snapshot_.num_scans);
85  snapshot_.scan_start[0] = ros::Time(105);
86  snapshot_.scan_start[1] = ros::Time(110);
87  snapshot_.scan_start[2] = ros::Time(115.1);
88 
89  // Build Feature Vector
90  features_.image_points.resize(P);
91 
92  // Boundaries
93  features_.image_points[0] = Point(0,0);
94  features_.image_points[1] = Point(3,0);
95  features_.image_points[2] = Point(0,2);
96  features_.image_points[3] = Point(3,2);
97 
98  features_.image_points[4] = Point(2.5, .5);
99  features_.image_points[5] = Point(2, 1);
100  features_.image_points[6] = Point(1.25, 1.5);
101  }
102 
103  void testProcessing()
104  {
105  bool success;
106  // Run the processing step
107  success = processor_.processLaserData(snapshot_, features_, result_, ros::Duration(2,0));
108  EXPECT_TRUE(success);
109  }
110 
111  geometry_msgs::Point Point(float x, float y)
112  {
113  geometry_msgs::Point point;
114  point.x = x;
115  point.y = y;
116  return point;
117  }
118 
119  static const unsigned int P = 7;
120  calibration_msgs::JointStateCalibrationPattern result_;
121  calibration_msgs::CalibrationPattern features_;
122  calibration_msgs::DenseLaserSnapshot snapshot_;
123  LaserJointProcessor processor_;
124 };
125 
126 TEST_F(LaserJointProcessor_EasyTests, earlyTest)
127 {
128  EXPECT_FALSE(processor_.isSnapshotEarly(snapshot_));
129  EXPECT_FALSE(processor_.isSnapshotLate(snapshot_));
130  snapshot_.scan_start[2] = ros::Time(300);
131  EXPECT_TRUE(processor_.isSnapshotEarly(snapshot_));
132  EXPECT_FALSE(processor_.isSnapshotLate(snapshot_));
133 }
134 
135 TEST_F(LaserJointProcessor_EasyTests, lateTest)
136 {
137  EXPECT_FALSE(processor_.isSnapshotLate(snapshot_));
138  EXPECT_FALSE(processor_.isSnapshotEarly(snapshot_));
139  snapshot_.scan_start[0] = ros::Time(10);
140  EXPECT_TRUE(processor_.isSnapshotLate(snapshot_));
141  EXPECT_FALSE(processor_.isSnapshotEarly(snapshot_));
142 }
143 
144 /*
145  * Joint 1:
146  * 5.00 5.50 6.00 6.50
147  * 10.00 10.50 11.00 11.50
148  * 15.10 15.60 16.10 16.60
149  *
150  * Joint 2:
151  * 50.00 55.00 60.00 65.00
152  * 100.00 105.00 110.00 115.00
153  * 151.00 156.00 161.00 166.00
154  */
155 
156 TEST_F(LaserJointProcessor_EasyTests, jointTest)
157 {
158  testProcessing();
159 
160  ASSERT_EQ(result_.joint_points.size(), (unsigned int) P);
161  for (unsigned int i=0; i<result_.joint_points.size(); i++)
162  {
163  ASSERT_EQ(result_.joint_points[i].name.size(), (unsigned int) 4);
164  ASSERT_EQ(result_.joint_points[i].position.size(), (unsigned int) 4);
165  EXPECT_EQ(result_.joint_points[i].name[0], "JointA");
166  EXPECT_EQ(result_.joint_points[i].name[1], "JointB");
167  }
168 
169  // point #0 = (0,0)
170  // t=105
171  EXPECT_NEAR(result_.joint_points[0].position[0], 5.0, eps);
172  EXPECT_NEAR(result_.joint_points[0].position[1], 50.0, eps);
173 
174  // point #1 = (3,0)
175  // t=1.5
176  EXPECT_NEAR(result_.joint_points[1].position[0], 6.5, eps);
177  EXPECT_NEAR(result_.joint_points[1].position[1], 65, eps);
178 
179  // point #2 = (0,2)
180  // t=115.1
181  EXPECT_NEAR(result_.joint_points[2].position[0], 15.1, eps);
182  EXPECT_NEAR(result_.joint_points[2].position[1], 151, eps);
183 
184  // point #3 = (3,2)
185  // t=116.6
186  EXPECT_NEAR(result_.joint_points[3].position[0], 16.6, eps);
187  EXPECT_NEAR(result_.joint_points[3].position[1], 166, eps);
188 
189  // point #4 = (2.5, .5)
190  EXPECT_NEAR(result_.joint_points[4].position[0], 8.75, eps);
191  EXPECT_NEAR(result_.joint_points[4].position[1], 87.5, eps);
192 
193  // point #5 = (2, 1)
194  EXPECT_NEAR(result_.joint_points[5].position[0], 11, eps);
195  EXPECT_NEAR(result_.joint_points[5].position[1], 110, eps);
196 
197  // point #6 = (1.25, 1.5)
198  EXPECT_NEAR(result_.joint_points[6].position[0], 13.175, eps);
199  EXPECT_NEAR(result_.joint_points[6].position[1], 131.75, eps);
200 }
201 
202 TEST_F(LaserJointProcessor_EasyTests, anglesTest)
203 {
204  testProcessing();
205 
206  ASSERT_EQ(result_.joint_points.size(), (unsigned int) P);
207  for (unsigned int i=0; i<P; i++)
208  {
209  ASSERT_EQ(result_.joint_points[i].name.size(), (unsigned int) 4);
210  ASSERT_EQ(result_.joint_points[i].position.size(), (unsigned int) 4);
211  EXPECT_EQ(result_.joint_points[i].name[2], "laser_angle_joint");
212  }
213 
214  EXPECT_NEAR(result_.joint_points[0].position[2], 10.0, eps);
215  EXPECT_NEAR(result_.joint_points[1].position[2], 13.0, eps);
216  EXPECT_NEAR(result_.joint_points[2].position[2], 10.0, eps);
217  EXPECT_NEAR(result_.joint_points[3].position[2], 13.0, eps);
218  EXPECT_NEAR(result_.joint_points[4].position[2], 12.5, eps);
219  EXPECT_NEAR(result_.joint_points[5].position[2], 12.0, eps);
220  EXPECT_NEAR(result_.joint_points[6].position[2], 11.25, eps);
221 }
222 
223 TEST_F(LaserJointProcessor_EasyTests, rangesTest)
224 {
225  testProcessing();
226 
227  ASSERT_EQ(result_.joint_points.size(), (unsigned int) P);
228  for (unsigned int i=0; i<P; i++)
229  {
230  ASSERT_EQ(result_.joint_points[i].name.size(), (unsigned int) 4);
231  ASSERT_EQ(result_.joint_points[i].position.size(), (unsigned int) 4);
232  EXPECT_EQ(result_.joint_points[i].name[3], "laser_range_joint");
233  }
234 
235  EXPECT_NEAR(result_.joint_points[0].position[3], 0.0, eps);
236  EXPECT_NEAR(result_.joint_points[1].position[3], 300.0, eps);
237  EXPECT_NEAR(result_.joint_points[2].position[3], 800.0, eps);
238  EXPECT_NEAR(result_.joint_points[3].position[3], 1100.0, eps);
239  EXPECT_NEAR(result_.joint_points[4].position[3], 450.0, eps);
240  EXPECT_NEAR(result_.joint_points[5].position[3], 600.0, eps);
241  EXPECT_NEAR(result_.joint_points[6].position[3], 725.0, eps);
242 }
243 
244 int main(int argc, char **argv){
245  testing::InitGoogleTest(&argc, argv);
246  return RUN_ALL_TESTS();
247 }
laser_joint_processor::LaserJointProcessor
Definition: laser_joint_processor.h:51
laser_joint_processor.h
LaserJointProcessor_EasyTests::snapshot_
calibration_msgs::DenseLaserSnapshot snapshot_
Definition: laser_joint_processor_unittest.cpp:122
LaserJointProcessor_EasyTests
Definition: laser_joint_processor_unittest.cpp:43
eps
static const float eps
Definition: laser_joint_processor_unittest.cpp:41
LaserJointProcessor_EasyTests::result_
calibration_msgs::JointStateCalibrationPattern result_
Definition: laser_joint_processor_unittest.cpp:120
TEST_F
TEST_F(LaserJointProcessor_EasyTests, earlyTest)
Definition: laser_joint_processor_unittest.cpp:126
LaserJointProcessor_EasyTests::SetUp
virtual void SetUp()
Definition: laser_joint_processor_unittest.cpp:46
LaserJointProcessor_EasyTests::testProcessing
void testProcessing()
Definition: laser_joint_processor_unittest.cpp:103
LaserJointProcessor_EasyTests::processor_
LaserJointProcessor processor_
Definition: laser_joint_processor_unittest.cpp:123
laser_joint_processor
Definition: joint_image_interpolator.h:44
LaserJointProcessor_EasyTests::Point
geometry_msgs::Point Point(float x, float y)
Definition: laser_joint_processor_unittest.cpp:111
main
int main(int argc, char **argv)
Definition: laser_joint_processor_unittest.cpp:244
LaserJointProcessor_EasyTests::features_
calibration_msgs::CalibrationPattern features_
Definition: laser_joint_processor_unittest.cpp:121

laser_joint_processor
Author(s): Vijay Pradeep
autogenerated on Tue Jun 1 2021 02:50:55