collision_matrix.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, E. Gil Jones */
00036 
00037 #include <moveit/collision_detection/collision_matrix.h>
00038 #include <boost/bind.hpp>
00039 #include <iomanip>
00040 
00041 collision_detection::AllowedCollisionMatrix::AllowedCollisionMatrix()
00042 {
00043 }
00044 
00045 collision_detection::AllowedCollisionMatrix::AllowedCollisionMatrix(const std::vector<std::string>& names, bool allowed)
00046 {
00047   for (std::size_t i = 0 ; i < names.size() ; ++i)
00048     for (std::size_t j = i; j < names.size() ; ++j)
00049       setEntry(names[i], names[j], allowed);
00050 }
00051 
00052 collision_detection::AllowedCollisionMatrix::AllowedCollisionMatrix(const moveit_msgs::AllowedCollisionMatrix &msg)
00053 {
00054   if (msg.entry_names.size() != msg.entry_values.size() || msg.default_entry_names.size() != msg.default_entry_values.size())
00055     logError("The number of links does not match the number of entries in AllowedCollisionMatrix message");
00056   else
00057   {
00058     for (std::size_t i = 0 ; i < msg.entry_names.size() ; ++i)
00059       if (msg.entry_values[i].enabled.size() != msg.entry_names.size())
00060         logError("Number of entries is incorrect for link '%s' in AllowedCollisionMatrix message", msg.entry_names[i].c_str());
00061       else
00062         for (std::size_t j = i + 1 ; j < msg.entry_values[i].enabled.size() ; ++j)
00063           setEntry(msg.entry_names[i], msg.entry_names[j], msg.entry_values[i].enabled[j]);
00064 
00065     for (std::size_t i = 0 ; i < msg.default_entry_names.size() ; ++i)
00066       setDefaultEntry(msg.default_entry_names[i], msg.default_entry_values[i]);
00067   }
00068 }
00069 
00070 collision_detection::AllowedCollisionMatrix::AllowedCollisionMatrix(const AllowedCollisionMatrix& acm)
00071 {
00072   entries_ = acm.entries_;
00073   allowed_contacts_ = acm.allowed_contacts_;
00074   default_entries_ = acm.default_entries_;
00075   default_allowed_contacts_ = acm.default_allowed_contacts_;
00076 }
00077 
00078 bool collision_detection::AllowedCollisionMatrix::getEntry(const std::string& name1, const std::string& name2, DecideContactFn &fn) const
00079 {
00080   std::map<std::string, std::map<std::string, DecideContactFn> >::const_iterator it1 = allowed_contacts_.find(name1);
00081   if (it1 == allowed_contacts_.end())
00082     return false;
00083   std::map<std::string, DecideContactFn>::const_iterator it2 = it1->second.find(name2);
00084   if (it2 == it1->second.end())
00085     return false;
00086   fn = it2->second;
00087   return true;
00088 }
00089 
00090 bool collision_detection::AllowedCollisionMatrix::getEntry(const std::string& name1, const std::string& name2, AllowedCollision::Type& allowed_collision) const
00091 {
00092   std::map<std::string, std::map<std::string, AllowedCollision::Type> >::const_iterator it1 = entries_.find(name1);
00093   if (it1 == entries_.end())
00094     return false;
00095   std::map<std::string, AllowedCollision::Type>::const_iterator it2 = it1->second.find(name2);
00096   if (it2 == it1->second.end())
00097     return false;
00098   allowed_collision = it2->second;
00099   return true;
00100 }
00101 
00102 bool collision_detection::AllowedCollisionMatrix::hasEntry(const std::string& name) const
00103 {
00104   return entries_.find(name) != entries_.end();
00105 }
00106 
00107 bool collision_detection::AllowedCollisionMatrix::hasEntry(const std::string& name1, const std::string& name2) const
00108 {
00109   std::map<std::string, std::map<std::string, AllowedCollision::Type> >::const_iterator it1 = entries_.find(name1);
00110   if (it1 == entries_.end())
00111     return false;
00112   std::map<std::string, AllowedCollision::Type>::const_iterator it2 = it1->second.find(name2);
00113   if (it2 == it1->second.end())
00114     return false;
00115   return true;
00116 }
00117 
00118 void collision_detection::AllowedCollisionMatrix::setEntry(const std::string &name1, const std::string &name2, bool allowed)
00119 {
00120   const AllowedCollision::Type v = allowed ? AllowedCollision::ALWAYS : AllowedCollision::NEVER;
00121   entries_[name1][name2] = entries_[name2][name1] = v;
00122 
00123   // remove boost::function pointers, if any
00124   std::map<std::string, std::map<std::string, DecideContactFn> >::iterator it = allowed_contacts_.find(name1);
00125   if (it != allowed_contacts_.end())
00126   {
00127     std::map<std::string, DecideContactFn>::iterator jt = it->second.find(name2);
00128     if (jt != it->second.end())
00129       it->second.erase(jt);
00130   }
00131   it = allowed_contacts_.find(name2);
00132   if (it != allowed_contacts_.end())
00133   {
00134     std::map<std::string, DecideContactFn>::iterator jt = it->second.find(name1);
00135     if (jt != it->second.end())
00136       it->second.erase(jt);
00137   }
00138 }
00139 
00140 void collision_detection::AllowedCollisionMatrix::setEntry(const std::string& name1, const std::string& name2, const DecideContactFn &fn)
00141 {
00142   entries_[name1][name2] = entries_[name2][name1] = AllowedCollision::CONDITIONAL;
00143   allowed_contacts_[name1][name2] = allowed_contacts_[name2][name1] = fn;
00144 }
00145 
00146 void collision_detection::AllowedCollisionMatrix::removeEntry(const std::string& name)
00147 {
00148   entries_.erase(name);
00149   allowed_contacts_.erase(name);
00150   for (std::map<std::string, std::map<std::string, AllowedCollision::Type> >::iterator it = entries_.begin() ; it != entries_.end() ; ++it)
00151     it->second.erase(name);
00152   for (std::map<std::string, std::map<std::string, DecideContactFn> >::iterator it = allowed_contacts_.begin() ; it != allowed_contacts_.end() ; ++it)
00153     it->second.erase(name);
00154 }
00155 
00156 void collision_detection::AllowedCollisionMatrix::removeEntry(const std::string& name1, const std::string &name2)
00157 {
00158   std::map<std::string, std::map<std::string, AllowedCollision::Type> >::iterator jt = entries_.find(name1);
00159   if (jt != entries_.end())
00160   {
00161     std::map<std::string, AllowedCollision::Type>::iterator it = jt->second.find(name2);
00162     if (it != jt->second.end())
00163       jt->second.erase(it);
00164   }
00165   jt = entries_.find(name2);
00166   if (jt != entries_.end())
00167   {
00168     std::map<std::string, AllowedCollision::Type>::iterator it = jt->second.find(name1);
00169     if (it != jt->second.end())
00170       jt->second.erase(it);
00171   }
00172 
00173   std::map<std::string, std::map<std::string, DecideContactFn> >::iterator it = allowed_contacts_.find(name1);
00174   if (it != allowed_contacts_.end())
00175   {
00176     std::map<std::string, DecideContactFn>::iterator jt = it->second.find(name2);
00177     if (jt != it->second.end())
00178       it->second.erase(jt);
00179   }
00180   it = allowed_contacts_.find(name2);
00181   if (it != allowed_contacts_.end())
00182   {
00183     std::map<std::string, DecideContactFn>::iterator jt = it->second.find(name1);
00184     if (jt != it->second.end())
00185       it->second.erase(jt);
00186   }
00187 }
00188 
00189 void collision_detection::AllowedCollisionMatrix::setEntry(const std::string& name, const std::vector<std::string>& other_names, bool allowed)
00190 {
00191   for (std::size_t i = 0 ; i < other_names.size() ; ++i)
00192     if (other_names[i] != name)
00193       setEntry(other_names[i], name, allowed);
00194 }
00195 
00196 void collision_detection::AllowedCollisionMatrix::setEntry(const std::vector<std::string>& names1, const std::vector<std::string>& names2, bool allowed)
00197 {
00198   for (std::size_t i = 0 ; i < names1.size() ; ++i)
00199     setEntry(names1[i], names2, allowed);
00200 }
00201 
00202 void collision_detection::AllowedCollisionMatrix::setEntry(const std::string& name, bool allowed)
00203 {
00204   std::string last = name;
00205   for (std::map<std::string, std::map<std::string, AllowedCollision::Type> >::iterator it = entries_.begin() ; it != entries_.end() ; ++it)
00206     if (name != it->first && last != it->first)
00207     {
00208       last = it->first;
00209       setEntry(name, it->first, allowed);
00210     }
00211 }
00212 
00213 void collision_detection::AllowedCollisionMatrix::setEntry(bool allowed)
00214 {
00215   const AllowedCollision::Type v = allowed ? AllowedCollision::ALWAYS : AllowedCollision::NEVER;
00216   for (std::map<std::string, std::map<std::string, AllowedCollision::Type> >::iterator it1 = entries_.begin() ; it1 != entries_.end() ; ++it1)
00217     for (std::map<std::string, AllowedCollision::Type>::iterator it2 = it1->second.begin() ; it2 != it1->second.end() ; ++it2)
00218       it2->second = v;
00219 }
00220 
00221 void collision_detection::AllowedCollisionMatrix::setDefaultEntry(const std::string &name, bool allowed)
00222 {
00223   const AllowedCollision::Type v = allowed ? AllowedCollision::ALWAYS : AllowedCollision::NEVER;
00224   default_entries_[name] = v;
00225   default_allowed_contacts_.erase(name);
00226 }
00227 
00228 void collision_detection::AllowedCollisionMatrix::setDefaultEntry(const std::string &name, const DecideContactFn &fn)
00229 {
00230   default_entries_[name] = AllowedCollision::CONDITIONAL;
00231   default_allowed_contacts_[name] = fn;
00232 }
00233 
00234 bool collision_detection::AllowedCollisionMatrix::getDefaultEntry(const std::string &name, AllowedCollision::Type &allowed_collision) const
00235 {
00236   std::map<std::string, AllowedCollision::Type>::const_iterator it = default_entries_.find(name);
00237   if (it == default_entries_.end())
00238     return false;
00239   allowed_collision = it->second;
00240   return true;
00241 }
00242 
00243 bool collision_detection::AllowedCollisionMatrix::getDefaultEntry(const std::string &name, DecideContactFn &fn) const
00244 {
00245   std::map<std::string, DecideContactFn>::const_iterator it = default_allowed_contacts_.find(name);
00246   if (it == default_allowed_contacts_.end())
00247     return false;
00248   fn = it->second;
00249   return true;
00250 }
00251 
00252 namespace collision_detection
00253 {
00254   static bool andDecideContact(const DecideContactFn &f1, const DecideContactFn &f2, collision_detection::Contact &contact)
00255   {
00256     return f1(contact) && f2(contact);
00257   }
00258 }
00259 
00260 bool collision_detection::AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const std::string& name2, DecideContactFn &fn) const
00261 {
00262   DecideContactFn fn1, fn2;
00263   bool found1 = getDefaultEntry(name1, fn1);
00264   bool found2 = getDefaultEntry(name2, fn2);
00265 
00266   if (!found1 && !found2)
00267     return getEntry(name1, name2, fn);
00268   else
00269   {
00270     if (found1 && !found2)
00271       fn = fn1;
00272     else
00273       if (!found1 && found2)
00274         fn = fn2;
00275       else
00276         if (found1 && found2)
00277           fn = boost::bind(&andDecideContact, fn1, fn2, _1);
00278         else
00279           return false;
00280     return true;
00281   }
00282 }
00283 
00284 bool collision_detection::AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const std::string& name2, AllowedCollision::Type& allowed_collision) const
00285 {
00286   AllowedCollision::Type t1, t2;
00287   bool found1 = getDefaultEntry(name1, t1);
00288   bool found2 = getDefaultEntry(name2, t2);
00289 
00290   if (!found1 && !found2)
00291     return getEntry(name1, name2, allowed_collision);
00292   else
00293   {
00294     if (found1 && !found2)
00295       allowed_collision = t1;
00296     else
00297       if (!found1 && found2)
00298         allowed_collision = t2;
00299       else
00300         if (found1 && found2)
00301         {
00302           if (t1 == AllowedCollision::NEVER || t2 == AllowedCollision::NEVER)
00303             allowed_collision = AllowedCollision::NEVER;
00304           else
00305             if (t1 == AllowedCollision::CONDITIONAL || t2 == AllowedCollision::CONDITIONAL)
00306               allowed_collision = AllowedCollision::CONDITIONAL;
00307             else // ALWAYS is the only remaining case
00308               allowed_collision = AllowedCollision::ALWAYS;
00309         }
00310         else
00311           return false;
00312     return true;
00313   }
00314 }
00315 
00316 void collision_detection::AllowedCollisionMatrix::clear()
00317 {
00318   entries_.clear();
00319   allowed_contacts_.clear();
00320   default_entries_.clear();
00321   default_allowed_contacts_.clear();
00322 }
00323 
00324 void collision_detection::AllowedCollisionMatrix::getAllEntryNames(std::vector<std::string>& names) const
00325 {
00326   names.clear();
00327   for (std::map<std::string, std::map<std::string, AllowedCollision::Type> >::const_iterator it = entries_.begin() ; it != entries_.end() ; ++it)
00328     if (!names.empty() && names.back() == it->first)
00329       continue;
00330     else
00331       names.push_back(it->first);
00332 }
00333 
00334 void collision_detection::AllowedCollisionMatrix::getMessage(moveit_msgs::AllowedCollisionMatrix &msg) const
00335 {
00336   msg.entry_names.clear();
00337   msg.entry_values.clear();
00338   msg.default_entry_names.clear();
00339   msg.default_entry_values.clear();
00340 
00341   getAllEntryNames(msg.entry_names);
00342   std::sort(msg.entry_names.begin(), msg.entry_names.end());
00343 
00344   msg.entry_values.resize(msg.entry_names.size());
00345   for (std::size_t i = 0 ; i < msg.entry_names.size() ; ++i)
00346     msg.entry_values[i].enabled.resize(msg.entry_names.size(), false);
00347 
00348   // there is an approximation here: if we use a boost function to decide
00349   // whether a collision is allowed or not, we just assume the collision is not allowed.
00350   for (std::size_t i = 0 ; i < msg.entry_names.size() ; ++i)
00351   {
00352     AllowedCollision::Type dtype;
00353     bool dfound = getDefaultEntry(msg.entry_names[i], dtype);
00354     if (dfound)
00355     {
00356       msg.default_entry_names.push_back(msg.entry_names[i]);
00357       msg.default_entry_values.push_back(dtype == AllowedCollision::ALWAYS);
00358     }
00359 
00360     for (std::size_t j = i ; j < msg.entry_names.size() ; ++j)
00361     {
00362       AllowedCollision::Type type;
00363       bool found = getEntry(msg.entry_names[i], msg.entry_names[j], type);
00364       if (found)
00365         msg.entry_values[i].enabled[j] = msg.entry_values[j].enabled[i] = type == AllowedCollision::ALWAYS;
00366     }
00367   }
00368 }
00369 
00370 void collision_detection::AllowedCollisionMatrix::print(std::ostream& out) const
00371 {
00372   std::vector<std::string> names;
00373   getAllEntryNames(names);
00374   std::sort(names.begin(), names.end());
00375 
00376   std::size_t L = 4;
00377   for (std::size_t i = 0 ; i < names.size() ; ++i)
00378   {
00379     std::size_t l = names[i].length();
00380     if (l > L)
00381       L = l;
00382   }
00383   ++L;
00384 
00385   std::size_t D = 2;
00386   while (names.size() > pow(10,D)-1) D++;
00387 
00388   // print indices along the top of the matrix
00389   for (std::size_t j = 0 ; j < D ; ++j)
00390   {
00391     out << std::setw(L+D+4) << "";
00392     for (std::size_t i = 0 ; i < names.size() ; ++i)
00393     {
00394       std::stringstream ss;
00395       ss << std::setw(D) << i;
00396       out << std::setw(3) << ss.str().c_str()[j];
00397     }
00398     out << std::endl;
00399   }
00400 
00401   for (std::size_t i = 0 ; i < names.size() ; ++i)
00402   {
00403     out << std::setw(L) << names[i];
00404     out << std::setw(D+1) << i;
00405     out << " | ";
00406     for (std::size_t j = 0 ; j < names.size() ; ++j)
00407     {
00408       AllowedCollision::Type type;
00409       bool found = getAllowedCollision(names[i], names[j], type);
00410       if (found)
00411         out << std::setw(3) << (type == AllowedCollision::ALWAYS ? '1' : (type == AllowedCollision::NEVER ? '0' : '?'));
00412       else
00413         out << std::setw(3) << '-';
00414     }
00415     out << std::endl;
00416   }
00417 }


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