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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Nov 11 2019 04:01:08