collision_tools.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 <tf2_eigen/tf2_eigen.h>
39 
40 namespace collision_detection
41 {
42 void getCostMarkers(visualization_msgs::MarkerArray& arr, const std::string& frame_id,
43  std::set<CostSource>& cost_sources)
44 {
45  std_msgs::ColorRGBA color;
46  color.r = 1.0f;
47  color.g = 0.5f;
48  color.b = 0.0f;
49  color.a = 0.4f;
50  getCostMarkers(arr, frame_id, cost_sources, color, ros::Duration(60.0));
51 }
52 
53 void getCollisionMarkersFromContacts(visualization_msgs::MarkerArray& arr, const std::string& frame_id,
54  const CollisionResult::ContactMap& con)
55 {
56  std_msgs::ColorRGBA color;
57  color.r = 1.0f;
58  color.g = 0.0f;
59  color.b = 0.0f;
60  color.a = 0.8f;
61  getCollisionMarkersFromContacts(arr, frame_id, con, color, ros::Duration(60.0));
62 }
63 
64 void getCostMarkers(visualization_msgs::MarkerArray& arr, const std::string& frame_id,
65  std::set<CostSource>& cost_sources, const std_msgs::ColorRGBA& color, const ros::Duration& lifetime)
66 {
67  int id = 0;
68  for (const auto& cost_source : cost_sources)
69  {
70  visualization_msgs::Marker mk;
71  mk.header.stamp = ros::Time::now();
72  mk.header.frame_id = frame_id;
73  mk.ns = "cost_source";
74  mk.id = id++;
75  mk.type = visualization_msgs::Marker::CUBE;
76  mk.action = visualization_msgs::Marker::ADD;
77  mk.pose.position.x = (cost_source.aabb_max[0] + cost_source.aabb_min[0]) / 2.0;
78  mk.pose.position.y = (cost_source.aabb_max[1] + cost_source.aabb_min[1]) / 2.0;
79  mk.pose.position.z = (cost_source.aabb_max[2] + cost_source.aabb_min[2]) / 2.0;
80  mk.pose.orientation.x = 0.0;
81  mk.pose.orientation.y = 0.0;
82  mk.pose.orientation.z = 0.0;
83  mk.pose.orientation.w = 1.0;
84  mk.scale.x = cost_source.aabb_max[0] - cost_source.aabb_min[0];
85  mk.scale.y = cost_source.aabb_max[1] - cost_source.aabb_min[1];
86  mk.scale.z = cost_source.aabb_max[2] - cost_source.aabb_min[2];
87  mk.color = color;
88  if (mk.color.a == 0.0)
89  mk.color.a = 1.0;
90  mk.lifetime = lifetime;
91  arr.markers.push_back(mk);
92  }
93 }
94 
95 void getCollisionMarkersFromContacts(visualization_msgs::MarkerArray& arr, const std::string& frame_id,
96  const CollisionResult::ContactMap& con, const std_msgs::ColorRGBA& color,
97  const ros::Duration& lifetime, double radius)
98 
99 {
100  std::map<std::string, unsigned> ns_counts;
101  for (const auto& collision : con)
102  {
103  for (const auto& contact : collision.second)
104  {
105  std::string ns_name = contact.body_name_1 + "=" + contact.body_name_2;
106  if (ns_counts.find(ns_name) == ns_counts.end())
107  ns_counts[ns_name] = 0;
108  else
109  ns_counts[ns_name]++;
110  visualization_msgs::Marker mk;
111  mk.header.stamp = ros::Time::now();
112  mk.header.frame_id = frame_id;
113  mk.ns = ns_name;
114  mk.id = ns_counts[ns_name];
115  mk.type = visualization_msgs::Marker::SPHERE;
116  mk.action = visualization_msgs::Marker::ADD;
117  mk.pose.position.x = contact.pos.x();
118  mk.pose.position.y = contact.pos.y();
119  mk.pose.position.z = contact.pos.z();
120  mk.pose.orientation.x = 0.0;
121  mk.pose.orientation.y = 0.0;
122  mk.pose.orientation.z = 0.0;
123  mk.pose.orientation.w = 1.0;
124  mk.scale.x = mk.scale.y = mk.scale.z = radius * 2.0;
125  mk.color = color;
126  if (mk.color.a == 0.0)
127  mk.color.a = 1.0;
128  mk.lifetime = lifetime;
129  arr.markers.push_back(mk);
130  }
131  }
132 }
133 
134 bool getSensorPositioning(geometry_msgs::Point& point, const std::set<CostSource>& cost_sources)
135 {
136  if (cost_sources.empty())
137  return false;
138  auto it = cost_sources.begin();
139  for (std::size_t i = 0; i < 4 * cost_sources.size() / 5; ++i)
140  ++it;
141  point.x = (it->aabb_max[0] + it->aabb_min[0]) / 2.0;
142  point.y = (it->aabb_max[1] + it->aabb_min[1]) / 2.0;
143  point.z = (it->aabb_max[2] + it->aabb_min[2]) / 2.0;
144  return true;
145 }
146 
147 double getTotalCost(const std::set<CostSource>& cost_sources)
148 {
149  double cost = 0.0;
150  for (const auto& cost_source : cost_sources)
151  cost += cost_source.getVolume() * cost_source.cost;
152  return cost;
153 }
154 
155 void intersectCostSources(std::set<CostSource>& cost_sources, const std::set<CostSource>& a,
156  const std::set<CostSource>& b)
157 {
158  cost_sources.clear();
159  CostSource tmp;
160  for (const auto& source_a : a)
161  for (const auto& source_b : b)
162  {
163  tmp.aabb_min[0] = std::max(source_a.aabb_min[0], source_b.aabb_min[0]);
164  tmp.aabb_min[1] = std::max(source_a.aabb_min[1], source_b.aabb_min[1]);
165  tmp.aabb_min[2] = std::max(source_a.aabb_min[2], source_b.aabb_min[2]);
166 
167  tmp.aabb_max[0] = std::min(source_a.aabb_max[0], source_b.aabb_max[0]);
168  tmp.aabb_max[1] = std::min(source_a.aabb_max[1], source_b.aabb_max[1]);
169  tmp.aabb_max[2] = std::min(source_a.aabb_max[2], source_b.aabb_max[2]);
170 
171  if (tmp.aabb_min[0] >= tmp.aabb_max[0] || tmp.aabb_min[1] >= tmp.aabb_max[1] || tmp.aabb_min[2] >= tmp.aabb_max[2])
172  continue;
173  tmp.cost = std::max(source_a.cost, source_b.cost);
174  cost_sources.insert(tmp);
175  }
176 }
177 
178 void removeOverlapping(std::set<CostSource>& cost_sources, double overlap_fraction)
179 {
180  double p[3], q[3];
181  for (auto it = cost_sources.begin(); it != cost_sources.end(); ++it)
182  {
183  double vol = it->getVolume() * overlap_fraction;
184  std::vector<std::set<CostSource>::iterator> remove;
185  auto it1 = it;
186  for (auto jt = ++it1; jt != cost_sources.end(); ++jt)
187  {
188  p[0] = std::max(it->aabb_min[0], jt->aabb_min[0]);
189  p[1] = std::max(it->aabb_min[1], jt->aabb_min[1]);
190  p[2] = std::max(it->aabb_min[2], jt->aabb_min[2]);
191 
192  q[0] = std::min(it->aabb_max[0], jt->aabb_max[0]);
193  q[1] = std::min(it->aabb_max[1], jt->aabb_max[1]);
194  q[2] = std::min(it->aabb_max[2], jt->aabb_max[2]);
195 
196  if (p[0] >= q[0] || p[1] >= q[1] || p[2] >= q[2])
197  continue;
198 
199  double intersect_volume = (q[0] - p[0]) * (q[1] - p[1]) * (q[2] - p[2]);
200  if (intersect_volume >= vol)
201  remove.push_back(jt);
202  }
203  for (auto& r : remove)
204  cost_sources.erase(r);
205  }
206 }
207 
208 void removeCostSources(std::set<CostSource>& cost_sources, const std::set<CostSource>& cost_sources_to_remove,
209  double overlap_fraction)
210 {
211  // remove all the boxes that overlap with the intersection previously computed in \e rem
212  double p[3], q[3];
213  for (const auto& source_remove : cost_sources_to_remove)
214  {
215  std::vector<std::set<CostSource>::iterator> remove;
216  std::set<CostSource> add;
217  for (auto it = cost_sources.begin(); it != cost_sources.end(); ++it)
218  {
219  p[0] = std::max(it->aabb_min[0], source_remove.aabb_min[0]);
220  p[1] = std::max(it->aabb_min[1], source_remove.aabb_min[1]);
221  p[2] = std::max(it->aabb_min[2], source_remove.aabb_min[2]);
222 
223  q[0] = std::min(it->aabb_max[0], source_remove.aabb_max[0]);
224  q[1] = std::min(it->aabb_max[1], source_remove.aabb_max[1]);
225  q[2] = std::min(it->aabb_max[2], source_remove.aabb_max[2]);
226 
227  if (p[0] >= q[0] || p[1] >= q[1] || p[2] >= q[2])
228  continue;
229 
230  double intersect_volume = (q[0] - p[0]) * (q[1] - p[1]) * (q[2] - p[2]);
231  if (intersect_volume >= it->getVolume() * overlap_fraction)
232  remove.push_back(it);
233  else
234  {
235  // there is some overlap, but not too large, so we split the cost source into multiple ones
236  for (int i = 0; i < 3; ++i)
237  {
238  // is there a box above axis i in the intersection?
239  if (it->aabb_max[i] > q[i])
240  {
241  CostSource cs = *it;
242  cs.aabb_min[i] = q[i];
243  add.insert(cs);
244  }
245  // is there a box below axis i in the intersection?
246  if (it->aabb_min[i] < p[i])
247  {
248  CostSource cs = *it;
249  cs.aabb_max[i] = p[i];
250  add.insert(cs);
251  }
252  }
253  }
254  }
255  for (auto& r : remove)
256  cost_sources.erase(r);
257  cost_sources.insert(add.begin(), add.end());
258  }
259 }
260 
261 void costSourceToMsg(const CostSource& cost_source, moveit_msgs::CostSource& msg)
262 {
263  msg.cost_density = cost_source.cost;
264  msg.aabb_min.x = cost_source.aabb_min[0];
265  msg.aabb_min.y = cost_source.aabb_min[1];
266  msg.aabb_min.z = cost_source.aabb_min[2];
267  msg.aabb_max.x = cost_source.aabb_max[0];
268  msg.aabb_max.y = cost_source.aabb_max[1];
269  msg.aabb_max.z = cost_source.aabb_max[2];
270 }
271 
272 void contactToMsg(const Contact& contact, moveit_msgs::ContactInformation& msg)
273 {
274  msg.position = tf2::toMsg(contact.pos);
275  tf2::toMsg(contact.normal, msg.normal);
276  msg.depth = contact.depth;
277  msg.contact_body_1 = contact.body_name_1;
278  msg.contact_body_2 = contact.body_name_2;
279  if (contact.body_type_1 == BodyTypes::ROBOT_LINK)
281  else if (contact.body_type_1 == BodyTypes::ROBOT_ATTACHED)
283  else
285  if (contact.body_type_2 == BodyTypes::ROBOT_LINK)
287  else if (contact.body_type_2 == BodyTypes::ROBOT_ATTACHED)
289  else
291 }
292 
293 } // end of namespace collision_detection
void getCostMarkers(visualization_msgs::MarkerArray &arr, const std::string &frame_id, std::set< CostSource > &cost_sources)
double cost
The partial cost (the probability of existance for the object there is a collision with) ...
boost::array< double, 3 > aabb_min
The minimum bound of the AABB defining the volume responsible for this partial cost.
BodyType body_type_2
The type of the second body involved in the contact.
void getCollisionMarkersFromContacts(visualization_msgs::MarkerArray &arr, const std::string &frame_id, const CollisionResult::ContactMap &con, const std_msgs::ColorRGBA &color, const ros::Duration &lifetime, const double radius=0.035)
double getTotalCost(const std::set< CostSource > &cost_sources)
void costSourceToMsg(const CostSource &cost_source, moveit_msgs::CostSource &msg)
void contactToMsg(const Contact &contact, moveit_msgs::ContactInformation &msg)
bool add(const actionlib::TwoIntsGoal &req, actionlib::TwoIntsResult &res)
Generic interface to collision detection.
std::map< std::pair< std::string, std::string >, std::vector< Contact > > ContactMap
void intersectCostSources(std::set< CostSource > &cost_sources, const std::set< CostSource > &a, const std::set< CostSource > &b)
bool getSensorPositioning(geometry_msgs::Point &point, const std::set< CostSource > &cost_sources)
boost::array< double, 3 > aabb_max
The maximum bound of the AABB defining the volume responsible for this partial cost.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Vector3d pos
contact position
std::string body_name_2
The id of the second body involved in the contact.
Eigen::Vector3d normal
normal unit vector at contact
void removeOverlapping(std::set< CostSource > &cost_sources, double overlap_fraction)
void removeCostSources(std::set< CostSource > &cost_sources, const std::set< CostSource > &cost_sources_to_remove, double overlap_fraction)
B toMsg(const A &a)
std::string body_name_1
The id of the first body involved in the contact.
static Time now()
BodyType body_type_1
The type of the first body involved in the contact.
When collision costs are computed, this structure contains information about the partial cost incurre...
r


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Nov 23 2020 03:52:30