moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
collision_matrix.hpp
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/msg/allowed_collision_matrix.hpp>
42#include <iostream>
43#include <vector>
44#include <string>
45#include <map>
46
47namespace collision_detection
48{
50namespace AllowedCollision
51{
52enum Type
53{
57
61
66};
67} // namespace AllowedCollision
68
71using DecideContactFn = std::function<bool(collision_detection::Contact&)>;
72
73MOVEIT_CLASS_FORWARD(AllowedCollisionMatrix); // Defines AllowedCollisionMatrixPtr, ConstPtr, WeakPtr... etc
74
80{
81public:
83
88 AllowedCollisionMatrix(const std::vector<std::string>& names, bool allowed = false);
89
91 AllowedCollisionMatrix(const srdf::Model& srdf);
92
94 AllowedCollisionMatrix(const moveit_msgs::msg::AllowedCollisionMatrix& msg);
95
98
101
107 bool getEntry(const std::string& name1, const std::string& name2,
108 AllowedCollision::Type& allowed_collision_type) const;
109
117 bool getEntry(const std::string& name1, const std::string& name2, DecideContactFn& fn) const;
118
122 bool hasEntry(const std::string& name) const;
123
128 bool hasEntry(const std::string& name1, const std::string& name2) const;
129
134 void removeEntry(const std::string& name1, const std::string& name2);
135
138 void removeEntry(const std::string& name);
139
146 void setEntry(const std::string& name1, const std::string& name2, bool allowed);
147
154 void setEntry(const std::string& name1, const std::string& name2, DecideContactFn& fn);
155
164 void setEntry(const std::string& name, bool allowed);
165
173 void setEntry(const std::string& name, const std::vector<std::string>& other_names, bool allowed);
174
181 void setEntry(const std::vector<std::string>& names1, const std::vector<std::string>& names2, bool allowed);
182
187 void setEntry(bool allowed);
188
190 void getAllEntryNames(std::vector<std::string>& names) const;
191
193 void getMessage(moveit_msgs::msg::AllowedCollisionMatrix& msg) const;
194
196 void clear();
197
199 std::size_t getSize() const
200 {
201 return entries_.size();
202 }
203
209 void setDefaultEntry(const std::string& name, bool allowed);
210
215 void setDefaultEntry(const std::string& name, DecideContactFn& fn);
216
221 bool getDefaultEntry(const std::string& name, AllowedCollision::Type& allowed_collision) const;
222
227 bool getDefaultEntry(const std::string& name, DecideContactFn& fn) const;
228
235 bool getAllowedCollision(const std::string& name1, const std::string& name2, DecideContactFn& fn) const;
236
243 bool getAllowedCollision(const std::string& name1, const std::string& name2,
244 AllowedCollision::Type& allowed_collision) const;
245
247 void print(std::ostream& out) const;
248
249private:
250 bool getDefaultEntry(const std::string& name1, const std::string& name2,
251 AllowedCollision::Type& allowed_collision) const;
252
253 std::map<std::string, std::map<std::string, AllowedCollision::Type> > entries_;
254 std::map<std::string, std::map<std::string, DecideContactFn> > allowed_contacts_;
255
256 std::map<std::string, AllowedCollision::Type> default_entries_;
257 std::map<std::string, DecideContactFn> default_allowed_contacts_;
258};
259} // namespace collision_detection
#define MOVEIT_CLASS_FORWARD(C)
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
void getAllEntryNames(std::vector< std::string > &names) const
Get all the names known to the collision matrix.
void clear()
Clear the allowed collision matrix.
void setEntry(const std::string &name1, const std::string &name2, bool allowed)
Set an entry corresponding to a pair of elements.
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::size_t getSize() const
Get the size of the allowed collision matrix (number of specified entries)
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...
void print(std::ostream &out) const
Print the allowed collision matrix.
void setDefaultEntry(const std::string &name, bool allowed)
Set the default value for entries that include name but are not set explicitly with setEntry().
AllowedCollisionMatrix & operator=(const AllowedCollisionMatrix &)=default
Copy assignment.
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...
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...
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...
void getMessage(moveit_msgs::msg::AllowedCollisionMatrix &msg) const
Get the allowed collision matrix as a message.
AllowedCollisionMatrix(const AllowedCollisionMatrix &acm)=default
Copy constructor.
@ CONDITIONAL
The collision is allowed depending on a predicate evaluated on the produced contact....
@ NEVER
Collisions between the pair of bodies is never ok, i.e., if two bodies are in contact in a particular...
@ ALWAYS
Collisions between a particular pair of bodies does not imply that the robot configuration is in coll...
std::function< bool(collision_detection::Contact &)> DecideContactFn
Signature of predicate that decides whether a contact is allowed or not (when AllowedCollision::Type ...
Definition of a contact point.