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/class_forward.h>
#include <moveit_msgs/PlanningScene.h>
#include <moveit_msgs/RobotTrajectory.h>
#include <moveit_msgs/Constraints.h>
#include <moveit_msgs/PlanningSceneComponents.h>
#include <octomap_msgs/OctomapWithPose.h>
#include <boost/noncopyable.hpp>
#include <boost/function.hpp>
#include <boost/concept_check.hpp>
#include <memory>

namespace planning_scene
{
MOVEIT_CLASS_FORWARD(PlanningScene);  // Defines PlanningScenePtr, ConstPtr, WeakPtr... etc

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 std::enable_shared_from_this<PlanningScene>
{
public:
  PlanningScene(
      const robot_model::RobotModelConstPtr& robot_model,
      const collision_detection::WorldPtr& world = collision_detection::WorldPtr(new collision_detection::World()));

  PlanningScene(
      const urdf::ModelInterfaceSharedPtr& urdf_model, const srdf::ModelConstSharedPtr& srdf_model,
      const 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 robot_model_;
  }

  const robot_state::RobotState& getCurrentState() const
  {
    // if we have an updated state, return it; otherwise, return the parent one
    return robot_state_ ? *robot_state_ : 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 scene_transforms_ ? scene_transforms_->getTargetFrame() : parent_->getPlanningFrame();
  }

  const robot_state::Transforms& getTransforms() const
  {
    if (scene_transforms_ || !parent_)
    {
      return *scene_transforms_;
    }

    // if this planning scene is a child of another, and doesn't have its own custom transforms
    return parent_->getTransforms();
  }

  const robot_state::Transforms& getTransforms();

  robot_state::Transforms& getTransformsNonConst();

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

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

  const Eigen::Isometry3d& getFrameTransform(robot_state::RobotState& state, const std::string& id) const
  {
    state.updateLinkTransforms();
    return getFrameTransform(static_cast<const robot_state::RobotState&>(state), id);
  }

  const Eigen::Isometry3d& 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(static_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& robot_state) const
  {
    robot_state.updateCollisionBodyTransforms();
    checkCollision(req, res, static_cast<const robot_state::RobotState&>(robot_state));
  }

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

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

  void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
                      const robot_state::RobotState& robot_state,
                      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& robot_state) const
  {
    checkCollisionUnpadded(req, res, robot_state, getAllowedCollisionMatrix());
  }

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

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

  void checkCollisionUnpadded(const collision_detection::CollisionRequest& req,
                              collision_detection::CollisionResult& res, const robot_state::RobotState& robot_state,
                              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& robot_state) const
  {
    robot_state.updateCollisionBodyTransforms();
    checkSelfCollision(req, res, static_cast<const robot_state::RobotState&>(robot_state), getAllowedCollisionMatrix());
  }

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

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

