world.h
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: Ioan Sucan, Acorn Pooley, Sachin Chitta */
00036 
00037 #ifndef MOVEIT_COLLISION_DETECTION_WORLD_
00038 #define MOVEIT_COLLISION_DETECTION_WORLD_
00039 
00040 #include <moveit/macros/class_forward.h>
00041 
00042 #include <string>
00043 #include <vector>
00044 #include <map>
00045 #include <boost/function.hpp>
00046 #include <Eigen/Geometry>
00047 #include <eigen_stl_containers/eigen_stl_vector_container.h>
00048 #include <geometric_shapes/shapes.h>
00049 
00050 namespace collision_detection
00051 {
00052 MOVEIT_CLASS_FORWARD(World);
00053 
00055 class World
00056 {
00057 public:
00059   World();
00060 
00064   World(const World& other);
00065 
00066   ~World();
00067 
00068   /**********************************************************************/
00069   /* Collision Bodies                                                   */
00070   /**********************************************************************/
00071 
00072   struct Object;
00073   typedef boost::shared_ptr<Object> ObjectPtr;
00074   typedef boost::shared_ptr<Object> ObjectConstPtr;
00075 
00077   struct Object
00078   {
00079     Object(const std::string& id) : id_(id)
00080     {
00081     }
00082 
00083     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00084 
00086     std::string id_;
00087 
00099     std::vector<shapes::ShapeConstPtr> shapes_;
00100 
00104     EigenSTL::vector_Affine3d shape_poses_;
00105   };
00106 
00108   std::vector<std::string> getObjectIds() const;
00109 
00111   ObjectConstPtr getObject(const std::string& id) const;
00112 
00114   typedef std::map<std::string, ObjectConstPtr>::const_iterator const_iterator;
00116   const_iterator begin() const
00117   {
00118     return objects_.begin();
00119   }
00121   const_iterator end() const
00122   {
00123     return objects_.end();
00124   }
00126   std::size_t size() const
00127   {
00128     return objects_.size();
00129   }
00131   const_iterator find(const std::string& id) const
00132   {
00133     return objects_.find(id);
00134   }
00135 
00137   bool hasObject(const std::string& id) const;
00138 
00144   void addToObject(const std::string& id, const std::vector<shapes::ShapeConstPtr>& shapes,
00145                    const EigenSTL::vector_Affine3d& poses);
00146 
00151   void addToObject(const std::string& id, const shapes::ShapeConstPtr& shape, const Eigen::Affine3d& pose);
00152 
00155   bool moveShapeInObject(const std::string& id, const shapes::ShapeConstPtr& shape, const Eigen::Affine3d& pose);
00156 
00163   bool removeShapeFromObject(const std::string& id, const shapes::ShapeConstPtr& shape);
00164 
00169   bool removeObject(const std::string& id);
00170 
00174   void clearObjects();
00175 
00176   enum ActionBits
00177   {
00178     UNINITIALIZED = 0,
00179     CREATE = 1,        
00180     DESTROY = 2,       
00181     MOVE_SHAPE = 4,    
00182     ADD_SHAPE = 8,     
00183     REMOVE_SHAPE = 16, 
00184   };
00185 
00189   class Action
00190   {
00191   public:
00192     Action() : action_(UNINITIALIZED)
00193     {
00194     }
00195     Action(int v) : action_(v)
00196     {
00197     }
00198     operator ActionBits() const
00199     {
00200       return ActionBits(action_);
00201     }
00202 
00203   private:
00204     int action_;
00205   };
00206 
00207 private:
00208   class Observer;
00209 
00210 public:
00211   class ObserverHandle
00212   {
00213   public:
00214     ObserverHandle() : observer_(NULL)
00215     {
00216     }
00217 
00218   private:
00219     ObserverHandle(const Observer* o) : observer_(o)
00220     {
00221     }
00222     const Observer* observer_;
00223     friend class World;
00224   };
00225 
00226   typedef boost::function<void(const ObjectConstPtr&, Action)> ObserverCallbackFn;
00227 
00232   ObserverHandle addObserver(const ObserverCallbackFn& callback);
00233 
00235   void removeObserver(const ObserverHandle observer_handle);
00236 
00239   void notifyObserverAllObjects(const ObserverHandle observer_handle, Action action) const;
00240 
00241 private:
00243   void notify(const ObjectConstPtr&, Action);
00244 
00246   void notifyAll(Action action);
00247 
00251   void ensureUnique(ObjectPtr& obj);
00252 
00253   /* Add a shape with no checking */
00254   virtual void addToObjectInternal(const ObjectPtr& obj, const shapes::ShapeConstPtr& shape,
00255                                    const Eigen::Affine3d& pose);
00256 
00258   std::map<std::string, ObjectPtr> objects_;
00259 
00260   /* observers to call when something changes */
00261   class Observer
00262   {
00263   public:
00264     Observer(const ObserverCallbackFn& callback) : callback_(callback)
00265     {
00266     }
00267     ObserverCallbackFn callback_;
00268   };
00269   std::vector<Observer*> observers_;
00270 };
00271 }
00272 
00273 #endif


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jan 17 2018 03:31:36