collision_matrix.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, E. Gil Jones */
36 
38 #include <boost/bind.hpp>
39 #include <iomanip>
40 
41 namespace collision_detection
42 {
44 {
45 }
46 
47 AllowedCollisionMatrix::AllowedCollisionMatrix(const std::vector<std::string>& names, bool allowed)
48 {
49  for (std::size_t i = 0; i < names.size(); ++i)
50  for (std::size_t j = i; j < names.size(); ++j)
51  setEntry(names[i], names[j], allowed);
52 }
53 
54 AllowedCollisionMatrix::AllowedCollisionMatrix(const moveit_msgs::AllowedCollisionMatrix& msg)
55 {
56  if (msg.entry_names.size() != msg.entry_values.size() ||
57  msg.default_entry_names.size() != msg.default_entry_values.size())
58  ROS_ERROR_NAMED("collision_detection", "The number of links does not match the number of entries "
59  "in AllowedCollisionMatrix message");
60  else
61  {
62  for (std::size_t i = 0; i < msg.entry_names.size(); ++i)
63  if (msg.entry_values[i].enabled.size() != msg.entry_names.size())
64  ROS_ERROR_NAMED("collision_detection",
65  "Number of entries is incorrect for link '%s' in AllowedCollisionMatrix message",
66  msg.entry_names[i].c_str());
67  else
68  for (std::size_t j = i + 1; j < msg.entry_values[i].enabled.size(); ++j)
69  setEntry(msg.entry_names[i], msg.entry_names[j], msg.entry_values[i].enabled[j]);
70 
71  for (std::size_t i = 0; i < msg.default_entry_names.size(); ++i)
72  setDefaultEntry(msg.default_entry_names[i], msg.default_entry_values[i]);
73  }
74 }
75 
77 {
78  entries_ = acm.entries_;
82 }
83 
84 bool AllowedCollisionMatrix::getEntry(const std::string& name1, const std::string& name2, DecideContactFn& fn) const
85 {
86  auto it1 = allowed_contacts_.find(name1);
87  if (it1 == allowed_contacts_.end())
88  return false;
89  auto it2 = it1->second.find(name2);
90  if (it2 == it1->second.end())
91  return false;
92  fn = it2->second;
93  return true;
94 }
95 
96 bool AllowedCollisionMatrix::getEntry(const std::string& name1, const std::string& name2,
97  AllowedCollision::Type& allowed_collision) const
98 {
99  auto it1 = entries_.find(name1);
100  if (it1 == entries_.end())
101  return false;
102  auto it2 = it1->second.find(name2);
103  if (it2 == it1->second.end())
104  return false;
105  allowed_collision = it2->second;
106  return true;
107 }
108 
109 bool AllowedCollisionMatrix::hasEntry(const std::string& name) const
110 {
111  return entries_.find(name) != entries_.end();
112 }
113 
114 bool AllowedCollisionMatrix::hasEntry(const std::string& name1, const std::string& name2) const
115 {
116  auto it1 = entries_.find(name1);
117  if (it1 == entries_.end())
118  return false;
119  auto it2 = it1->second.find(name2);
120  return it2 != it1->second.end();
121 }
122 
123 void AllowedCollisionMatrix::setEntry(const std::string& name1, const std::string& name2, bool allowed)
124 {
126  entries_[name1][name2] = entries_[name2][name1] = v;
127 
128  // remove boost::function pointers, if any
129  auto it = allowed_contacts_.find(name1);
130  if (it != allowed_contacts_.end())
131  {
132  auto jt = it->second.find(name2);
133  if (jt != it->second.end())
134  it->second.erase(jt);
135  }
136  it = allowed_contacts_.find(name2);
137  if (it != allowed_contacts_.end())
138  {
139  auto jt = it->second.find(name1);
140  if (jt != it->second.end())
141  it->second.erase(jt);
142  }
143 }
144 
145 void AllowedCollisionMatrix::setEntry(const std::string& name1, const std::string& name2, const DecideContactFn& fn)
146 {
147  entries_[name1][name2] = entries_[name2][name1] = AllowedCollision::CONDITIONAL;
148  allowed_contacts_[name1][name2] = allowed_contacts_[name2][name1] = fn;
149 }
150 
151 void AllowedCollisionMatrix::removeEntry(const std::string& name)
152 {
153  entries_.erase(name);
154  allowed_contacts_.erase(name);
155  for (auto& entry : entries_)
156  entry.second.erase(name);
157  for (auto& allowed_contact : allowed_contacts_)
158  allowed_contact.second.erase(name);
159 }
160 
161 void AllowedCollisionMatrix::removeEntry(const std::string& name1, const std::string& name2)
162 {
163  auto jt = entries_.find(name1);
164  if (jt != entries_.end())
165  {
166  auto it = jt->second.find(name2);
167  if (it != jt->second.end())
168  jt->second.erase(it);
169  }
170  jt = entries_.find(name2);
171  if (jt != entries_.end())
172  {
173  auto it = jt->second.find(name1);
174  if (it != jt->second.end())
175  jt->second.erase(it);
176  }
177 
178  auto it = allowed_contacts_.find(name1);
179  if (it != allowed_contacts_.end())
180  {
181  auto jt = it->second.find(name2);
182  if (jt != it->second.end())
183  it->second.erase(jt);
184  }
185  it = allowed_contacts_.find(name2);
186  if (it != allowed_contacts_.end())
187  {
188  auto jt = it->second.find(name1);
189  if (jt != it->second.end())
190  it->second.erase(jt);
191  }
192 }
193 
194 void AllowedCollisionMatrix::setEntry(const std::string& name, const std::vector<std::string>& other_names,
195  bool allowed)
196 {
197  for (const auto& other_name : other_names)
198  if (other_name != name)
199  setEntry(other_name, name, allowed);
200 }
201 
202 void AllowedCollisionMatrix::setEntry(const std::vector<std::string>& names1, const std::vector<std::string>& names2,
203  bool allowed)
204 {
205  for (const auto& name1 : names1)
206  setEntry(name1, names2, allowed);
207 }
208 
209 void AllowedCollisionMatrix::setEntry(const std::string& name, bool allowed)
210 {
211  std::string last = name;
212  for (auto& entry : entries_)
213  if (name != entry.first && last != entry.first)
214  {
215  last = entry.first;
216  setEntry(name, entry.first, allowed);
217  }
218 }
219 
221 {
223  for (auto& entry : entries_)
224  for (auto& it2 : entry.second)
225  it2.second = v;
226 }
227 
228 void AllowedCollisionMatrix::setDefaultEntry(const std::string& name, bool allowed)
229 {
231  default_entries_[name] = v;
232  default_allowed_contacts_.erase(name);
233 }
234 
235 void AllowedCollisionMatrix::setDefaultEntry(const std::string& name, const DecideContactFn& fn)
236 {
238  default_allowed_contacts_[name] = fn;
239 }
240 
241 bool AllowedCollisionMatrix::getDefaultEntry(const std::string& name, AllowedCollision::Type& allowed_collision) const
242 {
243  auto it = default_entries_.find(name);
244  if (it == default_entries_.end())
245  return false;
246  allowed_collision = it->second;
247  return true;
248 }
249 
250 bool AllowedCollisionMatrix::getDefaultEntry(const std::string& name, DecideContactFn& fn) const
251 {
252  auto it = default_allowed_contacts_.find(name);
253  if (it == default_allowed_contacts_.end())
254  return false;
255  fn = it->second;
256  return true;
257 }
258 
259 static bool andDecideContact(const DecideContactFn& f1, const DecideContactFn& f2, Contact& contact)
260 {
261  return f1(contact) && f2(contact);
262 }
263 
264 bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const std::string& name2,
265  DecideContactFn& fn) const
266 {
267  DecideContactFn fn1, fn2;
268  bool found1 = getDefaultEntry(name1, fn1);
269  bool found2 = getDefaultEntry(name2, fn2);
270 
271  if (!found1 && !found2)
272  return getEntry(name1, name2, fn);
273  else
274  {
275  if (found1 && !found2)
276  fn = fn1;
277  else if (!found1 && found2)
278  fn = fn2;
279  else if (found1 && found2)
280  fn = boost::bind(&andDecideContact, fn1, fn2, _1);
281  else
282  return false;
283  return true;
284  }
285 }
286 
287 bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const std::string& name2,
288  AllowedCollision::Type& allowed_collision) const
289 {
290  AllowedCollision::Type t1, t2;
291  bool found1 = getDefaultEntry(name1, t1);
292  bool found2 = getDefaultEntry(name2, t2);
293 
294  if (!found1 && !found2)
295  return getEntry(name1, name2, allowed_collision);
296  else
297  {
298  if (found1 && !found2)
299  allowed_collision = t1;
300  else if (!found1 && found2)
301  allowed_collision = t2;
302  else if (found1 && found2)
303  {
305  allowed_collision = AllowedCollision::NEVER;
307  allowed_collision = AllowedCollision::CONDITIONAL;
308  else // ALWAYS is the only remaining case
309  allowed_collision = AllowedCollision::ALWAYS;
310  }
311  else
312  return false;
313  return true;
314  }
315 }
316 
318 {
319  entries_.clear();
320  allowed_contacts_.clear();
321  default_entries_.clear();
323 }
324 
325 void AllowedCollisionMatrix::getAllEntryNames(std::vector<std::string>& names) const
326 {
327  names.clear();
328  for (const auto& entry : entries_)
329  if (!names.empty() && names.back() == entry.first)
330  continue;
331  else
332  names.push_back(entry.first);
333 }
334 
335 void AllowedCollisionMatrix::getMessage(moveit_msgs::AllowedCollisionMatrix& msg) const
336 {
337  msg.entry_names.clear();
338  msg.entry_values.clear();
339  msg.default_entry_names.clear();
340  msg.default_entry_values.clear();
341 
342  getAllEntryNames(msg.entry_names);
343  std::sort(msg.entry_names.begin(), msg.entry_names.end());
344 
345  msg.entry_values.resize(msg.entry_names.size());
346  for (std::size_t i = 0; i < msg.entry_names.size(); ++i)
347  msg.entry_values[i].enabled.resize(msg.entry_names.size(), false);
348 
349  // there is an approximation here: if we use a boost function to decide
350  // whether a collision is allowed or not, we just assume the collision is not allowed.
351  for (std::size_t i = 0; i < msg.entry_names.size(); ++i)
352  {
354  bool dfound = getDefaultEntry(msg.entry_names[i], dtype);
355  if (dfound)
356  {
357  msg.default_entry_names.push_back(msg.entry_names[i]);
358  msg.default_entry_values.push_back(dtype == AllowedCollision::ALWAYS);
359  }
360 
361  for (std::size_t j = i; j < msg.entry_names.size(); ++j)
362  {
364  bool found = getEntry(msg.entry_names[i], msg.entry_names[j], type);
365  if (found)
366  msg.entry_values[i].enabled[j] = msg.entry_values[j].enabled[i] = type == AllowedCollision::ALWAYS;
367  }
368  }
369 }
370 
371 void AllowedCollisionMatrix::print(std::ostream& out) const
372 {
373  std::vector<std::string> names;
374  getAllEntryNames(names);
375  std::sort(names.begin(), names.end());
376 
377  std::size_t L = 4;
378  for (auto& name : names)
379  {
380  std::size_t l = name.length();
381  if (l > L)
382  L = l;
383  }
384  ++L;
385 
386  std::size_t D = 2;
387  while (names.size() > pow(10, D) - 1)
388  D++;
389 
390  // print indices along the top of the matrix
391  for (std::size_t j = 0; j < D; ++j)
392  {
393  out << std::setw(L + D + 4) << "";
394  for (std::size_t i = 0; i < names.size(); ++i)
395  {
396  std::stringstream ss;
397  ss << std::setw(D) << i;
398  out << std::setw(3) << ss.str().c_str()[j];
399  }
400  out << std::endl;
401  }
402 
403  for (std::size_t i = 0; i < names.size(); ++i)
404  {
405  out << std::setw(L) << names[i];
406  out << std::setw(D + 1) << i;
407  out << " | ";
408  for (std::size_t j = 0; j < names.size(); ++j)
409  {
411  bool found = getAllowedCollision(names[i], names[j], type);
412  if (found)
413  out << std::setw(3) << (type == AllowedCollision::ALWAYS ? '1' : (type == AllowedCollision::NEVER ? '0' : '?'));
414  else
415  out << std::setw(3) << '-';
416  }
417  out << std::endl;
418  }
419 }
420 
421 } // end of namespace collision_detection
std::map< std::string, std::map< std::string, AllowedCollision::Type > > entries_
Collisions between the pair of bodies is never ok, i.e., if two bodies are in contact in a particular...
void getMessage(moveit_msgs::AllowedCollisionMatrix &msg) const
Get the allowed collision matrix as a message.
void print(std::ostream &out) const
Print the allowed collision matrix.
static bool andDecideContact(const DecideContactFn &f1, const DecideContactFn &f2, Contact &contact)
bool hasEntry(const std::string &name) const
Check if the allowed collision matrix has an entry at all for an element. Returns true if the element...
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 ...
bool getEntry(const std::string &name1, const std::string &name2, AllowedCollision::Type &allowed_collision_type) const
Get the type of the allowed collision between two elements. Return true if the entry is included in t...
std::map< std::string, AllowedCollision::Type > default_entries_
Generic interface to collision detection.
std::map< std::string, DecideContactFn > default_allowed_contacts_
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.
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
std::map< std::string, std::map< std::string, DecideContactFn > > allowed_contacts_
bool getDefaultEntry(const std::string &name, AllowedCollision::Type &allowed_collision) const
Get the type of the allowed collision between to be considered by default for an element. Return true if a default value was found for the specified element. Return false otherwise.
void setDefaultEntry(const std::string &name, bool allowed)
Set the default value for entries that include name. If such a default value is set, queries to getAllowedCollision() that include name will return this value instead, even if a pair that includes name was previously specified with setEntry().
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
void getAllEntryNames(std::vector< std::string > &names) const
Get all the names known to the collision matrix.
void setEntry(const std::string &name1, const std::string &name2, bool allowed)
Set an entry corresponding to a pair of elements.
#define ROS_ERROR_NAMED(name,...)
Collisions between a particular pair of bodies does not imply that the robot configuration is in coll...
void clear()
Clear the allowed collision matrix.
void removeEntry(const std::string &name1, const std::string &name2)
Remove an entry corresponding to a pair of elements. Nothing happens if the pair does not exist in th...


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jul 10 2019 04:03:04