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 the 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, std::set<CostSource> &cost_sources)
00041 {
00042   std_msgs::ColorRGBA color;
00043   color.r = 1.0f; color.g = 0.5f; color.b = 0.0f; color.a = 0.4f;
00044   getCostMarkers(arr, frame_id, cost_sources, color, ros::Duration(60.0));
00045 }
00046 
00047 void collision_detection::getCollisionMarkersFromContacts(visualization_msgs::MarkerArray& arr,
00048                                                           const std::string& frame_id,
00049                                                           const CollisionResult::ContactMap &con)
00050 {
00051   std_msgs::ColorRGBA color;
00052   color.r = 1.0f; color.g = 0.0f; color.b = 0.0f; color.a = 0.8f;
00053   getCollisionMarkersFromContacts(arr, frame_id, con, color, ros::Duration(60.0));
00054 }
00055 
00056 void collision_detection::getCostMarkers(visualization_msgs::MarkerArray& arr,
00057                                          const std::string& frame_id,
00058                                          std::set<CostSource> &cost_sources,
00059                                          const std_msgs::ColorRGBA& color,
00060                                          const ros::Duration& lifetime)
00061 {
00062   int id = 0;
00063   for (std::set<CostSource>::iterator it = cost_sources.begin() ; it != cost_sources.end() ; ++it)
00064   {
00065     visualization_msgs::Marker mk;
00066     mk.header.stamp = ros::Time::now();
00067     mk.header.frame_id = frame_id;
00068     mk.ns = "cost_source";
00069     mk.id = id++;
00070     mk.type = visualization_msgs::Marker::CUBE;
00071     mk.action = visualization_msgs::Marker::ADD;
00072     mk.pose.position.x = (it->aabb_max[0] + it->aabb_min[0]) / 2.0;
00073     mk.pose.position.y = (it->aabb_max[1] + it->aabb_min[1]) / 2.0;
00074     mk.pose.position.z = (it->aabb_max[2] + it->aabb_min[2]) / 2.0;
00075     mk.pose.orientation.x = 0.0;
00076     mk.pose.orientation.y = 0.0;
00077     mk.pose.orientation.z = 0.0;
00078     mk.pose.orientation.w = 1.0;
00079     mk.scale.x = it->aabb_max[0] - it->aabb_min[0];
00080     mk.scale.y = it->aabb_max[1] - it->aabb_min[1];
00081     mk.scale.z = it->aabb_max[2] - it->aabb_min[2];
00082     mk.color = color;
00083     if (mk.color.a == 0.0)
00084       mk.color.a = 1.0;
00085     mk.lifetime = lifetime;
00086     arr.markers.push_back(mk);
00087   }
00088 }
00089 
00090 void collision_detection::getCollisionMarkersFromContacts(visualization_msgs::MarkerArray& arr,
00091                                                           const std::string& frame_id,
00092                                                           const CollisionResult::ContactMap& con,
00093                                                           const std_msgs::ColorRGBA& color,
00094                                                           const ros::Duration& lifetime,
00095                                                           double radius)
00096 
00097 {
00098   std::map<std::string, unsigned> ns_counts;
00099   for(CollisionResult::ContactMap::const_iterator it = con.begin(); it != con.end(); ++it)
00100   {
00101     for(unsigned int i = 0; i < it->second.size(); ++i)
00102     {
00103       std::string ns_name = it->second[i].body_name_1 + "=" + it->second[i].body_name_2;
00104       if (ns_counts.find(ns_name) == ns_counts.end())
00105         ns_counts[ns_name] = 0;
00106       else
00107         ns_counts[ns_name]++;
00108       visualization_msgs::Marker mk;
00109       mk.header.stamp = ros::Time::now();
00110       mk.header.frame_id = frame_id;
00111       mk.ns = ns_name;
00112       mk.id = ns_counts[ns_name];
00113       mk.type = visualization_msgs::Marker::SPHERE;
00114       mk.action = visualization_msgs::Marker::ADD;
00115       mk.pose.position.x = it->second[i].pos.x();
00116       mk.pose.position.y = it->second[i].pos.y();
00117       mk.pose.position.z = it->second[i].pos.z();
00118       mk.pose.orientation.x = 0.0;
00119       mk.pose.orientation.y = 0.0;
00120       mk.pose.orientation.z = 0.0;
00121       mk.pose.orientation.w = 1.0;
00122       mk.scale.x = mk.scale.y = mk.scale.z = radius * 2.0;
00123       mk.color = color;
00124       if(mk.color.a == 0.0)
00125         mk.color.a = 1.0;
00126       mk.lifetime = lifetime;
00127       arr.markers.push_back(mk);
00128     }
00129   }
00130 }
00131 
00132 bool collision_detection::getSensorPositioning(geometry_msgs::Point &point,
00133                                                const std::set<CostSource> &cost_sources)
00134 {
00135   if (cost_sources.empty())
00136     return false;
00137   std::set<CostSource>::const_iterator it = cost_sources.begin();
00138   for (std::size_t i = 0 ; i < 4*cost_sources.size()/5 ; ++i)
00139     ++it;
00140   point.x = (it->aabb_max[0] + it->aabb_min[0]) / 2.0;
00141   point.y = (it->aabb_max[1] + it->aabb_min[1]) / 2.0;
00142   point.z = (it->aabb_max[2] + it->aabb_min[2]) / 2.0;
00143   return true;
00144 }
00145 
00146 double collision_detection::getTotalCost(const std::set<CostSource> &cost_sources)
00147 {
00148   double cost = 0.0;
00149   for (std::set<collision_detection::CostSource>::const_iterator it = cost_sources.begin() ; it != cost_sources.end() ; ++it)
00150     cost += it->getVolume() * it->cost;
00151   return cost;
00152 }
00153 
00154 
00155 void collision_detection::intersectCostSources(std::set<CostSource> &cost_sources, const std::set<CostSource> &a, const std::set<CostSource> &b)
00156 {
00157   cost_sources.clear();
00158   CostSource tmp;
00159   for (std::set<CostSource>::const_iterator it = a.begin() ; it != a.end() ; ++it)
00160     for (std::set<CostSource>::const_iterator jt = b.begin() ; jt != b.end() ; ++jt)
00161     {
00162       tmp.aabb_min[0] = std::max(it->aabb_min[0], jt->aabb_min[0]);
00163       tmp.aabb_min[1] = std::max(it->aabb_min[1], jt->aabb_min[1]);
00164       tmp.aabb_min[2] = std::max(it->aabb_min[2], jt->aabb_min[2]);
00165 
00166       tmp.aabb_max[0] = std::min(it->aabb_max[0], jt->aabb_max[0]);
00167       tmp.aabb_max[1] = std::min(it->aabb_max[1], jt->aabb_max[1]);
00168       tmp.aabb_max[2] = std::min(it->aabb_max[2], jt->aabb_max[2]);
00169 
00170       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])
00171         continue;
00172       tmp.cost = std::max(it->cost, jt->cost);
00173       cost_sources.insert(tmp);
00174     }
00175 }
00176 
00177 void collision_detection::removeOverlapping(std::set<CostSource> &cost_sources, double overlap_fraction)
00178 {
00179   double p[3], q[3];
00180   for (std::set<CostSource>::iterator it = cost_sources.begin() ; it != cost_sources.end() ; ++it)
00181   {
00182     double vol = it->getVolume() * overlap_fraction;
00183     std::vector<std::set<CostSource>::iterator> remove;
00184     std::set<CostSource>::iterator it1 = it;
00185     for (std::set<CostSource>::iterator jt = ++it1 ; jt != cost_sources.end() ; ++jt)
00186     {
00187       p[0] = std::max(it->aabb_min[0], jt->aabb_min[0]);
00188       p[1] = std::max(it->aabb_min[1], jt->aabb_min[1]);
00189       p[2] = std::max(it->aabb_min[2], jt->aabb_min[2]);
00190 
00191       q[0] = std::min(it->aabb_max[0], jt->aabb_max[0]);
00192       q[1] = std::min(it->aabb_max[1], jt->aabb_max[1]);
00193       q[2] = std::min(it->aabb_max[2], jt->aabb_max[2]);
00194 
00195       if (p[0] >= q[0] || p[1] >= q[1] || p[2] >= q[2])
00196         continue;
00197 
00198       double intersect_volume = (q[0] - p[0]) * (q[1] - p[1]) * (q[2] - p[2]);
00199       if (intersect_volume >= vol)
00200         remove.push_back(jt);
00201     }
00202     for (std::size_t i = 0 ; i < remove.size() ; ++i)
00203       cost_sources.erase(remove[i]);
00204   }
00205 }
00206 
00207 
00208 void collision_detection::removeCostSources(std::set<CostSource> &cost_sources, const std::set<CostSource> &cost_sources_to_remove, double overlap_fraction)
00209 {
00210   // remove all the boxes that overlap with the intersection previously computed in \e rem
00211   double p[3], q[3];
00212   for (std::set<CostSource>::const_iterator jt = cost_sources_to_remove.begin() ; jt != cost_sources_to_remove.end() ; ++jt)
00213   {
00214     std::vector<std::set<CostSource>::iterator> remove;
00215     std::set<CostSource> add;
00216     for (std::set<CostSource>::iterator it = cost_sources.begin() ; it != cost_sources.end() ; ++it)
00217     {
00218       p[0] = std::max(it->aabb_min[0], jt->aabb_min[0]);
00219       p[1] = std::max(it->aabb_min[1], jt->aabb_min[1]);
00220       p[2] = std::max(it->aabb_min[2], jt->aabb_min[2]);
00221 
00222       q[0] = std::min(it->aabb_max[0], jt->aabb_max[0]);
00223       q[1] = std::min(it->aabb_max[1], jt->aabb_max[1]);
00224       q[2] = std::min(it->aabb_max[2], jt->aabb_max[2]);
00225 
00226       if (p[0] >= q[0] || p[1] >= q[1] || p[2] >= q[2])
00227         continue;
00228 
00229       double intersect_volume = (q[0] - p[0]) * (q[1] - p[1]) * (q[2] - p[2]);
00230       if (intersect_volume >= it->getVolume() * overlap_fraction)
00231         remove.push_back(it);
00232       else
00233       {
00234         // there is some overlap, but not too large, so we split the cost source into multiple ones
00235         for (int i = 0 ; i < 3 ; ++i)
00236         {
00237           // is there a box above axis i in the intersection?
00238           if (it->aabb_max[i] > q[i])
00239           {
00240             CostSource cs = *it;
00241             cs.aabb_min[i] = q[i];
00242             add.insert(cs);
00243           }
00244           // is there a box below axis i in the intersection?
00245           if (it->aabb_min[i] < p[i])
00246           {
00247             CostSource cs = *it;
00248             cs.aabb_max[i] = p[i];
00249             add.insert(cs);
00250           }
00251         }
00252       }
00253     }
00254     for (std::size_t i = 0 ; i < remove.size() ; ++i)
00255       cost_sources.erase(remove[i]);
00256     cost_sources.insert(add.begin(), add.end());
00257   }
00258 }
00259 
00260 void collision_detection::costSourceToMsg(const CostSource &cost_source, moveit_msgs::CostSource &msg)
00261 {
00262   msg.cost_density = cost_source.cost;
00263   msg.aabb_min.x = cost_source.aabb_min[0];
00264   msg.aabb_min.y = cost_source.aabb_min[1];
00265   msg.aabb_min.z = cost_source.aabb_min[2];
00266   msg.aabb_max.x = cost_source.aabb_max[0];
00267   msg.aabb_max.y = cost_source.aabb_max[1];
00268   msg.aabb_max.z = cost_source.aabb_max[2];
00269 }
00270 
00271 void collision_detection::contactToMsg(const Contact& contact, moveit_msgs::ContactInformation &msg)
00272 {
00273   tf::pointEigenToMsg(contact.pos, msg.position);
00274   tf::vectorEigenToMsg(contact.normal, msg.normal);
00275   msg.depth = contact.depth;
00276   msg.contact_body_1 = contact.body_name_1;
00277   msg.contact_body_2 = contact.body_name_2;
00278   if (contact.body_type_1 == BodyTypes::ROBOT_LINK)
00279     msg.body_type_1 = moveit_msgs::ContactInformation::ROBOT_LINK;
00280   else
00281     if (contact.body_type_1 == BodyTypes::ROBOT_ATTACHED)
00282       msg.body_type_1 = moveit_msgs::ContactInformation::ROBOT_ATTACHED;
00283     else
00284       msg.body_type_1 = moveit_msgs::ContactInformation::WORLD_OBJECT;
00285   if (contact.body_type_2 == BodyTypes::ROBOT_LINK)
00286     msg.body_type_2 = moveit_msgs::ContactInformation::ROBOT_LINK;
00287   else
00288     if (contact.body_type_2 == BodyTypes::ROBOT_ATTACHED)
00289       msg.body_type_2 = moveit_msgs::ContactInformation::ROBOT_ATTACHED;
00290     else
00291       msg.body_type_2 = moveit_msgs::ContactInformation::WORLD_OBJECT;
00292 }


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Oct 6 2014 02:24:46