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 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 
00044 collision_detection::World::World(const World& other)
00045 {
00046   objects_ = other.objects_;
00047 }
00048 
00049 collision_detection::World::~World()
00050 {
00051   while (!observers_.empty())
00052     removeObserver(observers_.front());
00053 }
00054 
00055 inline void collision_detection::World::addToObjectInternal(const ObjectPtr& obj, 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, const std::vector<shapes::ShapeConstPtr>& shapes,
00063                                              const EigenSTL::vector_Affine3d& poses)
00064 {
00065   if (shapes.size() != poses.size())
00066   {
00067     logError("Number of shapes and number of poses do not match. Not adding this object to collision world.");
00068     return;
00069   }
00070 
00071   if (shapes.empty())
00072     return;
00073 
00074   int action = ADD_SHAPE;
00075 
00076   ObjectPtr& obj = objects_[id];
00077   if (!obj)
00078   {
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, const shapes::ShapeConstPtr& shape,
00092                                              const Eigen::Affine3d& pose)
00093 {
00094   int action = ADD_SHAPE;
00095 
00096   ObjectPtr& obj = objects_[id];
00097   if (!obj)
00098   {
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, const shapes::ShapeConstPtr& shape,
00138                                                    const Eigen::Affine3d& pose)
00139 {
00140   std::map<std::string, ObjectPtr>::iterator it = objects_.find(id);
00141   if (it != objects_.end())
00142   {
00143     unsigned int n = it->second->shapes_.size();
00144     for (unsigned int i = 0; i < n; ++i)
00145       if (it->second->shapes_[i] == shape)
00146       {
00147         ensureUnique(it->second);
00148         it->second->shape_poses_[i] = pose;
00149 
00150         notify(it->second, MOVE_SHAPE);
00151         return true;
00152       }
00153   }
00154   return false;
00155 }
00156 
00157 bool collision_detection::World::removeShapeFromObject(const std::string& id, const shapes::ShapeConstPtr& shape)
00158 {
00159   std::map<std::string, ObjectPtr>::iterator it = objects_.find(id);
00160   if (it != objects_.end())
00161   {
00162     unsigned int n = it->second->shapes_.size();
00163     for (unsigned int i = 0; i < n; ++i)
00164       if (it->second->shapes_[i] == shape)
00165       {
00166         ensureUnique(it->second);
00167         it->second->shapes_.erase(it->second->shapes_.begin() + i);
00168         it->second->shape_poses_.erase(it->second->shape_poses_.begin() + i);
00169 
00170         if (it->second->shapes_.empty())
00171         {
00172           notify(it->second, DESTROY);
00173           objects_.erase(it);
00174         }
00175         else
00176         {
00177           notify(it->second, REMOVE_SHAPE);
00178         }
00179         return true;
00180       }
00181   }
00182   return false;
00183 }
00184 
00185 bool collision_detection::World::removeObject(const std::string& id)
00186 {
00187   std::map<std::string, ObjectPtr>::iterator it = objects_.find(id);
00188   if (it != objects_.end())
00189   {
00190     notify(it->second, DESTROY);
00191     objects_.erase(it);
00192     return true;
00193   }
00194   return false;
00195 }
00196 
00197 void collision_detection::World::clearObjects()
00198 {
00199   notifyAll(DESTROY);
00200   objects_.clear();
00201 }
00202 
00203 collision_detection::World::ObserverHandle collision_detection::World::addObserver(const ObserverCallbackFn& callback)
00204 {
00205   Observer* o = new Observer(callback);
00206   observers_.push_back(o);
00207   return ObserverHandle(o);
00208 }
00209 
00210 void collision_detection::World::removeObserver(ObserverHandle observer_handle)
00211 {
00212   for (std::vector<Observer*>::iterator obs = observers_.begin(); obs != observers_.end(); ++obs)
00213   {
00214     if (*obs == observer_handle.observer_)
00215     {
00216       delete *obs;
00217       observers_.erase(obs);
00218       return;
00219     }
00220   }
00221 }
00222 
00223 void collision_detection::World::notifyAll(Action action)
00224 {
00225   for (std::map<std::string, ObjectPtr>::const_iterator it = objects_.begin(); it != objects_.end(); ++it)
00226     notify(it->second, action);
00227 }
00228 
00229 void collision_detection::World::notify(const ObjectConstPtr& obj, Action action)
00230 {
00231   for (std::vector<Observer*>::const_iterator obs = observers_.begin(); obs != observers_.end(); ++obs)
00232     (*obs)->callback_(obj, action);
00233 }
00234 
00235 void collision_detection::World::notifyObserverAllObjects(const ObserverHandle observer_handle, Action action) const
00236 {
00237   for (std::vector<Observer*>::const_iterator obs = observers_.begin(); obs != observers_.end(); ++obs)
00238   {
00239     if (*obs == observer_handle.observer_)
00240     {
00241       // call the callback for each object
00242       for (std::map<std::string, ObjectPtr>::const_iterator obj = objects_.begin(); obj != objects_.end(); ++obj)
00243         (*obs)->callback_(obj->second, action);
00244       break;
00245     }
00246   }
00247 }


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