planning_scene.h

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011, 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 */

#ifndef MOVEIT_PLANNING_SCENE_PLANNING_SCENE_
#define MOVEIT_PLANNING_SCENE_PLANNING_SCENE_

#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit/transforms/transforms.h>
#include <moveit/collision_detection/collision_detector_allocator.h>
#include <moveit/collision_detection/world_diff.h>
#include <moveit/kinematic_constraints/kinematic_constraint.h>
#include <moveit/kinematics_base/kinematics_base.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/macros/deprecation.h>
#include <moveit_msgs/PlanningScene.h>
#include <moveit_msgs/RobotTrajectory.h>
#include <moveit_msgs/Constraints.h>
#include <moveit_msgs/PlanningSceneComponents.h>
#include <boost/enable_shared_from_this.hpp>
#include <boost/noncopyable.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/function.hpp>
#include <boost/concept_check.hpp>

namespace planning_scene
{

class PlanningScene;
typedef boost::shared_ptr<PlanningScene> PlanningScenePtr;
typedef boost::shared_ptr<const PlanningScene> PlanningSceneConstPtr;

typedef boost::function<bool(const robot_state::RobotState&, bool)> StateFeasibilityFn;

typedef boost::function<bool(const robot_state::RobotState&, const robot_state::RobotState&, bool)> MotionFeasibilityFn;

typedef std::map<std::string, std_msgs::ColorRGBA> ObjectColorMap;

typedef std::map<std::string, object_recognition_msgs::ObjectType> ObjectTypeMap;

class PlanningScene : private boost::noncopyable,
                      public boost::enable_shared_from_this<PlanningScene>
{
public:

  PlanningScene(const robot_model::RobotModelConstPtr &robot_model,
                collision_detection::WorldPtr world = collision_detection::WorldPtr(new collision_detection::World()));

  PlanningScene(const boost::shared_ptr<const urdf::ModelInterface> &urdf_model,
                const boost::shared_ptr<const srdf::Model> &srdf_model,
                collision_detection::WorldPtr world = collision_detection::WorldPtr(new collision_detection::World()));

  static const std::string OCTOMAP_NS;
  static const std::string DEFAULT_SCENE_NAME;

  ~PlanningScene();

  const std::string& getName() const
  {
    return name_;
  }

  void setName(const std::string &name)
  {
    name_ = name;
  }

  PlanningScenePtr diff() const;

  PlanningScenePtr diff(const moveit_msgs::PlanningScene &msg) const;

  const PlanningSceneConstPtr& getParent() const
  {
    return parent_;
  }

  const robot_model::RobotModelConstPtr& getRobotModel() const
  {
    // the kinematic model does not change
    return kmodel_;
  }

  const robot_state::RobotState& getCurrentState() const
  {
    // if we have an updated state, return it; otherwise, return the parent one
    return kstate_ ? *kstate_ : parent_->getCurrentState();
  }
  robot_state::RobotState& getCurrentStateNonConst();

  robot_state::RobotStatePtr getCurrentStateUpdated(const moveit_msgs::RobotState &update) const;

  const std::string& getPlanningFrame() const
  {
    // if we have an updated set of transforms, return it; otherwise, return the parent one
    return ftf_ ? ftf_->getTargetFrame() : parent_->getPlanningFrame();
  }

  const robot_state::Transforms& getTransforms() const
  {
    // if we have updated transforms, return those
    return (ftf_ || !parent_) ? *ftf_ : parent_->getTransforms();
  }

  const robot_state::Transforms& getTransforms();
  
  robot_state::Transforms& getTransformsNonConst();

  const Eigen::Affine3d& getFrameTransform(const std::string &id) const;

  const Eigen::Affine3d& getFrameTransform(const std::string &id);

  const Eigen::Affine3d& getFrameTransform(robot_state::RobotState &state, const std::string &id) const
  {
    state.updateLinkTransforms();
    return getFrameTransform(const_cast<const robot_state::RobotState&>(state), id);
  }
  
  const Eigen::Affine3d& getFrameTransform(const robot_state::RobotState &state, const std::string &id) const;
  
  bool knowsFrameTransform(const std::string &id) const;

  bool knowsFrameTransform(const robot_state::RobotState &state, const std::string &id) const;

  void addCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr& allocator);

