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 
00040 bool collision_space::EnvironmentModel::getCollisionContacts(std::vector<Contact> &contacts, unsigned int max_count)
00041 {
00042   std::vector<AllowedContact> allowed;
00043   return getCollisionContacts(allowed, contacts, max_count);
00044 }
00045 
00046 bool collision_space::EnvironmentModel::getVerbose(void) const
00047 {
00048   return m_verbose;
00049 }
00050 
00051 void collision_space::EnvironmentModel::setVerbose(bool verbose)
00052 {
00053   m_verbose = verbose;
00054 }
00055 
00056 const collision_space::EnvironmentObjects* collision_space::EnvironmentModel::getObjects(void) const
00057 {
00058   return m_objects;
00059 }
00060 
00061 const boost::shared_ptr<const planning_models::KinematicModel>& collision_space::EnvironmentModel::getRobotModel(void) const
00062 {
00063   return m_robotModel;
00064 }
00065 
00066 double collision_space::EnvironmentModel::getRobotScale(void) const
00067 {
00068   return m_robotScale;
00069 }
00070 
00071 double collision_space::EnvironmentModel::getRobotPadding(void) const
00072 {
00073   return m_robotPadd;
00074 }
00075         
00076 void collision_space::EnvironmentModel::setRobotModel(const boost::shared_ptr<const planning_models::KinematicModel> &model, 
00077                                                       const std::vector<std::string> &links, 
00078                                                       const std::map<std::string, double>& link_padding_map,
00079                                                       double default_padding,
00080                                                       double scale) 
00081 {
00082   m_robotModel = model;
00083   m_collisionLinks = links;
00084   m_selfCollisionTest.resize(links.size());
00085   for (unsigned int i = 0 ; i < links.size() ; ++i)
00086   {
00087     m_selfCollisionTest[i].resize(links.size(), true);
00088     m_collisionLinkIndex[links[i]] = i;
00089   }
00090   m_robotScale = scale;
00091   m_robotPadd = default_padding;
00092 }
00093 
00094 void collision_space::EnvironmentModel::addSelfCollisionGroup(const std::vector<std::string>& group1,
00095                                                               const std::vector<std::string>& group2) 
00096 {
00097   for(std::vector<std::string>::const_iterator stit1 = group1.begin();
00098       stit1 != group1.end();
00099       stit1++) {
00100     if (m_collisionLinkIndex.find(*stit1) == m_collisionLinkIndex.end())
00101     {
00102       ROS_WARN("Unknown link '%s'", (*stit1).c_str());
00103       continue;
00104     }
00105     for(std::vector<std::string>::const_iterator stit2 = group2.begin();
00106         stit2 != group2.end();
00107         stit2++) {
00108       if (m_collisionLinkIndex.find(*stit2) == m_collisionLinkIndex.end())
00109       {
00110         ROS_WARN("Unknown link '%s'", (*stit2).c_str());
00111         continue;
00112       }
00113       m_selfCollisionTest[m_collisionLinkIndex[*stit1]][m_collisionLinkIndex[*stit2]] = false;
00114       m_selfCollisionTest[m_collisionLinkIndex[*stit2]][m_collisionLinkIndex[*stit1]] = false;
00115     }
00116   }
00117 }
00118 
00119 void collision_space::EnvironmentModel::removeSelfCollisionGroup(const std::vector<std::string>& group1,
00120                                                                  const std::vector<std::string>& group2) 
00121 {
00122   for(std::vector<std::string>::const_iterator stit1 = group1.begin();
00123       stit1 != group1.end();
00124       stit1++) {
00125     if (m_collisionLinkIndex.find(*stit1) == m_collisionLinkIndex.end())
00126     {
00127       ROS_WARN("Unknown link '%s'", (*stit1).c_str());
00128       continue;
00129     }
00130     for(std::vector<std::string>::const_iterator stit2 = group2.begin();
00131         stit2 != group2.end();
00132         stit2++) {
00133       if (m_collisionLinkIndex.find(*stit2) == m_collisionLinkIndex.end())
00134       {
00135         ROS_WARN("Unknown link '%s'", (*stit2).c_str());
00136         continue;
00137       }
00138       m_selfCollisionTest[m_collisionLinkIndex[*stit1]][m_collisionLinkIndex[*stit2]] = true;
00139       m_selfCollisionTest[m_collisionLinkIndex[*stit2]][m_collisionLinkIndex[*stit1]] = true;
00140     }
00141   }
00142 }
00143 
00144 void collision_space::EnvironmentModel::lock(void)
00145 {
00146   m_lock.lock();
00147 }
00148 
00149 void collision_space::EnvironmentModel::unlock(void)
00150 {
00151   m_lock.unlock();
00152 }
00153 
00154 void collision_space::EnvironmentModel::setSelfCollision(bool selfCollision)
00155 {
00156   m_selfCollision = selfCollision;
00157 }
00158 
00159 bool collision_space::EnvironmentModel::getSelfCollision(void) const
00160 {
00161   return m_selfCollision;
00162 }
00163 
00164 void collision_space::EnvironmentModel::setAllowedCollisionMatrix(const std::vector<std::vector<bool> > &matrix,
00165                                                                   const std::map<std::string, unsigned int> &ind){
00166   use_set_collision_matrix_ = true;
00167   set_collision_matrix_ = matrix;
00168   set_collision_ind_ = ind;
00169 }
00170 
00171 void collision_space::EnvironmentModel::revertAllowedCollisionMatrix() {
00172   use_set_collision_matrix_ = false;
00173   set_collision_matrix_.clear();
00174   set_collision_ind_.clear();
00175 }
00176 
00177 void collision_space::EnvironmentModel::getCurrentAllowedCollisionMatrix(std::vector<std::vector<bool> > &matrix,
00178                                                                          std::map<std::string, unsigned int> &ind) const {
00179   if(use_set_collision_matrix_) {
00180     matrix = set_collision_matrix_;
00181     ind = set_collision_ind_;
00182   } else {
00183     getDefaultAllowedCollisionMatrix(matrix, ind);
00184   }
00185 }
00186 
00187 void collision_space::EnvironmentModel::setRobotLinkPadding(const std::map<std::string, double>& new_link_padding) {
00188   for(std::map<std::string, double>::const_iterator it = new_link_padding.begin();
00189       it != new_link_padding.end();
00190       it++) {
00191     altered_link_padding_[it->first] = it->second;
00192   }
00193 }
00194 
00195 void collision_space::EnvironmentModel::revertRobotLinkPadding() {
00196   altered_link_padding_.clear();
00197 }
00198 
00199 double collision_space::EnvironmentModel::getCurrentLinkPadding(std::string name) const {
00200   if(altered_link_padding_.find(name) != altered_link_padding_.end()) {
00201     return altered_link_padding_.find(name)->second;
00202   }
00203   if(link_padding_map_.find(name) != link_padding_map_.end()) {
00204     return link_padding_map_.find(name)->second;
00205   }
00206   return 0.0;
00207 }