world.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, 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: Acorn Pooley, Ioan Sucan */
00036 
00037 #include <moveit/collision_detection/world.h>
00038 #include <console_bridge/console.h>
00039 
00040 collision_detection::World::World()
00041 { }
00042 
00043 collision_detection::World::World(const World &other)
00044 {
00045   objects_ = other.objects_;
00046 }
00047 
00048 collision_detection::World::~World()
00049 {
00050   while (!observers_.empty())
00051     removeObserver(observers_.front());
00052 }
00053 
00054 inline void collision_detection::World::addToObjectInternal(const ObjectPtr &obj,
00055                                                             const shapes::ShapeConstPtr &shape,
00056                                                             const Eigen::Affine3d &pose)
00057 {
00058   obj->shapes_.push_back(shape);
00059   obj->shape_poses_.push_back(pose);
00060 }
00061 
00062 void collision_detection::World::addToObject(const std::string &id,
00063                                              const std::vector<shapes::ShapeConstPtr> &shapes,
00064                                              const EigenSTL::vector_Affine3d &poses)
00065 {
00066   if (shapes.size() != poses.size())
00067   {
00068     logError("Number of shapes and number of poses do not match. Not adding this object to collision world.");
00069     return;
00070   }
00071 
00072   if (!shapes.size())
00073     return;
00074 
00075   int action = ADD_SHAPE;
00076 
00077   ObjectPtr& obj = objects_[id];
00078   if (!obj) {
00079     obj.reset(new Object(id));
00080     action |= CREATE;
00081   }
00082 
00083   ensureUnique(obj);
00084 
00085   for (std::size_t i = 0 ; i < shapes.size() ; ++i)
00086     addToObjectInternal(obj, shapes[i], poses[i]);
00087 
00088   notify(obj, Action(action));
00089 }
00090 
00091 void collision_detection::World::addToObject(const std::string &id,
00092                                              const shapes::ShapeConstPtr &shape,
00093                                              const Eigen::Affine3d &pose)
00094 {
00095   int action = ADD_SHAPE;
00096 
00097   ObjectPtr& obj = objects_[id];
00098   if (!obj) {
00099     obj.reset(new Object(id));
00100     action |= CREATE;
00101   }
00102 
00103   ensureUnique(obj);
00104   addToObjectInternal(obj, shape, pose);
00105 
00106   notify(obj, Action(action));
00107 }
00108 
00109 std::vector<std::string> collision_detection::World::getObjectIds() const
00110 {
00111   std::vector<std::string> id;
00112   for (std::map<std::string, ObjectPtr>::const_iterator it = objects_.begin() ; it != objects_.end() ; ++it)
00113     id.push_back(it->first);
00114   return id;
00115 }
00116 
00117 collision_detection::World::ObjectConstPtr collision_detection::World::getObject(const std::string &id) const
00118 {
00119   std::map<std::string, ObjectPtr>::const_iterator it = objects_.find(id);
00120   if (it == objects_.end())
00121     return ObjectConstPtr();
00122   else
00123     return it->second;
00124 }
00125 
00126 void collision_detection::World::ensureUnique(ObjectPtr &obj)
00127 {
00128   if (obj && !obj.unique())
00129     obj.reset(new Object(*obj));
00130 }
00131 
00132 bool collision_detection::World::hasObject(const std::string &id) const
00133 {
00134   return objects_.find(id) != objects_.end();
00135 }
00136 
00137 bool collision_detection::World::moveShapeInObject(const std::string &id,
00138                                                    const shapes::ShapeConstPtr &shape,
00139                                                    const Eigen::Affine3d &pose)
00140 {
00141   std::map<std::string, ObjectPtr>::iterator it = objects_.find(id);
00142   if (it != objects_.end())
00143   {
00144     unsigned int n = it->second->shapes_.size();
00145     for (unsigned int i = 0 ; i < n ; ++i)
00146       if (it->second->shapes_[i] == shape)
00147       {
00148         ensureUnique(it->second);
00149         it->second->shape_poses_[i] = pose;
00150 
00151         notify(it->second, MOVE_SHAPE);
00152         return true;
00153       }
00154   }
00155   return false;
00156 }
00157 
00158 bool collision_detection::World::removeShapeFromObject(const std::string &id,
00159                                                        const shapes::ShapeConstPtr &shape)
00160 {
00161   std::map<std::string, ObjectPtr>::iterator it = objects_.find(id);
00162   if (it != objects_.end())
00163   {
00164     unsigned int n = it->second->shapes_.size();
00165     for (unsigned int i = 0 ; i < n ; ++i)
00166       if (it->second->shapes_[i] == shape)
00167       {
00168         ensureUnique(it->second);
00169         it->second->shapes_.erase(it->second->shapes_.begin() + i);
00170         it->second->shape_poses_.erase(it->second->shape_poses_.begin() + i);
00171 
00172         if (it->second->shapes_.empty())
00173         {
00174           notify(it->second, DESTROY);
00175           objects_.erase(it);
00176         }
00177         else
00178         {
00179           notify(it->second, REMOVE_SHAPE);
00180         }
00181         return true;
00182       }
00183   }
00184   return false;
00185 }
00186 
00187 bool collision_detection::World::removeObject(const std::string &id)
00188 {
00189   std::map<std::string, ObjectPtr>::iterator it = objects_.find(id);
00190   if (it != objects_.end())
00191   {
00192     notify(it->second, DESTROY);
00193     objects_.erase(it);
00194     return true;
00195   }
00196   return false;
00197 }
00198 
00199 void collision_detection::World::clearObjects()
00200 {
00201   notifyAll(DESTROY);
00202   objects_.clear();
00203 }
00204 
00205 collision_detection::World::ObserverHandle collision_detection::World::addObserver(const ObserverCallbackFn &callback)
00206 {
00207   Observer *o = new Observer(callback);
00208   observers_.push_back(o);
00209   return ObserverHandle(o);
00210 }
00211 
00212 void collision_detection::World::removeObserver(ObserverHandle observer_handle)
00213 {
00214   for (std::vector<Observer*>::iterator obs = observers_.begin() ; obs != observers_.end() ; ++obs)
00215   {
00216     if (*obs == observer_handle.observer_)
00217     {
00218       delete *obs;
00219       observers_.erase(obs);
00220       return;
00221     }
00222   }
00223 }
00224 
00225 void collision_detection::World::notifyAll(Action action)
00226 {
00227   for (std::map<std::string, ObjectPtr>::const_iterator it = objects_.begin() ; it != objects_.end() ; ++it)
00228     notify(it->second, action);
00229 }
00230 
00231 void collision_detection::World::notify(const ObjectConstPtr& obj, Action action)
00232 {
00233   for (std::vector<Observer*>::const_iterator obs = observers_.begin() ; obs != observers_.end() ; ++obs)
00234     (*obs)->callback_(obj, action);
00235 }
00236 
00237 void collision_detection::World::notifyObserverAllObjects(const ObserverHandle observer_handle, Action action) const
00238 {
00239   for (std::vector<Observer*>::const_iterator obs = observers_.begin() ; obs != observers_.end() ; ++obs)
00240   {
00241     if (*obs == observer_handle.observer_)
00242     {
00243       // call the callback for each object
00244       for (std::map<std::string, ObjectPtr>::const_iterator obj = objects_.begin() ; obj != objects_.end() ; ++obj)
00245         (*obs)->callback_(obj->second, action);
00246       break;
00247     }
00248   }
00249 }


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