collision_matrix.h
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 
37 #pragma once
38 
41 #include <moveit_msgs/AllowedCollisionMatrix.h>
42 #include <boost/function.hpp>
43 #include <iostream>
44 #include <vector>
45 #include <string>
46 #include <map>
47 
48 namespace collision_detection
49 {
51 namespace AllowedCollision
52 {
53 enum Type
54 {
57  NEVER,
58 
61  ALWAYS,
62 
67 };
68 } // namespace AllowedCollision
69 
72 using DecideContactFn = boost::function<bool(collision_detection::Contact&)>;
73 
74 MOVEIT_CLASS_FORWARD(AllowedCollisionMatrix); // Defines AllowedCollisionMatrixPtr, ConstPtr, WeakPtr... etc
75 
80 class AllowedCollisionMatrix
81 {
82 public:
84 
89  AllowedCollisionMatrix(const std::vector<std::string>& names, bool allowed = false);
90 
93 
95  AllowedCollisionMatrix(const moveit_msgs::AllowedCollisionMatrix& msg);
96 
98  AllowedCollisionMatrix(const AllowedCollisionMatrix& acm) = default;
99 
105  bool getEntry(const std::string& name1, const std::string& name2,
106  AllowedCollision::Type& allowed_collision_type) const;
107 
115  bool getEntry(const std::string& name1, const std::string& name2, DecideContactFn& fn) const;
116 
120  bool hasEntry(const std::string& name) const;
121 
126  bool hasEntry(const std::string& name1, const std::string& name2) const;
127 
132  void removeEntry(const std::string& name1, const std::string& name2);
133 
136  void removeEntry(const std::string& name);
137 
144  void setEntry(const std::string& name1, const std::string& name2, bool allowed);
145 
152  void setEntry(const std::string& name1, const std::string& name2, const DecideContactFn& fn);
153 
162  void setEntry(const std::string& name, bool allowed);
163 
171  void setEntry(const std::string& name, const std::vector<std::string>& other_names, bool allowed);
172 
179  void setEntry(const std::vector<std::string>& names1, const std::vector<std::string>& names2, bool allowed);
180 
185  void setEntry(bool allowed);
186 
188  void getAllEntryNames(std::vector<std::string>& names) const;
189 
191  void getMessage(moveit_msgs::AllowedCollisionMatrix& msg) const;
192 
194  void clear();
195 
197  std::size_t getSize() const
198  {
199  return entries_.size();
200  }
201 
207  void setDefaultEntry(const std::string& name, bool allowed);
208 
213  void setDefaultEntry(const std::string& name, const DecideContactFn& fn);
214 
219  bool getDefaultEntry(const std::string& name, AllowedCollision::Type& allowed_collision) const;
220 
225  bool getDefaultEntry(const std::string& name, DecideContactFn& fn) const;
226 
233  bool getAllowedCollision(const std::string& name1, const std::string& name2, DecideContactFn& fn) const;
234 
241  bool getAllowedCollision(const std::string& name1, const std::string& name2,
242  AllowedCollision::Type& allowed_collision) const;
243 
245  void print(std::ostream& out) const;
246 
247 private:
248  bool getDefaultEntry(const std::string& name1, const std::string& name2,
249  AllowedCollision::Type& allowed_collision) const;
250 
251  std::map<std::string, std::map<std::string, AllowedCollision::Type> > entries_;
252  std::map<std::string, std::map<std::string, DecideContactFn> > allowed_contacts_;
253 
254  std::map<std::string, AllowedCollision::Type> default_entries_;
255  std::map<std::string, DecideContactFn> default_allowed_contacts_;
256 };
257 } // namespace collision_detection
collision_detection::AllowedCollisionMatrix::setDefaultEntry
void setDefaultEntry(const std::string &name, bool allowed)
Set the default value for entries that include name but are not set explicity with setEntry().
Definition: collision_matrix.cpp:276
collision_detection::AllowedCollisionMatrix::hasEntry
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...
Definition: collision_matrix.cpp:158
collision_detection::AllowedCollisionMatrix::entries_
std::map< std::string, std::map< std::string, AllowedCollision::Type > > entries_
Definition: collision_matrix.h:283
collision_detection::AllowedCollisionMatrix::getDefaultEntry
bool getDefaultEntry(const std::string &name, AllowedCollision::Type &allowed_collision) const
Get the type of the allowed collision to be considered by default for an element. Return true if a de...
Definition: collision_matrix.cpp:289
collision_detection::AllowedCollisionMatrix::getSize
std::size_t getSize() const
Get the size of the allowed collision matrix (number of specified entries)
Definition: collision_matrix.h:229
collision_detection::MOVEIT_CLASS_FORWARD
MOVEIT_CLASS_FORWARD(AllowedCollisionMatrix)
collision_detection::AllowedCollisionMatrix::getAllowedCollision
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...
Definition: collision_matrix.cpp:312
collision_detection::AllowedCollisionMatrix::getEntry
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...
Definition: collision_matrix.cpp:145
collision_detection::AllowedCollisionMatrix::removeEntry
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...
Definition: collision_matrix.cpp:210
collision_detection::AllowedCollision::ALWAYS
@ ALWAYS
Collisions between a particular pair of bodies does not imply that the robot configuration is in coll...
Definition: collision_matrix.h:157
class_forward.h
collision_detection::Contact
Definition of a contact point.
Definition: include/moveit/collision_detection/collision_common.h:105
collision_detection::AllowedCollisionMatrix::getMessage
void getMessage(moveit_msgs::AllowedCollisionMatrix &msg) const
Get the allowed collision matrix as a message.
Definition: collision_matrix.cpp:385
collision_common.h
collision_detection::AllowedCollisionMatrix
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
Definition: collision_matrix.h:112
collision_detection::AllowedCollisionMatrix::AllowedCollisionMatrix
AllowedCollisionMatrix()
Definition: collision_matrix.cpp:75
collision_detection::AllowedCollision::Type
Type
Definition: collision_matrix.h:117
collision_detection::AllowedCollision::CONDITIONAL
@ CONDITIONAL
The collision is allowed depending on a predicate evaluated on the produced contact....
Definition: collision_matrix.h:162
collision_detection::AllowedCollisionMatrix::print
void print(std::ostream &out) const
Print the allowed collision matrix.
Definition: collision_matrix.cpp:419
collision_detection::AllowedCollisionMatrix::clear
void clear()
Clear the allowed collision matrix.
Definition: collision_matrix.cpp:363
collision_detection::AllowedCollisionMatrix::allowed_contacts_
std::map< std::string, std::map< std::string, DecideContactFn > > allowed_contacts_
Definition: collision_matrix.h:284
collision_detection::AllowedCollisionMatrix::default_entries_
std::map< std::string, AllowedCollision::Type > default_entries_
Definition: collision_matrix.h:286
srdf
collision_detection::AllowedCollisionMatrix::default_allowed_contacts_
std::map< std::string, DecideContactFn > default_allowed_contacts_
Definition: collision_matrix.h:287
collision_detection::AllowedCollisionMatrix::getAllEntryNames
void getAllEntryNames(std::vector< std::string > &names) const
Get all the names known to the collision matrix.
Definition: collision_matrix.cpp:371
srdf::Model
collision_detection::DecideContactFn
boost::function< bool(collision_detection::Contact &)> DecideContactFn
Signature of predicate that decides whether a contact is allowed or not (when AllowedCollision::Type ...
Definition: collision_matrix.h:104
collision_detection::AllowedCollision::NEVER
@ NEVER
Collisions between the pair of bodies is never ok, i.e., if two bodies are in contact in a particular...
Definition: collision_matrix.h:153
collision_detection::AllowedCollisionMatrix::setEntry
void setEntry(const std::string &name1, const std::string &name2, bool allowed)
Set an entry corresponding to a pair of elements.
Definition: collision_matrix.cpp:172
collision_detection
Definition: collision_detector_allocator_allvalid.h:42


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Sep 18 2022 02:26:15