GCC Code Coverage Report

Directory: ./
File: test/utility.h
Date: 2025-04-13 14:25:19
Exec Total Coverage
Lines: 5 5 100.0%
Branches: 0 0 -%
Line Branch Exec Source
1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011-2014, Willow Garage, Inc.
5 * Copyright (c) 2014-2015, 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 #ifndef TEST_COAL_UTILITY_H
39 #define TEST_COAL_UTILITY_H
40
41 #include "coal/math/transform.h"
42 #include "coal/collision_data.h"
43 #include "coal/collision_object.h"
44 #include "coal/broadphase/default_broadphase_callbacks.h"
45 #include "coal/shape/convex.h"
46
47 #ifdef COAL_HAS_OCTOMAP
48 #include "coal/octree.h"
49 #endif
50
51 #ifdef _WIN32
52 #include <windows.h>
53 #else
54 #include <sys/time.h>
55 #endif
56
57 #define EIGEN_VECTOR_IS_APPROX(Va, Vb, precision) \
58 BOOST_CHECK_MESSAGE(((Va) - (Vb)).isZero(precision), \
59 "check " #Va ".isApprox(" #Vb ") failed at precision " \
60 << precision << " [\n" \
61 << (Va).transpose() << "\n!=\n" \
62 << (Vb).transpose() << "\n]")
63
64 #define EIGEN_MATRIX_IS_APPROX(Va, Vb, precision) \
65 BOOST_CHECK_MESSAGE(((Va) - (Vb)).isZero(precision), \
66 "check " #Va ".isApprox(" #Vb ") failed at precision " \
67 << precision << " [\n" \
68 << (Va) << "\n!=\n" \
69 << (Vb) << "\n]")
70
71 #define Scalar_IS_APPROX(Va, Vb, precision) \
72 BOOST_CHECK_MESSAGE(std::abs((Va) - (Vb)) < precision, \
73 "check " #Va ".isApprox(" #Vb ") failed at precision " \
74 << precision << " [\n" \
75 << Va << "\n!=\n" \
76 << Vb << "\n]")
77
78 namespace octomap {
79 #ifdef COAL_HAS_OCTOMAP
80 typedef coal::shared_ptr<octomap::OcTree> OcTreePtr_t;
81 #endif
82 } // namespace octomap
83 namespace coal {
84
85 class BenchTimer {
86 public:
87 BenchTimer();
88 ~BenchTimer();
89
90 void start(); ///< start timer
91 void stop(); ///< stop the timer
92 double getElapsedTime(); ///< get elapsed time in milli-second
93 double getElapsedTimeInSec(); ///< get elapsed time in second (same as
94 ///< getElapsedTime)
95 double getElapsedTimeInMilliSec(); ///< get elapsed time in milli-second
96 double getElapsedTimeInMicroSec(); ///< get elapsed time in micro-second
97
98 private:
99 double startTimeInMicroSec; ///< starting time in micro-second
100 double endTimeInMicroSec; ///< ending time in micro-second
101 int stopped; ///< stop flag
102 #ifdef _WIN32
103 LARGE_INTEGER frequency; ///< ticks per second
104 LARGE_INTEGER startCount;
105 LARGE_INTEGER endCount;
106 #else
107 timeval startCount;
108 timeval endCount;
109 #endif
110 };
111
112 struct TStruct {
113 std::vector<double> records;
114 double overall_time;
115
116 459 TStruct() { overall_time = 0; }
117
118 36540 void push_back(double t) {
119 36540 records.push_back(t);
120 36540 overall_time += t;
121 36540 }
122 };
123
124 extern const Eigen::IOFormat vfmt;
125 extern const Eigen::IOFormat pyfmt;
126 typedef Eigen::AngleAxis<Scalar> AngleAxis;
127 extern const Vec3s UnitX;
128 extern const Vec3s UnitY;
129 extern const Vec3s UnitZ;
130
131 /// @brief Load an obj mesh file
132 void loadOBJFile(const char* filename, std::vector<Vec3s>& points,
133 std::vector<Triangle>& triangles);
134
135 void saveOBJFile(const char* filename, std::vector<Vec3s>& points,
136 std::vector<Triangle>& triangles);
137
138 #ifdef COAL_HAS_OCTOMAP
139 coal::OcTree loadOctreeFile(const std::string& filename,
140 const Scalar& resolution);
141 #endif
142
143 /// @brief Generate one random transform whose translation is constrained by
144 /// extents and rotation without constraints. The translation is (x, y, z), and
145 /// extents[0] <= x <= extents[3], extents[1] <= y <= extents[4], extents[2] <=
146 /// z <= extents[5]
147 void generateRandomTransform(Scalar extents[6], Transform3s& transform);
148
149 /// @brief Generate n random transforms whose translations are constrained by
150 /// extents.
151 void generateRandomTransforms(Scalar extents[6],
152 std::vector<Transform3s>& transforms,
153 std::size_t n);
154
155 /// @brief Generate n random transforms whose translations are constrained by
156 /// extents. Also generate another transforms2 which have additional random
157 /// translation & rotation to the transforms generated.
158 void generateRandomTransforms(Scalar extents[6], Scalar delta_trans[3],
159 Scalar delta_rot,
160 std::vector<Transform3s>& transforms,
161 std::vector<Transform3s>& transforms2,
162 std::size_t n);
163
164 /// @ brief Structure for minimum distance between two meshes and the
165 /// corresponding nearest point pair
166 struct DistanceRes {
167 Scalar distance;
168 Vec3s p1;
169 Vec3s p2;
170 };
171
172 /// @brief Default collision callback for two objects o1 and o2 in broad phase.
173 /// return value means whether the broad phase can stop now.
174 bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2,
175 void* cdata);
176
177 /// @brief Default distance callback for two objects o1 and o2 in broad phase.
178 /// return value means whether the broad phase can stop now. also return dist,
179 /// i.e. the bmin distance till now
180 bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2,
181 void* cdata, Scalar& dist);
182
183 std::string getNodeTypeName(NODE_TYPE node_type);
184
185 Quats makeQuat(Scalar w, Scalar x, Scalar y, Scalar z);
186
187 std::ostream& operator<<(std::ostream& os, const Transform3s& tf);
188
189 /// Get the argument --nb-run from argv
190 std::size_t getNbRun(const int& argc, char const* const* argv,
191 std::size_t defaultValue);
192
193 void generateEnvironments(std::vector<CollisionObject*>& env, Scalar env_scale,
194 std::size_t n);
195
196 void generateEnvironmentsMesh(std::vector<CollisionObject*>& env,
197 Scalar env_scale, std::size_t n);
198
199 /// @brief Constructs a box with halfsides (l, w, d), centered around 0.
200 /// The box is 2*l wide on the x-axis, 2*w wide on the y-axis and 2*d wide on
201 /// the z-axis.
202 Convex<Quadrilateral> buildBox(Scalar l, Scalar w, Scalar d);
203
204 /// @brief We give an ellipsoid as input. The output is a 20 faces polytope
205 /// which vertices belong to the original ellipsoid surface. The procedure is
206 /// simple: we construct a icosahedron, see
207 /// https://sinestesia.co/blog/tutorials/python-icospheres/ . We then apply an
208 /// ellipsoid tranformation to each vertex of the icosahedron.
209 Convex<Triangle> constructPolytopeFromEllipsoid(const Ellipsoid& ellipsoid);
210
211 Box makeRandomBox(Scalar min_size, Scalar max_size);
212
213 Sphere makeRandomSphere(Scalar min_size, Scalar max_size);
214
215 Ellipsoid makeRandomEllipsoid(Scalar min_size, Scalar max_size);
216
217 Capsule makeRandomCapsule(std::array<Scalar, 2> min_size,
218 std::array<Scalar, 2> max_size);
219
220 Cone makeRandomCone(std::array<Scalar, 2> min_size,
221 std::array<Scalar, 2> max_size);
222
223 Cylinder makeRandomCylinder(std::array<Scalar, 2> min_size,
224 std::array<Scalar, 2> max_size);
225
226 Convex<Triangle> makeRandomConvex(Scalar min_size, Scalar max_size);
227
228 Plane makeRandomPlane(Scalar min_size, Scalar max_size);
229
230 Halfspace makeRandomHalfspace(Scalar min_size, Scalar max_size);
231
232 std::shared_ptr<ShapeBase> makeRandomGeometry(NODE_TYPE node_type);
233
234 } // namespace coal
235
236 #endif
237