collision_robot.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 */
36 
38 #include <limits>
39 
40 static inline bool validateScale(double scale)
41 {
42  if (scale < std::numeric_limits<double>::epsilon())
43  {
44  ROS_ERROR_NAMED("collision_detection", "Scale must be positive");
45  return false;
46  }
47  if (scale > std::numeric_limits<double>::max())
48  {
49  ROS_ERROR_NAMED("collision_detection", "Scale must be finite");
50  return false;
51  }
52  return true;
53 }
54 
55 static inline bool validatePadding(double padding)
56 {
57  if (padding < 0.0)
58  {
59  ROS_ERROR_NAMED("collision_detection", "Padding cannot be negative");
60  return false;
61  }
62  if (padding > std::numeric_limits<double>::max())
63  {
64  ROS_ERROR_NAMED("collision_detection", "Padding must be finite");
65  return false;
66  }
67  return true;
68 }
69 
70 namespace collision_detection
71 {
72 CollisionRobot::CollisionRobot(const robot_model::RobotModelConstPtr& model, // NOLINT
73  double padding, double scale)
74  : robot_model_(model)
75 {
76  if (!validateScale(scale))
77  scale = 1.0;
78  if (!validatePadding(padding))
79  padding = 0.0;
80 
81  const std::vector<const robot_model::LinkModel*>& links = robot_model_->getLinkModelsWithCollisionGeometry();
82  for (auto link : links)
83  {
84  link_padding_[link->getName()] = padding;
85  link_scale_[link->getName()] = scale;
86  }
87 }
88 
90 {
92  link_scale_ = other.link_scale_;
93 }
94 
95 void CollisionRobot::setPadding(double padding)
96 {
97  if (!validatePadding(padding))
98  return;
99  std::vector<std::string> u;
100  const std::vector<const robot_model::LinkModel*>& links = robot_model_->getLinkModelsWithCollisionGeometry();
101  for (auto link : links)
102  {
103  if (getLinkPadding(link->getName()) != padding)
104  u.push_back(link->getName());
105  link_padding_[link->getName()] = padding;
106  }
107  if (!u.empty())
109 }
110 
111 void CollisionRobot::setScale(double scale)
112 {
113  if (!validateScale(scale))
114  return;
115  std::vector<std::string> u;
116  const std::vector<const robot_model::LinkModel*>& links = robot_model_->getLinkModelsWithCollisionGeometry();
117  for (auto link : links)
118  {
119  if (getLinkScale(link->getName()) != scale)
120  u.push_back(link->getName());
121  link_scale_[link->getName()] = scale;
122  }
123  if (!u.empty())
125 }
126 
127 void CollisionRobot::setLinkPadding(const std::string& link_name, double padding)
128 {
129  bool update = getLinkPadding(link_name) != padding;
130  link_padding_[link_name] = padding;
131  if (update)
132  {
133  std::vector<std::string> u(1, link_name);
135  }
136 }
137 
138 double CollisionRobot::getLinkPadding(const std::string& link_name) const
139 {
140  auto it = link_padding_.find(link_name);
141  if (it != link_padding_.end())
142  return it->second;
143  else
144  return 0.0;
145 }
146 
147 void CollisionRobot::setLinkPadding(const std::map<std::string, double>& padding)
148 {
149  std::vector<std::string> u;
150  for (const auto& link_pad_pair : padding)
151  {
152  bool update = getLinkPadding(link_pad_pair.first) != link_pad_pair.second;
153  link_padding_[link_pad_pair.first] = link_pad_pair.second;
154  if (update)
155  u.push_back(link_pad_pair.first);
156  }
157  if (!u.empty())
159 }
160 
161 const std::map<std::string, double>& CollisionRobot::getLinkPadding() const
162 {
163  return link_padding_;
164 }
165 
166 void CollisionRobot::setLinkScale(const std::string& link_name, double scale)
167 {
168  bool update = getLinkScale(link_name) != scale;
169  link_scale_[link_name] = scale;
170  if (update)
171  {
172  std::vector<std::string> u(1, link_name);
174  }
175 }
176 
177 double CollisionRobot::getLinkScale(const std::string& link_name) const
178 {
179  auto it = link_scale_.find(link_name);
180  if (it != link_scale_.end())
181  return it->second;
182  else
183  return 1.0;
184 }
185 
186 void CollisionRobot::setLinkScale(const std::map<std::string, double>& scale)
187 {
188  std::vector<std::string> u;
189  for (const auto& link_scale_pair : scale)
190  {
191  bool update = getLinkScale(link_scale_pair.first) != link_scale_pair.second;
192  link_scale_[link_scale_pair.first] = link_scale_pair.second;
193  if (update)
194  u.push_back(link_scale_pair.first);
195  }
196  if (!u.empty())
198 }
199 
200 const std::map<std::string, double>& CollisionRobot::getLinkScale() const
201 {
202  return link_scale_;
203 }
204 
205 void CollisionRobot::setPadding(const std::vector<moveit_msgs::LinkPadding>& padding)
206 {
207  std::vector<std::string> u;
208  for (const auto& p : padding)
209  {
210  bool update = getLinkPadding(p.link_name) != p.padding;
211  link_padding_[p.link_name] = p.padding;
212  if (update)
213  u.push_back(p.link_name);
214  }
215  if (!u.empty())
217 }
218 
219 void CollisionRobot::setScale(const std::vector<moveit_msgs::LinkScale>& scale)
220 {
221  std::vector<std::string> u;
222  for (const auto& s : scale)
223  {
224  bool update = getLinkScale(s.link_name) != s.scale;
225  link_scale_[s.link_name] = s.scale;
226  if (update)
227  u.push_back(s.link_name);
228  }
229  if (!u.empty())
231 }
232 
233 void CollisionRobot::getPadding(std::vector<moveit_msgs::LinkPadding>& padding) const
234 {
235  padding.clear();
236  for (const auto& lp : link_padding_)
237  {
238  moveit_msgs::LinkPadding lp_msg;
239  lp_msg.link_name = lp.first;
240  lp_msg.padding = lp.second;
241  padding.push_back(lp_msg);
242  }
243 }
244 
245 void CollisionRobot::getScale(std::vector<moveit_msgs::LinkScale>& scale) const
246 {
247  scale.clear();
248  for (const auto& ls : link_scale_)
249  {
250  moveit_msgs::LinkScale ls_msg;
251  ls_msg.link_name = ls.first;
252  ls_msg.scale = ls.second;
253  scale.push_back(ls_msg);
254  }
255 }
256 
257 void CollisionRobot::updatedPaddingOrScaling(const std::vector<std::string>& links)
258 {
259 }
260 
261 } // end of namespace collision_detection
void setScale(double scale)
Set the link scaling (for every link)
static bool validatePadding(double padding)
static bool validateScale(double scale)
std::map< std::string, double > link_scale_
The internally maintained map (from link names to scaling)
XmlRpcServer s
void setPadding(double padding)
Set the link padding (for every link)
const std::map< std::string, double > & getLinkPadding() const
Get the link paddings as a map (from link names to padding value)
CollisionRobot(const robot_model::RobotModelConstPtr &model, double padding=0.0, double scale=1.0)
Constructor.
virtual void updatedPaddingOrScaling(const std::vector< std::string > &links)
When the scale or padding is changed for a set of links by any of the functions in this class...
Generic interface to collision detection.
void getScale(std::vector< moveit_msgs::LinkScale > &scale) const
Get the link scaling as a vector of messages.
robot_model::RobotModelConstPtr robot_model_
The kinematic model corresponding to this collision model.
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
void getPadding(std::vector< moveit_msgs::LinkPadding > &padding) const
Get the link padding as a vector of messages.
void setLinkScale(const std::string &link_name, double scale)
Set the scaling for a particular link.
This class represents a collision model of the robot and can be used for self collision checks (to ch...
std::map< std::string, double > link_padding_
The internally maintained map (from link names to padding)
void setLinkPadding(const std::string &link_name, double padding)
Set the link padding for a particular link.
const std::map< std::string, double > & getLinkScale() const
Get the link scaling as a map (from link names to scale value)
#define ROS_ERROR_NAMED(name,...)


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Oct 18 2020 13:16:33