91class MOVEIT_PLANNING_SCENE_EXPORT
PlanningScene :
public std::enable_shared_from_this<PlanningScene>
105 PlanningScene(
const moveit::core::RobotModelConstPtr& robot_model,
106 const collision_detection::WorldPtr& world = std::make_shared<collision_detection::World>());
110 PlanningScene(
const urdf::ModelInterfaceSharedPtr& urdf_model,
const srdf::ModelConstSharedPtr& srdf_model,
111 const collision_detection::WorldPtr& world = std::make_shared<collision_detection::World>());
140 PlanningScenePtr diff()
const;
144 PlanningScenePtr diff(
const moveit_msgs::msg::PlanningScene& msg)
const;
163 return robot_state_.has_value() ? robot_state_.value() : parent_->getCurrentState();
169 moveit::core::RobotStatePtr getCurrentStateUpdated(
const moveit_msgs::msg::RobotState& update)
const;
180 return scene_transforms_.has_value() ? scene_transforms_.value()->getTargetFrame() : parent_->getPlanningFrame();
186 if (scene_transforms_.has_value() || !parent_)
188 return *scene_transforms_.value();
192 return parent_->getTransforms();
206 const Eigen::Isometry3d& getFrameTransform(
const std::string&
id)
const;
213 const Eigen::Isometry3d& getFrameTransform(
const std::string&
id);
234 bool knowsFrameTransform(
const std::string&
id)
const;
250 return collision_detector_ ? (collision_detector_->alloc_->getName()) :
"";
254 const collision_detection::WorldConstPtr&
getWorld()
const
270 return collision_detector_->getCollisionEnv();
276 return collision_detector_->getCollisionEnvUnpadded();
280 const collision_detection::CollisionEnvConstPtr& getCollisionEnv(
const std::string& collision_detector_name)
const;
284 const collision_detection::CollisionEnvConstPtr&
285 getCollisionEnvUnpadded(
const std::string& collision_detector_name)
const;
289 const collision_detection::CollisionEnvPtr& getCollisionEnvNonConst();
294 return acm_.has_value() ? acm_.value() : parent_->getAllowedCollisionMatrix();
313 bool isStateColliding(
const std::string& group =
"",
bool verbose =
false);
321 return isStateColliding(getCurrentState(), group, verbose);
338 bool verbose =
false)
const;
342 bool isStateColliding(
const moveit_msgs::msg::RobotState& state,
const std::string& group =
"",
343 bool verbose =
false)
const;
379 [[deprecated(
"Use new CollisionRequest flags to enable/ disable padding instead.")]]
void
384 [[deprecated(
"Use new CollisionRequest flags to enable/ disable padding instead.")]]
void
390 [[deprecated(
"Use new CollisionRequest flags to enable/ disable padding instead.")]]
void
397 [[deprecated(
"Use new CollisionRequest flags to enable/ disable padding instead.")]]
void
404 [[deprecated(
"Use new CollisionRequest flags to enable/ disable padding instead.")]]
void
411 [[deprecated(
"Use new CollisionRequest flags to enable/ disable padding instead.")]]
void
444 void getCollidingLinks(std::vector<std::string>& links);
449 getCollidingLinks(links, getCurrentState(), getAllowedCollisionMatrix());
463 getCollidingLinks(links, robot_state, getAllowedCollisionMatrix());
487 getCollidingPairs(contacts, getCurrentState(), getAllowedCollisionMatrix());
495 getCollidingPairs(contacts, robot_state, getAllowedCollisionMatrix(), group_name);
514 const std::string& group_name =
"")
const
525 const std::string& group_name =
"")
const;
548 return getCollisionEnv()->distanceRobot(robot_state, getAllowedCollisionMatrix());
563 return getCollisionEnvUnpadded()->distanceRobot(robot_state, getAllowedCollisionMatrix());
582 return getCollisionEnv()->distanceRobot(robot_state, acm);
601 return getCollisionEnvUnpadded()->distanceRobot(robot_state, acm);
607 void saveGeometryToStream(std::ostream& out)
const;
610 bool loadGeometryFromStream(std::istream& in);
613 bool loadGeometryFromStream(std::istream& in,
const Eigen::Isometry3d& offset);
619 void getPlanningSceneDiffMsg(moveit_msgs::msg::PlanningScene& scene)
const;
624 void getPlanningSceneMsg(moveit_msgs::msg::PlanningScene& scene)
const;
628 void getPlanningSceneMsg(moveit_msgs::msg::PlanningScene& scene,
629 const moveit_msgs::msg::PlanningSceneComponents& comp)
const;
633 bool getCollisionObjectMsg(moveit_msgs::msg::CollisionObject& collision_obj,
const std::string& ns)
const;
637 void getCollisionObjectMsgs(std::vector<moveit_msgs::msg::CollisionObject>& collision_objs)
const;
641 bool getAttachedCollisionObjectMsg(moveit_msgs::msg::AttachedCollisionObject& attached_collision_obj,
642 const std::string& ns)
const;
647 getAttachedCollisionObjectMsgs(std::vector<moveit_msgs::msg::AttachedCollisionObject>& attached_collision_objs)
const;
650 bool getOctomapMsg(octomap_msgs::msg::OctomapWithPose&
octomap)
const;
653 void getObjectColorMsgs(std::vector<moveit_msgs::msg::ObjectColor>& object_colors)
const;
660 bool setPlanningSceneDiffMsg(
const moveit_msgs::msg::PlanningScene& scene);
664 bool setPlanningSceneMsg(
const moveit_msgs::msg::PlanningScene& scene);
668 bool usePlanningSceneMsg(
const moveit_msgs::msg::PlanningScene& scene);
674 bool shapesAndPosesFromCollisionObjectMessage(
const moveit_msgs::msg::CollisionObject&
object,
675 Eigen::Isometry3d& object_pose_in_header_frame,
676 std::vector<shapes::ShapeConstPtr>&
shapes,
677 EigenSTL::vector_Isometry3d& shape_poses);
679 bool processCollisionObjectMsg(
const moveit_msgs::msg::CollisionObject&
object);
680 bool processAttachedCollisionObjectMsg(
const moveit_msgs::msg::AttachedCollisionObject&
object);
682 bool processPlanningSceneWorldMsg(
const moveit_msgs::msg::PlanningSceneWorld& world);
684 void processOctomapMsg(
const octomap_msgs::msg::OctomapWithPose& map);
685 void processOctomapMsg(
const octomap_msgs::msg::Octomap& map);
686 void processOctomapPtr(
const std::shared_ptr<const octomap::OcTree>& octree,
const Eigen::Isometry3d& t);
691 void removeAllCollisionObjects();
696 void setCurrentState(
const moveit_msgs::msg::RobotState& state);
707 bool hasObjectColor(
const std::string&
id)
const;
714 const std_msgs::msg::ColorRGBA& getObjectColor(
const std::string&
id)
const;
721 std::optional<std_msgs::msg::ColorRGBA> getOriginalObjectColor(
const std::string&
id)
const;
723 void setObjectColor(
const std::string&
id,
const std_msgs::msg::ColorRGBA& color);
724 void removeObjectColor(
const std::string&
id);
727 bool hasObjectType(
const std::string&
id)
const;
729 const object_recognition_msgs::msg::ObjectType& getObjectType(
const std::string&
id)
const;
730 void setObjectType(
const std::string&
id,
const object_recognition_msgs::msg::ObjectType& type);
731 void removeObjectType(
const std::string&
id);
743 void pushDiffs(
const PlanningScenePtr& scene);
748 void decoupleParent();
755 state_feasibility_ = fn;
762 return state_feasibility_;
769 motion_feasibility_ = fn;
776 return motion_feasibility_;
781 bool isStateFeasible(
const moveit_msgs::msg::RobotState& state,
bool verbose =
false)
const;
788 bool isStateConstrained(
const moveit_msgs::msg::RobotState& state,
const moveit_msgs::msg::Constraints& constr,
789 bool verbose =
false)
const;
793 bool verbose =
false)
const;
796 bool isStateConstrained(
const moveit_msgs::msg::RobotState& state,
805 bool isStateValid(
const moveit_msgs::msg::RobotState& state,
const std::string& group =
"",
806 bool verbose =
false)
const;
810 bool isStateValid(
const moveit::core::RobotState& state,
const std::string& group =
"",
bool verbose =
false)
const;
814 bool isStateValid(
const moveit_msgs::msg::RobotState& state,
const moveit_msgs::msg::Constraints& constr,
815 const std::string& group =
"",
bool verbose =
false)
const;
820 const std::string& group =
"",
bool verbose =
false)
const;
825 const std::string& group =
"",
bool verbose =
false)
const;
829 bool isPathValid(
const moveit_msgs::msg::RobotState& start_state,
const moveit_msgs::msg::RobotTrajectory& trajectory,
830 const std::string& group =
"",
bool verbose =
false,
831 std::vector<std::size_t>* invalid_index =
nullptr)
const;
836 bool isPathValid(
const moveit_msgs::msg::RobotState& start_state,
const moveit_msgs::msg::RobotTrajectory& trajectory,
837 const moveit_msgs::msg::Constraints& path_constraints,
const std::string& group =
"",
838 bool verbose =
false, std::vector<std::size_t>* invalid_index =
nullptr)
const;
843 bool isPathValid(
const moveit_msgs::msg::RobotState& start_state,
const moveit_msgs::msg::RobotTrajectory& trajectory,
844 const moveit_msgs::msg::Constraints& path_constraints,
845 const moveit_msgs::msg::Constraints& goal_constraints,
const std::string& group =
"",
846 bool verbose =
false, std::vector<std::size_t>* invalid_index =
nullptr)
const;
851 bool isPathValid(
const moveit_msgs::msg::RobotState& start_state,
const moveit_msgs::msg::RobotTrajectory& trajectory,
852 const moveit_msgs::msg::Constraints& path_constraints,
853 const std::vector<moveit_msgs::msg::Constraints>& goal_constraints,
const std::string& group =
"",
854 bool verbose =
false, std::vector<std::size_t>* invalid_index =
nullptr)
const;
860 const moveit_msgs::msg::Constraints& path_constraints,
861 const std::vector<moveit_msgs::msg::Constraints>& goal_constraints,
const std::string& group =
"",
862 bool verbose =
false, std::vector<std::size_t>* invalid_index =
nullptr)
const;
868 const moveit_msgs::msg::Constraints& path_constraints,
869 const moveit_msgs::msg::Constraints& goal_constraints,
const std::string& group =
"",
870 bool verbose =
false, std::vector<std::size_t>* invalid_index =
nullptr)
const;
875 const moveit_msgs::msg::Constraints& path_constraints,
const std::string& group =
"",
876 bool verbose =
false, std::vector<std::size_t>* invalid_index =
nullptr)
const;
881 bool verbose =
false, std::vector<std::size_t>* invalid_index =
nullptr)
const;
886 std::set<collision_detection::CostSource>& costs,
double overlap_fraction = 0.9)
const;
891 const std::string& group_name, std::set<collision_detection::CostSource>& costs,
892 double overlap_fraction = 0.9)
const;
896 std::set<collision_detection::CostSource>& costs)
const;
901 std::set<collision_detection::CostSource>& costs)
const;
904 void printKnownObjects(std::ostream& out = std::cout)
const;
907 static PlanningScenePtr clone(
const PlanningSceneConstPtr& scene);
924 allocateCollisionDetector(allocator,
nullptr );
936 bool processCollisionObjectAdd(
const moveit_msgs::msg::CollisionObject&
object);
937 bool processCollisionObjectRemove(
const moveit_msgs::msg::CollisionObject&
object);
938 bool processCollisionObjectMove(
const moveit_msgs::msg::CollisionObject&
object);
943 void allocateCollisionDetector(
const collision_detection::CollisionDetectorAllocatorPtr& allocator,
944 const CollisionDetectorPtr& parent_detector);
949 collision_detection::CollisionDetectorAllocatorPtr alloc_;
950 collision_detection::CollisionEnvPtr cenv_;
951 collision_detection::CollisionEnvConstPtr cenv_const_;
953 collision_detection::CollisionEnvPtr cenv_unpadded_;
954 collision_detection::CollisionEnvConstPtr cenv_unpadded_const_;
956 const collision_detection::CollisionEnvConstPtr& getCollisionEnv()
const
960 const collision_detection::CollisionEnvConstPtr& getCollisionEnvUnpadded()
const
962 return cenv_unpadded_const_;
970 PlanningSceneConstPtr parent_;
972 moveit::core::RobotModelConstPtr robot_model_;
974 std::optional<moveit::core::RobotState> robot_state_;
981 std::optional<moveit::core::TransformsPtr> scene_transforms_;
983 collision_detection::WorldPtr world_;
984 collision_detection::WorldConstPtr world_const_;
985 collision_detection::WorldDiffPtr world_diff_;
989 CollisionDetectorPtr collision_detector_;
991 std::optional<collision_detection::AllowedCollisionMatrix> acm_;
997 std::unique_ptr<ObjectColorMap> object_colors_;
998 std::unique_ptr<ObjectColorMap> original_object_colors_;
1001 std::optional<ObjectTypeMap> object_types_;