moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
collision_env_fcl.cpp
Go to the documentation of this file.
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, Willow Garage, Inc.
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 the copyright holder 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: Ioan Sucan, Jens Petit */
36
40
42#include <rclcpp/logger.hpp>
43#include <rclcpp/logging.hpp>
45
46#if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0))
47#include <fcl/broadphase/broadphase_dynamic_AABB_tree.h>
48#endif
49
50namespace collision_detection
51{
52const std::string CollisionDetectorAllocatorFCL::NAME("FCL");
53
54namespace
55{
56rclcpp::Logger getLogger()
57{
58 return moveit::getLogger("moveit.core.collision_detection_fcl");
59}
60
61// Check whether this FCL version supports the requested computations
62void checkFCLCapabilities(const DistanceRequest& req)
63{
64#if MOVEIT_FCL_VERSION < FCL_VERSION_CHECK(0, 6, 0)
65 if (req.enable_nearest_points)
66 {
67 // Known issues:
68 // https://github.com/flexible-collision-library/fcl/issues/171,
69 // https://github.com/flexible-collision-library/fcl/pull/288
70 rclcpp::Clock steady_clock(RCL_STEADY_TIME);
71 RCLCPP_ERROR_THROTTLE(getLogger(), steady_clock, 2000,
72 "You requested a distance check with enable_nearest_points=true, "
73 "but the FCL version MoveIt was compiled against (%d.%d.%d) "
74 "is known to return bogus nearest points. Please update your FCL "
75 "to at least 0.6.0.",
76 FCL_MAJOR_VERSION, FCL_MINOR_VERSION, FCL_PATCH_VERSION);
77 }
78#else
79 static_cast<void>(req); // silent -Wunused-parameter
80#endif
81}
82} // namespace
83
84CollisionEnvFCL::CollisionEnvFCL(const moveit::core::RobotModelConstPtr& model, double padding, double scale)
85 : CollisionEnv(model, padding, scale)
86{
87 const std::vector<const moveit::core::LinkModel*>& links = robot_model_->getLinkModelsWithCollisionGeometry();
88 std::size_t index;
89 robot_geoms_.resize(robot_model_->getLinkGeometryCount());
90 robot_fcl_objs_.resize(robot_model_->getLinkGeometryCount());
91 // we keep the same order of objects as what RobotState *::getLinkState() returns
92 for (auto link : links)
93 {
94 for (std::size_t j{ 0 }; j < link->getShapes().size(); ++j)
95 {
96 FCLGeometryConstPtr link_geometry = createCollisionGeometry(link->getShapes()[j], getLinkScale(link->getName()),
97 getLinkPadding(link->getName()), link, j);
98 if (link_geometry)
99 {
100 index = link->getFirstCollisionBodyTransformIndex() + j;
101 robot_geoms_[index] = link_geometry;
102
103 // Need to store the FCL object so the AABB does not get recreated every time.
104 // Every time this object is created, g->computeLocalAABB() is called which is
105 // very expensive and should only be calculated once. To update the AABB, use the
106 // collObj->setTransform and then call collObj->computeAABB() to transform the AABB.
108 std::make_shared<const fcl::CollisionObjectd>(link_geometry->collision_geometry_));
109 }
110 else
111 RCLCPP_ERROR(getLogger(), "Unable to construct collision geometry for link '%s'", link->getName().c_str());
112 }
113 }
114
115 manager_ = std::make_unique<fcl::DynamicAABBTreeCollisionManagerd>();
116
117 // request notifications about changes to new world
118 observer_handle_ = getWorld()->addObserver(
119 [this](const World::ObjectConstPtr& object, World::Action action) { notifyObjectChange(object, action); });
120}
121
122CollisionEnvFCL::CollisionEnvFCL(const moveit::core::RobotModelConstPtr& model, const WorldPtr& world, double padding,
123 double scale)
124 : CollisionEnv(model, world, padding, scale)
125{
126 const std::vector<const moveit::core::LinkModel*>& links = robot_model_->getLinkModelsWithCollisionGeometry();
127 std::size_t index;
128 robot_geoms_.resize(robot_model_->getLinkGeometryCount());
129 robot_fcl_objs_.resize(robot_model_->getLinkGeometryCount());
130 // we keep the same order of objects as what RobotState *::getLinkState() returns
131 for (auto link : links)
132 {
133 for (std::size_t j{ 0 }; j < link->getShapes().size(); ++j)
134 {
135 FCLGeometryConstPtr g = createCollisionGeometry(link->getShapes()[j], getLinkScale(link->getName()),
136 getLinkPadding(link->getName()), link, j);
137 if (g)
138 {
139 index = link->getFirstCollisionBodyTransformIndex() + j;
140 robot_geoms_[index] = g;
141
142 // Need to store the FCL object so the AABB does not get recreated every time.
143 // Every time this object is created, g->computeLocalAABB() is called which is
144 // very expensive and should only be calculated once. To update the AABB, use the
145 // collObj->setTransform and then call collObj->computeAABB() to transform the AABB.
146 robot_fcl_objs_[index] = std::make_shared<const fcl::CollisionObjectd>(g->collision_geometry_);
147 }
148 else
149 RCLCPP_ERROR(getLogger(), "Unable to construct collision geometry for link '%s'", link->getName().c_str());
150 }
151 }
152
153 manager_ = std::make_unique<fcl::DynamicAABBTreeCollisionManagerd>();
154
155 // request notifications about changes to new world
156 observer_handle_ = getWorld()->addObserver(
157 [this](const World::ObjectConstPtr& object, World::Action action) { notifyObjectChange(object, action); });
158 getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE);
159}
160
162{
163 getWorld()->removeObserver(observer_handle_);
164}
165
166CollisionEnvFCL::CollisionEnvFCL(const CollisionEnvFCL& other, const WorldPtr& world) : CollisionEnv(other, world)
167{
170
171 manager_ = std::make_unique<fcl::DynamicAABBTreeCollisionManagerd>();
172
173 fcl_objs_ = other.fcl_objs_;
174 for (auto& fcl_obj : fcl_objs_)
175 fcl_obj.second.registerTo(manager_.get());
176 // manager_->update();
177
178 // request notifications about changes to new world
179 observer_handle_ = getWorld()->addObserver(
180 [this](const World::ObjectConstPtr& object, World::Action action) { notifyObjectChange(object, action); });
181}
182
184 std::vector<FCLGeometryConstPtr>& geoms) const
185{
186 const std::vector<shapes::ShapeConstPtr>& shapes = ab->getShapes();
187 const size_t num_shapes{ shapes.size() };
188 geoms.reserve(num_shapes);
189 for (std::size_t i = 0; i < num_shapes; ++i)
190 {
191 FCLGeometryConstPtr co = createCollisionGeometry(shapes[i], getLinkScale(ab->getAttachedLinkName()),
192 getLinkPadding(ab->getAttachedLinkName()), ab, i);
193 if (co)
194 geoms.push_back(co);
195 }
196}
197
199{
200 for (std::size_t i{ 0 }; i < obj->shapes_.size(); ++i)
201 {
202 FCLGeometryConstPtr g = createCollisionGeometry(obj->shapes_[i], obj);
203 if (g)
204 {
205 auto co = new fcl::CollisionObjectd(g->collision_geometry_, transform2fcl(obj->global_shape_poses_[i]));
206 fcl_obj.collision_objects_.push_back(FCLCollisionObjectPtr(co));
207 fcl_obj.collision_geometry_.push_back(g);
208 }
209 }
210}
211
213{
214 fcl_obj.collision_objects_.reserve(robot_geoms_.size());
215 fcl::Transform3d fcl_tf;
216
217 for (std::size_t i{ 0 }; i < robot_geoms_.size(); ++i)
218 {
219 if (robot_geoms_[i] && robot_geoms_[i]->collision_geometry_)
220 {
221 transform2fcl(state.getCollisionBodyTransform(robot_geoms_[i]->collision_geometry_data_->ptr.link,
222 robot_geoms_[i]->collision_geometry_data_->shape_index),
223 fcl_tf);
224 auto coll_obj = new fcl::CollisionObjectd(*robot_fcl_objs_[i]);
225 coll_obj->setTransform(fcl_tf);
226 coll_obj->computeAABB();
227 fcl_obj.collision_objects_.push_back(FCLCollisionObjectPtr(coll_obj));
228 }
229 }
230
231 // TODO: Implement a method for caching fcl::CollisionObject's for moveit::core::AttachedBody's
232 std::vector<const moveit::core::AttachedBody*> ab;
233 state.getAttachedBodies(ab);
234 for (auto& body : ab)
235 {
236 std::vector<FCLGeometryConstPtr> objs;
237 getAttachedBodyObjects(body, objs);
238 const EigenSTL::vector_Isometry3d& ab_t = body->getGlobalCollisionBodyTransforms();
239 for (std::size_t k = 0; k < objs.size(); ++k)
240 {
241 if (objs[k]->collision_geometry_)
242 {
243 transform2fcl(ab_t[k], fcl_tf);
244 fcl_obj.collision_objects_.push_back(
245 std::make_shared<fcl::CollisionObjectd>(objs[k]->collision_geometry_, fcl_tf));
246 // we copy the shared ptr to the CollisionGeometryData, as this is not stored by the class itself,
247 // and would be destroyed when objs goes out of scope.
248 fcl_obj.collision_geometry_.push_back(objs[k]);
249 }
250 }
251 }
252}
253
255{
256 manager.manager_ = std::make_unique<fcl::DynamicAABBTreeCollisionManagerd>();
257
258 constructFCLObjectRobot(state, manager.object_);
259 manager.object_.registerTo(manager.manager_.get());
260}
261
263 const moveit::core::RobotState& state) const
264{
265 checkSelfCollisionHelper(req, res, state, nullptr);
266}
267
269 const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const
270{
271 checkSelfCollisionHelper(req, res, state, &acm);
272}
273
275 const moveit::core::RobotState& state,
276 const AllowedCollisionMatrix* acm) const
277{
278 FCLManager manager;
279 allocSelfCollisionBroadPhase(state, manager);
280 CollisionData cd(&req, &res, acm);
282 manager.manager_->collide(&cd, &collisionCallback);
283 if (req.distance)
284 {
285 DistanceRequest dreq;
286 DistanceResult dres;
287
288 dreq.group_name = req.group_name;
289 dreq.acm = acm;
291 distanceSelf(dreq, dres, state);
293 if (req.detailed_distance)
294 {
295 res.distance_result = dres;
296 }
297 }
298}
299
301 const moveit::core::RobotState& state) const
302{
303 checkRobotCollisionHelper(req, res, state, nullptr);
304}
305
307 const moveit::core::RobotState& state,
308 const AllowedCollisionMatrix& acm) const
309{
310 checkRobotCollisionHelper(req, res, state, &acm);
311}
312
314 const moveit::core::RobotState& /*state1*/,
315 const moveit::core::RobotState& /*state2*/) const
316{
317 RCLCPP_ERROR(getLogger(), "Continuous collision not implemented");
318}
319
321 const moveit::core::RobotState& /*state1*/,
322 const moveit::core::RobotState& /*state2*/,
323 const AllowedCollisionMatrix& /*acm*/) const
324{
325 RCLCPP_ERROR(getLogger(), "Not implemented");
326}
327
329 const moveit::core::RobotState& state,
330 const AllowedCollisionMatrix* acm) const
331{
332 FCLObject fcl_obj;
333 constructFCLObjectRobot(state, fcl_obj);
334
335 CollisionData cd(&req, &res, acm);
337 for (std::size_t i = 0; !cd.done_ && i < fcl_obj.collision_objects_.size(); ++i)
338 manager_->collide(fcl_obj.collision_objects_[i].get(), &cd, &collisionCallback);
339
340 if (req.distance)
341 {
342 DistanceRequest dreq;
343 DistanceResult dres;
344
345 dreq.group_name = req.group_name;
346 dreq.acm = acm;
348 distanceRobot(dreq, dres, state);
350 if (req.detailed_distance)
351 {
352 res.distance_result = dres;
353 }
354 }
355}
356
358 const moveit::core::RobotState& state) const
359{
360 checkFCLCapabilities(req);
361
362 FCLManager manager;
363 allocSelfCollisionBroadPhase(state, manager);
364 DistanceData drd(&req, &res);
365
366 manager.manager_->distance(&drd, &distanceCallback);
367}
368
370 const moveit::core::RobotState& state) const
371{
372 checkFCLCapabilities(req);
373
374 FCLObject fcl_obj;
375 constructFCLObjectRobot(state, fcl_obj);
376
377 DistanceData drd(&req, &res);
378 for (std::size_t i = 0; !drd.done && i < fcl_obj.collision_objects_.size(); ++i)
379 manager_->distance(fcl_obj.collision_objects_[i].get(), &drd, &distanceCallback);
380}
381
382void CollisionEnvFCL::updateFCLObject(const std::string& id)
383{
384 // remove FCL objects that correspond to this object
385 auto jt = fcl_objs_.find(id);
386 if (jt != fcl_objs_.end())
387 {
388 jt->second.unregisterFrom(manager_.get());
389 jt->second.clear();
390 }
391
392 // check to see if we have this object
393 auto it = getWorld()->find(id);
394 if (it != getWorld()->end())
395 {
396 // construct FCL objects that correspond to this object
397 if (jt != fcl_objs_.end())
398 {
399 constructFCLObjectWorld(it->second.get(), jt->second);
400 jt->second.registerTo(manager_.get());
401 }
402 else
403 {
404 constructFCLObjectWorld(it->second.get(), fcl_objs_[id]);
405 fcl_objs_[id].registerTo(manager_.get());
406 }
407 }
408 else
409 {
410 if (jt != fcl_objs_.end())
411 fcl_objs_.erase(jt);
412 }
413
414 // manager_->update();
415}
416
417void CollisionEnvFCL::setWorld(const WorldPtr& world)
418{
419 if (world == getWorld())
420 return;
421
422 // turn off notifications about old world
423 getWorld()->removeObserver(observer_handle_);
424
425 // clear out objects from old world
426 manager_->clear();
427 fcl_objs_.clear();
429
431
432 // request notifications about changes to new world
433 observer_handle_ = getWorld()->addObserver(
434 [this](const World::ObjectConstPtr& object, World::Action action) { notifyObjectChange(object, action); });
435
436 // get notifications any objects already in the new world
437 getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE);
438}
439
440void CollisionEnvFCL::notifyObjectChange(const ObjectConstPtr& obj, World::Action action)
441{
442 if (action == World::DESTROY)
443 {
444 auto it = fcl_objs_.find(obj->id_);
445 if (it != fcl_objs_.end())
446 {
447 it->second.unregisterFrom(manager_.get());
448 it->second.clear();
449 fcl_objs_.erase(it);
450 }
452 }
453 else if (action == World::MOVE_SHAPE)
454 {
455 auto it = fcl_objs_.find(obj->id_);
456 if (it == fcl_objs_.end())
457 {
458 RCLCPP_ERROR(getLogger(), "Cannot move shapes of unknown FCL object: '%s'", obj->id_.c_str());
459 return;
460 }
461
462 if (obj->global_shape_poses_.size() != it->second.collision_objects_.size())
463 {
464 RCLCPP_ERROR(getLogger(),
465 "Cannot move shapes, shape size mismatch between FCL object and world object: '%s'. Respectively "
466 "%zu and %zu.",
467 obj->id_.c_str(), it->second.collision_objects_.size(), it->second.collision_objects_.size());
468 return;
469 }
470
471 for (std::size_t i = 0; i < it->second.collision_objects_.size(); ++i)
472 {
473 it->second.collision_objects_[i]->setTransform(transform2fcl(obj->global_shape_poses_[i]));
474
475 // compute AABB, order matters
476 it->second.collision_geometry_[i]->collision_geometry_->computeLocalAABB();
477 it->second.collision_objects_[i]->computeAABB();
478 }
479
480 // update AABB in the FCL broadphase manager tree
481 // see https://github.com/moveit/moveit/pull/3601 for benchmarks
482 it->second.unregisterFrom(manager_.get());
483 it->second.registerTo(manager_.get());
484 }
485 else
486 {
487 updateFCLObject(obj->id_);
488 if (action & (World::DESTROY | World::REMOVE_SHAPE))
490 }
491}
492
493void CollisionEnvFCL::updatedPaddingOrScaling(const std::vector<std::string>& links)
494{
495 std::size_t index;
496 for (const auto& link : links)
497 {
498 const moveit::core::LinkModel* lmodel = robot_model_->getLinkModel(link);
499 if (lmodel)
500 {
501 for (std::size_t j{ 0 }; j < lmodel->getShapes().size(); ++j)
502 {
503 FCLGeometryConstPtr g = createCollisionGeometry(lmodel->getShapes()[j], getLinkScale(lmodel->getName()),
504 getLinkPadding(lmodel->getName()), lmodel, j);
505 if (g)
506 {
507 index = lmodel->getFirstCollisionBodyTransformIndex() + j;
508 robot_geoms_[index] = g;
509 robot_fcl_objs_[index] = std::make_shared<const fcl::CollisionObjectd>(g->collision_geometry_);
510 }
511 }
512 }
513 else
514 RCLCPP_ERROR(getLogger(), "Updating padding or scaling for unknown link: '%s'", link.c_str());
515 }
516}
517
518} // end of namespace collision_detection
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
FCL implementation of the CollisionEnv.
std::vector< FCLCollisionObjectConstPtr > robot_fcl_objs_
Vector of shared pointers to the FCL collision objects which make up the robot.
void constructFCLObjectRobot(const moveit::core::RobotState &state, FCLObject &fcl_obj) const
Out of the current robot state and its attached bodies construct an FCLObject which can then be used ...
void checkSelfCollision(const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state) const override
Check for robot self collision. Any collision between any pair of links is checked for,...
void setWorld(const WorldPtr &world) override
std::unique_ptr< fcl::BroadPhaseCollisionManagerd > manager_
FCL collision manager which handles the collision checking process.
void distanceRobot(const DistanceRequest &req, DistanceResult &res, const moveit::core::RobotState &state) const override
Compute the distance between a robot and the world.
void checkRobotCollisionHelper(const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state, const AllowedCollisionMatrix *acm) const
Bundles the different checkRobotCollision functions into a single function.
void checkRobotCollision(const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state) const override
Check whether the robot model is in collision with the world. Any collisions between a robot link and...
void distanceSelf(const DistanceRequest &req, DistanceResult &res, const moveit::core::RobotState &state) const override
The distance to self-collision given the robot is at state state.
void updatedPaddingOrScaling(const std::vector< std::string > &links) override
Updates the FCL collision geometry and objects saved in the CollisionRobotFCL members to reflect a ne...
void allocSelfCollisionBroadPhase(const moveit::core::RobotState &state, FCLManager &manager) const
Prepares for the collision check through constructing an FCL collision object out of the current robo...
void updateFCLObject(const std::string &id)
Updates the specified object in \m fcl_objs_ and in the manager from new data available in the World.
void constructFCLObjectWorld(const World::Object *obj, FCLObject &fcl_obj) const
Construct an FCL collision object from MoveIt's World::Object.
void checkSelfCollisionHelper(const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state, const AllowedCollisionMatrix *acm) const
Bundles the different checkSelfCollision functions into a single function.
std::vector< FCLGeometryConstPtr > robot_geoms_
Vector of shared pointers to the FCL geometry for the objects in fcl_objs_.
std::map< std::string, FCLObject > fcl_objs_
void getAttachedBodyObjects(const moveit::core::AttachedBody *ab, std::vector< FCLGeometryConstPtr > &geoms) const
Converts all shapes which make up an attached body into a vector of FCLGeometryConstPtr.
Provides the interface to the individual collision checking libraries.
virtual void setWorld(const WorldPtr &world)
moveit::core::RobotModelConstPtr robot_model_
The kinematic model corresponding to this collision model.
const moveit::core::RobotModelConstPtr & getRobotModel() const
The kinematic model corresponding to this collision model.
const std::map< std::string, double > & getLinkScale() const
Get the link scaling as a map (from link names to scale value)
const std::map< std::string, double > & getLinkPadding() const
Get the link paddings as a map (from link names to padding value)
Represents an action that occurred on an object in the world. Several bits may be set indicating seve...
Definition world.h:268
Object defining bodies that can be attached to robot links.
const std::string & getAttachedLinkName() const
Get the name of the link this body is attached to.
const std::vector< shapes::ShapeConstPtr > & getShapes() const
Get the shapes that make up this attached body.
A link from the robot. Contains the constant transform applied to the link and its geometry.
Definition link_model.h:72
int getFirstCollisionBodyTransformIndex() const
Definition link_model.h:97
const std::string & getName() const
The name of this link.
Definition link_model.h:86
const std::vector< shapes::ShapeConstPtr > & getShapes() const
Get shape associated to the collision geometry for this link.
Definition link_model.h:176
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition robot_state.h:90
void getAttachedBodies(std::vector< const AttachedBody * > &attached_bodies) const
Get all bodies attached to the model corresponding to this state.
const Eigen::Isometry3d & getCollisionBodyTransform(const std::string &link_name, std::size_t index)
Get the link transform w.r.t. the root link (model frame) of the RobotModel. This is typically the ro...
FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr &shape, const moveit::core::LinkModel *link, int shape_index)
Create new FCLGeometry object out of robot link model.
void cleanCollisionGeometryCache()
Increases the counter of the caches which can trigger the cleaning of expired entries from them.
bool collisionCallback(fcl::CollisionObjectd *o1, fcl::CollisionObjectd *o2, void *data)
Callback function used by the FCLManager used for each pair of collision objects to calculate object ...
std::shared_ptr< fcl::CollisionObjectd > FCLCollisionObjectPtr
void transform2fcl(const Eigen::Isometry3d &b, fcl::Transform3d &f)
Transforms an Eigen Isometry3d to FCL coordinate transformation.
std::shared_ptr< const fcl::CollisionObjectd > FCLCollisionObjectConstPtr
bool distanceCallback(fcl::CollisionObjectd *o1, fcl::CollisionObjectd *o2, void *data, double &min_dist)
Callback function used by the FCLManager used for each pair of collision objects to calculate collisi...
fcl::CollisionObject CollisionObjectd
Definition fcl_compat.h:50
fcl::Transform3f Transform3d
Definition fcl_compat.h:54
rclcpp::Logger getLogger(const std::string &name)
Creates a namespaced logger.
Definition logger.cpp:79
Definition world.h:49
Data structure which is passed to the collision callback function of the collision manager.
void enableGroup(const moveit::core::RobotModelConstPtr &robot_model)
Compute active_components_only_ based on the joint group specified in req_.
bool done_
Flag indicating whether collision checking is complete.
Representation of a collision checking request.
std::string group_name
The group name to check collisions for (optional; if empty, assume the complete robot)....
bool detailed_distance
If true, return detailed distance information. Distance must be set to true as well.
bool distance
If true, compute proximity distance.
Representation of a collision checking result.
DistanceResult distance_result
Distance data for each link.
double distance
Closest distance between two bodies.
Data structure which is passed to the distance callback function of the collision manager.
bool done
Indicates if distance query is finished.
Representation of a distance-reporting request.
void enableGroup(const moveit::core::RobotModelConstPtr &robot_model)
std::string group_name
The group name.
const AllowedCollisionMatrix * acm
The allowed collision matrix used to filter checks.
Result of a distance request.
DistanceResultsData minimum_distance
ResultsData for the two objects with the minimum distance.
double distance
The distance between two objects. If two objects are in collision, distance <= 0.
Bundles an FCLObject and a broadphase FCL collision manager.
std::shared_ptr< fcl::BroadPhaseCollisionManagerd > manager_
A general high-level object which consists of multiple FCLCollisionObjects. It is the top level data ...
std::vector< FCLCollisionObjectPtr > collision_objects_
std::vector< FCLGeometryConstPtr > collision_geometry_
Geometry data corresponding to collision_objects_.
void registerTo(fcl::BroadPhaseCollisionManagerd *manager)
A representation of an object.
Definition world.h:79
EigenSTL::vector_Isometry3d global_shape_poses_
The poses of the corresponding entries in shapes_, relative to the world frame.
Definition world.h:106
std::vector< shapes::ShapeConstPtr > shapes_
All the shapes making up this object.
Definition world.h:96