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 
39 
40 void collision_detection::getCostMarkers(visualization_msgs::MarkerArray& arr, const std::string& frame_id,
41  std::set<CostSource>& cost_sources)
42 {
43  std_msgs::ColorRGBA color;
44  color.r = 1.0f;
45  color.g = 0.5f;
46  color.b = 0.0f;
47  color.a = 0.4f;
48  getCostMarkers(arr, frame_id, cost_sources, color, ros::Duration(60.0));
49 }
50 
51 void collision_detection::getCollisionMarkersFromContacts(visualization_msgs::MarkerArray& arr,
52  const std::string& frame_id,
53  const CollisionResult::ContactMap& con)
54 {
55  std_msgs::ColorRGBA color;
56  color.r = 1.0f;
57  color.g = 0.0f;
58  color.b = 0.0f;
59  color.a = 0.8f;
60  getCollisionMarkersFromContacts(arr, frame_id, con, color, ros::Duration(60.0));
61 }
62 
63 void collision_detection::getCostMarkers(visualization_msgs::MarkerArray& arr, const std::string& frame_id,
64  std::set<CostSource>& cost_sources, const std_msgs::ColorRGBA& color,
65  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 collision_detection::getCollisionMarkersFromContacts(visualization_msgs::MarkerArray& arr,
96  const std::string& frame_id,
97  const CollisionResult::ContactMap& con,
98  const std_msgs::ColorRGBA& color,
99  const ros::Duration& lifetime, double radius)
100 
101 {
102  std::map<std::string, unsigned> ns_counts;
103  for (const auto& collision : con)
104  {
105  for (const auto& contact : collision.second)
106  {
107  std::string ns_name = contact.body_name_1 + "=" + contact.body_name_2;
108  if (ns_counts.find(ns_name) == ns_counts.end())
109  ns_counts[ns_name] = 0;
110  else
111  ns_counts[ns_name]++;
112  visualization_msgs::Marker mk;
113  mk.header.stamp = ros::Time::now();
114  mk.header.frame_id = frame_id;
115  mk.ns = ns_name;
116  mk.id = ns_counts[ns_name];
117  mk.type = visualization_msgs::Marker::SPHERE;
118  mk.action = visualization_msgs::Marker::ADD;
119  mk.pose.position.x = contact.pos.x();
120  mk.pose.position.y = contact.pos.y();
121  mk.pose.position.z = contact.pos.z();
122  mk.pose.orientation.x = 0.0;
123  mk.pose.orientation.y = 0.0;
124  mk.pose.orientation.z = 0.0;
125  mk.pose.orientation.w = 1.0;
126  mk.scale.x = mk.scale.y = mk.scale.z = radius * 2.0;
127  mk.color = color;
128  if (mk.color.a == 0.0)
129  mk.color.a = 1.0;
130  mk.lifetime = lifetime;
131  arr.markers.push_back(mk);
132  }
133  }
134 }
135 
136 bool collision_detection::getSensorPositioning(geometry_msgs::Point& point, const std::set<CostSource>& cost_sources)
137 {
138  if (cost_sources.empty())
139  return false;
140  auto it = cost_sources.begin();
141  for (std::size_t i = 0; i < 4 * cost_sources.size() / 5; ++i)
142  ++it;
143  point.x = (it->aabb_max[0] + it->aabb_min[0]) / 2.0;
144  point.y = (it->aabb_max[1] + it->aabb_min[1]) / 2.0;
145  point.z = (it->aabb_max[2] + it->aabb_min[2]) / 2.0;
146  return true;
147 }
148 
149 double collision_detection::getTotalCost(const std::set<CostSource>& cost_sources)
150 {
151  double cost = 0.0;
152  for (const auto& cost_source : cost_sources)
153  cost += cost_source.getVolume() * cost_source.cost;
154  return cost;
155 }
156 
157 void collision_detection::intersectCostSources(std::set<CostSource>& cost_sources, const std::set<CostSource>& a,
158  const std::set<CostSource>& b)
159 {
160  cost_sources.clear();
161  CostSource tmp;
162  for (const auto& source_a : a)
163  for (const auto& source_b : b)
164  {
165  tmp.aabb_min[0] = std::max(source_a.aabb_min[0], source_b.aabb_min[0]);
166  tmp.aabb_min[1] = std::max(source_a.aabb_min[1], source_b.aabb_min[1]);
167  tmp.aabb_min[2] = std::max(source_a.aabb_min[2], source_b.aabb_min[2]);
168 
169  tmp.aabb_max[0] = std::min(source_a.aabb_max[0], source_b.aabb_max[0]);
170  tmp.aabb_max[1] = std::min(source_a.aabb_max[1], source_b.aabb_max[1]);
171  tmp.aabb_max[2] = std::min(source_a.aabb_max[2], source_b.aabb_max[2]);
172 
173  if (tmp.aabb_min[0] >= tmp.aabb_max[0] || tmp.aabb_min[1] >= tmp.aabb_max[1] ||
174  tmp.aabb_min[2] >= tmp.aabb_max[2])
175  continue;
176  tmp.cost = std::max(source_a.cost, source_b.cost);
177  cost_sources.insert(tmp);
178  }
179 }
180 
181 void collision_detection::removeOverlapping(std::set<CostSource>& cost_sources, double overlap_fraction)
182 {
183  double p[3], q[3];
184  for (auto it = cost_sources.begin(); it != cost_sources.end(); ++it)
185  {
186  double vol = it->getVolume() * overlap_fraction;
187  std::vector<std::set<CostSource>::iterator> remove;
188  auto it1 = it;
189  for (auto jt = ++it1; jt != cost_sources.end(); ++jt)
190  {
191  p[0] = std::max(it->aabb_min[0], jt->aabb_min[0]);
192  p[1] = std::max(it->aabb_min[1], jt->aabb_min[1]);
193  p[2] = std::max(it->aabb_min[2], jt->aabb_min[2]);
194 
195  q[0] = std::min(it->aabb_max[0], jt->aabb_max[0]);
196  q[1] = std::min(it->aabb_max[1], jt->aabb_max[1]);
197  q[2] = std::min(it->aabb_max[2], jt->aabb_max[2]);
198 
199  if (p[0] >= q[0] || p[1] >= q[1] || p[2] >= q[2])
200  continue;
201 
202  double intersect_volume = (q[0] - p[0]) * (q[1] - p[1]) * (q[2] - p[2]);
203  if (intersect_volume >= vol)
204  remove.push_back(jt);
205  }
206  for (auto& r : remove)
207  cost_sources.erase(r);
208  }
209 }
210 
211 void collision_detection::removeCostSources(std::set<CostSource>& cost_sources,
212  const std::set<CostSource>& cost_sources_to_remove, double overlap_fraction)
213 {
214  // remove all the boxes that overlap with the intersection previously computed in \e rem
215  double p[3], q[3];
216  for (const auto& source_remove : cost_sources_to_remove)
217  {
218  std::vector<std::set<CostSource>::iterator> remove;
219  std::set<CostSource> add;
220  for (auto it = cost_sources.begin(); it != cost_sources.end(); ++it)
221  {
222  p[0] = std::max(it->aabb_min[0], source_remove.aabb_min[0]);
223  p[1] = std::max(it->aabb_min[1], source_remove.aabb_min[1]);
224  p[2] = std::max(it->aabb_min[2], source_remove.aabb_min[2]);
225 
226  q[0] = std::min(it->aabb_max[0], source_remove.aabb_max[0]);
227  q[1] = std::min(it->aabb_max[1], source_remove.aabb_max[1]);
228  q[2] = std::min(it->aabb_max[2], source_remove.aabb_max[2]);
229 
230  if (p[0] >= q[0] || p[1] >= q[1] || p[2] >= q[2])
231  continue;
232 
233  double intersect_volume = (q[0] - p[0]) * (q[1] - p[1]) * (q[2] - p[2]);
234  if (intersect_volume >= it->getVolume() * overlap_fraction)
235  remove.push_back(it);
236  else
237  {
238  // there is some overlap, but not too large, so we split the cost source into multiple ones
239  for (int i = 0; i < 3; ++i)
240  {
241  // is there a box above axis i in the intersection?
242  if (it->aabb_max[i] > q[i])
243  {
244  CostSource cs = *it;
245  cs.aabb_min[i] = q[i];
246  add.insert(cs);
247  }
248  // is there a box below axis i in the intersection?
249  if (it->aabb_min[i] < p[i])
250  {
251  CostSource cs = *it;
252  cs.aabb_max[i] = p[i];
253  add.insert(cs);
254  }
255  }
256  }
257  }
258  for (auto& r : remove)
259  cost_sources.erase(r);
260  cost_sources.insert(add.begin(), add.end());
261  }
262 }
263 
264 void collision_detection::costSourceToMsg(const CostSource& cost_source, moveit_msgs::CostSource& msg)
265 {
266  msg.cost_density = cost_source.cost;
267  msg.aabb_min.x = cost_source.aabb_min[0];
268  msg.aabb_min.y = cost_source.aabb_min[1];
269  msg.aabb_min.z = cost_source.aabb_min[2];
270  msg.aabb_max.x = cost_source.aabb_max[0];
271  msg.aabb_max.y = cost_source.aabb_max[1];
272  msg.aabb_max.z = cost_source.aabb_max[2];
273 }
274 
275 void collision_detection::contactToMsg(const Contact& contact, moveit_msgs::ContactInformation& msg)
276 {
277  tf::pointEigenToMsg(contact.pos, msg.position);
278  tf::vectorEigenToMsg(contact.normal, msg.normal);
279  msg.depth = contact.depth;
280  msg.contact_body_1 = contact.body_name_1;
281  msg.contact_body_2 = contact.body_name_2;
282  if (contact.body_type_1 == BodyTypes::ROBOT_LINK)
284  else if (contact.body_type_1 == BodyTypes::ROBOT_ATTACHED)
286  else
288  if (contact.body_type_2 == BodyTypes::ROBOT_LINK)
290  else if (contact.body_type_2 == BodyTypes::ROBOT_ATTACHED)
292  else
294 }
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)
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
void vectorEigenToMsg(const Eigen::Vector3d &e, geometry_msgs::Vector3 &m)
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)
void pointEigenToMsg(const Eigen::Vector3d &e, geometry_msgs::Point &m)
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 Wed Apr 18 2018 02:49:03