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 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] ||
172  tmp.aabb_min[2] >= tmp.aabb_max[2])
173  continue;
174  tmp.cost = std::max(source_a.cost, source_b.cost);
175  cost_sources.insert(tmp);
176  }
177 }
178 
179 void removeOverlapping(std::set<CostSource>& cost_sources, double overlap_fraction)
180 {
181  double p[3], q[3];
182  for (auto it = cost_sources.begin(); it != cost_sources.end(); ++it)
183  {
184  double vol = it->getVolume() * overlap_fraction;
185  std::vector<std::set<CostSource>::iterator> remove;
186  auto it1 = it;
187  for (auto jt = ++it1; jt != cost_sources.end(); ++jt)
188  {
189  p[0] = std::max(it->aabb_min[0], jt->aabb_min[0]);
190  p[1] = std::max(it->aabb_min[1], jt->aabb_min[1]);
191  p[2] = std::max(it->aabb_min[2], jt->aabb_min[2]);
192 
193  q[0] = std::min(it->aabb_max[0], jt->aabb_max[0]);
194  q[1] = std::min(it->aabb_max[1], jt->aabb_max[1]);
195  q[2] = std::min(it->aabb_max[2], jt->aabb_max[2]);
196 
197  if (p[0] >= q[0] || p[1] >= q[1] || p[2] >= q[2])
198  continue;
199 
200  double intersect_volume = (q[0] - p[0]) * (q[1] - p[1]) * (q[2] - p[2]);
201  if (intersect_volume >= vol)
202  remove.push_back(jt);
203  }
204  for (auto& r : remove)
205  cost_sources.erase(r);
206  }
207 }
208 
209 void removeCostSources(std::set<CostSource>& cost_sources, const std::set<CostSource>& cost_sources_to_remove,
210  double overlap_fraction)
211 {
212  // remove all the boxes that overlap with the intersection previously computed in \e rem
213  double p[3], q[3];
214  for (const auto& source_remove : cost_sources_to_remove)
215  {
216  std::vector<std::set<CostSource>::iterator> remove;
217  std::set<CostSource> add;
218  for (auto it = cost_sources.begin(); it != cost_sources.end(); ++it)
219  {
220  p[0] = std::max(it->aabb_min[0], source_remove.aabb_min[0]);
221  p[1] = std::max(it->aabb_min[1], source_remove.aabb_min[1]);
222  p[2] = std::max(it->aabb_min[2], source_remove.aabb_min[2]);
223 
224  q[0] = std::min(it->aabb_max[0], source_remove.aabb_max[0]);
225  q[1] = std::min(it->aabb_max[1], source_remove.aabb_max[1]);
226  q[2] = std::min(it->aabb_max[2], source_remove.aabb_max[2]);
227 
228  if (p[0] >= q[0] || p[1] >= q[1] || p[2] >= q[2])
229  continue;
230 
231  double intersect_volume = (q[0] - p[0]) * (q[1] - p[1]) * (q[2] - p[2]);
232  if (intersect_volume >= it->getVolume() * overlap_fraction)
233  remove.push_back(it);
234  else
235  {
236  // there is some overlap, but not too large, so we split the cost source into multiple ones
237  for (int i = 0; i < 3; ++i)
238  {
239  // is there a box above axis i in the intersection?
240  if (it->aabb_max[i] > q[i])
241  {
242  CostSource cs = *it;
243  cs.aabb_min[i] = q[i];
244  add.insert(cs);
245  }
246  // is there a box below axis i in the intersection?
247  if (it->aabb_min[i] < p[i])
248  {
249  CostSource cs = *it;
250  cs.aabb_max[i] = p[i];
251  add.insert(cs);
252  }
253  }
254  }
255  }
256  for (auto& r : remove)
257  cost_sources.erase(r);
258  cost_sources.insert(add.begin(), add.end());
259  }
260 }
261 
262 void costSourceToMsg(const CostSource& cost_source, moveit_msgs::CostSource& msg)
263 {
264  msg.cost_density = cost_source.cost;
265  msg.aabb_min.x = cost_source.aabb_min[0];
266  msg.aabb_min.y = cost_source.aabb_min[1];
267  msg.aabb_min.z = cost_source.aabb_min[2];
268  msg.aabb_max.x = cost_source.aabb_max[0];
269  msg.aabb_max.y = cost_source.aabb_max[1];
270  msg.aabb_max.z = cost_source.aabb_max[2];
271 }
272 
273 void contactToMsg(const Contact& contact, moveit_msgs::ContactInformation& msg)
274 {
275  tf::pointEigenToMsg(contact.pos, msg.position);
276  tf::vectorEigenToMsg(contact.normal, msg.normal);
277  msg.depth = contact.depth;
278  msg.contact_body_1 = contact.body_name_1;
279  msg.contact_body_2 = contact.body_name_2;
280  if (contact.body_type_1 == BodyTypes::ROBOT_LINK)
282  else if (contact.body_type_1 == BodyTypes::ROBOT_ATTACHED)
284  else
286  if (contact.body_type_2 == BodyTypes::ROBOT_LINK)
288  else if (contact.body_type_2 == BodyTypes::ROBOT_ATTACHED)
290  else
292 }
293 
294 } // 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)
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
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 Sun Oct 18 2020 13:16:33