38 #include <boost/bind.hpp> 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);
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");
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())
65 "Number of entries is incorrect for link '%s' in AllowedCollisionMatrix message",
66 msg.entry_names[i].c_str());
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]);
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]);
89 auto it2 = it1->second.find(name2);
90 if (it2 == it1->second.end())
102 auto it2 = it1->second.find(name2);
103 if (it2 == it1->second.end())
105 allowed_collision = it2->second;
119 auto it2 = it1->second.find(name2);
120 return it2 != it1->second.end();
132 auto jt = it->second.find(name2);
133 if (jt != it->second.end())
134 it->second.erase(jt);
139 auto jt = it->second.find(name1);
140 if (jt != it->second.end())
141 it->second.erase(jt);
156 entry.second.erase(name);
158 allowed_contact.second.erase(name);
166 auto it = jt->second.find(name2);
167 if (it != jt->second.end())
168 jt->second.erase(it);
173 auto it = jt->second.find(name1);
174 if (it != jt->second.end())
175 jt->second.erase(it);
181 auto jt = it->second.find(name2);
182 if (jt != it->second.end())
183 it->second.erase(jt);
188 auto jt = it->second.find(name1);
189 if (jt != it->second.end())
190 it->second.erase(jt);
197 for (
const auto& other_name : other_names)
198 if (other_name != name)
199 setEntry(other_name, name, allowed);
205 for (
const auto& name1 : names1)
211 std::string last = name;
213 if (name != entry.first && last != entry.first)
216 setEntry(name, entry.first, allowed);
224 for (
auto& it2 : entry.second)
246 allowed_collision = it->second;
261 return f1(contact) && f2(contact);
271 if (!found1 && !found2)
275 if (found1 && !found2)
277 else if (!found1 && found2)
279 else if (found1 && found2)
294 if (!found1 && !found2)
295 return getEntry(name1, name2, allowed_collision);
298 if (found1 && !found2)
299 allowed_collision = t1;
300 else if (!found1 && found2)
301 allowed_collision = t2;
302 else if (found1 && found2)
329 if (!names.empty() && names.back() == entry.first)
332 names.push_back(entry.first);
337 msg.entry_names.clear();
338 msg.entry_values.clear();
339 msg.default_entry_names.clear();
340 msg.default_entry_values.clear();
343 std::sort(msg.entry_names.begin(), msg.entry_names.end());
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);
351 for (std::size_t i = 0; i < msg.entry_names.size(); ++i)
357 msg.default_entry_names.push_back(msg.entry_names[i]);
361 for (std::size_t j = i; j < msg.entry_names.size(); ++j)
364 bool found =
getEntry(msg.entry_names[i], msg.entry_names[j], type);
373 std::vector<std::string> names;
375 std::sort(names.begin(), names.end());
378 for (
auto& name : names)
380 std::size_t l = name.length();
387 while (names.size() >
pow(10, D) - 1)
391 for (std::size_t j = 0; j < D; ++j)
393 out << std::setw(L + D + 4) <<
"";
394 for (std::size_t i = 0; i < names.size(); ++i)
396 std::stringstream ss;
397 ss << std::setw(D) << i;
398 out << std::setw(3) << ss.str().c_str()[j];
403 for (std::size_t i = 0; i < names.size(); ++i)
405 out << std::setw(L) << names[i];
406 out << std::setw(D + 1) << i;
408 for (std::size_t j = 0; j < names.size(); ++j)
415 out << std::setw(3) <<
'-';
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...