test_blockmem_gridmap.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2017, 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 <cstddef>
31 #include <limits>
32 
33 #include <gtest/gtest.h>
34 
36 
37 namespace planner_cspace
38 {
39 template <int BLOCK_WIDTH>
40 class BlockMemGridmapHelper : public BlockMemGridmap<int, 1, 1, BLOCK_WIDTH, false>
41 {
42 public:
43  size_t getBlockBit() const
44  {
45  return this->block_bit_;
46  }
47 };
48 TEST(BlockmemGridmap, BlockWidth)
49 {
50  ASSERT_EQ(4u, BlockMemGridmapHelper<0x10>().getBlockBit());
51  ASSERT_EQ(8u, BlockMemGridmapHelper<0x100>().getBlockBit());
52  ASSERT_EQ(12u, BlockMemGridmapHelper<0x1000>().getBlockBit());
53  ASSERT_EQ(16u, BlockMemGridmapHelper<0x10000>().getBlockBit());
54 }
55 
56 TEST(BlockmemGridmap, ResetClear)
57 {
59 
60  for (int s = 4; s <= 6; s += 2)
61  {
62  gm.reset(CyclicVecInt<3, 3>(s, s, s));
63  gm.clear(0.0);
64 
66  for (i[0] = 0; i[0] < s; ++i[0])
67  {
68  for (i[1] = 0; i[1] < s; ++i[1])
69  {
70  for (i[2] = 0; i[2] < s; ++i[2])
71  {
72  ASSERT_EQ(gm[i], 0.0);
73  }
74  }
75  }
76 
77  gm.clear(3.0);
78  for (i[0] = 0; i[0] < s; ++i[0])
79  {
80  for (i[1] = 0; i[1] < s; ++i[1])
81  {
82  for (i[2] = 0; i[2] < s; ++i[2])
83  {
84  ASSERT_EQ(gm[i], 3.0);
85  }
86  }
87  }
88  }
89 }
90 
91 TEST(BlockmemGridmap, ClearAndCopyPartially)
92 {
93  const CyclicVecInt<3, 3> base_size(17, 8, 3);
95  gm_base.reset(base_size);
96  for (CyclicVecInt<3, 3> p(0, 0, 0); p[0] < base_size[0]; ++p[0])
97  {
98  for (p[1] = 0; p[1] < base_size[1]; ++p[1])
99  {
100  for (p[2] = 0; p[2] < base_size[2]; ++p[2])
101  {
102  gm_base[p] = p[0] * base_size[1] * base_size[2] + p[1] * base_size[2] + p[2];
103  }
104  }
105  }
106 
108  gm = gm_base;
109 
110  const CyclicVecInt<3, 3> copy_min_pos(3, 5, 0);
111  const CyclicVecInt<3, 3> copy_max_pos(6, 7, 2);
112  gm.clear_partially(-1, copy_min_pos, copy_max_pos);
113 
114  for (CyclicVecInt<3, 3> p(0, 0, 0); p[0] < base_size[0]; ++p[0])
115  {
116  for (p[1] = 0; p[1] < base_size[1]; ++p[1])
117  {
118  for (p[2] = 0; p[2] < base_size[2]; ++p[2])
119  {
120  if ((copy_min_pos[0] <= p[0]) && (p[0] < copy_max_pos[0]) &&
121  (copy_min_pos[1] <= p[1]) && (p[1] < copy_max_pos[1]) &&
122  (copy_min_pos[2] <= p[2]) && (p[2] < copy_max_pos[2]))
123  {
124  EXPECT_EQ(gm[p], -1) << p[0] << "," << p[1] << "," << p[2];
125  }
126  else
127  {
128  EXPECT_EQ(gm[p], gm_base[p]) << p[0] << "," << p[1] << "," << p[2];
129  }
130  }
131  }
132  }
133 
135  gm_update.reset(base_size);
136  for (CyclicVecInt<3, 3> p(0, 0, 0); p[0] < base_size[0]; ++p[0])
137  {
138  for (p[1] = 0; p[1] < base_size[1]; ++p[1])
139  {
140  for (p[2] = 0; p[2] < base_size[2]; ++p[2])
141  {
142  gm_update[p] = p[0] * base_size[1] * base_size[2] + p[1] * base_size[2] + p[2] * -1;
143  }
144  }
145  }
146 
147  gm = gm_base;
148  gm.copy_partially(gm_update, copy_min_pos, copy_max_pos);
149 
150  for (CyclicVecInt<3, 3> p(0, 0, 0); p[0] < base_size[0]; ++p[0])
151  {
152  for (p[1] = 0; p[1] < base_size[1]; ++p[1])
153  {
154  for (p[2] = 0; p[2] < base_size[2]; ++p[2])
155  {
156  if ((copy_min_pos[0] <= p[0]) && (p[0] < copy_max_pos[0]) &&
157  (copy_min_pos[1] <= p[1]) && (p[1] < copy_max_pos[1]) &&
158  (copy_min_pos[2] <= p[2]) && (p[2] < copy_max_pos[2]))
159  {
160  ASSERT_EQ(gm[p], gm_update[p]);
161  }
162  else
163  {
164  ASSERT_EQ(gm[p], gm_base[p]);
165  }
166  }
167  }
168  }
169 }
170 
171 TEST(BlockmemGridmap, WriteRead)
172 {
174 
175  const int s = 4;
176  gm.reset(CyclicVecInt<3, 3>(s, s, s));
177  gm.clear(0.0);
178 
180  for (i[0] = 0; i[0] < s; ++i[0])
181  {
182  for (i[1] = 0; i[1] < s; ++i[1])
183  {
184  for (i[2] = 0; i[2] < s; ++i[2])
185  {
186  gm[i] = i[2] * 100 + i[1] * 10 + i[0];
187  }
188  }
189  }
190 
191  for (i[0] = 0; i[0] < s; ++i[0])
192  {
193  for (i[1] = 0; i[1] < s; ++i[1])
194  {
195  for (i[2] = 0; i[2] < s; ++i[2])
196  {
197  ASSERT_EQ(gm[i], i[2] * 100 + i[1] * 10 + i[0]);
198  }
199  }
200  }
201 }
202 
203 TEST(BlockmemGridmap, OuterBoundary)
204 {
206 
207  const int s = 0x30;
208  gm.reset(CyclicVecInt<3, 2>(s, s, s));
209  gm.clear(1.0);
210 
212  const int outer = 0x10;
213  for (i[0] = -outer; i[0] < s + outer; ++i[0])
214  {
215  for (i[1] = -outer; i[1] < s + outer; ++i[1])
216  {
217  for (i[2] = -outer; i[2] < s + outer; ++i[2])
218  {
219  if (i[0] >= 0 && i[1] >= 0 && i[2] >= 0 &&
220  i[0] < s && i[1] < s && i[2] < s)
221  {
222  ASSERT_TRUE(gm.validate(i));
223  }
224  else
225  {
226  ASSERT_FALSE(gm.validate(i));
227  }
228  // Confirm at least not dead
229  gm[i] = 1.0;
230  }
231  }
232  }
233 }
234 } // namespace planner_cspace
235 
236 int main(int argc, char** argv)
237 {
238  testing::InitGoogleTest(&argc, argv);
239 
240  return RUN_ALL_TESTS();
241 }
void reset(const CyclicVecInt< DIM, NONCYCLIC > &size)
TEST(BlockmemGridmap, BlockWidth)
bool validate(const CyclicVecInt< DIM, NONCYCLIC > &pos, const int tolerance=0) const
XmlRpcServer s
void clear_partially(const T zero, const CyclicVecInt< DIM, NONCYCLIC > &min, const CyclicVecInt< DIM, NONCYCLIC > &max)
int main(int argc, char **argv)
void copy_partially(const BlockMemGridmapBase< T, DIM, NONCYCLIC > &base, const CyclicVecInt< DIM, NONCYCLIC > &min, const CyclicVecInt< DIM, NONCYCLIC > &max)


planner_cspace
Author(s): Atsushi Watanabe
autogenerated on Wed May 12 2021 02:20:42