collision_tools.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
00036 
00037 #include <moveit/collision_detection/collision_tools.h>
00038 #include <eigen_conversions/eigen_msg.h>
00039 
00040 void collision_detection::getCostMarkers(visualization_msgs::MarkerArray& arr, const std::string& frame_id,
00041                                          std::set<CostSource>& cost_sources)
00042 {
00043   std_msgs::ColorRGBA color;
00044   color.r = 1.0f;
00045   color.g = 0.5f;
00046   color.b = 0.0f;
00047   color.a = 0.4f;
00048   getCostMarkers(arr, frame_id, cost_sources, color, ros::Duration(60.0));
00049 }
00050 
00051 void collision_detection::getCollisionMarkersFromContacts(visualization_msgs::MarkerArray& arr,
00052                                                           const std::string& frame_id,
00053                                                           const CollisionResult::ContactMap& con)
00054 {
00055   std_msgs::ColorRGBA color;
00056   color.r = 1.0f;
00057   color.g = 0.0f;
00058   color.b = 0.0f;
00059   color.a = 0.8f;
00060   getCollisionMarkersFromContacts(arr, frame_id, con, color, ros::Duration(60.0));
00061 }
00062 
00063 void collision_detection::getCostMarkers(visualization_msgs::MarkerArray& arr, const std::string& frame_id,
00064                                          std::set<CostSource>& cost_sources, const std_msgs::ColorRGBA& color,
00065                                          const ros::Duration& lifetime)
00066 {
00067   int id = 0;
00068   for (std::set<CostSource>::iterator it = cost_sources.begin(); it != cost_sources.end(); ++it)
00069   {
00070     visualization_msgs::Marker mk;
00071     mk.header.stamp = ros::Time::now();
00072     mk.header.frame_id = frame_id;
00073     mk.ns = "cost_source";
00074     mk.id = id++;
00075     mk.type = visualization_msgs::Marker::CUBE;
00076     mk.action = visualization_msgs::Marker::ADD;
00077     mk.pose.position.x = (it->aabb_max[0] + it->aabb_min[0]) / 2.0;
00078     mk.pose.position.y = (it->aabb_max[1] + it->aabb_min[1]) / 2.0;
00079     mk.pose.position.z = (it->aabb_max[2] + it->aabb_min[2]) / 2.0;
00080     mk.pose.orientation.x = 0.0;
00081     mk.pose.orientation.y = 0.0;
00082     mk.pose.orientation.z = 0.0;
00083     mk.pose.orientation.w = 1.0;
00084     mk.scale.x = it->aabb_max[0] - it->aabb_min[0];
00085     mk.scale.y = it->aabb_max[1] - it->aabb_min[1];
00086     mk.scale.z = it->aabb_max[2] - it->aabb_min[2];
00087     mk.color = color;
00088     if (mk.color.a == 0.0)
00089       mk.color.a = 1.0;
00090     mk.lifetime = lifetime;
00091     arr.markers.push_back(mk);
00092   }
00093 }
00094 
00095 void collision_detection::getCollisionMarkersFromContacts(visualization_msgs::MarkerArray& arr,
00096                                                           const std::string& frame_id,
00097                                                           const CollisionResult::ContactMap& con,
00098                                                           const std_msgs::ColorRGBA& color,
00099                                                           const ros::Duration& lifetime, double radius)
00100 
00101 {
00102   std::map<std::string, unsigned> ns_counts;
00103   for (CollisionResult::ContactMap::const_iterator it = con.begin(); it != con.end(); ++it)
00104   {
00105     for (unsigned int i = 0; i < it->second.size(); ++i)
00106     {
00107       std::string ns_name = it->second[i].body_name_1 + "=" + it->second[i].body_name_2;
00108       if (ns_counts.find(ns_name) == ns_counts.end())
00109         ns_counts[ns_name] = 0;
00110       else
00111         ns_counts[ns_name]++;
00112       visualization_msgs::Marker mk;
00113       mk.header.stamp = ros::Time::now();
00114       mk.header.frame_id = frame_id;
00115       mk.ns = ns_name;
00116       mk.id = ns_counts[ns_name];
00117       mk.type = visualization_msgs::Marker::SPHERE;
00118       mk.action = visualization_msgs::Marker::ADD;
00119       mk.pose.position.x = it->second[i].pos.x();
00120       mk.pose.position.y = it->second[i].pos.y();
00121       mk.pose.position.z = it->second[i].pos.z();
00122       mk.pose.orientation.x = 0.0;
00123       mk.pose.orientation.y = 0.0;
00124       mk.pose.orientation.z = 0.0;
00125       mk.pose.orientation.w = 1.0;
00126       mk.scale.x = mk.scale.y = mk.scale.z = radius * 2.0;
00127       mk.color = color;
00128       if (mk.color.a == 0.0)
00129         mk.color.a = 1.0;
00130       mk.lifetime = lifetime;
00131       arr.markers.push_back(mk);
00132     }
00133   }
00134 }
00135 
00136 bool collision_detection::getSensorPositioning(geometry_msgs::Point& point, const std::set<CostSource>& cost_sources)
00137 {
00138   if (cost_sources.empty())
00139     return false;
00140   std::set<CostSource>::const_iterator it = cost_sources.begin();
00141   for (std::size_t i = 0; i < 4 * cost_sources.size() / 5; ++i)
00142     ++it;
00143   point.x = (it->aabb_max[0] + it->aabb_min[0]) / 2.0;
00144   point.y = (it->aabb_max[1] + it->aabb_min[1]) / 2.0;
00145   point.z = (it->aabb_max[2] + it->aabb_min[2]) / 2.0;
00146   return true;
00147 }
00148 
00149 double collision_detection::getTotalCost(const std::set<CostSource>& cost_sources)
00150 {
00151   double cost = 0.0;
00152   for (std::set<collision_detection::CostSource>::const_iterator it = cost_sources.begin(); it != cost_sources.end();
00153        ++it)
00154     cost += it->getVolume() * it->cost;
00155   return cost;
00156 }
00157 
00158 void collision_detection::intersectCostSources(std::set<CostSource>& cost_sources, const std::set<CostSource>& a,
00159                                                const std::set<CostSource>& b)
00160 {
00161   cost_sources.clear();
00162   CostSource tmp;
00163   for (std::set<CostSource>::const_iterator it = a.begin(); it != a.end(); ++it)
00164     for (std::set<CostSource>::const_iterator jt = b.begin(); jt != b.end(); ++jt)
00165     {
00166       tmp.aabb_min[0] = std::max(it->aabb_min[0], jt->aabb_min[0]);
00167       tmp.aabb_min[1] = std::max(it->aabb_min[1], jt->aabb_min[1]);
00168       tmp.aabb_min[2] = std::max(it->aabb_min[2], jt->aabb_min[2]);
00169 
00170       tmp.aabb_max[0] = std::min(it->aabb_max[0], jt->aabb_max[0]);
00171       tmp.aabb_max[1] = std::min(it->aabb_max[1], jt->aabb_max[1]);
00172       tmp.aabb_max[2] = std::min(it->aabb_max[2], jt->aabb_max[2]);
00173 
00174       if (tmp.aabb_min[0] >= tmp.aabb_max[0] || tmp.aabb_min[1] >= tmp.aabb_max[1] ||
00175           tmp.aabb_min[2] >= tmp.aabb_max[2])
00176         continue;
00177       tmp.cost = std::max(it->cost, jt->cost);
00178       cost_sources.insert(tmp);
00179     }
00180 }
00181 
00182 void collision_detection::removeOverlapping(std::set<CostSource>& cost_sources, double overlap_fraction)
00183 {
00184   double p[3], q[3];
00185   for (std::set<CostSource>::iterator it = cost_sources.begin(); it != cost_sources.end(); ++it)
00186   {
00187     double vol = it->getVolume() * overlap_fraction;
00188     std::vector<std::set<CostSource>::iterator> remove;
00189     std::set<CostSource>::iterator it1 = it;
00190     for (std::set<CostSource>::iterator jt = ++it1; jt != cost_sources.end(); ++jt)
00191     {
00192       p[0] = std::max(it->aabb_min[0], jt->aabb_min[0]);
00193       p[1] = std::max(it->aabb_min[1], jt->aabb_min[1]);
00194       p[2] = std::max(it->aabb_min[2], jt->aabb_min[2]);
00195 
00196       q[0] = std::min(it->aabb_max[0], jt->aabb_max[0]);
00197       q[1] = std::min(it->aabb_max[1], jt->aabb_max[1]);
00198       q[2] = std::min(it->aabb_max[2], jt->aabb_max[2]);
00199 
00200       if (p[0] >= q[0] || p[1] >= q[1] || p[2] >= q[2])
00201         continue;
00202 
00203       double intersect_volume = (q[0] - p[0]) * (q[1] - p[1]) * (q[2] - p[2]);
00204       if (intersect_volume >= vol)
00205         remove.push_back(jt);
00206     }
00207     for (std::size_t i = 0; i < remove.size(); ++i)
00208       cost_sources.erase(remove[i]);
00209   }
00210 }
00211 
00212 void collision_detection::removeCostSources(std::set<CostSource>& cost_sources,
00213                                             const std::set<CostSource>& cost_sources_to_remove, double overlap_fraction)
00214 {
00215   // remove all the boxes that overlap with the intersection previously computed in \e rem
00216   double p[3], q[3];
00217   for (std::set<CostSource>::const_iterator jt = cost_sources_to_remove.begin(); jt != cost_sources_to_remove.end();
00218        ++jt)
00219   {
00220     std::vector<std::set<CostSource>::iterator> remove;
00221     std::set<CostSource> add;
00222     for (std::set<CostSource>::iterator it = cost_sources.begin(); it != cost_sources.end(); ++it)
00223     {
00224       p[0] = std::max(it->aabb_min[0], jt->aabb_min[0]);
00225       p[1] = std::max(it->aabb_min[1], jt->aabb_min[1]);
00226       p[2] = std::max(it->aabb_min[2], jt->aabb_min[2]);
00227 
00228       q[0] = std::min(it->aabb_max[0], jt->aabb_max[0]);
00229       q[1] = std::min(it->aabb_max[1], jt->aabb_max[1]);
00230       q[2] = std::min(it->aabb_max[2], jt->aabb_max[2]);
00231 
00232       if (p[0] >= q[0] || p[1] >= q[1] || p[2] >= q[2])
00233         continue;
00234 
00235       double intersect_volume = (q[0] - p[0]) * (q[1] - p[1]) * (q[2] - p[2]);
00236       if (intersect_volume >= it->getVolume() * overlap_fraction)
00237         remove.push_back(it);
00238       else
00239       {
00240         // there is some overlap, but not too large, so we split the cost source into multiple ones
00241         for (int i = 0; i < 3; ++i)
00242         {
00243           // is there a box above axis i in the intersection?
00244           if (it->aabb_max[i] > q[i])
00245           {
00246             CostSource cs = *it;
00247             cs.aabb_min[i] = q[i];
00248             add.insert(cs);
00249           }
00250           // is there a box below axis i in the intersection?
00251           if (it->aabb_min[i] < p[i])
00252           {
00253             CostSource cs = *it;
00254             cs.aabb_max[i] = p[i];
00255             add.insert(cs);
00256           }
00257         }
00258       }
00259     }
00260     for (std::size_t i = 0; i < remove.size(); ++i)
00261       cost_sources.erase(remove[i]);
00262     cost_sources.insert(add.begin(), add.end());
00263   }
00264 }
00265 
00266 void collision_detection::costSourceToMsg(const CostSource& cost_source, moveit_msgs::CostSource& msg)
00267 {
00268   msg.cost_density = cost_source.cost;
00269   msg.aabb_min.x = cost_source.aabb_min[0];
00270   msg.aabb_min.y = cost_source.aabb_min[1];
00271   msg.aabb_min.z = cost_source.aabb_min[2];
00272   msg.aabb_max.x = cost_source.aabb_max[0];
00273   msg.aabb_max.y = cost_source.aabb_max[1];
00274   msg.aabb_max.z = cost_source.aabb_max[2];
00275 }
00276 
00277 void collision_detection::contactToMsg(const Contact& contact, moveit_msgs::ContactInformation& msg)
00278 {
00279   tf::pointEigenToMsg(contact.pos, msg.position);
00280   tf::vectorEigenToMsg(contact.normal, msg.normal);
00281   msg.depth = contact.depth;
00282   msg.contact_body_1 = contact.body_name_1;
00283   msg.contact_body_2 = contact.body_name_2;
00284   if (contact.body_type_1 == BodyTypes::ROBOT_LINK)
00285     msg.body_type_1 = moveit_msgs::ContactInformation::ROBOT_LINK;
00286   else if (contact.body_type_1 == BodyTypes::ROBOT_ATTACHED)
00287     msg.body_type_1 = moveit_msgs::ContactInformation::ROBOT_ATTACHED;
00288   else
00289     msg.body_type_1 = moveit_msgs::ContactInformation::WORLD_OBJECT;
00290   if (contact.body_type_2 == BodyTypes::ROBOT_LINK)
00291     msg.body_type_2 = moveit_msgs::ContactInformation::ROBOT_LINK;
00292   else if (contact.body_type_2 == BodyTypes::ROBOT_ATTACHED)
00293     msg.body_type_2 = moveit_msgs::ContactInformation::ROBOT_ATTACHED;
00294   else
00295     msg.body_type_2 = moveit_msgs::ContactInformation::WORLD_OBJECT;
00296 }


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Jul 24 2017 02:20:43