Definition of a structure for the allowed collision matrix. All elements in the collision world are referred to by their names. This class represents which collisions are allowed to happen and which are not. More...
#include <collision_matrix.h>
Public Member Functions | |
AllowedCollisionMatrix () | |
AllowedCollisionMatrix (const std::vector< std::string > &names, bool allowed=false) | |
Instantiate using a vector of names (corresponding to all the elements in the collision world). | |
AllowedCollisionMatrix (const moveit_msgs::AllowedCollisionMatrix &msg) | |
Construct the structure from a message representation. | |
AllowedCollisionMatrix (const AllowedCollisionMatrix &acm) | |
Copy constructor. | |
void | clear () |
Clear the allowed collision matrix. | |
void | getAllEntryNames (std::vector< std::string > &names) const |
Get all the names known to the collision matrix. | |
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 included in the collision matrix (if the type is AllowedCollision::CONDITIONAL) or if one was computed from defaults. Return false if the entry is not found. Default values take precedence. If a default value is specified for either name1 or name2, that value is returned instead (if both elements have default values specified, both predicates must be satisfied). If no default values are specified, getEntry() is used to look for the pair (name1, name2). | |
bool | getAllowedCollision (const std::string &name1, const std::string &name2, AllowedCollision::Type &allowed_collision) const |
Get the type of the allowed collision between two elements. Return true if the entry is included in the collision matrix or if specified defaults were found. Return false if the entry is not found. Default values take precedence. If a default value is specified for either name1 or name2, that value is returned instead (if both elements have default values specified, AllowedCollision::NEVER takes precedence). If no default values are specified, getEntry() is used to look for the pair (name1, name2). | |
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. | |
bool | getDefaultEntry (const std::string &name, DecideContactFn &fn) 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. | |
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 the collision matrix. Return false if the entry is not found. | |
bool | getEntry (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 included in the collision matrix (if the type is AllowedCollision::CONDITIONAL). Return false if the entry is not found. | |
void | getMessage (moveit_msgs::AllowedCollisionMatrix &msg) const |
Get the allowed collision matrix as a message. | |
std::size_t | getSize () const |
Get the size of the allowed collision matrix (number of specified entries) | |
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 is included. | |
bool | hasEntry (const std::string &name1, const std::string &name2) const |
Check if the allowed collision matrix has an entry for a pair of elements. Returns true if the pair is included. | |
void | print (std::ostream &out) const |
Print 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 the collision matrix. | |
void | removeEntry (const std::string &name) |
Remove all entries corresponding to a name (all pairs that include this name) | |
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(). | |
void | setDefaultEntry (const std::string &name, const DecideContactFn &fn) |
Set the default value for entries that include name to be AllowedCollision::CONDITIONAL and specify the allowed contact predicate to be fn. If this function is called, queries to getAllowedCollision() that include name will return this value instead, even if a pair that includes name was previously specified with setEntry(). | |
void | setEntry (const std::string &name1, const std::string &name2, bool allowed) |
Set an entry corresponding to a pair of elements. | |
void | setEntry (const std::string &name1, const std::string &name2, const DecideContactFn &fn) |
Set an entry corresponding to a pair of elements. This sets the type of the entry to AllowedCollision::CONDITIONAL. | |
void | setEntry (const std::string &name, bool allowed) |
Set the entries corresponding to a name. With each of the the known names in the collision matrix, form a pair using the name specified as argument to this function and set the entry as indicated by allowed. | |
void | setEntry (const std::string &name, const std::vector< std::string > &other_names, bool allowed) |
Set multiple entries. Pairs of names are formed using name and other_names. | |
void | setEntry (const std::vector< std::string > &names1, const std::vector< std::string > &names2, bool allowed) |
Set an entry corresponding to all possible pairs between two sets of elements. | |
void | setEntry (bool allowed) |
Set an entry corresponding to all known pairs. | |
Private Attributes | |
std::map< std::string, std::map< std::string, DecideContactFn > > | allowed_contacts_ |
std::map< std::string, DecideContactFn > | default_allowed_contacts_ |
std::map< std::string, AllowedCollision::Type > | default_entries_ |
std::map< std::string, std::map< std::string, AllowedCollision::Type > > | entries_ |
Definition of a structure for the allowed collision matrix. All elements in the collision world are referred to by their names. This class represents which collisions are allowed to happen and which are not.
Definition at line 77 of file collision_matrix.h.
Definition at line 41 of file collision_matrix.cpp.
collision_detection::AllowedCollisionMatrix::AllowedCollisionMatrix | ( | const std::vector< std::string > & | names, |
bool | allowed = false |
||
) |
Instantiate using a vector of names (corresponding to all the elements in the collision world).
names | a vector of names (corresponding to object IDs in the collision world). |
allowed | If false, indicates that collisions between all elements must be checked for and no collisions will be ignored. |
Definition at line 45 of file collision_matrix.cpp.
collision_detection::AllowedCollisionMatrix::AllowedCollisionMatrix | ( | const moveit_msgs::AllowedCollisionMatrix & | msg | ) |
Construct the structure from a message representation.
Definition at line 52 of file collision_matrix.cpp.
collision_detection::AllowedCollisionMatrix::AllowedCollisionMatrix | ( | const AllowedCollisionMatrix & | acm | ) |
Copy constructor.
Definition at line 70 of file collision_matrix.cpp.
Clear the allowed collision matrix.
Definition at line 316 of file collision_matrix.cpp.
void collision_detection::AllowedCollisionMatrix::getAllEntryNames | ( | std::vector< std::string > & | names | ) | const |
Get all the names known to the collision matrix.
Definition at line 324 of file collision_matrix.cpp.
bool collision_detection::AllowedCollisionMatrix::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 included in the collision matrix (if the type is AllowedCollision::CONDITIONAL) or if one was computed from defaults. Return false if the entry is not found. Default values take precedence. If a default value is specified for either name1 or name2, that value is returned instead (if both elements have default values specified, both predicates must be satisfied). If no default values are specified, getEntry() is used to look for the pair (name1, name2).
name1 | name of first element |
name2 | name of second element |
fn | A callback function that is used to decide if collisions are allowed between the two elements is filled here |
Definition at line 260 of file collision_matrix.cpp.
bool collision_detection::AllowedCollisionMatrix::getAllowedCollision | ( | const std::string & | name1, |
const std::string & | name2, | ||
AllowedCollision::Type & | allowed_collision | ||
) | const |
Get the type of the allowed collision between two elements. Return true if the entry is included in the collision matrix or if specified defaults were found. Return false if the entry is not found. Default values take precedence. If a default value is specified for either name1 or name2, that value is returned instead (if both elements have default values specified, AllowedCollision::NEVER takes precedence). If no default values are specified, getEntry() is used to look for the pair (name1, name2).
name1 | name of first element |
name2 | name of second element |
allowed_collision | The allowed collision type will be filled here |
Definition at line 284 of file collision_matrix.cpp.
bool collision_detection::AllowedCollisionMatrix::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.
name | name of the element |
allowed_collision | The default allowed collision type will be filled here |
Definition at line 234 of file collision_matrix.cpp.
bool collision_detection::AllowedCollisionMatrix::getDefaultEntry | ( | const std::string & | name, |
DecideContactFn & | fn | ||
) | 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.
name | name of the element |
fn | A callback function that is used to decide if collisions are allowed between the two elements is filled here. |
Definition at line 243 of file collision_matrix.cpp.
bool collision_detection::AllowedCollisionMatrix::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 the collision matrix. Return false if the entry is not found.
name1 | name of first element |
name2 | name of second element |
allowed_collision_type | The allowed collision type will be filled here |
Definition at line 90 of file collision_matrix.cpp.
bool collision_detection::AllowedCollisionMatrix::getEntry | ( | 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 included in the collision matrix (if the type is AllowedCollision::CONDITIONAL). Return false if the entry is not found.
name1 | name of first element |
name2 | name of second element |
fn | A callback function that is used to decide if collisions are allowed between the two elements is filled here |
Definition at line 78 of file collision_matrix.cpp.
void collision_detection::AllowedCollisionMatrix::getMessage | ( | moveit_msgs::AllowedCollisionMatrix & | msg | ) | const |
Get the allowed collision matrix as a message.
Definition at line 334 of file collision_matrix.cpp.
std::size_t collision_detection::AllowedCollisionMatrix::getSize | ( | ) | const [inline] |
Get the size of the allowed collision matrix (number of specified entries)
Definition at line 182 of file collision_matrix.h.
bool collision_detection::AllowedCollisionMatrix::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 is included.
name | name of the element |
Definition at line 102 of file collision_matrix.cpp.
bool collision_detection::AllowedCollisionMatrix::hasEntry | ( | const std::string & | name1, |
const std::string & | name2 | ||
) | const |
Check if the allowed collision matrix has an entry for a pair of elements. Returns true if the pair is included.
name1 | name of first element |
name2 | name of second element |
Definition at line 107 of file collision_matrix.cpp.
void collision_detection::AllowedCollisionMatrix::print | ( | std::ostream & | out | ) | const |
Print the allowed collision matrix.
Definition at line 370 of file collision_matrix.cpp.
void collision_detection::AllowedCollisionMatrix::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 the collision matrix.
name1 | name of first element |
name2 | name of second element |
Definition at line 156 of file collision_matrix.cpp.
void collision_detection::AllowedCollisionMatrix::removeEntry | ( | const std::string & | name | ) |
Remove all entries corresponding to a name (all pairs that include this name)
name | namespace |
Definition at line 146 of file collision_matrix.cpp.
void collision_detection::AllowedCollisionMatrix::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().
name | The name of the element for which to set the default value |
allowed | If false, indicates that collisions between name and any other element must be checked for and no collisions will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between name and any other element are ok and an explicit collision computation is not necessary (AllowedCollision::ALWAYS). |
Definition at line 221 of file collision_matrix.cpp.
void collision_detection::AllowedCollisionMatrix::setDefaultEntry | ( | const std::string & | name, |
const DecideContactFn & | fn | ||
) |
Set the default value for entries that include name to be AllowedCollision::CONDITIONAL and specify the allowed contact predicate to be fn. If this function is called, queries to getAllowedCollision() that include name will return this value instead, even if a pair that includes name was previously specified with setEntry().
name | The name of the element for which to set the default value |
fn | A callback function that is used to decide if collisions are allowed between name and some other element is expected here. |
Definition at line 228 of file collision_matrix.cpp.
void collision_detection::AllowedCollisionMatrix::setEntry | ( | const std::string & | name1, |
const std::string & | name2, | ||
bool | allowed | ||
) |
Set an entry corresponding to a pair of elements.
name1 | name of first element |
name2 | name of second element |
allowed | If false, indicates that collisions between two elements must be checked for and no collisions will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between two elements are ok and an explicit collision computation is not necessary (AllowedCollision::ALWAYS). |
Definition at line 118 of file collision_matrix.cpp.
void collision_detection::AllowedCollisionMatrix::setEntry | ( | const std::string & | name1, |
const std::string & | name2, | ||
const DecideContactFn & | fn | ||
) |
Set an entry corresponding to a pair of elements. This sets the type of the entry to AllowedCollision::CONDITIONAL.
name1 | name of first element |
name2 | name of second element |
fn | A callback function that is used to decide if collisions are allowed between the two elements is expected here |
Definition at line 140 of file collision_matrix.cpp.
void collision_detection::AllowedCollisionMatrix::setEntry | ( | const std::string & | name, |
bool | allowed | ||
) |
Set the entries corresponding to a name. With each of the the known names in the collision matrix, form a pair using the name specified as argument to this function and set the entry as indicated by allowed.
name | the object name |
allowed | If false, indicates that collisions between two elements must be checked for and no collisions will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between two elements are ok and an explicit collision computation is not necessary (AllowedCollision::ALWAYS). |
Definition at line 202 of file collision_matrix.cpp.
void collision_detection::AllowedCollisionMatrix::setEntry | ( | const std::string & | name, |
const std::vector< std::string > & | other_names, | ||
bool | allowed | ||
) |
Set multiple entries. Pairs of names are formed using name and other_names.
name | name of first element |
other_names | names of all other elements to pair with first element. The collision matrix entries will be set for all such pairs. |
allowed | If false, indicates that collisions between two elements must be checked for and no collisions will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between two elements are ok and an explicit collision computation is not necessary (AllowedCollision::ALWAYS). |
Definition at line 189 of file collision_matrix.cpp.
void collision_detection::AllowedCollisionMatrix::setEntry | ( | const std::vector< std::string > & | names1, |
const std::vector< std::string > & | names2, | ||
bool | allowed | ||
) |
Set an entry corresponding to all possible pairs between two sets of elements.
names1 | First set of names |
names2 | Second set of names |
allowed | If false, indicates that collisions between two elements must be checked for and no collisions will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between two elements are ok and an explicit collision computation is not necessary (AllowedCollision::ALWAYS). |
Definition at line 196 of file collision_matrix.cpp.
void collision_detection::AllowedCollisionMatrix::setEntry | ( | bool | allowed | ) |
Set an entry corresponding to all known pairs.
allowed | If false, indicates that collisions between two elements must be checked for and no collisions will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between two elements are ok and an explicit collision computation is not necessary (AllowedCollision::ALWAYS). |
Definition at line 213 of file collision_matrix.cpp.
std::map<std::string, std::map<std::string, DecideContactFn> > collision_detection::AllowedCollisionMatrix::allowed_contacts_ [private] |
Definition at line 239 of file collision_matrix.h.
std::map<std::string, DecideContactFn> collision_detection::AllowedCollisionMatrix::default_allowed_contacts_ [private] |
Definition at line 242 of file collision_matrix.h.
std::map<std::string, AllowedCollision::Type> collision_detection::AllowedCollisionMatrix::default_entries_ [private] |
Definition at line 241 of file collision_matrix.h.
std::map<std::string, std::map<std::string, AllowedCollision::Type> > collision_detection::AllowedCollisionMatrix::entries_ [private] |
Definition at line 238 of file collision_matrix.h.