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 */
 
#pragma once
 
#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/collision_detection/collision_env.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/msg/planning_scene.hpp>
#include <moveit_msgs/msg/robot_trajectory.hpp>
#include <moveit_msgs/msg/constraints.hpp>
#include <moveit_msgs/msg/planning_scene_components.hpp>
#include <octomap_msgs/msg/octomap_with_pose.hpp>
#include <memory>
#include <functional>
#include <optional>
#include <thread>
#include <variant>
#include <rclcpp/rclcpp.hpp>
 
#include <moveit_planning_scene_export.h>
 
namespace planning_scene
{
MOVEIT_CLASS_FORWARD(PlanningScene);  // Defines PlanningScenePtr, ConstPtr, WeakPtr... etc
 
typedef std::function<bool(const moveit::core::RobotState&, bool)> StateFeasibilityFn;
 
using MotionFeasibilityFn = std::function<bool(const moveit::core::RobotState&, const moveit::core::RobotState&, bool)>;
 
using ObjectColorMap = std::map<std::string, std_msgs::msg::ColorRGBA>;
 
using ObjectTypeMap = std::map<std::string, object_recognition_msgs::msg::ObjectType>;
 
class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_from_this<PlanningScene>
{
public:
  PlanningScene(const PlanningScene&) = delete;
 
  PlanningScene& operator=(const PlanningScene&) = delete;
 
  PlanningScene(const moveit::core::RobotModelConstPtr& robot_model,
                const collision_detection::WorldPtr& world = std::make_shared<collision_detection::World>());
 
  PlanningScene(const urdf::ModelInterfaceSharedPtr& urdf_model, const srdf::ModelConstSharedPtr& srdf_model,
                const collision_detection::WorldPtr& world = std::make_shared<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::msg::PlanningScene& msg) const;
 
  const PlanningSceneConstPtr& getParent() const
  {
    return parent_;
  }
 
  const moveit::core::RobotModelConstPtr& getRobotModel() const
  {
    // the kinematic model does not change
    return robot_model_;
  }
 
  const moveit::core::RobotState& getCurrentState() const
  {
    // if we have an updated state, return it; otherwise, return the parent one
    return robot_state_ ? *robot_state_ : parent_->getCurrentState();
  }
  moveit::core::RobotState& getCurrentStateNonConst();
 
  moveit::core::RobotStatePtr getCurrentStateUpdated(const moveit_msgs::msg::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 moveit::core::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 moveit::core::Transforms& getTransforms();
 
  moveit::core::Transforms& getTransformsNonConst();
 
  const Eigen::Isometry3d& getFrameTransform(const std::string& id) const;
 
  const Eigen::Isometry3d& getFrameTransform(const std::string& id);
 
  const Eigen::Isometry3d& getFrameTransform(moveit::core::RobotState& state, const std::string& id) const
  {
    state.updateLinkTransforms();
    return getFrameTransform(static_cast<const moveit::core::RobotState&>(state), id);
  }
 
  const Eigen::Isometry3d& getFrameTransform(const moveit::core::RobotState& state, const std::string& id) const;
 
  bool knowsFrameTransform(const std::string& id) const;
 
  bool knowsFrameTransform(const moveit::core::RobotState& state, const std::string& id) const;
 
  const std::string getCollisionDetectorName() const
  {
    // If no collision detector is allocated, return an empty string
    return collision_detector_ ? (collision_detector_->alloc_->getName()) : "";
  }
 
  const collision_detection::WorldConstPtr& getWorld() const
  {
    // we always have a world representation
    return world_const_;
  }
 
  const collision_detection::WorldPtr& getWorldNonConst()
  {
    // we always have a world representation
    return world_;
  }
 
  const collision_detection::CollisionEnvConstPtr& getCollisionEnv() const
  {
    return collision_detector_->getCollisionEnv();
  }
 
  const collision_detection::CollisionEnvConstPtr& getCollisionEnvUnpadded() const
  {
    return collision_detector_->getCollisionEnvUnpadded();
  }
 
  const collision_detection::CollisionEnvConstPtr& getCollisionEnv(const std::string& collision_detector_name) const;
 
  const collision_detection::CollisionEnvConstPtr&
  getCollisionEnvUnpadded(const std::string& collision_detector_name) const;
 
  const collision_detection::CollisionEnvPtr& getCollisionEnvNonConst();
 
  const collision_detection::AllowedCollisionMatrix& getAllowedCollisionMatrix() const
  {
    return acm_ ? *acm_ : parent_->getAllowedCollisionMatrix();
  }
 
  collision_detection::AllowedCollisionMatrix& getAllowedCollisionMatrixNonConst();
 
  void setAllowedCollisionMatrix(const collision_detection::AllowedCollisionMatrix& acm);
 
  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(moveit::core::RobotState& state, const std::string& group = "", bool verbose = false) const
  {
    state.updateCollisionBodyTransforms();
    return isStateColliding(static_cast<const moveit::core::RobotState&>(state), group, verbose);
  }
 
  bool isStateColliding(const moveit::core::RobotState& state, const std::string& group = "",
                        bool verbose = false) const;
 
  bool isStateColliding(const moveit_msgs::msg::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,
                      moveit::core::RobotState& robot_state) const
  {
    robot_state.updateCollisionBodyTransforms();
    checkCollision(req, res, static_cast<const moveit::core::RobotState&>(robot_state));
  }
 
  void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
                      const moveit::core::RobotState& robot_state) const;
 
