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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jun 19 2019 19:23:49