loader.cpp
Go to the documentation of this file.
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  * Copyright (c) 2016, CNRS - LAAS
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Open Source Robotics Foundation nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  */
36 
39 
40 #include <boost/filesystem.hpp>
41 
42 #ifdef HPP_FCL_HAS_OCTOMAP
43 #include <hpp/fcl/octree.h>
44 #endif
45 
46 #include <hpp/fcl/BV/BV.h>
47 
48 namespace hpp {
49 namespace fcl {
51  const CachedMeshLoader::Key& a = *this;
52  for (int i = 0; i < 3; ++i) {
53  if (a.scale[i] < b.scale[i])
54  return true;
55  else if (a.scale[i] > b.scale[i])
56  return false;
57  }
58  return std::less<std::string>()(a.filename, b.filename);
59 }
60 
61 template <typename BV>
62 BVHModelPtr_t _load(const std::string& filename, const Vec3f& scale) {
63  shared_ptr<BVHModel<BV> > polyhedron(new BVHModel<BV>);
64  loadPolyhedronFromResource(filename, scale, polyhedron);
65  return polyhedron;
66 }
67 
69  const Vec3f& scale) {
70  switch (bvType_) {
71  case BV_AABB:
72  return _load<AABB>(filename, scale);
73  case BV_OBB:
74  return _load<OBB>(filename, scale);
75  case BV_RSS:
76  return _load<RSS>(filename, scale);
77  case BV_kIOS:
78  return _load<kIOS>(filename, scale);
79  case BV_OBBRSS:
80  return _load<OBBRSS>(filename, scale);
81  case BV_KDOP16:
82  return _load<KDOP<16> >(filename, scale);
83  case BV_KDOP18:
84  return _load<KDOP<18> >(filename, scale);
85  case BV_KDOP24:
86  return _load<KDOP<24> >(filename, scale);
87  default:
88  throw std::invalid_argument("Unhandled bouding volume type.");
89  }
90 }
91 
93 #ifdef HPP_FCL_HAS_OCTOMAP
94  shared_ptr<octomap::OcTree> octree(new octomap::OcTree(filename));
96 #else
97  throw std::logic_error(
98  "hpp-fcl compiled without OctoMap. Cannot create OcTrees.");
99 #endif
100 }
101 
103  const Vec3f& scale) {
104  Key key(filename, scale);
105 
106  std::time_t mtime = 0;
107  try {
108  mtime = boost::filesystem::last_write_time(filename);
109 
110  Cache_t::const_iterator _cached = cache_.find(key);
111  if (_cached != cache_.end() && _cached->second.mtime == mtime)
112  // File found in cache and mtime is the same
113  return _cached->second.model;
114  } catch (boost::filesystem::filesystem_error&) {
115  // Could not stat. Make sure we will try to load the file so that
116  // there will be a file not found error.
117  }
118 
119  BVHModelPtr_t geom = MeshLoader::load(filename, scale);
120  Value val;
121  val.model = geom;
122  val.mtime = mtime;
123  cache_[key] = val;
124  return geom;
125 }
126 } // namespace fcl
127 
128 } // namespace hpp
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
hpp::fcl::BV_KDOP24
@ BV_KDOP24
Definition: collision_object.h:74
hpp::fcl::loadPolyhedronFromResource
void loadPolyhedronFromResource(const std::string &resource_path, const fcl::Vec3f &scale, const shared_ptr< BVHModel< BoundingVolume > > &polyhedron)
Read a mesh file and convert it to a polyhedral mesh.
Definition: assimp.h:118
hpp::fcl::CachedMeshLoader::Key::operator<
bool operator<(const CachedMeshLoader::Key &b) const
Definition: loader.cpp:50
hpp::fcl::BV_RSS
@ BV_RSS
Definition: collision_object.h:69
hpp::fcl::CachedMeshLoader::Value::model
BVHModelPtr_t model
Definition: loader.h:91
hpp::fcl::BV_OBB
@ BV_OBB
Definition: collision_object.h:68
octree
Definition: octree.py:1
hpp::fcl::CollisionGeometryPtr_t
shared_ptr< CollisionGeometry > CollisionGeometryPtr_t
Definition: include/hpp/fcl/fwd.hh:96
hpp::fcl::_load
BVHModelPtr_t _load(const std::string &filename, const Vec3f &scale)
Definition: loader.cpp:62
hpp::fcl::CachedMeshLoader::Value
Definition: loader.h:90
a
list a
octomap::OcTree
hpp::fcl::CachedMeshLoader::Key
Definition: loader.h:82
hpp::fcl::BV_OBBRSS
@ BV_OBBRSS
Definition: collision_object.h:71
hpp::fcl::BV_AABB
@ BV_AABB
Definition: collision_object.h:67
collision-bench.filename
filename
Definition: collision-bench.py:6
hpp::fcl::CachedMeshLoader::Value::mtime
std::time_t mtime
Definition: loader.h:92
BV.h
hpp
Main namespace.
Definition: broadphase_bruteforce.h:44
hpp::fcl::CachedMeshLoader::load
virtual BVHModelPtr_t load(const std::string &filename, const Vec3f &scale)
Definition: loader.cpp:102
generate_distance_plot.b
float b
Definition: generate_distance_plot.py:7
hpp::fcl::MeshLoader::bvType_
const NODE_TYPE bvType_
Definition: loader.h:67
hpp::fcl::BVHModel
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition: BVH/BVH_model.h:273
hpp::fcl::BV_KDOP18
@ BV_KDOP18
Definition: collision_object.h:73
assimp.h
loader.h
hpp::fcl::MeshLoader::load
virtual BVHModelPtr_t load(const std::string &filename, const Vec3f &scale=Vec3f::Ones())
Definition: loader.cpp:68
hpp::fcl::OcTree
Octree is one type of collision geometry which can encode uncertainty information in the sensor data.
Definition: octree.h:53
octree.h
hpp::fcl::BV_kIOS
@ BV_kIOS
Definition: collision_object.h:70
hpp::fcl::CachedMeshLoader::cache_
Cache_t cache_
Definition: loader.h:99
hpp::fcl::MeshLoader::loadOctree
virtual CollisionGeometryPtr_t loadOctree(const std::string &filename)
Definition: loader.cpp:92
hpp::fcl::BV_KDOP16
@ BV_KDOP16
Definition: collision_object.h:72
hpp::fcl::BVHModelPtr_t
shared_ptr< BVHModelBase > BVHModelPtr_t
Definition: include/hpp/fcl/fwd.hh:103


hpp-fcl
Author(s):
autogenerated on Fri Jan 26 2024 03:46:14