| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
/* |
||
2 |
* Software License Agreement (BSD License) |
||
3 |
* |
||
4 |
* Copyright (c) 2019, CNRS-LAAS. |
||
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 Willow Garage, Inc. 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 |
/** \author Florent Lamiraux <florent@laas.fr> */ |
||
36 |
|||
37 |
#define BOOST_TEST_MODULE FCL_OCTREE |
||
38 |
#include <fstream> |
||
39 |
#include <boost/test/included/unit_test.hpp> |
||
40 |
#include <boost/filesystem.hpp> |
||
41 |
|||
42 |
#include <hpp/fcl/BVH/BVH_model.h> |
||
43 |
#include <hpp/fcl/collision.h> |
||
44 |
#include <hpp/fcl/distance.h> |
||
45 |
#include <hpp/fcl/shape/geometric_shapes.h> |
||
46 |
#include <hpp/fcl/internal/BV_splitter.h> |
||
47 |
|||
48 |
#include "utility.h" |
||
49 |
#include "fcl_resources/config.h" |
||
50 |
|||
51 |
namespace utf = boost::unit_test::framework; |
||
52 |
|||
53 |
using hpp::fcl::BVHModel; |
||
54 |
using hpp::fcl::BVSplitter; |
||
55 |
using hpp::fcl::CollisionRequest; |
||
56 |
using hpp::fcl::CollisionResult; |
||
57 |
using hpp::fcl::FCL_REAL; |
||
58 |
using hpp::fcl::OBBRSS; |
||
59 |
using hpp::fcl::OcTree; |
||
60 |
using hpp::fcl::Transform3f; |
||
61 |
using hpp::fcl::Triangle; |
||
62 |
using hpp::fcl::Vec3f; |
||
63 |
|||
64 |
2 |
void makeMesh(const std::vector<Vec3f>& vertices, |
|
65 |
const std::vector<Triangle>& triangles, BVHModel<OBBRSS>& model) { |
||
66 |
2 |
hpp::fcl::SplitMethodType split_method(hpp::fcl::SPLIT_METHOD_MEAN); |
|
67 |
✓✗ | 2 |
model.bv_splitter.reset(new BVSplitter<OBBRSS>(split_method)); |
68 |
✓✗ | 2 |
model.bv_splitter.reset(new BVSplitter<OBBRSS>(split_method)); |
69 |
|||
70 |
2 |
model.beginModel(); |
|
71 |
2 |
model.addSubModel(vertices, triangles); |
|
72 |
2 |
model.endModel(); |
|
73 |
2 |
} |
|
74 |
|||
75 |
hpp::fcl::OcTree makeOctree(const BVHModel<OBBRSS>& mesh, |
||
76 |
const FCL_REAL& resolution) { |
||
77 |
Vec3f m(mesh.aabb_local.min_); |
||
78 |
Vec3f M(mesh.aabb_local.max_); |
||
79 |
hpp::fcl::Box box(resolution, resolution, resolution); |
||
80 |
CollisionRequest request(hpp::fcl::CONTACT | hpp::fcl::DISTANCE_LOWER_BOUND, |
||
81 |
1); |
||
82 |
CollisionResult result; |
||
83 |
Transform3f tfBox; |
||
84 |
octomap::OcTreePtr_t octree(new octomap::OcTree(resolution)); |
||
85 |
|||
86 |
for (FCL_REAL x = resolution * floor(m[0] / resolution); x <= M[0]; |
||
87 |
x += resolution) { |
||
88 |
for (FCL_REAL y = resolution * floor(m[1] / resolution); y <= M[1]; |
||
89 |
y += resolution) { |
||
90 |
for (FCL_REAL z = resolution * floor(m[2] / resolution); z <= M[2]; |
||
91 |
z += resolution) { |
||
92 |
Vec3f center(x + .5 * resolution, y + .5 * resolution, |
||
93 |
z + .5 * resolution); |
||
94 |
tfBox.setTranslation(center); |
||
95 |
hpp::fcl::collide(&box, tfBox, &mesh, Transform3f(), request, result); |
||
96 |
if (result.isCollision()) { |
||
97 |
octomap::point3d p((float)center[0], (float)center[1], |
||
98 |
(float)center[2]); |
||
99 |
octree->updateNode(p, true); |
||
100 |
result.clear(); |
||
101 |
} |
||
102 |
} |
||
103 |
} |
||
104 |
} |
||
105 |
|||
106 |
octree->updateInnerOccupancy(); |
||
107 |
octree->writeBinary("./env.octree"); |
||
108 |
return OcTree(octree); |
||
109 |
} |
||
110 |
|||
111 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(OCTREE) { |
112 |
Eigen::IOFormat tuple(Eigen::FullPrecision, Eigen::DontAlignCols, "", ", ", |
||
113 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
6 |
"", "", "(", ")"); |
114 |
2 |
FCL_REAL resolution(10.); |
|
115 |
4 |
std::vector<Vec3f> pRob, pEnv; |
|
116 |
4 |
std::vector<Triangle> tRob, tEnv; |
|
117 |
✓✗ | 4 |
boost::filesystem::path path(TEST_RESOURCES_DIR); |
118 |
✓✗✓✗ ✓✗ |
2 |
loadOBJFile((path / "rob.obj").string().c_str(), pRob, tRob); |
119 |
✓✗✓✗ ✓✗ |
2 |
loadOBJFile((path / "env.obj").string().c_str(), pEnv, tEnv); |
120 |
|||
121 |
✓✗✓✗ |
4 |
BVHModel<OBBRSS> robMesh, envMesh; |
122 |
// Build meshes with robot and environment |
||
123 |
✓✗ | 2 |
makeMesh(pRob, tRob, robMesh); |
124 |
✓✗ | 2 |
makeMesh(pEnv, tEnv, envMesh); |
125 |
// Build octomap with environment |
||
126 |
✓✗ | 2 |
envMesh.computeLocalAABB(); |
127 |
// Load octree built from envMesh by makeOctree(envMesh, resolution) |
||
128 |
OcTree envOctree( |
||
129 |
✓✗✓✗ ✓✗ |
6 |
hpp::fcl::loadOctreeFile((path / "env.octree").string(), resolution)); |
130 |
|||
131 |
✓✗✓✗ |
2 |
std::cout << "Finished loading octree." << std::endl; |
132 |
|||
133 |
4 |
std::vector<Transform3f> transforms; |
|
134 |
2 |
FCL_REAL extents[] = {-2000, -2000, 0, 2000, 2000, 2000}; |
|
135 |
#ifndef NDEBUG // if debug mode |
||
136 |
2 |
std::size_t N = 100; |
|
137 |
#else |
||
138 |
std::size_t N = 10000; |
||
139 |
#endif |
||
140 |
✓✗ | 2 |
N = hpp::fcl::getNbRun(utf::master_test_suite().argc, |
141 |
✓✗✓✗ |
2 |
utf::master_test_suite().argv, N); |
142 |
|||
143 |
✓✗ | 2 |
generateRandomTransforms(extents, transforms, 2 * N); |
144 |
|||
145 |
CollisionRequest request(hpp::fcl::CONTACT | hpp::fcl::DISTANCE_LOWER_BOUND, |
||
146 |
✓✗ | 2 |
1); |
147 |
✓✓ | 202 |
for (std::size_t i = 0; i < N; ++i) { |
148 |
✓✗ | 400 |
CollisionResult resultMesh; |
149 |
✓✗ | 400 |
CollisionResult resultOctree; |
150 |
✓✗ | 200 |
Transform3f tf1(transforms[2 * i]); |
151 |
✓✗ | 200 |
Transform3f tf2(transforms[2 * i + 1]); |
152 |
; |
||
153 |
// Test collision between meshes with random transform for robot. |
||
154 |
✓✗ | 200 |
hpp::fcl::collide(&robMesh, tf1, &envMesh, tf2, request, resultMesh); |
155 |
// Test collision between mesh and octree for the same transform. |
||
156 |
✓✗ | 200 |
hpp::fcl::collide(&robMesh, tf1, &envOctree, tf2, request, resultOctree); |
157 |
200 |
bool resMesh(resultMesh.isCollision()); |
|
158 |
200 |
bool resOctree(resultOctree.isCollision()); |
|
159 |
✓✗✓✗ ✓✗✓✓ ✓✗✓✗ ✓✗✗✓ |
200 |
BOOST_CHECK(!resMesh || resOctree); |
160 |
✓✓✗✓ |
200 |
if (!resMesh && resOctree) { |
161 |
hpp::fcl::DistanceRequest dreq; |
||
162 |
hpp::fcl::DistanceResult dres; |
||
163 |
hpp::fcl::distance(&robMesh, tf1, &envMesh, tf2, dreq, dres); |
||
164 |
std::cout << "distance mesh mesh: " << dres.min_distance << std::endl; |
||
165 |
BOOST_CHECK(dres.min_distance < sqrt(2.) * resolution); |
||
166 |
} |
||
167 |
} |
||
168 |
2 |
} |
| Generated by: GCOVR (Version 4.2) |