  void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
                      moveit::core::RobotState& robot_state,
                      const collision_detection::AllowedCollisionMatrix& acm) const
  {
    robot_state.updateCollisionBodyTransforms();
    checkCollision(req, res, static_cast<const moveit::core::RobotState&>(robot_state), acm);
  }
 
  void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
                      const moveit::core::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 moveit::core::RobotState& robot_state) const
  {
    checkCollisionUnpadded(req, res, robot_state, getAllowedCollisionMatrix());
  }
 
  void checkCollisionUnpadded(const collision_detection::CollisionRequest& req,
                              collision_detection::CollisionResult& res, moveit::core::RobotState& robot_state) const
  {
    robot_state.updateCollisionBodyTransforms();
    checkCollisionUnpadded(req, res, static_cast<const moveit::core::RobotState&>(robot_state),
                           getAllowedCollisionMatrix());
  }
 
  void checkCollisionUnpadded(const collision_detection::CollisionRequest& req,
                              collision_detection::CollisionResult& res, moveit::core::RobotState& robot_state,
                              const collision_detection::AllowedCollisionMatrix& acm) const
  {
    robot_state.updateCollisionBodyTransforms();
    checkCollisionUnpadded(req, res, static_cast<const moveit::core::RobotState&>(robot_state), acm);
  }
 
  void checkCollisionUnpadded(const collision_detection::CollisionRequest& req,
                              collision_detection::CollisionResult& res, const moveit::core::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,
                          moveit::core::RobotState& robot_state) const
  {
    robot_state.updateCollisionBodyTransforms();
    checkSelfCollision(req, res, static_cast<const moveit::core::RobotState&>(robot_state), getAllowedCollisionMatrix());
  }
 
  void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
                          const moveit::core::RobotState& robot_state) const
  {
    // do self-collision checking with the unpadded version of the robot
    getCollisionEnvUnpadded()->checkSelfCollision(req, res, robot_state, getAllowedCollisionMatrix());
  }
 
  void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
                          moveit::core::RobotState& robot_state,
                          const collision_detection::AllowedCollisionMatrix& acm) const
  {
    robot_state.updateCollisionBodyTransforms();
    checkSelfCollision(req, res, static_cast<const moveit::core::RobotState&>(robot_state), acm);
  }
 
  void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
                          const moveit::core::RobotState& robot_state,
                          const collision_detection::AllowedCollisionMatrix& acm) const
  {
    // do self-collision checking with the unpadded version of the robot
    getCollisionEnvUnpadded()->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, moveit::core::RobotState& robot_state) const
  {
    robot_state.updateCollisionBodyTransforms();
    getCollidingLinks(links, static_cast<const moveit::core::RobotState&>(robot_state), getAllowedCollisionMatrix());
  }
 
  void getCollidingLinks(std::vector<std::string>& links, const moveit::core::RobotState& robot_state) const
  {
    getCollidingLinks(links, robot_state, getAllowedCollisionMatrix());
  }
 
  void getCollidingLinks(std::vector<std::string>& links, moveit::core::RobotState& robot_state,
                         const collision_detection::AllowedCollisionMatrix& acm) const
  {
    robot_state.updateCollisionBodyTransforms();
    getCollidingLinks(links, static_cast<const moveit::core::RobotState&>(robot_state), acm);
  }
 
  void getCollidingLinks(std::vector<std::string>& links, const moveit::core::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 moveit::core::RobotState& robot_state, const std::string& group_name = "") const
  {
    getCollidingPairs(contacts, robot_state, getAllowedCollisionMatrix(), group_name);
  }
 
  void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts,
                         moveit::core::RobotState& robot_state, const std::string& group_name = "") const
  {
    robot_state.updateCollisionBodyTransforms();
    getCollidingPairs(contacts, static_cast<const moveit::core::RobotState&>(robot_state), getAllowedCollisionMatrix(),
                      group_name);
  }
 
  void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts,
                         moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm,
                         const std::string& group_name = "") const
  {
    robot_state.updateCollisionBodyTransforms();
    getCollidingPairs(contacts, static_cast<const moveit::core::RobotState&>(robot_state), acm, group_name);
  }
 
  void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts,
                         const moveit::core::RobotState& robot_state,
                         const collision_detection::AllowedCollisionMatrix& acm,
                         const std::string& group_name = "") const;
 
  double distanceToCollision(moveit::core::RobotState& robot_state) const
  {
    robot_state.updateCollisionBodyTransforms();
    return distanceToCollision(static_cast<const moveit::core::RobotState&>(robot_state));
  }
 
  double distanceToCollision(const moveit::core::RobotState& robot_state) const
  {
    return getCollisionEnv()->distanceRobot(robot_state, getAllowedCollisionMatrix());
  }
 
  double distanceToCollisionUnpadded(moveit::core::RobotState& robot_state) const
  {
    robot_state.updateCollisionBodyTransforms();
    return distanceToCollisionUnpadded(static_cast<const moveit::core::RobotState&>(robot_state));
  }
 
  double distanceToCollisionUnpadded(const moveit::core::RobotState& robot_state) const
  {
    return getCollisionEnvUnpadded()->distanceRobot(robot_state, getAllowedCollisionMatrix());
  }
 
  double distanceToCollision(moveit::core::RobotState& robot_state,
                             const collision_detection::AllowedCollisionMatrix& acm) const
  {
    robot_state.updateCollisionBodyTransforms();
    return distanceToCollision(static_cast<const moveit::core::RobotState&>(robot_state), acm);
  }
 
  double distanceToCollision(const moveit::core::RobotState& robot_state,
                             const collision_detection::AllowedCollisionMatrix& acm) const
  {
    return getCollisionEnv()->distanceRobot(robot_state, acm);
  }
 
  double distanceToCollisionUnpadded(moveit::core::RobotState& robot_state,
                                     const collision_detection::AllowedCollisionMatrix& acm) const
  {
    robot_state.updateCollisionBodyTransforms();
    return distanceToCollisionUnpadded(static_cast<const moveit::core::RobotState&>(robot_state), acm);
  }
 
  double distanceToCollisionUnpadded(const moveit::core::RobotState& robot_state,
                                     const collision_detection::AllowedCollisionMatrix& acm) const
  {
    return getCollisionEnvUnpadded()->distanceRobot(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::msg::PlanningScene& scene) const;
 
  void getPlanningSceneMsg(moveit_msgs::msg::PlanningScene& scene) const;
 
  void getPlanningSceneMsg(moveit_msgs::msg::PlanningScene& scene,
                           const moveit_msgs::msg::PlanningSceneComponents& comp) const;
 
  bool getCollisionObjectMsg(moveit_msgs::msg::CollisionObject& collision_obj, const std::string& ns) const;
 
  void getCollisionObjectMsgs(std::vector<moveit_msgs::msg::CollisionObject>& collision_objs) const;
 
  bool getAttachedCollisionObjectMsg(moveit_msgs::msg::AttachedCollisionObject& attached_collision_obj,
                                     const std::string& ns) const;
 
  void
  getAttachedCollisionObjectMsgs(std::vector<moveit_msgs::msg::AttachedCollisionObject>& attached_collision_objs) const;
 
  bool getOctomapMsg(octomap_msgs::msg::OctomapWithPose& octomap) const;
 
  void getObjectColorMsgs(std::vector<moveit_msgs::msg::ObjectColor>& object_colors) const;
 
  bool setPlanningSceneDiffMsg(const moveit_msgs::msg::PlanningScene& scene);
 
  bool setPlanningSceneMsg(const moveit_msgs::msg::PlanningScene& scene);
 
  bool usePlanningSceneMsg(const moveit_msgs::msg::PlanningScene& scene);
 
  bool shapesAndPosesFromCollisionObjectMessage(const moveit_msgs::msg::CollisionObject& object,
                                                Eigen::Isometry3d& object_pose_in_header_frame,
                                                std::vector<shapes::ShapeConstPtr>& shapes,
                                                EigenSTL::vector_Isometry3d& shape_poses);
 
  bool processCollisionObjectMsg(const moveit_msgs::msg::CollisionObject& object);
  bool processAttachedCollisionObjectMsg(const moveit_msgs::msg::AttachedCollisionObject& object);
 
  bool processPlanningSceneWorldMsg(const moveit_msgs::msg::PlanningSceneWorld& world);
 
  void processOctomapMsg(const octomap_msgs::msg::OctomapWithPose& map);
  void processOctomapMsg(const octomap_msgs::msg::Octomap& map);
  void processOctomapPtr(const std::shared_ptr<const octomap::OcTree>& octree, const Eigen::Isometry3d& t);
 
  void removeAllCollisionObjects();
 
  void setCurrentState(const moveit_msgs::msg::RobotState& state);
 
  void setCurrentState(const moveit::core::RobotState& state);
 
  void setAttachedBodyUpdateCallback(const moveit::core::AttachedBodyCallback& callback);
 
  void setCollisionObjectUpdateCallback(const collision_detection::World::ObserverCallbackFn& callback);
 
  bool hasObjectColor(const std::string& id) const;
 
  const std_msgs::msg::ColorRGBA& getObjectColor(const std::string& id) const;
 
  std::optional<std_msgs::msg::ColorRGBA> getOriginalObjectColor(const std::string& id) const;
 
  void setObjectColor(const std::string& id, const std_msgs::msg::ColorRGBA& color);
  void removeObjectColor(const std::string& id);
  void getKnownObjectColors(ObjectColorMap& kc) const;
 
  bool hasObjectType(const std::string& id) const;
 
  const object_recognition_msgs::msg::ObjectType& getObjectType(const std::string& id) const;
  void setObjectType(const std::string& id, const object_recognition_msgs::msg::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::msg::RobotState& state, bool verbose = false) const;
 
  bool isStateFeasible(const moveit::core::RobotState& state, bool verbose = false) const;
 
  bool isStateConstrained(const moveit_msgs::msg::RobotState& state, const moveit_msgs::msg::Constraints& constr,
                          bool verbose = false) const;
 
  bool isStateConstrained(const moveit::core::RobotState& state, const moveit_msgs::msg::Constraints& constr,
                          bool verbose = false) const;
 
  bool isStateConstrained(const moveit_msgs::msg::RobotState& state,
                          const kinematic_constraints::KinematicConstraintSet& constr, bool verbose = false) const;
 
  bool isStateConstrained(const moveit::core::RobotState& state,
                          const kinematic_constraints::KinematicConstraintSet& constr, bool verbose = false) const;
 
  bool isStateValid(const moveit_msgs::msg::RobotState& state, const std::string& group = "",
                    bool verbose = false) const;
 
  bool isStateValid(const moveit::core::RobotState& state, const std::string& group = "", bool verbose = false) const;
 
  bool isStateValid(const moveit_msgs::msg::RobotState& state, const moveit_msgs::msg::Constraints& constr,
                    const std::string& group = "", bool verbose = false) const;
 
  bool isStateValid(const moveit::core::RobotState& state, const moveit_msgs::msg::Constraints& constr,
                    const std::string& group = "", bool verbose = false) const;
 
  bool isStateValid(const moveit::core::RobotState& state, const kinematic_constraints::KinematicConstraintSet& constr,
                    const std::string& group = "", bool verbose = false) const;
 
  bool isPathValid(const moveit_msgs::msg::RobotState& start_state, const moveit_msgs::msg::RobotTrajectory& trajectory,
                   const std::string& group = "", bool verbose = false,
                   std::vector<std::size_t>* invalid_index = nullptr) const;
 
  bool isPathValid(const moveit_msgs::msg::RobotState& start_state, const moveit_msgs::msg::RobotTrajectory& trajectory,
                   const moveit_msgs::msg::Constraints& path_constraints, const std::string& group = "",
                   bool verbose = false, std::vector<std::size_t>* invalid_index = nullptr) const;
 
  bool isPathValid(const moveit_msgs::msg::RobotState& start_state, const moveit_msgs::msg::RobotTrajectory& trajectory,
                   const moveit_msgs::msg::Constraints& path_constraints,
                   const moveit_msgs::msg::Constraints& goal_constraints, const std::string& group = "",
                   bool verbose = false, std::vector<std::size_t>* invalid_index = nullptr) const;
 
  bool isPathValid(const moveit_msgs::msg::RobotState& start_state, const moveit_msgs::msg::RobotTrajectory& trajectory,
                   const moveit_msgs::msg::Constraints& path_constraints,
                   const std::vector<moveit_msgs::msg::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::msg::Constraints& path_constraints,
                   const std::vector<moveit_msgs::msg::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::msg::Constraints& path_constraints,
                   const moveit_msgs::msg::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::msg::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 moveit::core::RobotState& state, std::size_t max_costs,
                      std::set<collision_detection::CostSource>& costs) const;
 
  void getCostSources(const moveit::core::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 PlanningScenePtr clone(const PlanningSceneConstPtr& scene);
 
  void allocateCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr& allocator)
  {
    allocateCollisionDetector(allocator, nullptr /* no parent_detector */);
  }
 
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 functions for processing collision objects */
  bool processCollisionObjectAdd(const moveit_msgs::msg::CollisionObject& object);
  bool processCollisionObjectRemove(const moveit_msgs::msg::CollisionObject& object);
  bool processCollisionObjectMove(const moveit_msgs::msg::CollisionObject& object);
 
  MOVEIT_STRUCT_FORWARD(CollisionDetector);
 
  /* Construct a new CollisionDector from allocator, copy-construct environments from parent_detector if not nullptr */
  void allocateCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr& allocator,
                                 const CollisionDetectorPtr& parent_detector);
 
  /* \brief A set of compatible collision detectors */
  struct CollisionDetector
  {
    collision_detection::CollisionDetectorAllocatorPtr alloc_;
    collision_detection::CollisionEnvPtr cenv_;  // never nullptr
    collision_detection::CollisionEnvConstPtr cenv_const_;
 
    collision_detection::CollisionEnvPtr cenv_unpadded_;
    collision_detection::CollisionEnvConstPtr cenv_unpadded_const_;
 
    const collision_detection::CollisionEnvConstPtr& getCollisionEnv() const
    {
      return cenv_const_;
    }
    const collision_detection::CollisionEnvConstPtr& getCollisionEnvUnpadded() const
    {
      return cenv_unpadded_const_;
    }
    void copyPadding(const CollisionDetector& src);
  };
  friend struct CollisionDetector;
 
  std::string name_;  // may be empty
 
  PlanningSceneConstPtr parent_;  // Null unless this is a diff scene
 
  moveit::core::RobotModelConstPtr robot_model_;  // Never null (may point to same model as parent)
 
  moveit::core::RobotStatePtr robot_state_;  // if nullptr use parent's
 
  // Called when changes are made to attached bodies
  moveit::core::AttachedBodyCallback current_state_attached_body_callback_;
 
  // This variable is not necessarily used by child planning scenes
  // This Transforms class is actually a SceneTransforms class
  moveit::core::TransformsPtr scene_transforms_;  // if nullptr use parent's
 
  collision_detection::WorldPtr world_;             // never nullptr, never shared with parent/child
  collision_detection::WorldConstPtr world_const_;  // copy of world_
  collision_detection::WorldDiffPtr world_diff_;    // nullptr 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_;
 
  CollisionDetectorPtr collision_detector_;  // Never nullptr.
 
  collision_detection::AllowedCollisionMatrixPtr acm_;  // if nullptr use parent's
 
  StateFeasibilityFn state_feasibility_;
  MotionFeasibilityFn motion_feasibility_;
 
  // Maps of current and original object colors (to manage attaching/detaching objects)
  std::unique_ptr<ObjectColorMap> object_colors_;
  std::unique_ptr<ObjectColorMap> original_object_colors_;
 
  // a map of object types
  std::unique_ptr<ObjectTypeMap> object_types_;
};
}  // namespace planning_scene