1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011-2014, Willow Garage, Inc.
5 * Copyright (c) 2014-2016, Open Source Robotics Foundation
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of Open Source Robotics Foundation nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 */
35
36 /** @author Jia Pan */
37
38 #include <gtest/gtest.h>
39
40 #include "fcl/config.h"
41 #include "fcl/geometry/octree/octree.h"
42 #include "fcl/narrowphase/collision.h"
43 #include "fcl/broadphase/broadphase_bruteforce.h"
44 #include "fcl/broadphase/broadphase_spatialhash.h"
45 #include "fcl/broadphase/broadphase_SaP.h"
46 #include "fcl/broadphase/broadphase_SSaP.h"
47 #include "fcl/broadphase/broadphase_interval_tree.h"
48 #include "fcl/broadphase/broadphase_dynamic_AABB_tree.h"
49 #include "fcl/broadphase/broadphase_dynamic_AABB_tree_array.h"
50 #include "fcl/broadphase/default_broadphase_callbacks.h"
51 #include "fcl/geometry/geometric_shape_to_BVH_model.h"
52 #include "test_fcl_utility.h"
53 #include "fcl_resources/config.h"
54
55 using namespace fcl;
56
57 /// @brief Octomap collision with an environment with 3 * env_size objects, compute cost
58 template <typename S>
59 void octomap_cost_test(S env_scale, std::size_t env_size, std::size_t num_max_cost_sources, bool use_mesh, bool use_mesh_octomap, double resolution = 0.1);
60
61 template <typename S>
test_octomap_cost()62 void test_octomap_cost()
63 {
64 #ifdef NDEBUG
65 octomap_cost_test<S>(200, 100, 10, false, false);
66 octomap_cost_test<S>(200, 1000, 10, false, false);
67 #else
68 octomap_cost_test<S>(200, 10, 10, false, false, 0.1);
69 octomap_cost_test<S>(200, 100, 10, false, false, 0.1);
70 #endif
71 }
72
GTEST_TEST(FCL_OCTOMAP,test_octomap_cost)73 GTEST_TEST(FCL_OCTOMAP, test_octomap_cost)
74 {
75 // test_octomap_cost<float>();
76 test_octomap_cost<double>();
77 }
78
79 template <typename S>
test_octomap_cost_mesh()80 void test_octomap_cost_mesh()
81 {
82 #ifdef NDEBUG
83 octomap_cost_test<S>(200, 100, 10, true, false);
84 octomap_cost_test<S>(200, 1000, 10, true, false);
85 #else
86 octomap_cost_test<S>(200, 2, 4, true, false, 1.0);
87 octomap_cost_test<S>(200, 5, 4, true, false, 1.0);
88 #endif
89 }
90
GTEST_TEST(FCL_OCTOMAP,test_octomap_cost_mesh)91 GTEST_TEST(FCL_OCTOMAP, test_octomap_cost_mesh)
92 {
93 // test_octomap_cost_mesh<float>();
94 test_octomap_cost_mesh<double>();
95 }
96
97 template <typename S>
octomap_cost_test(S env_scale,std::size_t env_size,std::size_t num_max_cost_sources,bool use_mesh,bool use_mesh_octomap,double resolution)98 void octomap_cost_test(S env_scale, std::size_t env_size, std::size_t num_max_cost_sources, bool use_mesh, bool use_mesh_octomap, double resolution)
99 {
100 std::vector<CollisionObject<S>*> env;
101 if(use_mesh)
102 test::generateEnvironmentsMesh(env, env_scale, env_size);
103 else
104 test::generateEnvironments(env, env_scale, env_size);
105
106 OcTree<S>* tree = new OcTree<S>(std::shared_ptr<const octomap::OcTree>(test::generateOcTree(resolution)));
107 CollisionObject<S> tree_obj((std::shared_ptr<CollisionGeometry<S>>(tree)));
108
109 DynamicAABBTreeCollisionManager<S>* manager = new DynamicAABBTreeCollisionManager<S>();
110 manager->registerObjects(env);
111 manager->setup();
112
113 DefaultCollisionData<S> cdata;
114 cdata.request.enable_cost = true;
115 cdata.request.num_max_cost_sources = num_max_cost_sources;
116
117 test::TStruct t1;
118 test::Timer timer1;
119 timer1.start();
120 manager->octree_as_geometry_collide = false;
121 manager->octree_as_geometry_distance = false;
122 manager->collide(&tree_obj, &cdata, DefaultCollisionFunction);
123 timer1.stop();
124 t1.push_back(timer1.getElapsedTime());
125
126 DefaultCollisionData<S> cdata3;
127 cdata3.request.enable_cost = true;
128 cdata3.request.num_max_cost_sources = num_max_cost_sources;
129
130 test::TStruct t3;
131 test::Timer timer3;
132 timer3.start();
133 manager->octree_as_geometry_collide = true;
134 manager->octree_as_geometry_distance = true;
135 manager->collide(&tree_obj, &cdata3, DefaultCollisionFunction);
136 timer3.stop();
137 t3.push_back(timer3.getElapsedTime());
138
139 test::TStruct t2;
140 test::Timer timer2;
141 timer2.start();
142 std::vector<CollisionObject<S>*> boxes;
143 if(use_mesh_octomap)
144 test::generateBoxesFromOctomapMesh(boxes, *tree);
145 else
146 test::generateBoxesFromOctomap(boxes, *tree);
147 timer2.stop();
148 t2.push_back(timer2.getElapsedTime());
149
150 timer2.start();
151 DynamicAABBTreeCollisionManager<S>* manager2 = new DynamicAABBTreeCollisionManager<S>();
152 manager2->registerObjects(boxes);
153 manager2->setup();
154 timer2.stop();
155 t2.push_back(timer2.getElapsedTime());
156
157 DefaultCollisionData<S> cdata2;
158 cdata2.request.enable_cost = true;
159 cdata3.request.num_max_cost_sources = num_max_cost_sources;
160
161 timer2.start();
162 manager->collide(manager2, &cdata2, DefaultCollisionFunction);
163 timer2.stop();
164 t2.push_back(timer2.getElapsedTime());
165
166 std::cout << cdata.result.numContacts() << " " << cdata3.result.numContacts() << " " << cdata2.result.numContacts() << std::endl;
167 std::cout << cdata.result.numCostSources() << " " << cdata3.result.numCostSources() << " " << cdata2.result.numCostSources() << std::endl;
168
169 {
170 std::vector<CostSource<S>> cost_sources;
171 cdata.result.getCostSources(cost_sources);
172 for(std::size_t i = 0; i < cost_sources.size(); ++i)
173 {
174 std::cout << cost_sources[i].aabb_min.transpose() << " " << cost_sources[i].aabb_max.transpose() << " " << cost_sources[i].cost_density << std::endl;
175 }
176
177 std::cout << std::endl;
178
179 cdata3.result.getCostSources(cost_sources);
180 for(std::size_t i = 0; i < cost_sources.size(); ++i)
181 {
182 std::cout << cost_sources[i].aabb_min.transpose() << " " << cost_sources[i].aabb_max.transpose() << " " << cost_sources[i].cost_density << std::endl;
183 }
184
185 std::cout << std::endl;
186
187 cdata2.result.getCostSources(cost_sources);
188 for(std::size_t i = 0; i < cost_sources.size(); ++i)
189 {
190 std::cout << cost_sources[i].aabb_min.transpose() << " " << cost_sources[i].aabb_max.transpose() << " " << cost_sources[i].cost_density << std::endl;
191 }
192
193 std::cout << std::endl;
194
195 }
196
197 if(use_mesh) EXPECT_TRUE((cdata.result.numContacts() > 0) >= (cdata2.result.numContacts() > 0));
198 else EXPECT_TRUE(cdata.result.numContacts() >= cdata2.result.numContacts());
199
200 delete manager;
201 delete manager2;
202 for(std::size_t i = 0; i < boxes.size(); ++i)
203 delete boxes[i];
204
205 std::cout << "collision cost" << std::endl;
206 std::cout << "1) octomap overall time: " << t1.overall_time << std::endl;
207 std::cout << "1') octomap overall time (as geometry): " << t3.overall_time << std::endl;
208 std::cout << "2) boxes overall time: " << t2.overall_time << std::endl;
209 std::cout << " a) to boxes: " << t2.records[0] << std::endl;
210 std::cout << " b) structure init: " << t2.records[1] << std::endl;
211 std::cout << " c) collision: " << t2.records[2] << std::endl;
212 std::cout << "Note: octomap may need more collides when using mesh, because octomap collision uses box primitive inside" << std::endl;
213 }
214
215 //==============================================================================
main(int argc,char * argv[])216 int main(int argc, char* argv[])
217 {
218 ::testing::InitGoogleTest(&argc, argv);
219 return RUN_ALL_TESTS();
220 }
221