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  unsigned int from = map_.size();
369  for (auto it = map_.begin(); it != map_.end();)
370  {
371  auto nit = it;
372  ++nit;
373  if (it->first.expired())
374  map_.erase(it);
375  it = nit;
376  }
377  // ROS_DEBUG_NAMED("collision_detection.fcl", "Cleaning up cache for FCL objects that correspond to static
378  // shapes. Cache size
379  // reduced from %u
380  // to %u", from, (unsigned int)map_.size());
381  }
382  }
383 
384  static const unsigned int MAX_CLEAN_COUNT = 100; // every this many uses of the cache, a cleaning operation is
385  // executed (this is only removal of expired entries)
387  unsigned int clean_count_;
388  boost::mutex lock_;
389 };
390 
391 bool distanceCallback(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* data, double& min_dist)
392 {
393  DistanceData* cdata = reinterpret_cast<DistanceData*>(data);
394 
395  const CollisionGeometryData* cd1 = static_cast<const CollisionGeometryData*>(o1->collisionGeometry()->getUserData());
396  const CollisionGeometryData* cd2 = static_cast<const CollisionGeometryData*>(o2->collisionGeometry()->getUserData());
397 
398  // do not distance check for geoms part of the same object / link / attached body
399  if (cd1->sameObject(*cd2))
400  return false;
401 
402  // If active components are specified
403  if (cdata->req->active_components_only)
404  {
405  const robot_model::LinkModel* l1 =
406  cd1->type == BodyTypes::ROBOT_LINK ?
407  cd1->ptr.link :
408  (cd1->type == BodyTypes::ROBOT_ATTACHED ? cd1->ptr.ab->getAttachedLink() : nullptr);
409  const robot_model::LinkModel* l2 =
410  cd2->type == BodyTypes::ROBOT_LINK ?
411  cd2->ptr.link :
412  (cd2->type == BodyTypes::ROBOT_ATTACHED ? cd2->ptr.ab->getAttachedLink() : nullptr);
413 
414  // If neither of the involved components is active
415  if ((!l1 || cdata->req->active_components_only->find(l1) == cdata->req->active_components_only->end()) &&
416  (!l2 || cdata->req->active_components_only->find(l2) == cdata->req->active_components_only->end()))
417  {
418  return false;
419  }
420  }
421 
422  // use the collision matrix (if any) to avoid certain distance checks
423  bool always_allow_collision = false;
424  if (cdata->req->acm)
425  {
427 
428  bool found = cdata->req->acm->getAllowedCollision(cd1->getID(), cd2->getID(), type);
429  if (found)
430  {
431  // if we have an entry in the collision matrix, we read it
432  if (type == AllowedCollision::ALWAYS)
433  {
434  always_allow_collision = true;
435  if (cdata->req->verbose)
436  ROS_DEBUG_NAMED("collision_detection.fcl",
437  "Collision between '%s' and '%s' is always allowed. No distances are computed.",
438  cd1->getID().c_str(), cd2->getID().c_str());
439  }
440  }
441  }
442 
443  // check if a link is touching an attached object
445  {
446  const std::set<std::string>& tl = cd2->ptr.ab->getTouchLinks();
447  if (tl.find(cd1->getID()) != tl.end())
448  {
449  always_allow_collision = true;
450  if (cdata->req->verbose)
451  ROS_DEBUG_NAMED("collision_detection.fcl",
452  "Robot link '%s' is allowed to touch attached object '%s'. No distances are computed.",
453  cd1->getID().c_str(), cd2->getID().c_str());
454  }
455  }
456  else
457  {
459  {
460  const std::set<std::string>& tl = cd1->ptr.ab->getTouchLinks();
461  if (tl.find(cd2->getID()) != tl.end())
462  {
463  always_allow_collision = true;
464  if (cdata->req->verbose)
465  ROS_DEBUG_NAMED("collision_detection.fcl",
466  "Robot link '%s' is allowed to touch attached object '%s'. No distances are computed.",
467  cd2->getID().c_str(), cd1->getID().c_str());
468  }
469  }
470  }
471 
472  if (always_allow_collision)
473  {
474  return false;
475  }
476  if (cdata->req->verbose)
477  ROS_DEBUG_NAMED("collision_detection.fcl", "Actually checking collisions between %s and %s", cd1->getID().c_str(),
478  cd2->getID().c_str());
479 
480  fcl::DistanceResult fcl_result;
481  DistanceResultsData dist_result;
482  double dist_threshold = cdata->req->distance_threshold;
483 
484  const std::pair<std::string, std::string>& pc = cd1->getID() < cd2->getID() ?
485  std::make_pair(cd1->getID(), cd2->getID()) :
486  std::make_pair(cd2->getID(), cd1->getID());
487 
488  DistanceMap::iterator it = cdata->res->distances.find(pc);
489 
490  if (it != cdata->res->distances.end())
491  {
492  if (cdata->req->type == DistanceRequestType::LIMITED)
493  {
494  // If at the limit for a given pair just return
495  if (it->second.size() >= cdata->req->max_contacts_per_body)
496  {
497  return cdata->done;
498  }
499  }
500  else if (cdata->req->type == DistanceRequestType::GLOBAL)
501  {
502  dist_threshold = cdata->res->minimum_distance.distance;
503  }
504  else if (cdata->req->type == DistanceRequestType::SINGLE)
505  {
506  dist_threshold = it->second[0].distance;
507  }
508  }
509 
510  fcl_result.min_distance = dist_threshold;
511  double d = fcl::distance(o1, o2, fcl::DistanceRequest(cdata->req->enable_nearest_points), fcl_result);
512 
513  // Check if either object is already in the map. If not add it or if present
514  // check to see if the new distance is closer. If closer remove the existing
515  // one and add the new distance information.
516  if (d < dist_threshold)
517  {
518  dist_result.distance = fcl_result.min_distance;
519  dist_result.nearest_points[0] = Eigen::Vector3d(fcl_result.nearest_points[0].data.vs);
520  dist_result.nearest_points[1] = Eigen::Vector3d(fcl_result.nearest_points[1].data.vs);
521  dist_result.link_names[0] = cd1->ptr.obj->id_;
522  dist_result.link_names[1] = cd2->ptr.obj->id_;
523  dist_result.body_types[0] = cd1->type;
524  dist_result.body_types[1] = cd2->type;
525  if (cdata->req->enable_nearest_points)
526  {
527  dist_result.normal = (dist_result.nearest_points[1] - dist_result.nearest_points[0]).normalized();
528  }
529 
530  if (d <= 0 && cdata->req->enable_signed_distance)
531  {
532  dist_result.nearest_points[0].setZero();
533  dist_result.nearest_points[1].setZero();
534  dist_result.normal.setZero();
535 
536  fcl::CollisionRequest coll_req;
537  fcl::CollisionResult coll_res;
538  coll_req.enable_contact = true;
539  coll_req.num_max_contacts = 200;
540  std::size_t contacts = fcl::collide(o1, o2, coll_req, coll_res);
541  if (contacts > 0)
542  {
543  double max_dist = 0;
544  int max_index = 0;
545  for (int i = 0; i < contacts; ++i)
546  {
547  const fcl::Contact& contact = coll_res.getContact(i);
548  if (contact.penetration_depth > max_dist)
549  {
550  max_dist = contact.penetration_depth;
551  max_index = i;
552  }
553  }
554 
555  const fcl::Contact& contact = coll_res.getContact(max_index);
556  dist_result.distance = -contact.penetration_depth;
557  dist_result.nearest_points[0] = Eigen::Vector3d(contact.pos.data.vs);
558  dist_result.nearest_points[1] = Eigen::Vector3d(contact.pos.data.vs);
559  dist_result.normal = Eigen::Vector3d(contact.normal.data.vs);
560  }
561  }
562 
563  if (dist_result.distance < cdata->res->minimum_distance.distance)
564  {
565  cdata->res->minimum_distance = dist_result;
566  }
567 
568  if (dist_result.distance <= 0)
569  {
570  cdata->res->collision = true;
571  }
572 
573  if (cdata->req->type != DistanceRequestType::GLOBAL)
574  {
575  if (it == cdata->res->distances.end())
576  {
577  std::vector<DistanceResultsData> data;
578  data.reserve(cdata->req->type == DistanceRequestType::SINGLE ? 1 : cdata->req->max_contacts_per_body);
579  data.push_back(dist_result);
580  cdata->res->distances.insert(std::make_pair(pc, data));
581  }
582  else
583  {
584  if (cdata->req->type == DistanceRequestType::ALL)
585  {
586  it->second.push_back(dist_result);
587  }
588  else if (cdata->req->type == DistanceRequestType::SINGLE)
589  {
590  if (it->second[0].distance < dist_result.distance)
591  it->second[0] = dist_result;
592  }
593  else if (cdata->req->type == DistanceRequestType::LIMITED)
594  {
595  assert(it->second.size() < cdata->req->max_contacts_per_body);
596  it->second.push_back(dist_result);
597  }
598  }
599  }
600 
601  if (!cdata->req->enable_signed_distance && cdata->res->collision)
602  {
603  cdata->done = true;
604  }
605  }
606 
607  return cdata->done;
608 }
609 
610 /* We template the function so we get a different cache for each of the template arguments combinations */
611 template <typename BV, typename T>
613 {
614  static FCLShapeCache cache;
615  return cache;
616 }
617 
618 template <typename T1, typename T2>
620 {
621  enum
622  {
623  value = 0
624  };
625 };
626 
627 template <typename T>
628 struct IfSameType<T, T>
629 {
630  enum
631  {
632  value = 1
633  };
634 };
635 
636 template <typename BV, typename T>
637 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const T* data, int shape_index)
638 {
639  using ShapeKey = std::weak_ptr<const shapes::Shape>;
640  using ShapeMap = std::map<ShapeKey, FCLGeometryConstPtr, std::owner_less<ShapeKey>>;
641 
642  FCLShapeCache& cache = GetShapeCache<BV, T>();
643 
644  std::weak_ptr<const shapes::Shape> wptr(shape);
645  {
646  boost::mutex::scoped_lock slock(cache.lock_);
647  ShapeMap::const_iterator cache_it = cache.map_.find(wptr);
648  if (cache_it != cache.map_.end())
649  {
650  if (cache_it->second->collision_geometry_data_->ptr.raw == (void*)data)
651  {
652  // ROS_DEBUG_NAMED("collision_detection.fcl", "Collision data structures for object %s retrieved from
653  // cache.",
654  // cache_it->second->collision_geometry_data_->getID().c_str());
655  return cache_it->second;
656  }
657  else if (cache_it->second.unique())
658  {
659  const_cast<FCLGeometry*>(cache_it->second.get())->updateCollisionGeometryData(data, shape_index, false);
660  // ROS_DEBUG_NAMED("collision_detection.fcl", "Collision data structures for object %s retrieved from
661  // cache after updating
662  // the source
663  // object.", cache_it->second->collision_geometry_data_->getID().c_str());
664  return cache_it->second;
665  }
666  }
667  }
668 
669  // attached objects could have previously been World::Object; we try to move them
670  // from their old cache to the new one, if possible. the code is not pretty, but should help
671  // when we attach/detach objects that are in the world
673  {
674  // get the cache that corresponds to objects; maybe this attached object used to be a world object
675  FCLShapeCache& othercache = GetShapeCache<BV, World::Object>();
676 
677  // attached bodies could be just moved from the environment.
678  othercache.lock_.lock(); // lock manually to avoid having 2 simultaneous locks active (avoids possible deadlock)
679  auto cache_it = othercache.map_.find(wptr);
680  if (cache_it != othercache.map_.end())
681  {
682  if (cache_it->second.unique())
683  {
684  // remove from old cache
685  FCLGeometryConstPtr obj_cache = cache_it->second;
686  othercache.map_.erase(cache_it);
687  othercache.lock_.unlock();
688 
689  // update the CollisionGeometryData; nobody has a pointer to this, so we can safely modify it
690  const_cast<FCLGeometry*>(obj_cache.get())->updateCollisionGeometryData(data, shape_index, true);
691 
692  // ROS_DEBUG_NAMED("collision_detection.fcl", "Collision data structures for attached body %s retrieved
693  // from the cache for
694  // world objects.",
695  // obj_cache->collision_geometry_data_->getID().c_str());
696 
697  // add to the new cache
698  boost::mutex::scoped_lock slock(cache.lock_);
699  cache.map_[wptr] = obj_cache;
700  cache.bumpUseCount();
701  return obj_cache;
702  }
703  }
704  othercache.lock_.unlock();
705  }
706  else
707  // world objects could have previously been attached objects; 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, robot_state::AttachedBody>();
714 
715  // attached bodies could be just moved from the environment.
716  othercache.lock_.lock(); // lock manually to avoid having 2 simultaneous locks active (avoids possible deadlock)
717  auto cache_it = othercache.map_.find(wptr);
718  if (cache_it != othercache.map_.end())
719  {
720  if (cache_it->second.unique())
721  {
722  // remove from old cache
723  FCLGeometryConstPtr obj_cache = cache_it->second;
724  othercache.map_.erase(cache_it);
725  othercache.lock_.unlock();
726 
727  // update the CollisionGeometryData; nobody has a pointer to this, so we can safely modify it
728  const_cast<FCLGeometry*>(obj_cache.get())->updateCollisionGeometryData(data, shape_index, true);
729 
730  // ROS_DEBUG_NAMED("collision_detection.fcl", "Collision data structures for world object %s retrieved
731  // from the cache for
732  // attached
733  // bodies.",
734  // obj_cache->collision_geometry_data_->getID().c_str());
735 
736  // add to the new cache
737  boost::mutex::scoped_lock slock(cache.lock_);
738  cache.map_[wptr] = obj_cache;
739  cache.bumpUseCount();
740  return obj_cache;
741  }
742  }
743  othercache.lock_.unlock();
744  }
745 
746  fcl::CollisionGeometry* cg_g = nullptr;
747  if (shape->type == shapes::PLANE) // shapes that directly produce CollisionGeometry
748  {
749  // handle cases individually
750  switch (shape->type)
751  {
752  case shapes::PLANE:
753  {
754  const shapes::Plane* p = static_cast<const shapes::Plane*>(shape.get());
755  cg_g = new fcl::Plane(p->a, p->b, p->c, p->d);
756  }
757  break;
758  default:
759  break;
760  }
761  }
762  else
763  {
764  switch (shape->type)
765  {
766  case shapes::SPHERE:
767  {
768  const shapes::Sphere* s = static_cast<const shapes::Sphere*>(shape.get());
769  cg_g = new fcl::Sphere(s->radius);
770  }
771  break;
772  case shapes::BOX:
773  {
774  const shapes::Box* s = static_cast<const shapes::Box*>(shape.get());
775  const double* size = s->size;
776  cg_g = new fcl::Box(size[0], size[1], size[2]);
777  }
778  break;
779  case shapes::CYLINDER:
780  {
781  const shapes::Cylinder* s = static_cast<const shapes::Cylinder*>(shape.get());
782  cg_g = new fcl::Cylinder(s->radius, s->length);
783  }
784  break;
785  case shapes::CONE:
786  {
787  const shapes::Cone* s = static_cast<const shapes::Cone*>(shape.get());
788  cg_g = new fcl::Cone(s->radius, s->length);
789  }
790  break;
791  case shapes::MESH:
792  {
793  auto g = new fcl::BVHModel<BV>();
794  const shapes::Mesh* mesh = static_cast<const shapes::Mesh*>(shape.get());
795  if (mesh->vertex_count > 0 && mesh->triangle_count > 0)
796  {
797  std::vector<fcl::Triangle> tri_indices(mesh->triangle_count);
798  for (unsigned int i = 0; i < mesh->triangle_count; ++i)
799  tri_indices[i] =
800  fcl::Triangle(mesh->triangles[3 * i], mesh->triangles[3 * i + 1], mesh->triangles[3 * i + 2]);
801 
802  std::vector<fcl::Vec3f> points(mesh->vertex_count);
803  for (unsigned int i = 0; i < mesh->vertex_count; ++i)
804  points[i] = fcl::Vec3f(mesh->vertices[3 * i], mesh->vertices[3 * i + 1], mesh->vertices[3 * i + 2]);
805 
806  g->beginModel();
807  g->addSubModel(points, tri_indices);
808  g->endModel();
809  }
810  cg_g = g;
811  }
812  break;
813  case shapes::OCTREE:
814  {
815  const shapes::OcTree* g = static_cast<const shapes::OcTree*>(shape.get());
816  cg_g = new fcl::OcTree(g->octree);
817  }
818  break;
819  default:
820  ROS_ERROR_NAMED("collision_detection.fcl", "This shape type (%d) is not supported using FCL yet",
821  (int)shape->type);
822  cg_g = nullptr;
823  }
824  }
825  if (cg_g)
826  {
827  cg_g->computeLocalAABB();
828  FCLGeometryConstPtr res(new FCLGeometry(cg_g, data, shape_index));
829  boost::mutex::scoped_lock slock(cache.lock_);
830  cache.map_[wptr] = res;
831  cache.bumpUseCount();
832  return res;
833  }
834  return FCLGeometryConstPtr();
835 }
836 
838 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const robot_model::LinkModel* link,
839  int shape_index)
840 {
841  return createCollisionGeometry<fcl::OBBRSS, robot_model::LinkModel>(shape, link, shape_index);
842 }
843 
844 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const robot_state::AttachedBody* ab,
845  int shape_index)
846 {
847  return createCollisionGeometry<fcl::OBBRSS, robot_state::AttachedBody>(shape, ab, shape_index);
848 }
849 
850 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const World::Object* obj)
851 {
852  return createCollisionGeometry<fcl::OBBRSS, World::Object>(shape, obj, 0);
853 }
854 
855 template <typename BV, typename T>
856 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding,
857  const T* data, int shape_index)
858 {
859  if (fabs(scale - 1.0) <= std::numeric_limits<double>::epsilon() &&
860  fabs(padding) <= std::numeric_limits<double>::epsilon())
861  return createCollisionGeometry<BV, T>(shape, data, shape_index);
862  else
863  {
864  shapes::ShapePtr scaled_shape(shape->clone());
865  scaled_shape->scaleAndPadd(scale, padding);
866  return createCollisionGeometry<BV, T>(scaled_shape, data, shape_index);
867  }
868 }
869 
870 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding,
871  const robot_model::LinkModel* link, int shape_index)
872 {
873  return createCollisionGeometry<fcl::OBBRSS, robot_model::LinkModel>(shape, scale, padding, link, shape_index);
874 }
875 
876 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding,
877  const robot_state::AttachedBody* ab, int shape_index)
878 {
879  return createCollisionGeometry<fcl::OBBRSS, robot_state::AttachedBody>(shape, scale, padding, ab, shape_index);
880 }
881 
882 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding,
883  const World::Object* obj)
884 {
885  return createCollisionGeometry<fcl::OBBRSS, World::Object>(shape, scale, padding, obj, 0);
886 }
887 
889 {
890  FCLShapeCache& cache1 = GetShapeCache<fcl::OBBRSS, World::Object>();
891  {
892  boost::mutex::scoped_lock slock(cache1.lock_);
893  cache1.bumpUseCount(true);
894  }
895  FCLShapeCache& cache2 = GetShapeCache<fcl::OBBRSS, robot_state::AttachedBody>();
896  {
897  boost::mutex::scoped_lock slock(cache2.lock_);
898  cache2.bumpUseCount(true);
899  }
900 }
901 }
902 
903 void collision_detection::CollisionData::enableGroup(const robot_model::RobotModelConstPtr& kmodel)
904 {
905  if (kmodel->hasJointModelGroup(req_->group_name))
906  active_components_only_ = &kmodel->getJointModelGroup(req_->group_name)->getUpdatedLinkModelsSet();
907  else
908  active_components_only_ = nullptr;
909 }
910 
911 void collision_detection::FCLObject::registerTo(fcl::BroadPhaseCollisionManager* manager)
912 {
913  std::vector<fcl::CollisionObject*> collision_objects(collision_objects_.size());
914  for (std::size_t i = 0; i < collision_objects_.size(); ++i)
915  collision_objects[i] = collision_objects_[i].get();
916  if (!collision_objects.empty())
917  manager->registerObjects(collision_objects);
918 }
919 
920 void collision_detection::FCLObject::unregisterFrom(fcl::BroadPhaseCollisionManager* manager)
921 {
922  for (auto& collision_object : collision_objects_)
923  manager->unregisterObject(collision_object.get());
924 }
925 
927 {
928  collision_objects_.clear();
929  collision_geometry_.clear();
930 }
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.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW std::string id_
The id for this object.
Definition: world.h:89
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 Tue Jun 12 2018 02:47:30