moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
planning_scene.hpp
Go to the documentation of this file.
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34
35/* Author: Ioan Sucan, Acorn Pooley */
36
37#pragma once
38
49#include <moveit_msgs/msg/planning_scene.hpp>
50#include <moveit_msgs/msg/robot_trajectory.hpp>
51#include <moveit_msgs/msg/constraints.hpp>
52#include <moveit_msgs/msg/planning_scene_components.hpp>
53#include <octomap_msgs/msg/octomap_with_pose.hpp>
54#include <memory>
55#include <functional>
56#include <optional>
57#include <thread>
58#include <variant>
59#include <optional>
60#include <rclcpp/rclcpp.hpp>
61
62#include <moveit_planning_scene_export.h>
63
66{
67MOVEIT_CLASS_FORWARD(PlanningScene); // Defines PlanningScenePtr, ConstPtr, WeakPtr... etc
68
73typedef std::function<bool(const moveit::core::RobotState&, bool)> StateFeasibilityFn;
74
80using MotionFeasibilityFn = std::function<bool(const moveit::core::RobotState&, const moveit::core::RobotState&, bool)>;
81
83using ObjectColorMap = std::map<std::string, std_msgs::msg::ColorRGBA>;
84
86using ObjectTypeMap = std::map<std::string, object_recognition_msgs::msg::ObjectType>;
87
91class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_from_this<PlanningScene>
92{
93public:
97 PlanningScene(const PlanningScene&) = delete;
98
103
105 PlanningScene(const moveit::core::RobotModelConstPtr& robot_model,
106 const collision_detection::WorldPtr& world = std::make_shared<collision_detection::World>());
107
110 PlanningScene(const urdf::ModelInterfaceSharedPtr& urdf_model, const srdf::ModelConstSharedPtr& srdf_model,
111 const collision_detection::WorldPtr& world = std::make_shared<collision_detection::World>());
112
113 static const std::string OCTOMAP_NS;
114 static const std::string DEFAULT_SCENE_NAME;
115
117
119 const std::string& getName() const
120 {
121 return name_;
122 }
123
125 void setName(const std::string& name)
126 {
127 name_ = name;
128 }
129
140 PlanningScenePtr diff() const;
141
144 PlanningScenePtr diff(const moveit_msgs::msg::PlanningScene& msg) const;
145
147 const PlanningSceneConstPtr& getParent() const
148 {
149 return parent_;
150 }
151
153 const moveit::core::RobotModelConstPtr& getRobotModel() const
154 {
155 // the kinematic model does not change
156 return robot_model_;
157 }
158
161 {
162 // if we have an updated state, return it; otherwise, return the parent one
163 return robot_state_.has_value() ? robot_state_.value() : parent_->getCurrentState();
164 }
166 moveit::core::RobotState& getCurrentStateNonConst();
167
169 moveit::core::RobotStatePtr getCurrentStateUpdated(const moveit_msgs::msg::RobotState& update) const;
170
177 const std::string& getPlanningFrame() const
178 {
179 // if we have an updated set of transforms, return it; otherwise, return the parent one
180 return scene_transforms_.has_value() ? scene_transforms_.value()->getTargetFrame() : parent_->getPlanningFrame();
181 }
182
185 {
186 if (scene_transforms_.has_value() || !parent_)
187 {
188 return *scene_transforms_.value();
189 }
190
191 // if this planning scene is a child of another, and doesn't have its own custom transforms
192 return parent_->getTransforms();
193 }
194
197 const moveit::core::Transforms& getTransforms();
198
200 moveit::core::Transforms& getTransformsNonConst();
201
206 const Eigen::Isometry3d& getFrameTransform(const std::string& id) const;
207
213 const Eigen::Isometry3d& getFrameTransform(const std::string& id);
214
220 const Eigen::Isometry3d& getFrameTransform(moveit::core::RobotState& state, const std::string& id) const
221 {
222 state.updateLinkTransforms();
223 return getFrameTransform(static_cast<const moveit::core::RobotState&>(state), id);
224 }
225
230 const Eigen::Isometry3d& getFrameTransform(const moveit::core::RobotState& state, const std::string& id) const;
231
234 bool knowsFrameTransform(const std::string& id) const;
235
238 bool knowsFrameTransform(const moveit::core::RobotState& state, const std::string& id) const;
239
247 const std::string getCollisionDetectorName() const
248 {
249 // If no collision detector is allocated, return an empty string
250 return collision_detector_ ? (collision_detector_->alloc_->getName()) : "";
251 }
252
254 const collision_detection::WorldConstPtr& getWorld() const
255 {
256 // we always have a world representation
257 return world_const_;
258 }
259
261 const collision_detection::WorldPtr& getWorldNonConst()
262 {
263 // we always have a world representation
264 return world_;
265 }
266
268 const collision_detection::CollisionEnvConstPtr& getCollisionEnv() const
269 {
270 return collision_detector_->getCollisionEnv();
271 }
272
274 const collision_detection::CollisionEnvConstPtr& getCollisionEnvUnpadded() const
275 {
276 return collision_detector_->getCollisionEnvUnpadded();
277 }
278
280 const collision_detection::CollisionEnvConstPtr& getCollisionEnv(const std::string& collision_detector_name) const;
281
284 const collision_detection::CollisionEnvConstPtr&
285 getCollisionEnvUnpadded(const std::string& collision_detector_name) const;
286
289 const collision_detection::CollisionEnvPtr& getCollisionEnvNonConst();
290
293 {
294 return acm_.has_value() ? acm_.value() : parent_->getAllowedCollisionMatrix();
295 }
296
298 collision_detection::AllowedCollisionMatrix& getAllowedCollisionMatrixNonConst();
299
301 void setAllowedCollisionMatrix(const collision_detection::AllowedCollisionMatrix& acm);
302
313 bool isStateColliding(const std::string& group = "", bool verbose = false);
314
319 bool isStateColliding(const std::string& group = "", bool verbose = false) const
320 {
321 return isStateColliding(getCurrentState(), group, verbose);
322 }
323
328 bool isStateColliding(moveit::core::RobotState& state, const std::string& group = "", bool verbose = false) const
329 {
331 return isStateColliding(static_cast<const moveit::core::RobotState&>(state), group, verbose);
332 }
333
337 bool isStateColliding(const moveit::core::RobotState& state, const std::string& group = "",
338 bool verbose = false) const;
339
342 bool isStateColliding(const moveit_msgs::msg::RobotState& state, const std::string& group = "",
343 bool verbose = false) const;
344
348
350 void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res) const;
351
355 moveit::core::RobotState& robot_state) const;
356
361 const moveit::core::RobotState& robot_state) const;
362
367 moveit::core::RobotState& robot_state,
369
373 const moveit::core::RobotState& robot_state,
375
379 [[deprecated("Use new CollisionRequest flags to enable/ disable padding instead.")]] void
381
384 [[deprecated("Use new CollisionRequest flags to enable/ disable padding instead.")]] void
385 checkCollisionUnpadded(const collision_detection::CollisionRequest& req,
387
390 [[deprecated("Use new CollisionRequest flags to enable/ disable padding instead.")]] void
392 const moveit::core::RobotState& robot_state) const;
393
397 [[deprecated("Use new CollisionRequest flags to enable/ disable padding instead.")]] void
399 moveit::core::RobotState& robot_state) const;
400
404 [[deprecated("Use new CollisionRequest flags to enable/ disable padding instead.")]] void
406 moveit::core::RobotState& robot_state,
408
411 [[deprecated("Use new CollisionRequest flags to enable/ disable padding instead.")]] void
413 const moveit::core::RobotState& robot_state,
415
417 void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res);
418
420 void checkSelfCollision(const collision_detection::CollisionRequest& req,
422
425 moveit::core::RobotState& robot_state) const;
426
429 const moveit::core::RobotState& robot_state) const;
430
434 moveit::core::RobotState& robot_state,
436
440 const moveit::core::RobotState& robot_state,
442
444 void getCollidingLinks(std::vector<std::string>& links);
445
447 void getCollidingLinks(std::vector<std::string>& links) const
448 {
449 getCollidingLinks(links, getCurrentState(), getAllowedCollisionMatrix());
450 }
451
454 void getCollidingLinks(std::vector<std::string>& links, moveit::core::RobotState& robot_state) const
455 {
456 robot_state.updateCollisionBodyTransforms();
457 getCollidingLinks(links, static_cast<const moveit::core::RobotState&>(robot_state), getAllowedCollisionMatrix());
458 }
459
461 void getCollidingLinks(std::vector<std::string>& links, const moveit::core::RobotState& robot_state) const
462 {
463 getCollidingLinks(links, robot_state, getAllowedCollisionMatrix());
464 }
465
468 void getCollidingLinks(std::vector<std::string>& links, moveit::core::RobotState& robot_state,
470 {
471 robot_state.updateCollisionBodyTransforms();
472 getCollidingLinks(links, static_cast<const moveit::core::RobotState&>(robot_state), acm);
473 }
474
477 void getCollidingLinks(std::vector<std::string>& links, const moveit::core::RobotState& robot_state,
479
482 void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts);
483
486 {
487 getCollidingPairs(contacts, getCurrentState(), getAllowedCollisionMatrix());
488 }
489
493 const moveit::core::RobotState& robot_state, const std::string& group_name = "") const
494 {
495 getCollidingPairs(contacts, robot_state, getAllowedCollisionMatrix(), group_name);
496 }
497
502 moveit::core::RobotState& robot_state, const std::string& group_name = "") const
503 {
504 robot_state.updateCollisionBodyTransforms();
505 getCollidingPairs(contacts, static_cast<const moveit::core::RobotState&>(robot_state), getAllowedCollisionMatrix(),
506 group_name);
507 }
508
514 const std::string& group_name = "") const
515 {
516 robot_state.updateCollisionBodyTransforms();
517 getCollidingPairs(contacts, static_cast<const moveit::core::RobotState&>(robot_state), acm, group_name);
518 }
519
522 void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts,
523 const moveit::core::RobotState& robot_state,
525 const std::string& group_name = "") const;
526
538 {
539 robot_state.updateCollisionBodyTransforms();
540 return distanceToCollision(static_cast<const moveit::core::RobotState&>(robot_state));
541 }
542
546 double distanceToCollision(const moveit::core::RobotState& robot_state) const
547 {
548 return getCollisionEnv()->distanceRobot(robot_state, getAllowedCollisionMatrix());
549 }
550
554 {
555 robot_state.updateCollisionBodyTransforms();
556 return distanceToCollisionUnpadded(static_cast<const moveit::core::RobotState&>(robot_state));
557 }
558
562 {
563 return getCollisionEnvUnpadded()->distanceRobot(robot_state, getAllowedCollisionMatrix());
564 }
565
571 {
572 robot_state.updateCollisionBodyTransforms();
573 return distanceToCollision(static_cast<const moveit::core::RobotState&>(robot_state), acm);
574 }
575
581 {
582 return getCollisionEnv()->distanceRobot(robot_state, acm);
583 }
584
590 {
591 robot_state.updateCollisionBodyTransforms();
592 return distanceToCollisionUnpadded(static_cast<const moveit::core::RobotState&>(robot_state), acm);
593 }
594
600 {
601 return getCollisionEnvUnpadded()->distanceRobot(robot_state, acm);
602 }
603
607 void saveGeometryToStream(std::ostream& out) const;
608
610 bool loadGeometryFromStream(std::istream& in);
611
613 bool loadGeometryFromStream(std::istream& in, const Eigen::Isometry3d& offset);
614
619 void getPlanningSceneDiffMsg(moveit_msgs::msg::PlanningScene& scene) const;
620
624 void getPlanningSceneMsg(moveit_msgs::msg::PlanningScene& scene) const;
625
628 void getPlanningSceneMsg(moveit_msgs::msg::PlanningScene& scene,
629 const moveit_msgs::msg::PlanningSceneComponents& comp) const;
630
633 bool getCollisionObjectMsg(moveit_msgs::msg::CollisionObject& collision_obj, const std::string& ns) const;
634
637 void getCollisionObjectMsgs(std::vector<moveit_msgs::msg::CollisionObject>& collision_objs) const;
638
641 bool getAttachedCollisionObjectMsg(moveit_msgs::msg::AttachedCollisionObject& attached_collision_obj,
642 const std::string& ns) const;
643
646 void
647 getAttachedCollisionObjectMsgs(std::vector<moveit_msgs::msg::AttachedCollisionObject>& attached_collision_objs) const;
648
650 bool getOctomapMsg(octomap_msgs::msg::OctomapWithPose& octomap) const;
651
653 void getObjectColorMsgs(std::vector<moveit_msgs::msg::ObjectColor>& object_colors) const;
654
660 bool setPlanningSceneDiffMsg(const moveit_msgs::msg::PlanningScene& scene);
661
664 bool setPlanningSceneMsg(const moveit_msgs::msg::PlanningScene& scene);
665
668 bool usePlanningSceneMsg(const moveit_msgs::msg::PlanningScene& scene);
669
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);
678
679 bool processCollisionObjectMsg(const moveit_msgs::msg::CollisionObject& object);
680 bool processAttachedCollisionObjectMsg(const moveit_msgs::msg::AttachedCollisionObject& object);
681
682 bool processPlanningSceneWorldMsg(const moveit_msgs::msg::PlanningSceneWorld& world);
683
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);
687
691 void removeAllCollisionObjects();
692
696 void setCurrentState(const moveit_msgs::msg::RobotState& state);
697
699 void setCurrentState(const moveit::core::RobotState& state);
700
702 void setAttachedBodyUpdateCallback(const moveit::core::AttachedBodyCallback& callback);
703
705 void setCollisionObjectUpdateCallback(const collision_detection::World::ObserverCallbackFn& callback);
706
707 bool hasObjectColor(const std::string& id) const;
708
714 const std_msgs::msg::ColorRGBA& getObjectColor(const std::string& id) const;
715
721 std::optional<std_msgs::msg::ColorRGBA> getOriginalObjectColor(const std::string& id) const;
722
723 void setObjectColor(const std::string& id, const std_msgs::msg::ColorRGBA& color);
724 void removeObjectColor(const std::string& id);
725 void getKnownObjectColors(ObjectColorMap& kc) const;
726
727 bool hasObjectType(const std::string& id) const;
728
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);
732 void getKnownObjectTypes(ObjectTypeMap& kc) const;
733
738 void clearDiffs();
739
743 void pushDiffs(const PlanningScenePtr& scene);
744
748 void decoupleParent();
749
754 {
755 state_feasibility_ = fn;
756 }
757
761 {
762 return state_feasibility_;
763 }
764
768 {
769 motion_feasibility_ = fn;
770 }
771
775 {
776 return motion_feasibility_;
777 }
778
781 bool isStateFeasible(const moveit_msgs::msg::RobotState& state, bool verbose = false) const;
782
785 bool isStateFeasible(const moveit::core::RobotState& state, bool verbose = false) const;
786
788 bool isStateConstrained(const moveit_msgs::msg::RobotState& state, const moveit_msgs::msg::Constraints& constr,
789 bool verbose = false) const;
790
792 bool isStateConstrained(const moveit::core::RobotState& state, const moveit_msgs::msg::Constraints& constr,
793 bool verbose = false) const;
794
796 bool isStateConstrained(const moveit_msgs::msg::RobotState& state,
797 const kinematic_constraints::KinematicConstraintSet& constr, bool verbose = false) const;
798
800 bool isStateConstrained(const moveit::core::RobotState& state,
801 const kinematic_constraints::KinematicConstraintSet& constr, bool verbose = false) const;
802
805 bool isStateValid(const moveit_msgs::msg::RobotState& state, const std::string& group = "",
806 bool verbose = false) const;
807
810 bool isStateValid(const moveit::core::RobotState& state, const std::string& group = "", bool verbose = false) const;
811
814 bool isStateValid(const moveit_msgs::msg::RobotState& state, const moveit_msgs::msg::Constraints& constr,
815 const std::string& group = "", bool verbose = false) const;
816
819 bool isStateValid(const moveit::core::RobotState& state, const moveit_msgs::msg::Constraints& constr,
820 const std::string& group = "", bool verbose = false) const;
821
824 bool isStateValid(const moveit::core::RobotState& state, const kinematic_constraints::KinematicConstraintSet& constr,
825 const std::string& group = "", bool verbose = false) const;
826
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;
832
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;
839
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;
847
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;
855
859 bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory,
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;
863
867 bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory,
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;
871
874 bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory,
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;
877
880 bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory, const std::string& group = "",
881 bool verbose = false, std::vector<std::size_t>* invalid_index = nullptr) const;
882
885 void getCostSources(const robot_trajectory::RobotTrajectory& trajectory, std::size_t max_costs,
886 std::set<collision_detection::CostSource>& costs, double overlap_fraction = 0.9) const;
887
890 void getCostSources(const robot_trajectory::RobotTrajectory& trajectory, std::size_t max_costs,
891 const std::string& group_name, std::set<collision_detection::CostSource>& costs,
892 double overlap_fraction = 0.9) const;
893
895 void getCostSources(const moveit::core::RobotState& state, std::size_t max_costs,
896 std::set<collision_detection::CostSource>& costs) const;
897
900 void getCostSources(const moveit::core::RobotState& state, std::size_t max_costs, const std::string& group_name,
901 std::set<collision_detection::CostSource>& costs) const;
902
904 void printKnownObjects(std::ostream& out = std::cout) const;
905
907 static PlanningScenePtr clone(const PlanningSceneConstPtr& scene);
908
922 void allocateCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr& allocator)
923 {
924 allocateCollisionDetector(allocator, nullptr /* no parent_detector */);
925 }
926
927private:
928 /* Private constructor used by the diff() methods. */
929 PlanningScene(const PlanningSceneConstPtr& parent);
930
931 /* Initialize the scene. This should only be called by the constructors.
932 * Requires a valid robot_model_ */
933 void initialize();
934
935 /* Helper functions for processing collision objects */
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);
939
941
942 /* Construct a new CollisionDector from allocator, copy-construct environments from parent_detector if not nullptr */
943 void allocateCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr& allocator,
944 const CollisionDetectorPtr& parent_detector);
945
946 /* \brief A set of compatible collision detectors */
947 struct CollisionDetector
948 {
949 collision_detection::CollisionDetectorAllocatorPtr alloc_;
950 collision_detection::CollisionEnvPtr cenv_; // never nullptr
951 collision_detection::CollisionEnvConstPtr cenv_const_;
952
953 collision_detection::CollisionEnvPtr cenv_unpadded_;
954 collision_detection::CollisionEnvConstPtr cenv_unpadded_const_;
955
956 const collision_detection::CollisionEnvConstPtr& getCollisionEnv() const
957 {
958 return cenv_const_;
959 }
960 const collision_detection::CollisionEnvConstPtr& getCollisionEnvUnpadded() const
961 {
962 return cenv_unpadded_const_;
963 }
964 void copyPadding(const CollisionDetector& src);
965 };
966 friend struct CollisionDetector;
967
968 std::string name_; // may be empty
969
970 PlanningSceneConstPtr parent_; // Null unless this is a diff scene
971
972 moveit::core::RobotModelConstPtr robot_model_; // Never null (may point to same model as parent)
973
974 std::optional<moveit::core::RobotState> robot_state_; // if there is no value use parent's
975
976 // Called when changes are made to attached bodies
977 moveit::core::AttachedBodyCallback current_state_attached_body_callback_;
978
979 // This variable is not necessarily used by child planning scenes
980 // This Transforms class is actually a SceneTransforms class
981 std::optional<moveit::core::TransformsPtr> scene_transforms_; // if there is no value use parent's
982
983 collision_detection::WorldPtr world_; // never nullptr, never shared with parent/child
984 collision_detection::WorldConstPtr world_const_; // copy of world_
985 collision_detection::WorldDiffPtr world_diff_; // nullptr unless this is a diff scene
986 collision_detection::World::ObserverCallbackFn current_world_object_update_callback_;
987 collision_detection::World::ObserverHandle current_world_object_update_observer_handle_;
988
989 CollisionDetectorPtr collision_detector_; // Never nullptr.
990
991 std::optional<collision_detection::AllowedCollisionMatrix> acm_; // if there is no value use parent's
992
993 StateFeasibilityFn state_feasibility_;
994 MotionFeasibilityFn motion_feasibility_;
995
996 // Maps of current and original object colors (to manage attaching/detaching objects)
997 std::unique_ptr<ObjectColorMap> object_colors_;
998 std::unique_ptr<ObjectColorMap> original_object_colors_;
999
1000 // a map of object types
1001 std::optional<ObjectTypeMap> object_types_;
1002};
1003} // namespace planning_scene
#define MOVEIT_CLASS_FORWARD(C)
#define MOVEIT_STRUCT_FORWARD(C)
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
std::function< void(const ObjectConstPtr &, Action)> ObserverCallbackFn
Definition world.hpp:304
A class that contains many different constraints, and can check RobotState *versus the full set....
Representation of a robot's state. This includes position, velocity, acceleration and effort.
void updateLinkTransforms()
Update the reference frame transforms for links. This call is needed before using the transforms of l...
void updateCollisionBodyTransforms()
Update the transforms for the collision bodies. This call is needed before calling collision checking...
Provides an implementation of a snapshot of a transform tree that can be easily queried for transform...
This class maintains the representation of the environment as seen by a planning instance....
void setName(const std::string &name)
Set the name of the planning scene.
double distanceToCollisionUnpadded(const moveit::core::RobotState &robot_state) const
The distance between the robot model at state robot_state to the nearest collision (ignoring self-col...
double distanceToCollision(moveit::core::RobotState &robot_state, const collision_detection::AllowedCollisionMatrix &acm) const
The distance between the robot model at state robot_state to the nearest collision,...
void getCollidingPairs(collision_detection::CollisionResult::ContactMap &contacts) const
Get the names of the links that are involved in collisions for the current state.
void getCollidingPairs(collision_detection::CollisionResult::ContactMap &contacts, moveit::core::RobotState &robot_state, const collision_detection::AllowedCollisionMatrix &acm, const std::string &group_name="") const
Get the names of the links that are involved in collisions for the state robot_state given the allowe...
double distanceToCollision(moveit::core::RobotState &robot_state) const
The distance between the robot model at state robot_state to the nearest collision (ignoring self-col...
bool isStateColliding(moveit::core::RobotState &state, const std::string &group="", bool verbose=false) const
Check if a given state is in collision (with the environment or self collision) If a group name is sp...
const moveit::core::RobotModelConstPtr & getRobotModel() const
Get the kinematic model for which the planning scene is maintained.
const collision_detection::CollisionEnvConstPtr & getCollisionEnv() const
Get the active collision environment.
void getCollidingLinks(std::vector< std::string > &links, const moveit::core::RobotState &robot_state) const
Get the names of the links that are involved in collisions for the state robot_state.
const Eigen::Isometry3d & getFrameTransform(moveit::core::RobotState &state, const std::string &id) const
Get the transform corresponding to the frame id. This will be known if id is a link name,...
void getCollidingPairs(collision_detection::CollisionResult::ContactMap &contacts, const moveit::core::RobotState &robot_state, const std::string &group_name="") const
Get the names of the links that are involved in collisions for the state robot_state....
const PlanningSceneConstPtr & getParent() const
Get the parent scene (with respect to which the diffs are maintained). This may be empty.
void setMotionFeasibilityPredicate(const MotionFeasibilityFn &fn)
Specify a predicate that decides whether motion segments are considered valid or invalid for reasons ...
const collision_detection::WorldConstPtr & getWorld() const
Get the representation of the world.
void getCollidingLinks(std::vector< std::string > &links, moveit::core::RobotState &robot_state, const collision_detection::AllowedCollisionMatrix &acm) const
Get the names of the links that are involved in collisions for the state robot_state given the allowe...
const moveit::core::RobotState & getCurrentState() const
Get the state at which the robot is assumed to be.
PlanningScene(const PlanningScene &)=delete
PlanningScene cannot be copy-constructed.
double distanceToCollisionUnpadded(moveit::core::RobotState &robot_state, const collision_detection::AllowedCollisionMatrix &acm) const
The distance between the robot model at state robot_state to the nearest collision,...
double distanceToCollision(const moveit::core::RobotState &robot_state) const
The distance between the robot model at state robot_state to the nearest collision (ignoring self-col...
const std::string getCollisionDetectorName() const
void getCollidingPairs(collision_detection::CollisionResult::ContactMap &contacts, moveit::core::RobotState &robot_state, const std::string &group_name="") const
Get the names of the links that are involved in collisions for the state robot_state....
const collision_detection::CollisionEnvConstPtr & getCollisionEnvUnpadded() const
Get the active collision detector for the robot.
void getCollidingLinks(std::vector< std::string > &links) const
Get the names of the links that are involved in collisions for the current state.
const std::string & getPlanningFrame() const
Get the frame in which planning is performed.
void getCollidingLinks(std::vector< std::string > &links, moveit::core::RobotState &robot_state) const
Get the names of the links that are involved in collisions for the state robot_state....
void allocateCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr &allocator)
Allocate a new collision detector and replace the previous one if there was any.
const moveit::core::Transforms & getTransforms() const
Get the set of fixed transforms from known frames to the planning frame.
double distanceToCollisionUnpadded(const moveit::core::RobotState &robot_state, const collision_detection::AllowedCollisionMatrix &acm) const
The distance between the robot model at state robot_state to the nearest collision,...
double distanceToCollision(const moveit::core::RobotState &robot_state, const collision_detection::AllowedCollisionMatrix &acm) const
The distance between the robot model at state robot_state to the nearest collision,...
bool isStateColliding(const std::string &group="", bool verbose=false) const
Check if the current state is in collision (with the environment or self collision)....
const std::string & getName() const
Get the name of the planning scene. This is empty by default.
const StateFeasibilityFn & getStateFeasibilityPredicate() const
Get the predicate that decides whether states are considered valid or invalid for reasons beyond ones...
PlanningScene & operator=(const PlanningScene &)=delete
PlanningScene cannot be copy-assigned.
const collision_detection::WorldPtr & getWorldNonConst()
Get the representation of the world.
void setStateFeasibilityPredicate(const StateFeasibilityFn &fn)
Specify a predicate that decides whether states are considered valid or invalid for reasons beyond on...
double distanceToCollisionUnpadded(moveit::core::RobotState &robot_state) const
The distance between the robot model at state robot_state to the nearest collision (ignoring self-col...
static const std::string OCTOMAP_NS
static const std::string DEFAULT_SCENE_NAME
const MotionFeasibilityFn & getMotionFeasibilityPredicate() const
Get the predicate that decides whether motion segments are considered valid or invalid for reasons be...
const collision_detection::AllowedCollisionMatrix & getAllowedCollisionMatrix() const
Get the allowed collision matrix.
Maintain a sequence of waypoints and the time durations between these waypoints.
CollisionDetector
Enumerates the available collision detectors.
std::function< void(AttachedBody *body, bool attached)> AttachedBodyCallback
This namespace includes the central class for representing planning contexts.
std::function< bool(const moveit::core::RobotState &, bool)> StateFeasibilityFn
This is the function signature for additional feasibility checks to be imposed on states (in addition...
std::map< std::string, std_msgs::msg::ColorRGBA > ObjectColorMap
A map from object names (e.g., attached bodies, collision objects) to their colors.
std::function< bool(const moveit::core::RobotState &, const moveit::core::RobotState &, bool)> MotionFeasibilityFn
This is the function signature for additional feasibility checks to be imposed on motions segments be...
std::map< std::string, object_recognition_msgs::msg::ObjectType > ObjectTypeMap
A map from object names (e.g., attached bodies, collision objects) to their types.
Representation of a collision checking request.
Representation of a collision checking result.
std::map< std::pair< std::string, std::string >, std::vector< Contact > > ContactMap
A map returning the pairs of body ids in contact, plus their contact details.