  void setActiveCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr& allocator,
                                  bool exclusive = false);

  bool setActiveCollisionDetector(const std::string& collision_detector_name);

  const std::string& getActiveCollisionDetectorName() const
  {
    return active_collision_->alloc_->getName();
  }

  void getCollisionDetectorNames(std::vector<std::string>& names) const;

  const collision_detection::WorldConstPtr& getWorld() const
  {
    // we always have a world representation
    return world_const_;
  }

  //brief Get the representation of the world
  const collision_detection::WorldPtr& getWorldNonConst()
  {
    // we always have a world representation
    return world_;
  }

  const collision_detection::CollisionWorldConstPtr& getCollisionWorld() const
  {
    // we always have a world representation after configure is called.
    return active_collision_->cworld_const_;
  }

  const collision_detection::CollisionRobotConstPtr& getCollisionRobot() const
  {
    return active_collision_->getCollisionRobot();
  }

  const collision_detection::CollisionRobotConstPtr& getCollisionRobotUnpadded() const
  {
    return active_collision_->getCollisionRobotUnpadded();
  }

  const collision_detection::CollisionWorldConstPtr& getCollisionWorld(const std::string& collision_detector_name) const;

  const collision_detection::CollisionRobotConstPtr& getCollisionRobot(const std::string& collision_detector_name) const;

  const collision_detection::CollisionRobotConstPtr& getCollisionRobotUnpadded(const std::string& collision_detector_name) const;

  const collision_detection::CollisionRobotPtr& getCollisionRobotNonConst();

  void propogateRobotPadding();

  const collision_detection::AllowedCollisionMatrix& getAllowedCollisionMatrix() const
  {
    return acm_ ? *acm_ : parent_->getAllowedCollisionMatrix();
  }
  collision_detection::AllowedCollisionMatrix& getAllowedCollisionMatrixNonConst();

  bool isStateColliding(const std::string &group = "", bool verbose = false);

  bool isStateColliding(const std::string &group = "", bool verbose = false) const
  {
    return isStateColliding(getCurrentState(), group, verbose);
  }
  
  bool isStateColliding(robot_state::RobotState &state, const std::string &group = "", bool verbose = false) const
  {
    state.updateCollisionBodyTransforms();
    return isStateColliding(const_cast<const robot_state::RobotState&>(state), group, verbose);
  }

  bool isStateColliding(const robot_state::RobotState &state, const std::string &group = "", bool verbose = false) const;

  bool isStateColliding(const moveit_msgs::RobotState &state, const std::string &group = "", bool verbose = false) const;

  void checkCollision(const collision_detection::CollisionRequest& req,
                      collision_detection::CollisionResult &res);
  
  void checkCollision(const collision_detection::CollisionRequest& req,
                      collision_detection::CollisionResult &res) const
  {
    checkCollision(req, res, getCurrentState());
  }
  
  void checkCollision(const collision_detection::CollisionRequest& req,
                      collision_detection::CollisionResult &res,
                      robot_state::RobotState &kstate) const
  {
    kstate.updateCollisionBodyTransforms();
    checkCollision(req, res, const_cast<const robot_state::RobotState&>(kstate));
  }

  void checkCollision(const collision_detection::CollisionRequest& req,
                      collision_detection::CollisionResult &res,
                      const robot_state::RobotState &kstate) const;
  
  void checkCollision(const collision_detection::CollisionRequest& req,
                      collision_detection::CollisionResult &res,
                      robot_state::RobotState &kstate,
                      const collision_detection::AllowedCollisionMatrix& acm) const
  {
    kstate.updateCollisionBodyTransforms();
    checkCollision(req, res, const_cast<const robot_state::RobotState&>(kstate), acm);
  }

  void checkCollision(const collision_detection::CollisionRequest& req,
                      collision_detection::CollisionResult &res,
                      const robot_state::RobotState &kstate,
                      const collision_detection::AllowedCollisionMatrix& acm) const;

  void checkCollisionUnpadded(const collision_detection::CollisionRequest& req,
                              collision_detection::CollisionResult &res);
  
  void checkCollisionUnpadded(const collision_detection::CollisionRequest& req,
                              collision_detection::CollisionResult &res) const
  {
    checkCollisionUnpadded(req, res, getCurrentState(), getAllowedCollisionMatrix());
  }
  
  void checkCollisionUnpadded(const collision_detection::CollisionRequest& req,
                              collision_detection::CollisionResult &res,
                              const robot_state::RobotState &kstate) const
  {
    checkCollisionUnpadded(req, res, kstate, getAllowedCollisionMatrix());
  }

  void checkCollisionUnpadded(const collision_detection::CollisionRequest& req,
                              collision_detection::CollisionResult &res,
                              robot_state::RobotState &kstate) const
  {
    kstate.updateCollisionBodyTransforms();
    checkCollisionUnpadded(req, res, const_cast<const robot_state::RobotState&>(kstate), getAllowedCollisionMatrix());
  }
  
  void checkCollisionUnpadded(const collision_detection::CollisionRequest& req,
                              collision_detection::CollisionResult &res,
                              robot_state::RobotState &kstate,
                              const collision_detection::AllowedCollisionMatrix& acm) const
  {
    kstate.updateCollisionBodyTransforms();
    checkCollisionUnpadded(req, res, const_cast<const robot_state::RobotState&>(kstate), acm);
  }

  void checkCollisionUnpadded(const collision_detection::CollisionRequest& req,
                              collision_detection::CollisionResult &res,
                              const robot_state::RobotState &kstate,
                              const collision_detection::AllowedCollisionMatrix& acm) const;

  void checkSelfCollision(const collision_detection::CollisionRequest& req,
                          collision_detection::CollisionResult &res);
  
  void checkSelfCollision(const collision_detection::CollisionRequest& req,
                          collision_detection::CollisionResult &res) const
  {
    checkSelfCollision(req, res, getCurrentState());
  }

  void checkSelfCollision(const collision_detection::CollisionRequest& req,
                          collision_detection::CollisionResult &res,
                          robot_state::RobotState &kstate) const
  { 
    kstate.updateCollisionBodyTransforms();
    checkSelfCollision(req, res, const_cast<const robot_state::RobotState&>(kstate), getAllowedCollisionMatrix());
  }
  
  void checkSelfCollision(const collision_detection::CollisionRequest& req,
                          collision_detection::CollisionResult &res,
                          const robot_state::RobotState &kstate) const
  {
    // do self-collision checking with the unpadded version of the robot
    getCollisionRobotUnpadded()->checkSelfCollision(req, res, kstate, getAllowedCollisionMatrix());
  }

  void checkSelfCollision(const collision_detection::CollisionRequest& req,
                          collision_detection::CollisionResult &res,
                          robot_state::RobotState &kstate,
                          const collision_detection::AllowedCollisionMatrix& acm) const
  {
    kstate.updateCollisionBodyTransforms();
    checkSelfCollision(req, res, const_cast<const robot_state::RobotState&>(kstate), acm);
  }

  void checkSelfCollision(const collision_detection::CollisionRequest& req,
                          collision_detection::CollisionResult &res,
                          const robot_state::RobotState &kstate,
                          const collision_detection::AllowedCollisionMatrix& acm) const
  {
    // do self-collision checking with the unpadded version of the robot
    getCollisionRobotUnpadded()->checkSelfCollision(req, res, kstate, acm);
  }

  void getCollidingLinks(std::vector<std::string> &links);
  
  void getCollidingLinks(std::vector<std::string> &links) const
  {
    getCollidingLinks(links, getCurrentState(), getAllowedCollisionMatrix());
  }
  
  void getCollidingLinks(std::vector<std::string> &links, robot_state::RobotState &kstate) const
  {
    kstate.updateCollisionBodyTransforms();
    getCollidingLinks(links, const_cast<const robot_state::RobotState&>(kstate), getAllowedCollisionMatrix());
  }

  void getCollidingLinks(std::vector<std::string> &links, const robot_state::RobotState &kstate) const
  {
    getCollidingLinks(links, kstate, getAllowedCollisionMatrix());
  }

  void getCollidingLinks(std::vector<std::string> &links,
                         robot_state::RobotState &kstate,
                         const collision_detection::AllowedCollisionMatrix& acm) const
  {   
    kstate.updateCollisionBodyTransforms();
    getCollidingLinks(links, const_cast<const robot_state::RobotState&>(kstate), acm);
  }
  
  void getCollidingLinks(std::vector<std::string> &links,
                         const robot_state::RobotState &kstate,
                         const collision_detection::AllowedCollisionMatrix& acm) const;
  
  void getCollidingPairs(collision_detection::CollisionResult::ContactMap &contacts);
  
  void getCollidingPairs(collision_detection::CollisionResult::ContactMap &contacts) const
  {
    getCollidingPairs(contacts, getCurrentState(), getAllowedCollisionMatrix());
  }
  
  void getCollidingPairs(collision_detection::CollisionResult::ContactMap &contacts,
                         const robot_state::RobotState &kstate) const
  {
    getCollidingPairs(contacts, kstate, getAllowedCollisionMatrix());
  }

  void getCollidingPairs(collision_detection::CollisionResult::ContactMap &contacts,
                         robot_state::RobotState &kstate) const
  { 
    kstate.updateCollisionBodyTransforms();
    getCollidingPairs(contacts, const_cast<const robot_state::RobotState&>(kstate), getAllowedCollisionMatrix());
  }

  void getCollidingPairs(collision_detection::CollisionResult::ContactMap &contacts,
                         robot_state::RobotState &kstate,
                         const collision_detection::AllowedCollisionMatrix& acm) const
  { 
    kstate.updateCollisionBodyTransforms();
    getCollidingPairs(contacts, const_cast<const robot_state::RobotState&>(kstate), acm);
  }
  
  void getCollidingPairs(collision_detection::CollisionResult::ContactMap &contacts,
                         const robot_state::RobotState &kstate,
                         const collision_detection::AllowedCollisionMatrix& acm) const;

  double distanceToCollision(robot_state::RobotState &kstate) const
  {
    kstate.updateCollisionBodyTransforms();
    return distanceToCollision(const_cast<const robot_state::RobotState&>(kstate));
  }  
  
  double distanceToCollision(const robot_state::RobotState &kstate) const
  {
    return getCollisionWorld()->distanceRobot(*getCollisionRobot(), kstate, getAllowedCollisionMatrix());
  }  
  
  double distanceToCollisionUnpadded(robot_state::RobotState &kstate) const
  {
    kstate.updateCollisionBodyTransforms();
    return distanceToCollisionUnpadded(const_cast<const robot_state::RobotState&>(kstate));
  }
  
  double distanceToCollisionUnpadded(const robot_state::RobotState &kstate) const
  {
    return getCollisionWorld()->distanceRobot(*getCollisionRobotUnpadded(), kstate, getAllowedCollisionMatrix());
  }
  
  double distanceToCollision(robot_state::RobotState &kstate, const collision_detection::AllowedCollisionMatrix& acm) const
  {
    kstate.updateCollisionBodyTransforms();
    return distanceToCollision(const_cast<const robot_state::RobotState&>(kstate), acm);
  }

  double distanceToCollision(const robot_state::RobotState &kstate, const collision_detection::AllowedCollisionMatrix& acm) const
  {
    return getCollisionWorld()->distanceRobot(*getCollisionRobot(), kstate, acm);
  }
  
  double distanceToCollisionUnpadded(robot_state::RobotState &kstate, const collision_detection::AllowedCollisionMatrix& acm) const
  {
    kstate.updateCollisionBodyTransforms();
    return distanceToCollisionUnpadded(const_cast<const robot_state::RobotState&>(kstate), acm);
  }

  double distanceToCollisionUnpadded(const robot_state::RobotState &kstate, const collision_detection::AllowedCollisionMatrix& acm) const
  {
    return getCollisionWorld()->distanceRobot(*getCollisionRobotUnpadded(), kstate, acm);
  }
  
  void saveGeometryToStream(std::ostream &out) const;
  
  void loadGeometryFromStream(std::istream &in);
  
  void getPlanningSceneDiffMsg(moveit_msgs::PlanningScene &scene) const;

  void getPlanningSceneMsg(moveit_msgs::PlanningScene &scene) const;

  void getPlanningSceneMsg(moveit_msgs::PlanningScene &scene, const moveit_msgs::PlanningSceneComponents &comp) const;

  void setPlanningSceneDiffMsg(const moveit_msgs::PlanningScene &scene);

  void setPlanningSceneMsg(const moveit_msgs::PlanningScene &scene);

  void usePlanningSceneMsg(const moveit_msgs::PlanningScene &scene);

  bool processCollisionObjectMsg(const moveit_msgs::CollisionObject &object);
  bool processAttachedCollisionObjectMsg(const moveit_msgs::AttachedCollisionObject &object);

  void processPlanningSceneWorldMsg(const moveit_msgs::PlanningSceneWorld &world);

  void processOctomapMsg(const octomap_msgs::OctomapWithPose &map);
  void processOctomapMsg(const octomap_msgs::Octomap &map);
  void processOctomapPtr(const boost::shared_ptr<const octomap::OcTree> &octree, const Eigen::Affine3d &t);

  void setCurrentState(const moveit_msgs::RobotState &state);

  void setCurrentState(const robot_state::RobotState &state);

  void setAttachedBodyUpdateCallback(const robot_state::AttachedBodyCallback &callback);

  void setCollisionObjectUpdateCallback(const collision_detection::World::ObserverCallbackFn &callback);

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

  const std_msgs::ColorRGBA& getObjectColor(const std::string &id) const;
  void setObjectColor(const std::string &id, const std_msgs::ColorRGBA &color);
  void removeObjectColor(const std::string &id);
  void getKnownObjectColors(ObjectColorMap &kc) const;

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

  const object_recognition_msgs::ObjectType& getObjectType(const std::string &id) const;
  void setObjectType(const std::string &id, const object_recognition_msgs::ObjectType &type);
  void removeObjectType(const std::string &id);
  void getKnownObjectTypes(ObjectTypeMap &kc) const;

  void clearDiffs();

  void pushDiffs(const PlanningScenePtr &scene);

  void decoupleParent();

  void setStateFeasibilityPredicate(const StateFeasibilityFn &fn)
  {
    state_feasibility_ = fn;
  }

  const StateFeasibilityFn& getStateFeasibilityPredicate() const
  {
    return state_feasibility_;
  }

  void setMotionFeasibilityPredicate(const MotionFeasibilityFn &fn)
  {
    motion_feasibility_ = fn;
  }

  const MotionFeasibilityFn& getMotionFeasibilityPredicate() const
  {
    return motion_feasibility_;
  }

  bool isStateFeasible(const moveit_msgs::RobotState &state, bool verbose = false) const;

  bool isStateFeasible(const robot_state::RobotState &state, bool verbose = false) const;

  bool isStateConstrained(const moveit_msgs::RobotState &state, const moveit_msgs::Constraints &constr, bool verbose = false) const;

  bool isStateConstrained(const robot_state::RobotState &state, const moveit_msgs::Constraints &constr, bool verbose = false) const;

  bool isStateConstrained(const moveit_msgs::RobotState &state,  const kinematic_constraints::KinematicConstraintSet &constr, bool verbose = false) const;

  bool isStateConstrained(const robot_state::RobotState &state,  const kinematic_constraints::KinematicConstraintSet &constr, bool verbose = false) const;

  bool isStateValid(const moveit_msgs::RobotState &state, const std::string &group = "", bool verbose = false) const;

  bool isStateValid(const robot_state::RobotState &state, const std::string &group = "", bool verbose = false) const;

  bool isStateValid(const moveit_msgs::RobotState &state, const moveit_msgs::Constraints &constr, const std::string &group = "", bool verbose = false) const;

  bool isStateValid(const robot_state::RobotState &state, const moveit_msgs::Constraints &constr, const std::string &group = "", bool verbose = false) const;

  bool isStateValid(const robot_state::RobotState &state, const kinematic_constraints::KinematicConstraintSet &constr, const std::string &group = "", bool verbose = false) const;

  bool isPathValid(const moveit_msgs::RobotState &start_state,
                   const moveit_msgs::RobotTrajectory &trajectory,
                   const std::string &group = "", bool verbose = false, std::vector<std::size_t> *invalid_index = NULL) const;

  bool isPathValid(const moveit_msgs::RobotState &start_state,
                   const moveit_msgs::RobotTrajectory &trajectory,
                   const moveit_msgs::Constraints& path_constraints,
                   const std::string &group = "", bool verbose = false, std::vector<std::size_t> *invalid_index = NULL) const;

  bool isPathValid(const moveit_msgs::RobotState &start_state,
                   const moveit_msgs::RobotTrajectory &trajectory,
                   const moveit_msgs::Constraints& path_constraints,
                   const moveit_msgs::Constraints& goal_constraints,
                   const std::string &group = "", bool verbose = false, std::vector<std::size_t> *invalid_index = NULL) const;

  bool isPathValid(const moveit_msgs::RobotState &start_state,
                   const moveit_msgs::RobotTrajectory &trajectory,
                   const moveit_msgs::Constraints& path_constraints,
                   const std::vector<moveit_msgs::Constraints>& goal_constraints,
                   const std::string &group = "", bool verbose = false, std::vector<std::size_t> *invalid_index = NULL) const;

  bool isPathValid(const robot_trajectory::RobotTrajectory &trajectory,
                   const moveit_msgs::Constraints& path_constraints,
                   const std::vector<moveit_msgs::Constraints>& goal_constraints,
                   const std::string &group = "", bool verbose = false, std::vector<std::size_t> *invalid_index = NULL) const;

  bool isPathValid(const robot_trajectory::RobotTrajectory &trajectory,
                   const moveit_msgs::Constraints& path_constraints,
                   const moveit_msgs::Constraints& goal_constraints,
                   const std::string &group = "", bool verbose = false, std::vector<std::size_t> *invalid_index = NULL) const;

  bool isPathValid(const robot_trajectory::RobotTrajectory &trajectory,
                   const moveit_msgs::Constraints& path_constraints,
                   const std::string &group = "", bool verbose = false, std::vector<std::size_t> *invalid_index = NULL) const;

  bool isPathValid(const robot_trajectory::RobotTrajectory &trajectory,
                   const std::string &group = "", bool verbose = false, std::vector<std::size_t> *invalid_index = NULL) const;

  void getCostSources(const robot_trajectory::RobotTrajectory &trajectory, std::size_t max_costs,
                      std::set<collision_detection::CostSource> &costs, double overlap_fraction = 0.9) const;

  void getCostSources(const robot_trajectory::RobotTrajectory &trajectory, std::size_t max_costs,
                      const std::string &group_name, std::set<collision_detection::CostSource> &costs, double overlap_fraction = 0.9) const;

  void getCostSources(const robot_state::RobotState &state, std::size_t max_costs,
                      std::set<collision_detection::CostSource> &costs) const;

  void getCostSources(const robot_state::RobotState &state, std::size_t max_costs,
                      const std::string &group_name, std::set<collision_detection::CostSource> &costs) const;

  void printKnownObjects(std::ostream& out) const;

  static bool isEmpty(const moveit_msgs::PlanningScene &msg);

  static bool isEmpty(const moveit_msgs::PlanningSceneWorld &msg);

  static bool isEmpty(const moveit_msgs::RobotState &msg);

  static PlanningScenePtr clone(const PlanningSceneConstPtr &scene);

