collision_space_ccd: collision_space_ccd: environment.cpp Source File

00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, 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, Inc. 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 
00037 #include "collision_space_ccd/environment.h"
00038 #include <ros/console.h>
00039 
00040 namespace collision_space_ccd
00041 {
00042 
00043 EnvironmentModel::AllowedCollisionMatrix::AllowedCollisionMatrix(const std::vector<std::string>& names,
00044                                                                                   bool allowed) 
00045 {
00046   unsigned int ns = names.size();
00047   allowed_entries_.resize(ns);
00048   for(unsigned int i = 0; i < ns; i++)
00049   {
00050     allowed_entries_[i].resize(ns,allowed);
00051     allowed_entries_bimap_.insert(entry_type::value_type(names[i], i));
00052   }
00053   valid_ = true;
00054 }
00055 
00056 EnvironmentModel::AllowedCollisionMatrix::AllowedCollisionMatrix(const std::vector<std::vector<bool> >& all_coll_vectors,
00057                                                                                   const std::map<std::string, unsigned int>& all_coll_indices)
00058 {
00059   unsigned int num_outer = all_coll_vectors.size();
00060   valid_ = true;
00061   if(all_coll_indices.size() != all_coll_vectors.size())
00062   {
00063     valid_ = false;
00064     ROS_WARN_STREAM("Indices size " << all_coll_indices.size() << " not equal to num vecs " << all_coll_vectors.size());
00065     return;
00066   }
00067   for(std::map<std::string, unsigned int>::const_iterator it = all_coll_indices.begin();
00068       it != all_coll_indices.end();
00069       it++)
00070   {
00071     allowed_entries_bimap_.insert(entry_type::value_type(it->first, it->second));
00072   }
00073   
00074   if(allowed_entries_bimap_.left.size() != all_coll_indices.size())
00075   {
00076     valid_ = false;
00077     ROS_WARN_STREAM("Some strings or values in allowed collision matrix are repeated");
00078   }
00079   if(allowed_entries_bimap_.right.begin()->first != 0)
00080   {
00081     valid_ = false;
00082     ROS_WARN_STREAM("No entry with index 0 in map");
00083   }
00084   if(allowed_entries_bimap_.right.rbegin()->first != num_outer-1)
00085   {
00086     valid_ = false;
00087     ROS_WARN_STREAM("Last index should be " << num_outer << " but instead is " << allowed_entries_bimap_.right.rbegin()->first);
00088   }
00089   
00090   for(unsigned int i = 0; i < num_outer; i++)
00091   {
00092     if(num_outer != all_coll_vectors[i].size())
00093     {
00094       valid_ = false;
00095       ROS_WARN_STREAM("Entries size for " << allowed_entries_bimap_.right.at(i) << " is " << all_coll_vectors[i].size() << " instead of " << num_outer);
00096     }
00097   }
00098   allowed_entries_ = all_coll_vectors;
00099 }
00100 
00101 EnvironmentModel::AllowedCollisionMatrix::AllowedCollisionMatrix(const AllowedCollisionMatrix& acm)
00102 {
00103   valid_ = acm.valid_;
00104   allowed_entries_ = acm.allowed_entries_;
00105   allowed_entries_bimap_ = acm.allowed_entries_bimap_;
00106 }
00107 
00108 bool EnvironmentModel::AllowedCollisionMatrix::getAllowedCollision(const std::string& name1,
00109                                                                                     const std::string& name2,
00110                                                                                     bool& allowed_collision) const
00111 {
00112   entry_type::left_const_iterator it1 = allowed_entries_bimap_.left.find(name1);
00113   if(it1 == allowed_entries_bimap_.left.end())
00114   {
00115     return false;
00116   }
00117   entry_type::left_const_iterator it2 = allowed_entries_bimap_.left.find(name2);
00118   if(it2 == allowed_entries_bimap_.left.end())
00119   {
00120     return false;
00121   }
00122   if(it1->second > allowed_entries_.size())
00123   {
00124     ROS_INFO_STREAM("Something wrong with acm entry for " << name1);
00125     return false;
00126   } 
00127   if(it2->second > allowed_entries_[it1->second].size())
00128   {
00129     ROS_INFO_STREAM("Something wrong with acm entry for " << name2);
00130     return false;
00131   }
00132   allowed_collision = allowed_entries_[it1->second][it2->second];
00133   return true;
00134 }
00135 
00136 bool EnvironmentModel::AllowedCollisionMatrix::getAllowedCollision(unsigned int i, unsigned int j,
00137                                                                                     bool& allowed_collision) const
00138 {
00139   if(i > allowed_entries_.size() || j > allowed_entries_[i].size())
00140   {
00141     return false;
00142   }
00143   allowed_collision = allowed_entries_[i][j];
00144   return true;
00145 }
00146 
00147 bool EnvironmentModel::AllowedCollisionMatrix::hasEntry(const std::string& name) const
00148 {
00149   return(allowed_entries_bimap_.left.find(name) != allowed_entries_bimap_.left.end());
00150 }
00151 
00152 bool EnvironmentModel::AllowedCollisionMatrix::getEntryIndex(const std::string& name,
00153                                                                               unsigned int& index) const
00154 {
00155   entry_type::left_const_iterator it1 = allowed_entries_bimap_.left.find(name);
00156   if(it1 == allowed_entries_bimap_.left.end())
00157   {
00158     return false;
00159   }
00160   index = it1->second;
00161   return true;
00162 }
00163 
00164 bool EnvironmentModel::AllowedCollisionMatrix::getEntryName(const unsigned int ind,
00165                                                                              std::string& name) const
00166 {
00167   entry_type::right_const_iterator it1 = allowed_entries_bimap_.right.find(ind);
00168   if(it1 == allowed_entries_bimap_.right.end())
00169   {
00170     return false;
00171   }
00172   name = it1->second;
00173   return true;
00174 }
00175 
00176 bool EnvironmentModel::AllowedCollisionMatrix::removeEntry(const std::string& name)
00177 {
00178   if(allowed_entries_bimap_.left.find(name) == allowed_entries_bimap_.left.end())
00179   {
00180     return false;
00181   }
00182   unsigned int last_index = allowed_entries_bimap_.size()-1;
00183   unsigned int ind = allowed_entries_bimap_.left.find(name)->second;
00184   allowed_entries_.erase(allowed_entries_.begin()+ind);
00185   for(unsigned int i = 0; i < allowed_entries_.size(); i++)
00186   {
00187     allowed_entries_[i].erase(allowed_entries_[i].begin()+ind);
00188   }
00189   allowed_entries_bimap_.left.erase(name);
00190   //if this is last ind, no need to decrement
00191   if(ind != last_index) {
00192     //sanity checks
00193     entry_type::right_iterator it = allowed_entries_bimap_.right.find(last_index);
00194     if(it == allowed_entries_bimap_.right.end())
00195     {
00196       ROS_INFO_STREAM("Something wrong with last index " << last_index << " ind " << ind);
00197     }
00198     //now we need to decrement the index for everything after this
00199     for(unsigned int i = ind+1; i <= last_index; i++)
00200     {
00201       entry_type::right_iterator it = allowed_entries_bimap_.right.find(i);
00202       if(it == allowed_entries_bimap_.right.end())
00203       {
00204         ROS_WARN_STREAM("Problem in replace " << i);
00205         return false;
00206       }
00207       bool successful_replace = allowed_entries_bimap_.right.replace_key(it, i-1);
00208       if(!successful_replace) {
00209         ROS_WARN_STREAM("Can't replace");
00210         return false;
00211       } 
00212     }
00213   }
00214   return true;
00215 }
00216 
00217 bool EnvironmentModel::AllowedCollisionMatrix::addEntry(const std::string& name,
00218                                                                          bool allowed)
00219 {
00220   if(allowed_entries_bimap_.left.find(name) != allowed_entries_bimap_.left.end())
00221   {
00222     return false;
00223   }
00224   unsigned int ind = allowed_entries_.size();
00225   allowed_entries_bimap_.insert(entry_type::value_type(name,ind));
00226   std::vector<bool> new_entry(ind+1, allowed);
00227   allowed_entries_.resize(ind+1);
00228   allowed_entries_[ind] = new_entry;
00229   for(unsigned int i = 0; i < ind; i++)
00230   {
00231     allowed_entries_[i].resize(ind+1);
00232     allowed_entries_[i][ind] = allowed;
00233   }
00234   return true;
00235 }
00236 
00237 bool EnvironmentModel::AllowedCollisionMatrix::changeEntry(bool allowed)
00238 {
00239   for(unsigned int i = 0; i < allowed_entries_.size(); i++)
00240   {
00241     for(unsigned int j = 0; j < allowed_entries_[i].size(); j++)
00242     {
00243       allowed_entries_[i][j] = allowed;
00244       allowed_entries_[j][i] = allowed;
00245     }
00246   }
00247   return true;
00248 }
00249 
00250 bool EnvironmentModel::AllowedCollisionMatrix::changeEntry(const std::string& name1,
00251                                                                             const std::string& name2,
00252                                                                             bool allowed)
00253 {
00254   entry_type::left_const_iterator it1 = allowed_entries_bimap_.left.find(name1);
00255   if(it1 == allowed_entries_bimap_.left.end())
00256   {
00257     return false;
00258   }
00259   entry_type::left_const_iterator it2 = allowed_entries_bimap_.left.find(name2);
00260   if(it2 == allowed_entries_bimap_.left.end())
00261   {
00262     return false;
00263   }
00264   allowed_entries_[it1->second][it2->second] = allowed;
00265   allowed_entries_[it2->second][it1->second] = allowed;
00266   return true;
00267 }
00268 
00269 bool EnvironmentModel::AllowedCollisionMatrix::changeEntry(unsigned int i, unsigned int j,
00270                                                                             bool allowed) 
00271 {
00272   if(i > allowed_entries_.size() || j > allowed_entries_[i].size())
00273   {
00274     return false;
00275   }
00276   allowed_entries_[i][j] = allowed;
00277   allowed_entries_[j][i] = allowed;
00278   return true;
00279 }
00280 bool EnvironmentModel::AllowedCollisionMatrix::changeEntry(const std::string& name,
00281                                                                             bool allowed)
00282 {
00283   if(allowed_entries_bimap_.left.find(name) == allowed_entries_bimap_.left.end())
00284   {
00285     return false;
00286   }
00287   unsigned int ind = allowed_entries_bimap_.left.find(name)->second;
00288   for(unsigned int i = 0; i < allowed_entries_.size(); i++)
00289   {
00290     allowed_entries_[i][ind] = allowed;
00291     allowed_entries_[ind][i] = allowed;
00292   }
00293   return true;
00294 }
00295 
00296 bool EnvironmentModel::AllowedCollisionMatrix::changeEntry(const std::string& name,
00297                                                                             const std::vector<std::string>& change_names,
00298                                                                             bool allowed)
00299 {
00300   bool ok = true;
00301   if(allowed_entries_bimap_.left.find(name) == allowed_entries_bimap_.left.end())
00302   {
00303     ROS_DEBUG_STREAM("No entry for " << name);
00304     ok = false;
00305     return ok;
00306   }
00307   unsigned int ind_1 = allowed_entries_bimap_.left.find(name)->second;
00308   for(unsigned int i = 0; i < change_names.size(); i++)
00309   {
00310     if(allowed_entries_bimap_.left.find(change_names[i]) == allowed_entries_bimap_.left.end())
00311     {
00312       ROS_DEBUG_STREAM("No entry for " << change_names[i]);
00313       ok = false;
00314       continue;
00315     }
00316     unsigned int ind_2 = allowed_entries_bimap_.left.find(change_names[i])->second;
00317     if(ind_1 >= allowed_entries_.size())
00318     {
00319       ROS_ERROR_STREAM("Got an index entry for name " << name << " ind "  << ind_1 << " but only have " 
00320                        << allowed_entries_.size() << " in allowed collision matrix.");
00321       return false;
00322     }
00323     if(ind_2 >= allowed_entries_[ind_1].size())
00324     {
00325       ROS_ERROR_STREAM("Got an index entry for name " << change_names[i] << " index " << ind_2 << " but only have " << 
00326                        allowed_entries_[ind_1].size() << " in allowed collision matrix.");
00327       return false;
00328     }
00329     allowed_entries_[ind_1][ind_2] = allowed;
00330     allowed_entries_[ind_2][ind_1] = allowed;
00331   }
00332   return ok;
00333 }
00334 
00335 bool EnvironmentModel::AllowedCollisionMatrix::changeEntry(const std::vector<std::string>& change_names_1,
00336                                                                             const std::vector<std::string>& change_names_2,
00337                                                                             bool allowed)
00338 {
00339   bool ok = true;
00340   for(unsigned int i = 0; i < change_names_1.size(); i++)
00341   {
00342     if(!changeEntry(change_names_1[i], change_names_2, allowed))
00343     {
00344       ok = false;
00345     }
00346   }
00347   return ok;
00348 }
00349 
00350 bool EnvironmentModel::getCollisionContacts(std::vector<Contact> &contacts, unsigned int max_count) const
00351 {
00352   std::vector<AllowedContact> allowed;
00353   return getCollisionContacts(allowed, contacts, max_count);
00354 }
00355 
00356 bool EnvironmentModel::getVerbose(void) const
00357 {
00358   return verbose_;
00359 }
00360 
00361 void EnvironmentModel::setVerbose(bool verbose)
00362 {
00363   verbose_ = verbose;
00364 }
00365 
00366 const EnvironmentObjects* EnvironmentModel::getObjects(void) const
00367 {
00368   return objects_;
00369 }
00370 
00371 const planning_models::KinematicModel* EnvironmentModel::getRobotModel(void) const
00372 {
00373   return robot_model_;
00374 }
00375 
00376 double EnvironmentModel::getRobotScale(void) const
00377 {
00378   return robot_scale_;
00379 }
00380 
00381 double EnvironmentModel::getRobotPadding(void) const
00382 {
00383   return default_robot_padding_;
00384 }
00385         
00386 void EnvironmentModel::setRobotModel(const planning_models::KinematicModel* model,
00387                                                       const AllowedCollisionMatrix& acm,
00388                                                       const std::map<std::string, double>& link_padding_map,
00389                                                       double default_padding,
00390                                                       double scale) 
00391 {
00392   robot_model_ = model;
00393   default_collision_matrix_ = acm;
00394   robot_scale_ = scale;
00395   default_robot_padding_ = default_padding;
00396   default_link_padding_map_ = link_padding_map;
00397 }
00398 
00399 void EnvironmentModel::lock(void) const
00400 {
00401   lock_.lock();
00402 }
00403 
00404 void EnvironmentModel::unlock(void) const
00405 {
00406   lock_.unlock();
00407 }
00408 
00409 const EnvironmentModel::AllowedCollisionMatrix&
00410 EnvironmentModel::getDefaultAllowedCollisionMatrix() const
00411 {
00412   return default_collision_matrix_;
00413 }
00414 
00415 const EnvironmentModel::AllowedCollisionMatrix&
00416 EnvironmentModel::getCurrentAllowedCollisionMatrix() const
00417 {
00418   if(use_altered_collision_matrix_)
00419   {
00420     return altered_collision_matrix_;
00421   } 
00422   return default_collision_matrix_;
00423 }
00424 
00425 void EnvironmentModel::setAlteredCollisionMatrix(const AllowedCollisionMatrix& acm)
00426 {
00427   use_altered_collision_matrix_ = true;
00428   altered_collision_matrix_ = acm;
00429 }
00430 
00431 void EnvironmentModel::revertAlteredCollisionMatrix()
00432 {
00433   use_altered_collision_matrix_ = false;
00434 }
00435 
00436 void EnvironmentModel::setAlteredLinkPadding(const std::map<std::string, double>& new_link_padding)
00437 {
00438   altered_link_padding_map_.clear();
00439   for(std::map<std::string, double>::const_iterator it = new_link_padding.begin();
00440       it != new_link_padding.end();
00441       it++) {
00442     if(default_link_padding_map_.find(it->first) == default_link_padding_map_.end())
00443     {
00444       //don't have this currently
00445       continue;
00446     }
00447     //only putting in altered padding if it's different
00448     if(default_link_padding_map_.find(it->first)->second != it->second)
00449     {
00450       altered_link_padding_map_[it->first] = it->second;
00451     }
00452   }
00453   use_altered_link_padding_map_ = true;
00454 }
00455 
00456 void EnvironmentModel::revertAlteredLinkPadding()
00457 {
00458   altered_link_padding_map_.clear();
00459   use_altered_link_padding_map_ = false;
00460 }
00461 
00462 const std::map<std::string, double>& EnvironmentModel::getDefaultLinkPaddingMap() const
00463 {
00464   return default_link_padding_map_;
00465 }
00466 
00467 std::map<std::string, double> EnvironmentModel::getCurrentLinkPaddingMap() const
00468 {
00469   std::map<std::string, double> ret_map = default_link_padding_map_;
00470   for(std::map<std::string, double>::const_iterator it = altered_link_padding_map_.begin();
00471       it != altered_link_padding_map_.end();
00472       it++)
00473   {
00474     ret_map[it->first] = it->second;
00475   }
00476   return ret_map;
00477 }
00478 
00479 double EnvironmentModel::getCurrentLinkPadding(std::string name) const
00480 {
00481   if(altered_link_padding_map_.find(name) != altered_link_padding_map_.end())
00482   {
00483     return altered_link_padding_map_.find(name)->second;
00484   }
00485   else if(default_link_padding_map_.find(name) != default_link_padding_map_.end())
00486   {
00487     return default_link_padding_map_.find(name)->second;
00488   }
00489   return 0.0;
00490 }
00491 
00492 }