  void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
                          const robot_state::RobotState& robot_state,
                          const collision_detection::AllowedCollisionMatrix& acm) const
  {
    // do self-collision checking with the unpadded version of the robot
    getCollisionRobotUnpadded()->checkSelfCollision(req, res, robot_state, 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& robot_state) const
  {
    robot_state.updateCollisionBodyTransforms();
    getCollidingLinks(links, static_cast<const robot_state::RobotState&>(robot_state), getAllowedCollisionMatrix());
  }

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

  void getCollidingLinks(std::vector<std::string>& links, robot_state::RobotState& robot_state,
                         const collision_detection::AllowedCollisionMatrix& acm) const
  {
    robot_state.updateCollisionBodyTransforms();
    getCollidingLinks(links, static_cast<const robot_state::RobotState&>(robot_state), acm);
  }

  void getCollidingLinks(std::vector<std::string>& links, const robot_state::RobotState& robot_state,
                         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& robot_state) const
  {
    getCollidingPairs(contacts, robot_state, getAllowedCollisionMatrix());
  }

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

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

  void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts,
                         const robot_state::RobotState& robot_state,
                         const collision_detection::AllowedCollisionMatrix& acm) const;

  double distanceToCollision(robot_state::RobotState& robot_state) const
  {
    robot_state.updateCollisionBodyTransforms();
    return distanceToCollision(static_cast<const robot_state::RobotState&>(robot_state));
  }

  double distanceToCollision(const robot_state::RobotState& robot_state) const
  {
    return getCollisionWorld()->distanceRobot(*getCollisionRobot(), robot_state, getAllowedCollisionMatrix());
  }

  double distanceToCollisionUnpadded(robot_state::RobotState& robot_state) const
  {
    robot_state.updateCollisionBodyTransforms();
    return distanceToCollisionUnpadded(static_cast<const robot_state::RobotState&>(robot_state));
  }

  double distanceToCollisionUnpadded(const robot_state::RobotState& robot_state) const
  {
    return getCollisionWorld()->distanceRobot(*getCollisionRobotUnpadded(), robot_state, getAllowedCollisionMatrix());
  }

  double distanceToCollision(robot_state::RobotState& robot_state,
                             const collision_detection::AllowedCollisionMatrix& acm) const
  {
    robot_state.updateCollisionBodyTransforms();
    return distanceToCollision(static_cast<const robot_state::RobotState&>(robot_state), acm);
  }

  double distanceToCollision(const robot_state::RobotState& robot_state,
                             const collision_detection::AllowedCollisionMatrix& acm) const
  {
    return getCollisionWorld()->distanceRobot(*getCollisionRobot(), robot_state, acm);
  }

  double distanceToCollisionUnpadded(robot_state::RobotState& robot_state,
                                     const collision_detection::AllowedCollisionMatrix& acm) const
  {
    robot_state.updateCollisionBodyTransforms();
    return distanceToCollisionUnpadded(static_cast<const robot_state::RobotState&>(robot_state), acm);
  }

  double distanceToCollisionUnpadded(const robot_state::RobotState& robot_state,
                                     const collision_detection::AllowedCollisionMatrix& acm) const
  {
    return getCollisionWorld()->distanceRobot(*getCollisionRobotUnpadded(), robot_state, acm);
  }

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

  bool loadGeometryFromStream(std::istream& in);

  bool loadGeometryFromStream(std::istream& in, const Eigen::Isometry3d& offset);

  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;

  bool getCollisionObjectMsg(moveit_msgs::CollisionObject& collision_obj, const std::string& ns) const;

  void getCollisionObjectMsgs(std::vector<moveit_msgs::CollisionObject>& collision_objs) const;

  bool getAttachedCollisionObjectMsg(moveit_msgs::AttachedCollisionObject& attached_collision_obj,
                                     const std::string& ns) const;

  void getAttachedCollisionObjectMsgs(std::vector<moveit_msgs::AttachedCollisionObject>& attached_collision_objs) const;

  bool getOctomapMsg(octomap_msgs::OctomapWithPose& octomap) const;

  void getObjectColorMsgs(std::vector<moveit_msgs::ObjectColor>& object_colors) const;

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

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

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

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

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

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

  void removeAllCollisionObjects();

  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 = nullptr) 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 = nullptr) 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 = nullptr) 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 = nullptr) 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 = nullptr) 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 = nullptr) 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 = nullptr) const;

  bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory, const std::string& group = "",
                   bool verbose = false, std::vector<std::size_t>* invalid_index = nullptr) 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 = std::cout) 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 urdf::ModelInterfaceSharedPtr& urdf_model,
                                                     const srdf::ModelConstSharedPtr& srdf_model);

  /* Helper functions for processing collision objects */
  bool processCollisionObjectAdd(const moveit_msgs::CollisionObject& object);
  bool processCollisionObjectRemove(const moveit_msgs::CollisionObject& object);
  bool processCollisionObjectMove(const moveit_msgs::CollisionObject& object);

  static void poseMsgToEigen(const geometry_msgs::Pose& msg, Eigen::Isometry3d& out);

  MOVEIT_STRUCT_FORWARD(CollisionDetector);

  /* \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 robot_model_;  // Never null (may point to same model as parent)

  robot_state::RobotStatePtr robot_state_;  // if NULL use parent's

  // Called when changes are made to attached bodies
  robot_state::AttachedBodyCallback current_state_attached_body_callback_;

  // This variable is not necessarily used by child planning scenes
  // This Transforms class is actually a SceneTransforms class
  robot_state::TransformsPtr scene_transforms_;  // 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_;

  std::unique_ptr<ObjectColorMap> object_colors_;

  // a map of object types
  std::unique_ptr<ObjectTypeMap> object_types_;
};
}  // namespace planning_scene

#endif