private:

  /* Private constructor used by the diff() methods. */
  PlanningScene(const PlanningSceneConstPtr &parent);

  /* Initialize the scene.  This should only be called by the constructors.
   * Requires a valid robot_model_ */
  void initialize();

  /* helper function to create a RobotModel from a urdf/srdf. */
  static robot_model::RobotModelPtr createRobotModel(const boost::shared_ptr<const urdf::ModelInterface> &urdf_model,
                                                     const boost::shared_ptr<const srdf::Model> &srdf_model);

  void getPlanningSceneMsgCollisionObject(moveit_msgs::PlanningScene &scene, const std::string &ns) const;
  void getPlanningSceneMsgCollisionObjects(moveit_msgs::PlanningScene &scene) const;
  void getPlanningSceneMsgOctomap(moveit_msgs::PlanningScene &scene) const;
  void getPlanningSceneMsgObjectColors(moveit_msgs::PlanningScene &scene_msg) const;

  struct CollisionDetector;
  typedef boost::shared_ptr<CollisionDetector> CollisionDetectorPtr;
  typedef boost::shared_ptr<const CollisionDetector> CollisionDetectorConstPtr;

  /* \brief A set of compatible collision detectors */
  struct CollisionDetector
  {
    collision_detection::CollisionDetectorAllocatorPtr alloc_;
    collision_detection::CollisionRobotPtr             crobot_unpadded_;        // if NULL use parent's
    collision_detection::CollisionRobotConstPtr        crobot_unpadded_const_;
    collision_detection::CollisionRobotPtr             crobot_;                 // if NULL use parent's
    collision_detection::CollisionRobotConstPtr        crobot_const_;

    collision_detection::CollisionWorldPtr             cworld_;                 // never NULL
    collision_detection::CollisionWorldConstPtr        cworld_const_;

    CollisionDetectorConstPtr                          parent_;                 // may be NULL

    const collision_detection::CollisionRobotConstPtr& getCollisionRobot() const
    {
      return crobot_const_ ? crobot_const_ : parent_->getCollisionRobot();
    }
    const collision_detection::CollisionRobotConstPtr& getCollisionRobotUnpadded() const
    {
      return crobot_unpadded_const_ ? crobot_unpadded_const_ : parent_->getCollisionRobotUnpadded();
    }
    void findParent(const PlanningScene& scene);
    void copyPadding(const CollisionDetector& src);
  };
  friend struct CollisionDetector;

  typedef std::map<std::string, CollisionDetectorPtr>::iterator CollisionDetectorIterator;
  typedef std::map<std::string, CollisionDetectorPtr>::const_iterator CollisionDetectorConstIterator;

  void allocateCollisionDetectors();
  void allocateCollisionDetectors(CollisionDetector& detector);



  std::string                                    name_;         // may be empty

  PlanningSceneConstPtr                          parent_;       // Null unless this is a diff scene

  robot_model::RobotModelConstPtr                kmodel_;       // Never null (may point to same model as parent)

  robot_state::RobotStatePtr                     kstate_;       // if NULL use parent's
  robot_state::AttachedBodyCallback              current_state_attached_body_callback_; // called when changes are made to attached bodies

  robot_state::TransformsPtr                     ftf_;          // if NULL use parent's

  collision_detection::WorldPtr                  world_;        // never NULL, never shared with parent/child
  collision_detection::WorldConstPtr             world_const_;  // copy of world_
  collision_detection::WorldDiffPtr              world_diff_;   // NULL unless this is a diff scene
  collision_detection::World::ObserverCallbackFn current_world_object_update_callback_;
  collision_detection::World::ObserverHandle     current_world_object_update_observer_handle_;

  std::map<std::string, CollisionDetectorPtr>    collision_;          // never empty
  CollisionDetectorPtr                           active_collision_;   // copy of one of the entries in collision_.  Never NULL.

  collision_detection::AllowedCollisionMatrixPtr acm_;                // if NULL use parent's

  StateFeasibilityFn                             state_feasibility_;
  MotionFeasibilityFn                            motion_feasibility_;

  boost::scoped_ptr<ObjectColorMap>              object_colors_;

  // a map of object types
  boost::scoped_ptr<ObjectTypeMap>               object_types_;


};

}


#endif