world.h

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2013, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Ioan Sucan, Acorn Pooley, Sachin Chitta */

#ifndef MOVEIT_COLLISION_DETECTION_WORLD_
#define MOVEIT_COLLISION_DETECTION_WORLD_

#include <string>
#include <vector>
#include <map>
#include <boost/shared_ptr.hpp>
#include <boost/function.hpp>
#include <Eigen/Geometry>
#include <eigen_stl_containers/eigen_stl_vector_container.h>
#include <geometric_shapes/shapes.h>

namespace collision_detection
{

  class World
  {
  public:

    World();

    World(const World &other);

    ~World();

    /**********************************************************************/
    /* Collision Bodies                                                   */
    /**********************************************************************/

    struct Object
    {
      Object(const std::string &id) : id_(id) {}

      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

      std::string                         id_;

      std::vector<shapes::ShapeConstPtr> shapes_;

      EigenSTL::vector_Affine3d          shape_poses_;
    };

    typedef boost::shared_ptr<Object> ObjectPtr;
    typedef boost::shared_ptr<Object> ObjectConstPtr;

    std::vector<std::string> getObjectIds() const;

    ObjectConstPtr getObject(const std::string &id) const;

    typedef std::map<std::string, ObjectConstPtr>::const_iterator const_iterator;
    const_iterator begin() const
    {
      return objects_.begin();
    }
    const_iterator end() const
    {
      return objects_.end();
    }
    std::size_t size() const
    {
      return objects_.size();
    }
    const_iterator find(const std::string& id) const
    {
      return objects_.find(id);
    }


    bool hasObject(const std::string &id) const;

    void addToObject(const std::string &id,
                     const std::vector<shapes::ShapeConstPtr> &shapes,
                     const EigenSTL::vector_Affine3d &poses);

    void addToObject(const std::string &id,
                     const shapes::ShapeConstPtr &shape,
                     const Eigen::Affine3d &pose);

    bool moveShapeInObject(const std::string &id,
                           const shapes::ShapeConstPtr &shape,
                           const Eigen::Affine3d &pose);

    bool removeShapeFromObject(const std::string &id,
                               const shapes::ShapeConstPtr &shape);

    bool removeObject(const std::string &id);

    void clearObjects();

    enum ActionBits
    {
      UNINITIALIZED = 0,
      CREATE = 1,         
      DESTROY = 2,        
      MOVE_SHAPE = 4,     
      ADD_SHAPE = 8,      
      REMOVE_SHAPE = 16,  
    };

    class Action
    {
    public:
      Action() : action_(UNINITIALIZED) {}
      Action(int v) : action_(v) {}
      operator ActionBits() const { return ActionBits(action_); }
    private:
      int action_;
    };

  private:
    class Observer;
  public:
    class ObserverHandle
    {
    public:
      ObserverHandle() : observer_(NULL) {}
    private:
      ObserverHandle(const Observer* o) : observer_(o) {}
      const Observer *observer_;
      friend class World;
    };

    typedef boost::function<void (const ObjectConstPtr&, Action)> ObserverCallbackFn;

    ObserverHandle addObserver(const ObserverCallbackFn &callback);

    void removeObserver(const ObserverHandle observer_handle);

    void notifyObserverAllObjects(const ObserverHandle observer_handle, Action action) const;

  private:

    void notify(const ObjectConstPtr&, Action);

    void notifyAll(Action action);


    void ensureUnique(ObjectPtr &obj);

    /* Add a shape with no checking */
    virtual void addToObjectInternal(const ObjectPtr &obj,
                                     const shapes::ShapeConstPtr &shape,
                                     const Eigen::Affine3d &pose);


    std::map<std::string, ObjectPtr> objects_;

    /* observers to call when something changes */
    class Observer
    {
    public:
      Observer(const ObserverCallbackFn &callback) :
        callback_(callback)
      {}
      ObserverCallbackFn callback_;
    };
    std::vector<Observer*> observers_;

  };

  typedef boost::shared_ptr<World> WorldPtr;
  typedef boost::shared_ptr<const World> WorldConstPtr;

}


#endif