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