35
35
36
36
#include < coal/BVH/BVH_model.h>
37
37
#include < coal/mesh_loader/loader.h>
38
- #include < coal/shape/geometric_shapes.h>
39
38
#include < coal/octree.h>
39
+ #include < coal/shape/geometric_shapes.h>
40
40
41
41
#include < hpp/corbaserver/server-plugin.hh>
42
42
#include < hpp/pinocchio/collision-object.hh>
@@ -88,26 +88,28 @@ void Obstacle::loadPointCloudFromFilename(const char* objectName,
88
88
try {
89
89
core::ProblemSolverPtr_t ps = problemSolver ();
90
90
pinocchio::GeomModelPtr_t gm = ps->obstacleGeomModel ();
91
-
91
+
92
92
if (gm->existGeometryName (objectName)) {
93
93
// check that the existing geometry is of type coal::OcTree
94
94
::pinocchio::GeomIndex id = gm->getGeometryId (objectName);
95
95
coal::shared_ptr<coal::OcTree> octree =
96
- coal::dynamic_pointer_cast<coal::OcTree>
97
- ( gm->geometryObjects [id].geometry );
96
+ coal::dynamic_pointer_cast<coal::OcTree>(
97
+ gm->geometryObjects [id].geometry );
98
98
if (!octree) {
99
- HPP_THROW (Error, " Obstacle " << objectName << " exists but is not of the correct type." );
99
+ HPP_THROW (Error, " Obstacle "
100
+ << objectName
101
+ << " exists but is not of the correct type." );
100
102
}
101
- std::shared_ptr<octomap::OcTree> tree = std::const_pointer_cast<octomap::OcTree>(octree->getTree ());
103
+ std::shared_ptr<octomap::OcTree> tree =
104
+ std::const_pointer_cast<octomap::OcTree>(octree->getTree ());
102
105
// Update octree
103
106
tree->clear ();
104
107
tree->readBinary (filename);
105
108
octree->computeLocalAABB ();
106
109
} else {
107
110
std::shared_ptr<octomap::OcTree> octree (new octomap::OcTree (filename));
108
- coal::CollisionObject colobj (
109
- coal::CollisionGeometryPtr_t (new coal::OcTree (octree))
110
- );
111
+ coal::CollisionObject colobj (
112
+ coal::CollisionGeometryPtr_t (new coal::OcTree (octree)));
111
113
ps->addObstacle (objectName, colobj, true , true );
112
114
}
113
115
@@ -117,7 +119,7 @@ void Obstacle::loadPointCloudFromFilename(const char* objectName,
117
119
}
118
120
119
121
void Obstacle::loadPointCloudFromPoints (const char * objectName,
120
- const CORBA::Double resolution,
122
+ const CORBA::Double resolution,
121
123
const floatSeqSeq& points) {
122
124
auto N = points.length ();
123
125
if (N > 0 && points[0 ].length () != 3 ) {
@@ -126,32 +128,38 @@ void Obstacle::loadPointCloudFromPoints(const char* objectName,
126
128
try {
127
129
core::ProblemSolverPtr_t ps = problemSolver ();
128
130
pinocchio::GeomModelPtr_t gm = ps->obstacleGeomModel ();
129
-
131
+
130
132
coal::shared_ptr<coal::OcTree> octree;
131
133
std::shared_ptr<octomap::OcTree> tree;
132
134
if (gm->existGeometryName (objectName)) {
133
135
// check that the existing geometry is of type coal::OcTree
134
136
::pinocchio::GeomIndex id = gm->getGeometryId (objectName);
135
- octree = coal::dynamic_pointer_cast<coal::OcTree> (gm->geometryObjects [id].geometry );
137
+ octree = coal::dynamic_pointer_cast<coal::OcTree>(
138
+ gm->geometryObjects [id].geometry );
136
139
if (!octree) {
137
- HPP_THROW (Error, " Obstacle " << objectName << " exists but is not of the correct type." );
140
+ HPP_THROW (Error, " Obstacle "
141
+ << objectName
142
+ << " exists but is not of the correct type." );
138
143
}
139
144
tree = std::const_pointer_cast<octomap::OcTree>(octree->getTree ());
140
145
if (tree->getResolution () != resolution) {
141
- HPP_THROW (Error, " A point cloud with name " << objectName <<
142
- " exists with resolution " << tree->getResolution () <<
143
- " , which is different from the asked resolution (" << resolution
144
- << " ).\n You can either remove the obstacle or stick the existing "
145
- " resolution."
146
- );
146
+ HPP_THROW (Error,
147
+ " A point cloud with name "
148
+ << objectName << " exists with resolution "
149
+ << tree->getResolution ()
150
+ << " , which is different from the asked resolution ("
151
+ << resolution
152
+ << " ).\n You can either remove the obstacle or stick the "
153
+ " existing "
154
+ " resolution." );
147
155
}
148
156
tree->clear ();
149
157
} else {
150
158
tree = std::make_shared<octomap::OcTree>(resolution);
151
159
octree = std::make_shared<coal::OcTree>(tree);
152
- coal::CollisionObject colobj (
153
- octree,
154
- false // Dont compute the local AABB as the octree is not ready yet.
160
+ coal::CollisionObject colobj (
161
+ octree,
162
+ false // Dont compute the local AABB as the octree is not ready yet.
155
163
);
156
164
ps->addObstacle (objectName, colobj, true , true );
157
165
}
0 commit comments