collision_common.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 Willow Garage 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, Jia Pan */
36 
39 #include <fcl/BVH/BVH_model.h>
40 #include <fcl/shape/geometric_shapes.h>
41 #include <fcl/octree.h>
42 #include <boost/thread/mutex.hpp>
43 #include <memory>
44 
45 namespace collision_detection
46 {
47 bool collisionCallback(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* data)
48 {
49  CollisionData* cdata = reinterpret_cast<CollisionData*>(data);
50  if (cdata->done_)
51  return true;
52  const CollisionGeometryData* cd1 = static_cast<const CollisionGeometryData*>(o1->collisionGeometry()->getUserData());
53  const CollisionGeometryData* cd2 = static_cast<const CollisionGeometryData*>(o2->collisionGeometry()->getUserData());
54 
55  // do not collision check geoms part of the same object / link / attached body
56  if (cd1->sameObject(*cd2))
57  return false;
58 
59  // If active components are specified
60  if (cdata->active_components_only_)
61  {
62  const robot_model::LinkModel* l1 =
63  cd1->type == BodyTypes::ROBOT_LINK ?
64  cd1->ptr.link :
65  (cd1->type == BodyTypes::ROBOT_ATTACHED ? cd1->ptr.ab->getAttachedLink() : nullptr);
66  const robot_model::LinkModel* l2 =
67  cd2->type == BodyTypes::ROBOT_LINK ?
68  cd2->ptr.link :
69  (cd2->type == BodyTypes::ROBOT_ATTACHED ? cd2->ptr.ab->getAttachedLink() : nullptr);
70 
71  // If neither of the involved components is active
72  if ((!l1 || cdata->active_components_only_->find(l1) == cdata->active_components_only_->end()) &&
73  (!l2 || cdata->active_components_only_->find(l2) == cdata->active_components_only_->end()))
74  return false;
75  }
76 
77  // use the collision matrix (if any) to avoid certain collision checks
78  DecideContactFn dcf;
79  bool always_allow_collision = false;
80  if (cdata->acm_)
81  {
83  bool found = cdata->acm_->getAllowedCollision(cd1->getID(), cd2->getID(), type);
84  if (found)
85  {
86  // if we have an entry in the collision matrix, we read it
87  if (type == AllowedCollision::ALWAYS)
88  {
89  always_allow_collision = true;
90  if (cdata->req_->verbose)
92  "collision_detection.fcl", "Collision between '%s' (type '%s') and '%s' (type '%s') is always allowed. "
93  "No contacts are computed.",
94  cd1->getID().c_str(), cd1->getTypeString().c_str(), cd2->getID().c_str(), cd2->getTypeString().c_str());
95  }
96  else if (type == AllowedCollision::CONDITIONAL)
97  {
98  cdata->acm_->getAllowedCollision(cd1->getID(), cd2->getID(), dcf);
99  if (cdata->req_->verbose)
100  ROS_DEBUG_NAMED("collision_detection.fcl", "Collision between '%s' and '%s' is conditionally allowed",
101  cd1->getID().c_str(), cd2->getID().c_str());
102  }
103  }
104  }
105 
106  // check if a link is touching an attached object
108  {
109  const std::set<std::string>& tl = cd2->ptr.ab->getTouchLinks();
110  if (tl.find(cd1->getID()) != tl.end())
111  {
112  always_allow_collision = true;
113  if (cdata->req_->verbose)
114  ROS_DEBUG_NAMED("collision_detection.fcl",
115  "Robot link '%s' is allowed to touch attached object '%s'. No contacts are computed.",
116  cd1->getID().c_str(), cd2->getID().c_str());
117  }
118  }
119  else if (cd2->type == BodyTypes::ROBOT_LINK && cd1->type == BodyTypes::ROBOT_ATTACHED)
120  {
121  const std::set<std::string>& tl = cd1->ptr.ab->getTouchLinks();
122  if (tl.find(cd2->getID()) != tl.end())
123  {
124  always_allow_collision = true;
125  if (cdata->req_->verbose)
126  ROS_DEBUG_NAMED("collision_detection.fcl",
127  "Robot link '%s' is allowed to touch attached object '%s'. No contacts are computed.",
128  cd2->getID().c_str(), cd1->getID().c_str());
129  }
130  }
131  // bodies attached to the same link should not collide
133  {
134  if (cd1->ptr.ab->getAttachedLink() == cd2->ptr.ab->getAttachedLink())
135  always_allow_collision = true;
136  }
137 
138  // if collisions are always allowed, we are done
139  if (always_allow_collision)
140  return false;
141 
142  if (cdata->req_->verbose)
143  ROS_DEBUG_NAMED("collision_detection.fcl", "Actually checking collisions between %s and %s", cd1->getID().c_str(),
144  cd2->getID().c_str());
145 
146  // see if we need to compute a contact
147  std::size_t want_contact_count = 0;
148  if (cdata->req_->contacts)
149  if (cdata->res_->contact_count < cdata->req_->max_contacts)
150  {
151  std::size_t have;
152  if (cd1->getID() < cd2->getID())
153  {
154  std::pair<std::string, std::string> cp(cd1->getID(), cd2->getID());
155  have = cdata->res_->contacts.find(cp) != cdata->res_->contacts.end() ? cdata->res_->contacts[cp].size() : 0;
156  }
157  else
158  {
159  std::pair<std::string, std::string> cp(cd2->getID(), cd1->getID());
160  have = cdata->res_->contacts.find(cp) != cdata->res_->contacts.end() ? cdata->res_->contacts[cp].size() : 0;
161  }
162  if (have < cdata->req_->max_contacts_per_pair)
163  want_contact_count =
164  std::min(cdata->req_->max_contacts_per_pair - have, cdata->req_->max_contacts - cdata->res_->contact_count);
165  }
166 
167  if (dcf)
168  {
169  // if we have a decider for allowed contacts, we need to look at all the contacts
170  bool enable_cost = cdata->req_->cost;
171  std::size_t num_max_cost_sources = cdata->req_->max_cost_sources;
172  bool enable_contact = true;
173  fcl::CollisionResult col_result;
174  int num_contacts = fcl::collide(o1, o2, fcl::CollisionRequest(std::numeric_limits<size_t>::max(), enable_contact,
175  num_max_cost_sources, enable_cost),
176  col_result);
177  if (num_contacts > 0)
178  {
179  if (cdata->req_->verbose)
180  ROS_INFO_NAMED("collision_detection.fcl",
181  "Found %d contacts between '%s' and '%s'. "
182  "These contacts will be evaluated to check if they are accepted or not",
183  num_contacts, cd1->getID().c_str(), cd2->getID().c_str());
184  Contact c;
185  const std::pair<std::string, std::string>& pc = cd1->getID() < cd2->getID() ?
186  std::make_pair(cd1->getID(), cd2->getID()) :
187  std::make_pair(cd2->getID(), cd1->getID());
188  for (int i = 0; i < num_contacts; ++i)
189  {
190  fcl2contact(col_result.getContact(i), c);
191  // if the contact is not allowed, we have a collision
192  if (!dcf(c))
193  {
194  // store the contact, if it is needed
195  if (want_contact_count > 0)
196  {
197  --want_contact_count;
198  cdata->res_->contacts[pc].push_back(c);
199  cdata->res_->contact_count++;
200  if (cdata->req_->verbose)
201  ROS_INFO_NAMED("collision_detection.fcl",
202  "Found unacceptable contact between '%s' and '%s'. Contact was stored.",
203  cd1->getID().c_str(), cd2->getID().c_str());
204  }
205  else if (cdata->req_->verbose)
206  ROS_INFO_NAMED("collision_detection.fcl", "Found unacceptable contact between '%s' (type '%s') and '%s' "
207  "(type '%s'). Contact was stored.",
208  cd1->getID().c_str(), cd1->getTypeString().c_str(), cd2->getID().c_str(),
209  cd2->getTypeString().c_str());
210  cdata->res_->collision = true;
211  if (want_contact_count == 0)
212  break;
213  }
214  }
215  }
216 
217  if (enable_cost)
218  {
219  std::vector<fcl::CostSource> cost_sources;
220  col_result.getCostSources(cost_sources);
221 
222  CostSource cs;
223  for (auto& cost_source : cost_sources)
224  {
225  fcl2costsource(cost_source, cs);
226  cdata->res_->cost_sources.insert(cs);
227  while (cdata->res_->cost_sources.size() > cdata->req_->max_cost_sources)
228  cdata->res_->cost_sources.erase(--cdata->res_->cost_sources.end());
229  }
230  }
231  }
232  else
233  {
234  if (want_contact_count > 0)
235  {
236  // otherwise, we need to compute more things
237  bool enable_cost = cdata->req_->cost;
238  std::size_t num_max_cost_sources = cdata->req_->max_cost_sources;
239  bool enable_contact = true;
240 
241  fcl::CollisionResult col_result;
242  int num_contacts = fcl::collide(
243  o1, o2, fcl::CollisionRequest(want_contact_count, enable_contact, num_max_cost_sources, enable_cost),
244  col_result);
245  if (num_contacts > 0)
246  {
247  int num_contacts_initial = num_contacts;
248 
249  // make sure we don't get more contacts than we want
250  if (want_contact_count >= (std::size_t)num_contacts)
251  want_contact_count -= num_contacts;
252  else
253  {
254  num_contacts = want_contact_count;
255  want_contact_count = 0;
256  }
257 
258  if (cdata->req_->verbose)
259  ROS_INFO_NAMED("collision_detection.fcl", "Found %d contacts between '%s' (type '%s') and '%s' (type '%s'), "
260  "which constitute a collision. %d contacts will be stored",
261  num_contacts_initial, cd1->getID().c_str(), cd1->getTypeString().c_str(), cd2->getID().c_str(),
262  cd2->getTypeString().c_str(), num_contacts);
263 
264  const std::pair<std::string, std::string>& pc = cd1->getID() < cd2->getID() ?
265  std::make_pair(cd1->getID(), cd2->getID()) :
266  std::make_pair(cd2->getID(), cd1->getID());
267  cdata->res_->collision = true;
268  for (int i = 0; i < num_contacts; ++i)
269  {
270  Contact c;
271  fcl2contact(col_result.getContact(i), c);
272  cdata->res_->contacts[pc].push_back(c);
273  cdata->res_->contact_count++;
274  }
275  }
276 
277  if (enable_cost)
278  {
279  std::vector<fcl::CostSource> cost_sources;
280  col_result.getCostSources(cost_sources);
281 
282  CostSource cs;
283  for (auto& cost_source : cost_sources)
284  {
285  fcl2costsource(cost_source, cs);
286  cdata->res_->cost_sources.insert(cs);
287  while (cdata->res_->cost_sources.size() > cdata->req_->max_cost_sources)
288  cdata->res_->cost_sources.erase(--cdata->res_->cost_sources.end());
289  }
290  }
291  }
292  else
293  {
294  bool enable_cost = cdata->req_->cost;
295  std::size_t num_max_cost_sources = cdata->req_->max_cost_sources;
296  bool enable_contact = false;
297  fcl::CollisionResult col_result;
298  int num_contacts =
299  fcl::collide(o1, o2, fcl::CollisionRequest(1, enable_contact, num_max_cost_sources, enable_cost), col_result);
300  if (num_contacts > 0)
301  {
302  cdata->res_->collision = true;
303  if (cdata->req_->verbose)
304  ROS_INFO_NAMED("collision_detection.fcl", "Found a contact between '%s' (type '%s') and '%s' (type '%s'), "
305  "which constitutes a collision. "
306  "Contact information is not stored.",
307  cd1->getID().c_str(), cd1->getTypeString().c_str(), cd2->getID().c_str(),
308  cd2->getTypeString().c_str());
309  }
310 
311  if (enable_cost)
312  {
313  std::vector<fcl::CostSource> cost_sources;
314  col_result.getCostSources(cost_sources);
315 
316  CostSource cs;
317  for (auto& cost_source : cost_sources)
318  {
319  fcl2costsource(cost_source, cs);
320  cdata->res_->cost_sources.insert(cs);
321  while (cdata->res_->cost_sources.size() > cdata->req_->max_cost_sources)
322  cdata->res_->cost_sources.erase(--cdata->res_->cost_sources.end());
323  }
324  }
325  }
326  }
327 
328  if (cdata->res_->collision)
329  if (!cdata->req_->contacts || cdata->res_->contact_count >= cdata->req_->max_contacts)
330  {
331  if (!cdata->req_->cost)
332  cdata->done_ = true;
333  if (cdata->req_->verbose)
334  ROS_INFO_NAMED("collision_detection.fcl",
335  "Collision checking is considered complete (collision was found and %u contacts are stored)",
336  (unsigned int)cdata->res_->contact_count);
337  }
338 
339  if (!cdata->done_ && cdata->req_->is_done)
340  {
341  cdata->done_ = cdata->req_->is_done(*cdata->res_);
342  if (cdata->done_ && cdata->req_->verbose)
343  ROS_INFO_NAMED("collision_detection.fcl", "Collision checking is considered complete due to external callback. "
344  "%s was found. %u contacts are stored.",
345  cdata->res_->collision ? "Collision" : "No collision", (unsigned int)cdata->res_->contact_count);
346  }
347 
348  return cdata->done_;
349 }
350 
352 {
353  using ShapeKey = std::weak_ptr<const shapes::Shape>;
354  using ShapeMap = std::map<ShapeKey, FCLGeometryConstPtr, std::owner_less<ShapeKey>>;
355 
357  {
358  }
359 
360  void bumpUseCount(bool force = false)
361  {
362  clean_count_++;
363 
364  // clean-up for cache (we don't want to keep infinitely large number of weak ptrs stored)
365  if (clean_count_ > MAX_CLEAN_COUNT || force)
366  {
367  clean_count_ = 0;
368  for (auto it = map_.begin(); it != map_.end();)
369  {
370  auto nit = it;
371  ++nit;
372  if (it->first.expired())
373  map_.erase(it);
374  it = nit;
375  }
376  // ROS_DEBUG_NAMED("collision_detection.fcl", "Cleaning up cache for FCL objects that correspond to static
377  // shapes. Cache size
378  // reduced from %u
379  // to %u", from, (unsigned int)map_.size());
380  }
381  }
382 
383  static const unsigned int MAX_CLEAN_COUNT = 100; // every this many uses of the cache, a cleaning operation is
384  // executed (this is only removal of expired entries)
386  unsigned int clean_count_;
387  boost::mutex lock_;
388 };
389 
390 bool distanceCallback(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* data, double& min_dist)
391 {
392  DistanceData* cdata = reinterpret_cast<DistanceData*>(data);
393 
394  const CollisionGeometryData* cd1 = static_cast<const CollisionGeometryData*>(o1->collisionGeometry()->getUserData());
395  const CollisionGeometryData* cd2 = static_cast<const CollisionGeometryData*>(o2->collisionGeometry()->getUserData());
396 
397  // do not distance check for geoms part of the same object / link / attached body
398  if (cd1->sameObject(*cd2))
399  return false;
400 
401  // If active components are specified
402  if (cdata->req->active_components_only)
403  {
404  const robot_model::LinkModel* l1 =
405  cd1->type == BodyTypes::ROBOT_LINK ?
406  cd1->ptr.link :
407  (cd1->type == BodyTypes::ROBOT_ATTACHED ? cd1->ptr.ab->getAttachedLink() : nullptr);
408  const robot_model::LinkModel* l2 =
409  cd2->type == BodyTypes::ROBOT_LINK ?
410  cd2->ptr.link :
411  (cd2->type == BodyTypes::ROBOT_ATTACHED ? cd2->ptr.ab->getAttachedLink() : nullptr);
412 
413  // If neither of the involved components is active
414  if ((!l1 || cdata->req->active_components_only->find(l1) == cdata->req->active_components_only->end()) &&
415  (!l2 || cdata->req->active_components_only->find(l2) == cdata->req->active_components_only->end()))
416  {
417  return false;
418  }
419  }
420 
421  // use the collision matrix (if any) to avoid certain distance checks
422  bool always_allow_collision = false;
423  if (cdata->req->acm)
424  {
426 
427  bool found = cdata->req->acm->getAllowedCollision(cd1->getID(), cd2->getID(), type);
428  if (found)
429  {
430  // if we have an entry in the collision matrix, we read it
431  if (type == AllowedCollision::ALWAYS)
432  {
433  always_allow_collision = true;
434  if (cdata->req->verbose)
435  ROS_DEBUG_NAMED("collision_detection.fcl",
436  "Collision between '%s' and '%s' is always allowed. No distances are computed.",
437  cd1->getID().c_str(), cd2->getID().c_str());
438  }
439  }
440  }
441 
442  // check if a link is touching an attached object
444  {
445  const std::set<std::string>& tl = cd2->ptr.ab->getTouchLinks();
446  if (tl.find(cd1->getID()) != tl.end())
447  {
448  always_allow_collision = true;
449  if (cdata->req->verbose)
450  ROS_DEBUG_NAMED("collision_detection.fcl",
451  "Robot link '%s' is allowed to touch attached object '%s'. No distances are computed.",
452  cd1->getID().c_str(), cd2->getID().c_str());
453  }
454  }
455  else
456  {
458  {
459  const std::set<std::string>& tl = cd1->ptr.ab->getTouchLinks();
460  if (tl.find(cd2->getID()) != tl.end())
461  {
462  always_allow_collision = true;
463  if (cdata->req->verbose)
464  ROS_DEBUG_NAMED("collision_detection.fcl",
465  "Robot link '%s' is allowed to touch attached object '%s'. No distances are computed.",
466  cd2->getID().c_str(), cd1->getID().c_str());
467  }
468  }
469  }
470 
471  if (always_allow_collision)
472  {
473  return false;
474  }
475  if (cdata->req->verbose)
476  ROS_DEBUG_NAMED("collision_detection.fcl", "Actually checking collisions between %s and %s", cd1->getID().c_str(),
477  cd2->getID().c_str());
478 
479  fcl::DistanceResult fcl_result;
480  DistanceResultsData dist_result;
481  double dist_threshold = cdata->req->distance_threshold;
482 
483  const std::pair<std::string, std::string>& pc = cd1->getID() < cd2->getID() ?
484  std::make_pair(cd1->getID(), cd2->getID()) :
485  std::make_pair(cd2->getID(), cd1->getID());
486 
487  DistanceMap::iterator it = cdata->res->distances.find(pc);
488 
489  if (it != cdata->res->distances.end())
490  {
491  if (cdata->req->type == DistanceRequestType::LIMITED)
492  {
493  // If at the limit for a given pair just return
494  if (it->second.size() >= cdata->req->max_contacts_per_body)
495  {
496  return cdata->done;
497  }
498  }
499  else if (cdata->req->type == DistanceRequestType::GLOBAL)
500  {
501  dist_threshold = cdata->res->minimum_distance.distance;
502  }
503  else if (cdata->req->type == DistanceRequestType::SINGLE)
504  {
505  dist_threshold = it->second[0].distance;
506  }
507  }
508 
509  fcl_result.min_distance = dist_threshold;
510  double d = fcl::distance(o1, o2, fcl::DistanceRequest(cdata->req->enable_nearest_points), fcl_result);
511 
512  // Check if either object is already in the map. If not add it or if present
513  // check to see if the new distance is closer. If closer remove the existing
514  // one and add the new distance information.
515  if (d < dist_threshold)
516  {
517  dist_result.distance = fcl_result.min_distance;
518  dist_result.nearest_points[0] = Eigen::Vector3d(fcl_result.nearest_points[0].data.vs);
519  dist_result.nearest_points[1] = Eigen::Vector3d(fcl_result.nearest_points[1].data.vs);
520  dist_result.link_names[0] = cd1->getID();
521  dist_result.link_names[1] = cd2->getID();
522  dist_result.body_types[0] = cd1->type;
523  dist_result.body_types[1] = cd2->type;
524  if (cdata->req->enable_nearest_points)
525  {
526  dist_result.normal = (dist_result.nearest_points[1] - dist_result.nearest_points[0]).normalized();
527  }
528 
529  if (d <= 0 && cdata->req->enable_signed_distance)
530  {
531  dist_result.nearest_points[0].setZero();
532  dist_result.nearest_points[1].setZero();
533  dist_result.normal.setZero();
534 
535  fcl::CollisionRequest coll_req;
536  fcl::CollisionResult coll_res;
537  coll_req.enable_contact = true;
538  coll_req.num_max_contacts = 200;
539  std::size_t contacts = fcl::collide(o1, o2, coll_req, coll_res);
540  if (contacts > 0)
541  {
542  double max_dist = 0;
543  int max_index = 0;
544  for (std::size_t i = 0; i < contacts; ++i)
545  {
546  const fcl::Contact& contact = coll_res.getContact(i);
547  if (contact.penetration_depth > max_dist)
548  {
549  max_dist = contact.penetration_depth;
550  max_index = i;
551  }
552  }
553 
554  const fcl::Contact& contact = coll_res.getContact(max_index);
555  dist_result.distance = -contact.penetration_depth;
556  dist_result.nearest_points[0] = Eigen::Vector3d(contact.pos.data.vs);
557  dist_result.nearest_points[1] = Eigen::Vector3d(contact.pos.data.vs);
558  dist_result.normal = Eigen::Vector3d(contact.normal.data.vs);
559  }
560  }
561 
562  if (dist_result.distance < cdata->res->minimum_distance.distance)
563  {
564  cdata->res->minimum_distance = dist_result;
565  }
566 
567  if (dist_result.distance <= 0)
568  {
569  cdata->res->collision = true;
570  }
571 
572  if (cdata->req->type != DistanceRequestType::GLOBAL)
573  {
574  if (it == cdata->res->distances.end())
575  {
576  std::vector<DistanceResultsData> data;
577  data.reserve(cdata->req->type == DistanceRequestType::SINGLE ? 1 : cdata->req->max_contacts_per_body);
578  data.push_back(dist_result);
579  cdata->res->distances.insert(std::make_pair(pc, data));
580  }
581  else
582  {
583  if (cdata->req->type == DistanceRequestType::ALL)
584  {
585  it->second.push_back(dist_result);
586  }
587  else if (cdata->req->type == DistanceRequestType::SINGLE)
588  {
589  if (it->second[0].distance < dist_result.distance)
590  it->second[0] = dist_result;
591  }
592  else if (cdata->req->type == DistanceRequestType::LIMITED)
593  {
594  assert(it->second.size() < cdata->req->max_contacts_per_body);
595  it->second.push_back(dist_result);
596  }
597  }
598  }
599 
600  if (!cdata->req->enable_signed_distance && cdata->res->collision)
601  {
602  cdata->done = true;
603  }
604  }
605 
606  return cdata->done;
607 }
608 
609 /* We template the function so we get a different cache for each of the template arguments combinations */
610 template <typename BV, typename T>
612 {
613  static FCLShapeCache cache;
614  return cache;
615 }
616 
617 template <typename T1, typename T2>
619 {
620  enum
621  {
622  value = 0
623  };
624 };
625 
626 template <typename T>
627 struct IfSameType<T, T>
628 {
629  enum
630  {
631  value = 1
632  };
633 };
634 
635 template <typename BV, typename T>
636 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const T* data, int shape_index)
637 {
638  using ShapeKey = std::weak_ptr<const shapes::Shape>;
639  using ShapeMap = std::map<ShapeKey, FCLGeometryConstPtr, std::owner_less<ShapeKey>>;
640 
641  FCLShapeCache& cache = GetShapeCache<BV, T>();
642 
643  std::weak_ptr<const shapes::Shape> wptr(shape);
644  {
645  boost::mutex::scoped_lock slock(cache.lock_);
646  ShapeMap::const_iterator cache_it = cache.map_.find(wptr);
647  if (cache_it != cache.map_.end())
648  {
649  if (cache_it->second->collision_geometry_data_->ptr.raw == data)
650  {
651  // ROS_DEBUG_NAMED("collision_detection.fcl", "Collision data structures for object %s retrieved from
652  // cache.",
653  // cache_it->second->collision_geometry_data_->getID().c_str());
654  return cache_it->second;
655  }
656  else if (cache_it->second.unique())
657  {
658  const_cast<FCLGeometry*>(cache_it->second.get())->updateCollisionGeometryData(data, shape_index, false);
659  // ROS_DEBUG_NAMED("collision_detection.fcl", "Collision data structures for object %s retrieved from
660  // cache after updating
661  // the source
662  // object.", cache_it->second->collision_geometry_data_->getID().c_str());
663  return cache_it->second;
664  }
665  }
666  }
667 
668  // attached objects could have previously been World::Object; we try to move them
669  // from their old cache to the new one, if possible. the code is not pretty, but should help
670  // when we attach/detach objects that are in the world
672  {
673  // get the cache that corresponds to objects; maybe this attached object used to be a world object
674  FCLShapeCache& othercache = GetShapeCache<BV, World::Object>();
675 
676  // attached bodies could be just moved from the environment.
677  othercache.lock_.lock(); // lock manually to avoid having 2 simultaneous locks active (avoids possible deadlock)
678  auto cache_it = othercache.map_.find(wptr);
679  if (cache_it != othercache.map_.end())
680  {
681  if (cache_it->second.unique())
682  {
683  // remove from old cache
684  FCLGeometryConstPtr obj_cache = cache_it->second;
685  othercache.map_.erase(cache_it);
686  othercache.lock_.unlock();
687 
688  // update the CollisionGeometryData; nobody has a pointer to this, so we can safely modify it
689  const_cast<FCLGeometry*>(obj_cache.get())->updateCollisionGeometryData(data, shape_index, true);
690 
691  // ROS_DEBUG_NAMED("collision_detection.fcl", "Collision data structures for attached body %s retrieved
692  // from the cache for
693  // world objects.",
694  // obj_cache->collision_geometry_data_->getID().c_str());
695 
696  // add to the new cache
697  boost::mutex::scoped_lock slock(cache.lock_);
698  cache.map_[wptr] = obj_cache;
699  cache.bumpUseCount();
700  return obj_cache;
701  }
702  }
703  othercache.lock_.unlock();
704  }
705  else
706  // world objects could have previously been attached objects; we try to move them
707  // from their old cache to the new one, if possible. the code is not pretty, but should help
708  // when we attach/detach objects that are in the world
710  {
711  // get the cache that corresponds to objects; maybe this attached object used to be a world object
712  FCLShapeCache& othercache = GetShapeCache<BV, robot_state::AttachedBody>();
713 
714  // attached bodies could be just moved from the environment.
715  othercache.lock_.lock(); // lock manually to avoid having 2 simultaneous locks active (avoids possible deadlock)
716  auto cache_it = othercache.map_.find(wptr);
717  if (cache_it != othercache.map_.end())
718  {
719  if (cache_it->second.unique())
720  {
721  // remove from old cache
722  FCLGeometryConstPtr obj_cache = cache_it->second;
723  othercache.map_.erase(cache_it);
724  othercache.lock_.unlock();
725 
726  // update the CollisionGeometryData; nobody has a pointer to this, so we can safely modify it
727  const_cast<FCLGeometry*>(obj_cache.get())->updateCollisionGeometryData(data, shape_index, true);
728 
729  // ROS_DEBUG_NAMED("collision_detection.fcl", "Collision data structures for world object %s retrieved
730  // from the cache for
731  // attached
732  // bodies.",
733  // obj_cache->collision_geometry_data_->getID().c_str());
734 
735  // add to the new cache
736  boost::mutex::scoped_lock slock(cache.lock_);
737  cache.map_[wptr] = obj_cache;
738  cache.bumpUseCount();
739  return obj_cache;
740  }
741  }
742  othercache.lock_.unlock();
743  }
744 
745  fcl::CollisionGeometry* cg_g = nullptr;
746  if (shape->type == shapes::PLANE) // shapes that directly produce CollisionGeometry
747  {
748  // handle cases individually
749  switch (shape->type)
750  {
751  case shapes::PLANE:
752  {
753  const shapes::Plane* p = static_cast<const shapes::Plane*>(shape.get());
754  cg_g = new fcl::Plane(p->a, p->b, p->c, p->d);
755  }
756  break;
757  default:
758  break;
759  }
760  }
761  else
762  {
763  switch (shape->type)
764  {
765  case shapes::SPHERE:
766  {
767  const shapes::Sphere* s = static_cast<const shapes::Sphere*>(shape.get());
768  cg_g = new fcl::Sphere(s->radius);
769  }
770  break;
771  case shapes::BOX:
772  {
773  const shapes::Box* s = static_cast<const shapes::Box*>(shape.get());
774  const double* size = s->size;
775  cg_g = new fcl::Box(size[0], size[1], size[2]);
776  }
777  break;
778  case shapes::CYLINDER:
779  {
780  const shapes::Cylinder* s = static_cast<const shapes::Cylinder*>(shape.get());
781  cg_g = new fcl::Cylinder(s->radius, s->length);
782  }
783  break;
784  case shapes::CONE:
785  {
786  const shapes::Cone* s = static_cast<const shapes::Cone*>(shape.get());
787  cg_g = new fcl::Cone(s->radius, s->length);
788  }
789  break;
790  case shapes::MESH:
791  {
792  auto g = new fcl::BVHModel<BV>();
793  const shapes::Mesh* mesh = static_cast<const shapes::Mesh*>(shape.get());
794  if (mesh->vertex_count > 0 && mesh->triangle_count > 0)
795  {
796  std::vector<fcl::Triangle> tri_indices(mesh->triangle_count);
797  for (unsigned int i = 0; i < mesh->triangle_count; ++i)
798  tri_indices[i] =
799  fcl::Triangle(mesh->triangles[3 * i], mesh->triangles[3 * i + 1], mesh->triangles[3 * i + 2]);
800 
801  std::vector<fcl::Vec3f> points(mesh->vertex_count);
802  for (unsigned int i = 0; i < mesh->vertex_count; ++i)
803  points[i] = fcl::Vec3f(mesh->vertices[3 * i], mesh->vertices[3 * i + 1], mesh->vertices[3 * i + 2]);
804 
805  g->beginModel();
806  g->addSubModel(points, tri_indices);
807  g->endModel();
808  }
809  cg_g = g;
810  }
811  break;
812  case shapes::OCTREE:
813  {
814  const shapes::OcTree* g = static_cast<const shapes::OcTree*>(shape.get());
815  cg_g = new fcl::OcTree(g->octree);
816  }
817  break;
818  default:
819  ROS_ERROR_NAMED("collision_detection.fcl", "This shape type (%d) is not supported using FCL yet",
820  (int)shape->type);
821  cg_g = nullptr;
822  }
823  }
824  if (cg_g)
825  {
826  cg_g->computeLocalAABB();
827  FCLGeometryConstPtr res(new FCLGeometry(cg_g, data, shape_index));
828  boost::mutex::scoped_lock slock(cache.lock_);
829  cache.map_[wptr] = res;
830  cache.bumpUseCount();
831  return res;
832  }
833  return FCLGeometryConstPtr();
834 }
835 
837 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const robot_model::LinkModel* link,
838  int shape_index)
839 {
840  return createCollisionGeometry<fcl::OBBRSS, robot_model::LinkModel>(shape, link, shape_index);
841 }
842 
843 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const robot_state::AttachedBody* ab,
844  int shape_index)
845 {
846  return createCollisionGeometry<fcl::OBBRSS, robot_state::AttachedBody>(shape, ab, shape_index);
847 }
848 
849 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const World::Object* obj)
850 {
851  return createCollisionGeometry<fcl::OBBRSS, World::Object>(shape, obj, 0);
852 }
853 
854 template <typename BV, typename T>
855 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding,
856  const T* data, int shape_index)
857 {
858  if (fabs(scale - 1.0) <= std::numeric_limits<double>::epsilon() &&
859  fabs(padding) <= std::numeric_limits<double>::epsilon())
860  return createCollisionGeometry<BV, T>(shape, data, shape_index);
861  else
862  {
863  shapes::ShapePtr scaled_shape(shape->clone());
864  scaled_shape->scaleAndPadd(scale, padding);
865  return createCollisionGeometry<BV, T>(scaled_shape, data, shape_index);
866  }
867 }
868 
869 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding,
870  const robot_model::LinkModel* link, int shape_index)
871 {
872  return createCollisionGeometry<fcl::OBBRSS, robot_model::LinkModel>(shape, scale, padding, link, shape_index);
873 }
874 
875 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding,
876  const robot_state::AttachedBody* ab, int shape_index)
877 {
878  return createCollisionGeometry<fcl::OBBRSS, robot_state::AttachedBody>(shape, scale, padding, ab, shape_index);
879 }
880 
881 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding,
882  const World::Object* obj)
883 {
884  return createCollisionGeometry<fcl::OBBRSS, World::Object>(shape, scale, padding, obj, 0);
885 }
886 
888 {
889  FCLShapeCache& cache1 = GetShapeCache<fcl::OBBRSS, World::Object>();
890  {
891  boost::mutex::scoped_lock slock(cache1.lock_);
892  cache1.bumpUseCount(true);
893  }
894  FCLShapeCache& cache2 = GetShapeCache<fcl::OBBRSS, robot_state::AttachedBody>();
895  {
896  boost::mutex::scoped_lock slock(cache2.lock_);
897  cache2.bumpUseCount(true);
898  }
899 }
900 }
901 
902 void collision_detection::CollisionData::enableGroup(const robot_model::RobotModelConstPtr& kmodel)
903 {
904  if (kmodel->hasJointModelGroup(req_->group_name))
905  active_components_only_ = &kmodel->getJointModelGroup(req_->group_name)->getUpdatedLinkModelsSet();
906  else
907  active_components_only_ = nullptr;
908 }
909 
910 void collision_detection::FCLObject::registerTo(fcl::BroadPhaseCollisionManager* manager)
911 {
912  std::vector<fcl::CollisionObject*> collision_objects(collision_objects_.size());
913  for (std::size_t i = 0; i < collision_objects_.size(); ++i)
914  collision_objects[i] = collision_objects_[i].get();
915  if (!collision_objects.empty())
916  manager->registerObjects(collision_objects);
917 }
918 
919 void collision_detection::FCLObject::unregisterFrom(fcl::BroadPhaseCollisionManager* manager)
920 {
921  for (auto& collision_object : collision_objects_)
922  manager->unregisterObject(collision_object.get());
923 }
924 
926 {
927  collision_objects_.clear();
928  collision_geometry_.clear();
929 }
d
bool sameObject(const CollisionGeometryData &other) const
Check if two CollisionGeometryData objects point to the same source object.
const CollisionRequest * req_
The collision request passed by the user.
CollisionResult * res_
The user specified response location.
#define ROS_INFO_NAMED(name,...)
bool done_
Flag indicating whether collision checking is complete.
FCLShapeCache & GetShapeCache()
std::map< ShapeKey, FCLGeometryConstPtr, std::owner_less< ShapeKey >> ShapeMap
FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr &shape, const robot_model::LinkModel *link, int shape_index)
union collision_detection::CollisionGeometryData::@0 ptr
const AllowedCollisionMatrix * acm
The allowed collision matrix used to filter checks.
double size[3]
bool collisionCallback(fcl::CollisionObject *o1, fcl::CollisionObject *o2, void *data)
XmlRpcServer s
bool enable_nearest_points
Indicate if nearest point information should be calculated.
ContactMap contacts
A map returning the pairs of ids of the bodies in contact, plus information about the contacts themse...
DistanceResultsData minimum_distance
ResultsData for the two objects with the minimum distance.
bool getAllowedCollision(const std::string &name1, const std::string &name2, DecideContactFn &fn) const
Get the allowed collision predicate between two elements. Return true if a predicate for entry is inc...
boost::function< bool(collision_detection::Contact &)> DecideContactFn
Signature of predicate that decides whether a contact is allowed or not (when AllowedCollision::Type ...
std::shared_ptr< Shape > ShapePtr
Generic interface to collision detection.
bool enable_signed_distance
Indicate if a signed distance should be calculated in a collision.
void unregisterFrom(fcl::BroadPhaseCollisionManager *manager)
bool collision
True if collision was found, false otherwise.
const std::set< const robot_model::LinkModel * > * active_components_only
The set of active components to check.
std::set< CostSource > cost_sources
When costs are computed, the individual cost sources are.
boost::function< bool(const CollisionResult &)> is_done
Function call that decides whether collision detection should stop.
std::weak_ptr< const shapes::Shape > ShapeKey
#define ROS_DEBUG_NAMED(name,...)
std::shared_ptr< const octomap::OcTree > octree
The collision is allowed depending on a predicate evaluated on the produced contact. If the predicate returns true, this particular contact is deemed ok (or allowed), i.e., the contact does not imply that the two bodies are in collision.
std::size_t max_contacts
Overall maximum number of contacts to compute.
A representation of an object.
Definition: world.h:80
bool verbose
Flag indicating whether information about detected collisions should be reported. ...
DistanceMap distances
A map of distance data for each link in the req.active_components_only.
const std::set< std::string > & getTouchLinks() const
Get the links that the attached body is allowed to touch.
Definition: attached_body.h:94
const LinkModel * getAttachedLink() const
Get the model of the link this body is attached to.
Definition: attached_body.h:82
Find a limited(max_contacts_per_body) set of contacts for a given pair.
double distance
The distance between two objects. If two objects are in collision, distance <= 0. ...
bool collision
Indicates if two objects were in collision.
const AllowedCollisionMatrix * acm_
The user specified collision matrix (may be NULL)
void fcl2costsource(const fcl::CostSource &fcs, CostSource &cs)
std::size_t max_contacts_per_pair
Maximum number of contacts to compute per pair of bodies (multiple bodies may be in contact at differ...
std::size_t max_contacts_per_body
Maximum number of contacts to store for bodies (multiple bodies may be within distance threshold) ...
std::size_t max_cost_sources
When costs are computed, this value defines how many of the top cost sources should be returned...
void enableGroup(const robot_model::RobotModelConstPtr &kmodel)
Compute active_components_only_ based on req_.
#define ROS_ERROR_NAMED(name,...)
Collisions between a particular pair of bodies does not imply that the robot configuration is in coll...
static const double max_dist
A link from the robot. Contains the constant transform applied to the link and its geometry...
Definition: link_model.h:72
When collision costs are computed, this structure contains information about the partial cost incurre...
bool distanceCallback(fcl::CollisionObject *o1, fcl::CollisionObject *o2, void *data, double &min_dist)
Object defining bodies that can be attached to robot links. This is useful when handling objects pick...
Definition: attached_body.h:56
void registerTo(fcl::BroadPhaseCollisionManager *manager)
double distance(const urdf::Pose &transform)
Definition: pr2_arm_ik.h:55
static const unsigned int MAX_CLEAN_COUNT
const DistanceRequest * req
Distance query request information.
const std::set< const robot_model::LinkModel * > * active_components_only_
std::shared_ptr< const Shape > ShapeConstPtr


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Oct 18 2020 13:16:33