planning_scene.h
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 #ifndef MOVEIT_PLANNING_SCENE_PLANNING_SCENE_
38 #define MOVEIT_PLANNING_SCENE_PLANNING_SCENE_
39 
49 #include <moveit_msgs/PlanningScene.h>
50 #include <moveit_msgs/RobotTrajectory.h>
51 #include <moveit_msgs/Constraints.h>
52 #include <moveit_msgs/PlanningSceneComponents.h>
53 #include <octomap_msgs/OctomapWithPose.h>
54 #include <boost/noncopyable.hpp>
55 #include <boost/function.hpp>
56 #include <boost/concept_check.hpp>
57 #include <memory>
58 
60 namespace planning_scene
61 {
62 MOVEIT_CLASS_FORWARD(PlanningScene);
63 
68 typedef boost::function<bool(const robot_state::RobotState&, bool)> StateFeasibilityFn;
69 
75 typedef boost::function<bool(const robot_state::RobotState&, const robot_state::RobotState&, bool)> MotionFeasibilityFn;
76 
78 typedef std::map<std::string, std_msgs::ColorRGBA> ObjectColorMap;
79 
81 typedef std::map<std::string, object_recognition_msgs::ObjectType> ObjectTypeMap;
82 
86 class PlanningScene : private boost::noncopyable, public std::enable_shared_from_this<PlanningScene>
87 {
88 public:
91  const robot_model::RobotModelConstPtr& robot_model,
92  const collision_detection::WorldPtr& world = collision_detection::WorldPtr(new collision_detection::World()));
93 
97  const urdf::ModelInterfaceSharedPtr& urdf_model, const srdf::ModelConstSharedPtr& srdf_model,
98  const collision_detection::WorldPtr& world = collision_detection::WorldPtr(new collision_detection::World()));
99 
100  static const std::string OCTOMAP_NS;
101  static const std::string DEFAULT_SCENE_NAME;
102 
103  ~PlanningScene();
104 
106  const std::string& getName() const
107  {
108  return name_;
109  }
110 
112  void setName(const std::string& name)
113  {
114  name_ = name;
115  }
116 
127  PlanningScenePtr diff() const;
128 
131  PlanningScenePtr diff(const moveit_msgs::PlanningScene& msg) const;
132 
134  const PlanningSceneConstPtr& getParent() const
135  {
136  return parent_;
137  }
138 
140  const robot_model::RobotModelConstPtr& getRobotModel() const
141  {
142  // the kinematic model does not change
143  return robot_model_;
144  }
145 
148  {
149  // if we have an updated state, return it; otherwise, return the parent one
150  return robot_state_ ? *robot_state_ : parent_->getCurrentState();
151  }
154 
156  robot_state::RobotStatePtr getCurrentStateUpdated(const moveit_msgs::RobotState& update) const;
157 
164  const std::string& getPlanningFrame() const
165  {
166  // if we have an updated set of transforms, return it; otherwise, return the parent one
167  return scene_transforms_ ? scene_transforms_->getTargetFrame() : parent_->getPlanningFrame();
168  }
169 
172  {
173  if (scene_transforms_ || !parent_)
174  {
175  return *scene_transforms_;
176  }
177 
178  // if this planning scene is a child of another, and doesn't have its own custom transforms
179  return parent_->getTransforms();
180  }
181 
185 
188 
193  const Eigen::Isometry3d& getFrameTransform(const std::string& id) const;
194 
200  const Eigen::Isometry3d& getFrameTransform(const std::string& id);
201 
207  const Eigen::Isometry3d& getFrameTransform(robot_state::RobotState& state, const std::string& id) const
208  {
209  state.updateLinkTransforms();
210  return getFrameTransform(static_cast<const robot_state::RobotState&>(state), id);
211  }
212 
217  const Eigen::Isometry3d& getFrameTransform(const robot_state::RobotState& state, const std::string& id) const;
218 
221  bool knowsFrameTransform(const std::string& id) const;
222 
225  bool knowsFrameTransform(const robot_state::RobotState& state, const std::string& id) const;
226 
251  void addCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr& allocator);
252 
262  void setActiveCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr& allocator,
263  bool exclusive = false);
264 
271  bool setActiveCollisionDetector(const std::string& collision_detector_name);
272 
273  const std::string& getActiveCollisionDetectorName() const
274  {
275  return active_collision_->alloc_->getName();
276  }
277 
280  void getCollisionDetectorNames(std::vector<std::string>& names) const;
281 
283  const collision_detection::WorldConstPtr& getWorld() const
284  {
285  // we always have a world representation
286  return world_const_;
287  }
288 
289  // brief Get the representation of the world
290  const collision_detection::WorldPtr& getWorldNonConst()
291  {
292  // we always have a world representation
293  return world_;
294  }
295 
297  const collision_detection::CollisionWorldConstPtr& getCollisionWorld() const
298  {
299  // we always have a world representation after configure is called.
300  return active_collision_->cworld_const_;
301  }
302 
304  const collision_detection::CollisionRobotConstPtr& getCollisionRobot() const
305  {
306  return active_collision_->getCollisionRobot();
307  }
308 
310  const collision_detection::CollisionRobotConstPtr& getCollisionRobotUnpadded() const
311  {
312  return active_collision_->getCollisionRobotUnpadded();
313  }
314 
316  const collision_detection::CollisionWorldConstPtr&
317  getCollisionWorld(const std::string& collision_detector_name) const;
318 
320  const collision_detection::CollisionRobotConstPtr&
321  getCollisionRobot(const std::string& collision_detector_name) const;
322 
325  const collision_detection::CollisionRobotConstPtr&
326  getCollisionRobotUnpadded(const std::string& collision_detector_name) const;
327 
332  const collision_detection::CollisionRobotPtr& getCollisionRobotNonConst();
333 
337  void propogateRobotPadding();
338 
341  {
342  return acm_ ? *acm_ : parent_->getAllowedCollisionMatrix();
343  }
346 
357  bool isStateColliding(const std::string& group = "", bool verbose = false);
358 
362  bool isStateColliding(const std::string& group = "", bool verbose = false) const
363  {
364  return isStateColliding(getCurrentState(), group, verbose);
365  }
366 
371  bool isStateColliding(robot_state::RobotState& state, const std::string& group = "", bool verbose = false) const
372  {
374  return isStateColliding(static_cast<const robot_state::RobotState&>(state), group, verbose);
375  }
376 
380  bool isStateColliding(const robot_state::RobotState& state, const std::string& group = "",
381  bool verbose = false) const;
382 
385  bool isStateColliding(const moveit_msgs::RobotState& state, const std::string& group = "",
386  bool verbose = false) const;
387 
391 
394  {
395  checkCollision(req, res, getCurrentState());
396  }
397 
402  {
403  robot_state.updateCollisionBodyTransforms();
404  checkCollision(req, res, static_cast<const robot_state::RobotState&>(robot_state));
405  }
406 
411  const robot_state::RobotState& robot_state) const;
412 
417  robot_state::RobotState& robot_state,
419  {
420  robot_state.updateCollisionBodyTransforms();
421  checkCollision(req, res, static_cast<const robot_state::RobotState&>(robot_state), acm);
422  }
423 
427  const robot_state::RobotState& robot_state,
429 
435 
440  {
442  }
443 
448  const robot_state::RobotState& robot_state) const
449  {
450  checkCollisionUnpadded(req, res, robot_state, getAllowedCollisionMatrix());
451  }
452 
458  {
459  robot_state.updateCollisionBodyTransforms();
460  checkCollisionUnpadded(req, res, static_cast<const robot_state::RobotState&>(robot_state),
462  }
463 
470  {
471  robot_state.updateCollisionBodyTransforms();
472  checkCollisionUnpadded(req, res, static_cast<const robot_state::RobotState&>(robot_state), acm);
473  }
474 
480 
483 
487  {
488  checkSelfCollision(req, res, getCurrentState());
489  }
490 
493  robot_state::RobotState& robot_state) const
494  {
495  robot_state.updateCollisionBodyTransforms();
496  checkSelfCollision(req, res, static_cast<const robot_state::RobotState&>(robot_state), getAllowedCollisionMatrix());
497  }
498 
501  const robot_state::RobotState& robot_state) const
502  {
503  // do self-collision checking with the unpadded version of the robot
504  getCollisionRobotUnpadded()->checkSelfCollision(req, res, robot_state, getAllowedCollisionMatrix());
505  }
506 
510  robot_state::RobotState& robot_state,
512  {
513  robot_state.updateCollisionBodyTransforms();
514  checkSelfCollision(req, res, static_cast<const robot_state::RobotState&>(robot_state), acm);
515  }
516 
520  const robot_state::RobotState& robot_state,
522  {
523  // do self-collision checking with the unpadded version of the robot
524  getCollisionRobotUnpadded()->checkSelfCollision(req, res, robot_state, acm);
525  }
526 
528  void getCollidingLinks(std::vector<std::string>& links);
529 
531  void getCollidingLinks(std::vector<std::string>& links) const
532  {
534  }
535 
538  void getCollidingLinks(std::vector<std::string>& links, robot_state::RobotState& robot_state) const
539  {
540  robot_state.updateCollisionBodyTransforms();
541  getCollidingLinks(links, static_cast<const robot_state::RobotState&>(robot_state), getAllowedCollisionMatrix());
542  }
543 
545  void getCollidingLinks(std::vector<std::string>& links, const robot_state::RobotState& robot_state) const
546  {
547  getCollidingLinks(links, robot_state, getAllowedCollisionMatrix());
548  }
549 
552  void getCollidingLinks(std::vector<std::string>& links, robot_state::RobotState& robot_state,
554  {
555  robot_state.updateCollisionBodyTransforms();
556  getCollidingLinks(links, static_cast<const robot_state::RobotState&>(robot_state), acm);
557  }
558 
561  void getCollidingLinks(std::vector<std::string>& links, const robot_state::RobotState& robot_state,
563 
567 
570  {
572  }
573 
576  const robot_state::RobotState& robot_state) const
577  {
578  getCollidingPairs(contacts, robot_state, getAllowedCollisionMatrix());
579  }
580 
584  robot_state::RobotState& robot_state) const
585  {
586  robot_state.updateCollisionBodyTransforms();
587  getCollidingPairs(contacts, static_cast<const robot_state::RobotState&>(robot_state), getAllowedCollisionMatrix());
588  }
589 
593  robot_state::RobotState& robot_state,
595  {
596  robot_state.updateCollisionBodyTransforms();
597  getCollidingPairs(contacts, static_cast<const robot_state::RobotState&>(robot_state), acm);
598  }
599 
603  const robot_state::RobotState& robot_state,
605 
616  double distanceToCollision(robot_state::RobotState& robot_state) const
617  {
618  robot_state.updateCollisionBodyTransforms();
619  return distanceToCollision(static_cast<const robot_state::RobotState&>(robot_state));
620  }
621 
625  double distanceToCollision(const robot_state::RobotState& robot_state) const
626  {
627  return getCollisionWorld()->distanceRobot(*getCollisionRobot(), robot_state, getAllowedCollisionMatrix());
628  }
629 
633  {
634  robot_state.updateCollisionBodyTransforms();
635  return distanceToCollisionUnpadded(static_cast<const robot_state::RobotState&>(robot_state));
636  }
637 
640  double distanceToCollisionUnpadded(const robot_state::RobotState& robot_state) const
641  {
642  return getCollisionWorld()->distanceRobot(*getCollisionRobotUnpadded(), robot_state, getAllowedCollisionMatrix());
643  }
644 
650  {
651  robot_state.updateCollisionBodyTransforms();
652  return distanceToCollision(static_cast<const robot_state::RobotState&>(robot_state), acm);
653  }
654 
658  double distanceToCollision(const robot_state::RobotState& robot_state,
660  {
661  return getCollisionWorld()->distanceRobot(*getCollisionRobot(), robot_state, acm);
662  }
663 
669  {
670  robot_state.updateCollisionBodyTransforms();
671  return distanceToCollisionUnpadded(static_cast<const robot_state::RobotState&>(robot_state), acm);
672  }
673 
679  {
680  return getCollisionWorld()->distanceRobot(*getCollisionRobotUnpadded(), robot_state, acm);
681  }
682 
686  void saveGeometryToStream(std::ostream& out) const;
687 
689  bool loadGeometryFromStream(std::istream& in);
690 
692  bool loadGeometryFromStream(std::istream& in, const Eigen::Isometry3d& offset);
693 
698  void getPlanningSceneDiffMsg(moveit_msgs::PlanningScene& scene) const;
699 
703  void getPlanningSceneMsg(moveit_msgs::PlanningScene& scene) const;
704 
707  void getPlanningSceneMsg(moveit_msgs::PlanningScene& scene, const moveit_msgs::PlanningSceneComponents& comp) const;
708 
711  bool getCollisionObjectMsg(moveit_msgs::CollisionObject& collision_obj, const std::string& ns) const;
712 
715  void getCollisionObjectMsgs(std::vector<moveit_msgs::CollisionObject>& collision_objs) const;
716 
719  bool getAttachedCollisionObjectMsg(moveit_msgs::AttachedCollisionObject& attached_collision_obj,
720  const std::string& ns) const;
721 
724  void getAttachedCollisionObjectMsgs(std::vector<moveit_msgs::AttachedCollisionObject>& attached_collision_objs) const;
725 
727  bool getOctomapMsg(octomap_msgs::OctomapWithPose& octomap) const;
728 
730  void getObjectColorMsgs(std::vector<moveit_msgs::ObjectColor>& object_colors) const;
731 
737  bool setPlanningSceneDiffMsg(const moveit_msgs::PlanningScene& scene);
738 
741  bool setPlanningSceneMsg(const moveit_msgs::PlanningScene& scene);
742 
745  bool usePlanningSceneMsg(const moveit_msgs::PlanningScene& scene);
746 
747  bool processCollisionObjectMsg(const moveit_msgs::CollisionObject& object);
748  bool processAttachedCollisionObjectMsg(const moveit_msgs::AttachedCollisionObject& object);
749 
750  bool processPlanningSceneWorldMsg(const moveit_msgs::PlanningSceneWorld& world);
751 
752  void processOctomapMsg(const octomap_msgs::OctomapWithPose& map);
753  void processOctomapMsg(const octomap_msgs::Octomap& map);
754  void processOctomapPtr(const std::shared_ptr<const octomap::OcTree>& octree, const Eigen::Isometry3d& t);
755 
760 
764  void setCurrentState(const moveit_msgs::RobotState& state);
765 
767  void setCurrentState(const robot_state::RobotState& state);
768 
771 
774 
775  bool hasObjectColor(const std::string& id) const;
776 
777  const std_msgs::ColorRGBA& getObjectColor(const std::string& id) const;
778  void setObjectColor(const std::string& id, const std_msgs::ColorRGBA& color);
779  void removeObjectColor(const std::string& id);
780  void getKnownObjectColors(ObjectColorMap& kc) const;
781 
782  bool hasObjectType(const std::string& id) const;
783 
784  const object_recognition_msgs::ObjectType& getObjectType(const std::string& id) const;
785  void setObjectType(const std::string& id, const object_recognition_msgs::ObjectType& type);
786  void removeObjectType(const std::string& id);
787  void getKnownObjectTypes(ObjectTypeMap& kc) const;
788 
791  void clearDiffs();
792 
796  void pushDiffs(const PlanningScenePtr& scene);
797 
801  void decoupleParent();
802 
806  void setStateFeasibilityPredicate(const StateFeasibilityFn& fn)
807  {
808  state_feasibility_ = fn;
809  }
810 
813  const StateFeasibilityFn& getStateFeasibilityPredicate() const
814  {
815  return state_feasibility_;
816  }
817 
820  void setMotionFeasibilityPredicate(const MotionFeasibilityFn& fn)
821  {
822  motion_feasibility_ = fn;
823  }
824 
827  const MotionFeasibilityFn& getMotionFeasibilityPredicate() const
828  {
829  return motion_feasibility_;
830  }
831 
834  bool isStateFeasible(const moveit_msgs::RobotState& state, bool verbose = false) const;
835 
838  bool isStateFeasible(const robot_state::RobotState& state, bool verbose = false) const;
839 
841  bool isStateConstrained(const moveit_msgs::RobotState& state, const moveit_msgs::Constraints& constr,
842  bool verbose = false) const;
843 
845  bool isStateConstrained(const robot_state::RobotState& state, const moveit_msgs::Constraints& constr,
846  bool verbose = false) const;
847 
849  bool isStateConstrained(const moveit_msgs::RobotState& state,
850  const kinematic_constraints::KinematicConstraintSet& constr, bool verbose = false) const;
851 
854  const kinematic_constraints::KinematicConstraintSet& constr, bool verbose = false) const;
855 
857  bool isStateValid(const moveit_msgs::RobotState& state, const std::string& group = "", bool verbose = false) const;
858 
860  bool isStateValid(const robot_state::RobotState& state, const std::string& group = "", bool verbose = false) const;
861 
864  bool isStateValid(const moveit_msgs::RobotState& state, const moveit_msgs::Constraints& constr,
865  const std::string& group = "", bool verbose = false) const;
866 
869  bool isStateValid(const robot_state::RobotState& state, const moveit_msgs::Constraints& constr,
870  const std::string& group = "", bool verbose = false) const;
871 
875  const std::string& group = "", bool verbose = false) const;
876 
878  bool isPathValid(const moveit_msgs::RobotState& start_state, const moveit_msgs::RobotTrajectory& trajectory,
879  const std::string& group = "", bool verbose = false,
880  std::vector<std::size_t>* invalid_index = NULL) const;
881 
885  bool isPathValid(const moveit_msgs::RobotState& start_state, const moveit_msgs::RobotTrajectory& trajectory,
886  const moveit_msgs::Constraints& path_constraints, const std::string& group = "",
887  bool verbose = false, std::vector<std::size_t>* invalid_index = NULL) const;
888 
892  bool isPathValid(const moveit_msgs::RobotState& start_state, const moveit_msgs::RobotTrajectory& trajectory,
893  const moveit_msgs::Constraints& path_constraints, const moveit_msgs::Constraints& goal_constraints,
894  const std::string& group = "", bool verbose = false,
895  std::vector<std::size_t>* invalid_index = NULL) const;
896 
900  bool isPathValid(const moveit_msgs::RobotState& start_state, const moveit_msgs::RobotTrajectory& trajectory,
901  const moveit_msgs::Constraints& path_constraints,
902  const std::vector<moveit_msgs::Constraints>& goal_constraints, const std::string& group = "",
903  bool verbose = false, std::vector<std::size_t>* invalid_index = NULL) const;
904 
908  bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory,
909  const moveit_msgs::Constraints& path_constraints,
910  const std::vector<moveit_msgs::Constraints>& goal_constraints, const std::string& group = "",
911  bool verbose = false, std::vector<std::size_t>* invalid_index = NULL) const;
912 
916  bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory,
917  const moveit_msgs::Constraints& path_constraints, const moveit_msgs::Constraints& goal_constraints,
918  const std::string& group = "", bool verbose = false,
919  std::vector<std::size_t>* invalid_index = NULL) const;
920 
923  bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory,
924  const moveit_msgs::Constraints& path_constraints, const std::string& group = "",
925  bool verbose = false, std::vector<std::size_t>* invalid_index = NULL) const;
926 
928  bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory, const std::string& group = "",
929  bool verbose = false, std::vector<std::size_t>* invalid_index = NULL) const;
930 
933  void getCostSources(const robot_trajectory::RobotTrajectory& trajectory, std::size_t max_costs,
934  std::set<collision_detection::CostSource>& costs, double overlap_fraction = 0.9) const;
935 
938  void getCostSources(const robot_trajectory::RobotTrajectory& trajectory, std::size_t max_costs,
939  const std::string& group_name, std::set<collision_detection::CostSource>& costs,
940  double overlap_fraction = 0.9) const;
941 
943  void getCostSources(const robot_state::RobotState& state, std::size_t max_costs,
944  std::set<collision_detection::CostSource>& costs) const;
945 
948  void getCostSources(const robot_state::RobotState& state, std::size_t max_costs, const std::string& group_name,
949  std::set<collision_detection::CostSource>& costs) const;
950 
952  void printKnownObjects(std::ostream& out = std::cout) const;
953 
956  static bool isEmpty(const moveit_msgs::PlanningScene& msg);
957 
960  static bool isEmpty(const moveit_msgs::PlanningSceneWorld& msg);
961 
963  static bool isEmpty(const moveit_msgs::RobotState& msg);
964 
966  static PlanningScenePtr clone(const PlanningSceneConstPtr& scene);
967 
968 private:
969  /* Private constructor used by the diff() methods. */
970  PlanningScene(const PlanningSceneConstPtr& parent);
971 
972  /* Initialize the scene. This should only be called by the constructors.
973  * Requires a valid robot_model_ */
974  void initialize();
975 
976  /* helper function to create a RobotModel from a urdf/srdf. */
977  static robot_model::RobotModelPtr createRobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model,
978  const srdf::ModelConstSharedPtr& srdf_model);
979 
980  /* Helper functions for processing collision objects */
981  bool processCollisionObjectAdd(const moveit_msgs::CollisionObject& object);
982  bool processCollisionObjectRemove(const moveit_msgs::CollisionObject& object);
983  bool processCollisionObjectMove(const moveit_msgs::CollisionObject& object);
984 
986  static void poseMsgToEigen(const geometry_msgs::Pose& msg, Eigen::Isometry3d& out);
987 
989 
990  /* \brief A set of compatible collision detectors */
992  {
993  collision_detection::CollisionDetectorAllocatorPtr alloc_;
994  collision_detection::CollisionRobotPtr crobot_unpadded_; // if NULL use parent's
995  collision_detection::CollisionRobotConstPtr crobot_unpadded_const_;
996  collision_detection::CollisionRobotPtr crobot_; // if NULL use parent's
997  collision_detection::CollisionRobotConstPtr crobot_const_;
998 
999  collision_detection::CollisionWorldPtr cworld_; // never NULL
1000  collision_detection::CollisionWorldConstPtr cworld_const_;
1001 
1002  CollisionDetectorConstPtr parent_; // may be NULL
1003 
1004  const collision_detection::CollisionRobotConstPtr& getCollisionRobot() const
1005  {
1006  return crobot_const_ ? crobot_const_ : parent_->getCollisionRobot();
1007  }
1008  const collision_detection::CollisionRobotConstPtr& getCollisionRobotUnpadded() const
1009  {
1010  return crobot_unpadded_const_ ? crobot_unpadded_const_ : parent_->getCollisionRobotUnpadded();
1011  }
1012  void findParent(const PlanningScene& scene);
1013  void copyPadding(const CollisionDetector& src);
1014  };
1015  friend struct CollisionDetector;
1016 
1017  typedef std::map<std::string, CollisionDetectorPtr>::iterator CollisionDetectorIterator;
1018  typedef std::map<std::string, CollisionDetectorPtr>::const_iterator CollisionDetectorConstIterator;
1019 
1022 
1023  std::string name_; // may be empty
1024 
1025  PlanningSceneConstPtr parent_; // Null unless this is a diff scene
1026 
1027  robot_model::RobotModelConstPtr robot_model_; // Never null (may point to same model as parent)
1028 
1029  robot_state::RobotStatePtr robot_state_; // if NULL use parent's
1030 
1031  // Called when changes are made to attached bodies
1033 
1034  // This variable is not necessarily used by child planning scenes
1035  // This Transforms class is actually a SceneTransform class
1036  robot_state::TransformsPtr scene_transforms_; // if NULL use parent's
1037 
1038  collision_detection::WorldPtr world_; // never NULL, never shared with parent/child
1039  collision_detection::WorldConstPtr world_const_; // copy of world_
1040  collision_detection::WorldDiffPtr world_diff_; // NULL unless this is a diff scene
1043 
1044  std::map<std::string, CollisionDetectorPtr> collision_; // never empty
1045  CollisionDetectorPtr active_collision_; // copy of one of the entries in collision_. Never NULL.
1046 
1047  collision_detection::AllowedCollisionMatrixPtr acm_; // if NULL use parent's
1048 
1049  StateFeasibilityFn state_feasibility_;
1050  MotionFeasibilityFn motion_feasibility_;
1051 
1052  std::unique_ptr<ObjectColorMap> object_colors_;
1053 
1054  // a map of object types
1055  std::unique_ptr<ObjectTypeMap> object_types_;
1056 };
1057 }
1058 
1059 #endif
const collision_detection::AllowedCollisionMatrix & getAllowedCollisionMatrix() const
Get the allowed collision matrix.
MOVEIT_CLASS_FORWARD(PlanningScene)
bool isStateColliding(const std::string &group="", bool verbose=false) const
Check if the current state is in collision (with the environment or self collision). If a group name is specified, collision checking is done for that group only. It is expected the current state transforms are up to date.
void checkCollisionUnpadded(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, robot_state::RobotState &robot_state) const
Check whether a specified state (robot_state) is in collision, but use a collision_detection::Collisi...
bool hasObjectColor(const std::string &id) const
collision_detection::CollisionWorldConstPtr cworld_const_
void setStateFeasibilityPredicate(const StateFeasibilityFn &fn)
Specify a predicate that decides whether states are considered valid or invalid for reasons beyond on...
Core components of MoveIt!
void checkCollisionUnpadded(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, robot_state::RobotState &robot_state, const collision_detection::AllowedCollisionMatrix &acm) const
Check whether a specified state (robot_state) is in collision, with respect to a given allowed collis...
robot_state::RobotState & getCurrentStateNonConst()
Get the state at which the robot is assumed to be.
bool loadGeometryFromStream(std::istream &in)
Load the geometry of the planning scene from a stream.
const PlanningSceneConstPtr & getParent() const
Get the parent scene (whith respect to which the diffs are maintained). This may be empty...
bool isPathValid(const moveit_msgs::RobotState &start_state, const moveit_msgs::RobotTrajectory &trajectory, const std::string &group="", bool verbose=false, std::vector< std::size_t > *invalid_index=NULL) const
Check if a given path is valid. Each state is checked for validity (collision avoidance and feasibili...
void setObjectType(const std::string &id, const object_recognition_msgs::ObjectType &type)
void decoupleParent()
Make sure that all the data maintained in this scene is local. All unmodified data is copied from the...
void processOctomapMsg(const octomap_msgs::OctomapWithPose &map)
MotionFeasibilityFn motion_feasibility_
void getCollidingPairs(collision_detection::CollisionResult::ContactMap &contacts, robot_state::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...
static bool isEmpty(const moveit_msgs::PlanningScene &msg)
Check if a message includes any information about a planning scene, or it is just a default...
double distanceToCollisionUnpadded(const robot_state::RobotState &robot_state) const
The distance between the robot model at state robot_state to the nearest collision (ignoring self-col...
void checkSelfCollision(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, robot_state::RobotState &robot_state) const
Check whether a specified state (robot_state) is in self collision.
void removeObjectColor(const std::string &id)
bool isStateFeasible(const moveit_msgs::RobotState &state, bool verbose=false) const
Check if a given state is feasible, in accordance to the feasibility predicate specified by setStateF...
StateFeasibilityFn state_feasibility_
double distanceToCollision(robot_state::RobotState &robot_state, const collision_detection::AllowedCollisionMatrix &acm) const
The distance between the robot model at state robot_state to the nearest collision, ignoring self-collisions and elements that are allowed to collide.
bool getCollisionObjectMsg(moveit_msgs::CollisionObject &collision_obj, const std::string &ns) const
Construct a message (collision_object) with the collision object data from the planning_scene for the...
void checkSelfCollision(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, const robot_state::RobotState &robot_state) const
Check whether a specified state (robot_state) is in self collision.
void checkSelfCollision(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res)
Check whether the current state is in self collision.
const robot_state::Transforms & getTransforms() const
Get the set of fixed transforms from known frames to the planning frame.
static const std::string DEFAULT_SCENE_NAME
bool knowsFrameTransform(const std::string &id) const
Check if a transform to the frame id is known. This will be known if id is a link name...
void getPlanningSceneDiffMsg(moveit_msgs::PlanningScene &scene) const
Fill the message scene with the differences between this instance of PlanningScene with respect to th...
bool hasObjectType(const std::string &id) const
bool processCollisionObjectAdd(const moveit_msgs::CollisionObject &object)
const collision_detection::CollisionRobotConstPtr & getCollisionRobot() const
void getKnownObjectColors(ObjectColorMap &kc) const
Maintain a representation of the environment.
Definition: world.h:60
void saveGeometryToStream(std::ostream &out) const
Save the geometry of the planning scene to a stream, as plain text.
const StateFeasibilityFn & getStateFeasibilityPredicate() const
Get the predicate that decides whether states are considered valid or invalid for reasons beyond ones...
bool processAttachedCollisionObjectMsg(const moveit_msgs::AttachedCollisionObject &object)
A class that contains many different constraints, and can check RobotState *versus the full set...
bool processPlanningSceneWorldMsg(const moveit_msgs::PlanningSceneWorld &world)
void getCollidingLinks(std::vector< std::string > &links, const robot_state::RobotState &robot_state) const
Get the names of the links that are involved in collisions for the state robot_state.
void printKnownObjects(std::ostream &out=std::cout) const
Outputs debug information about the planning scene contents.
void getCollidingLinks(std::vector< std::string > &links, robot_state::RobotState &robot_state) const
Get the names of the links that are involved in collisions for the state robot_state. Update the link transforms for robot_state if needed.
void getCollidingLinks(std::vector< std::string > &links, robot_state::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...
void removeObjectType(const std::string &id)
const collision_detection::CollisionRobotPtr & getCollisionRobotNonConst()
Get the representation of the collision robot This can be used to set padding and link scale on the a...
std::map< std::string, CollisionDetectorPtr >::const_iterator CollisionDetectorConstIterator
const object_recognition_msgs::ObjectType & getObjectType(const std::string &id) const
void setObjectColor(const std::string &id, const std_msgs::ColorRGBA &color)
void getCostSources(const robot_trajectory::RobotTrajectory &trajectory, std::size_t max_costs, std::set< collision_detection::CostSource > &costs, double overlap_fraction=0.9) const
Get the top max_costs cost sources for a specified trajectory. The resulting costs are stored in cost...
void getAttachedCollisionObjectMsgs(std::vector< moveit_msgs::AttachedCollisionObject > &attached_collision_objs) const
Construct a vector of messages (attached_collision_objects) with the attached collision object data f...
collision_detection::CollisionRobotPtr crobot_
static const std::string OCTOMAP_NS
void getObjectColorMsgs(std::vector< moveit_msgs::ObjectColor > &object_colors) const
Construct a vector of messages (object_colors) with the colors of the objects from the planning_scene...
bool processCollisionObjectMsg(const moveit_msgs::CollisionObject &object)
geometry_msgs::TransformStamped t
Provides an implementation of a snapshot of a transform tree that can be easily queried for transform...
Definition: transforms.h:61
void copyPadding(const CollisionDetector &src)
void getKnownObjectTypes(ObjectTypeMap &kc) const
collision_detection::AllowedCollisionMatrixPtr acm_
collision_detection::CollisionWorldPtr cworld_
static robot_model::RobotModelPtr createRobotModel(const urdf::ModelInterfaceSharedPtr &urdf_model, const srdf::ModelConstSharedPtr &srdf_model)
PlanningScene(const robot_model::RobotModelConstPtr &robot_model, const collision_detection::WorldPtr &world=collision_detection::WorldPtr(new collision_detection::World()))
construct using an existing RobotModel
const robot_model::RobotModelConstPtr & getRobotModel() const
Get the kinematic model for which the planning scene is maintained.
const std::string & getName() const
Get the name of the planning scene. This is empty by default.
void setCollisionObjectUpdateCallback(const collision_detection::World::ObserverCallbackFn &callback)
Set the callback to be triggered when changes are made to the current scene world.
robot_model::RobotModelConstPtr robot_model_
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
collision_detection::WorldConstPtr world_const_
const collision_detection::WorldPtr & getWorldNonConst()
static PlanningScenePtr clone(const PlanningSceneConstPtr &scene)
Clone a planning scene. Even if the scene scene depends on a parent, the cloned scene will not...
robot_trajectory::RobotTrajectory trajectory(rmodel, "right_arm")
std::unique_ptr< ObjectColorMap > object_colors_
std::map< std::pair< std::string, std::string >, std::vector< Contact > > ContactMap
MOVEIT_STRUCT_FORWARD(CollisionDetector)
void processOctomapPtr(const std::shared_ptr< const octomap::OcTree > &octree, const Eigen::Isometry3d &t)
boost::function< void(const ObjectConstPtr &, Action)> ObserverCallbackFn
Definition: world.h:232
std::map< std::string, object_recognition_msgs::ObjectType > ObjectTypeMap
A map from object names (e.g., attached bodies, collision objects) to their types.
std::unique_ptr< ObjectTypeMap > object_types_
void setMotionFeasibilityPredicate(const MotionFeasibilityFn &fn)
Specify a predicate that decides whether motion segments are considered valid or invalid for reasons ...
bool verbose
robot_state::Transforms & getTransformsNonConst()
Get the set of fixed transforms from known frames to the planning frame.
robot_state::RobotStatePtr robot_state_
void getCollisionObjectMsgs(std::vector< moveit_msgs::CollisionObject > &collision_objs) const
Construct a vector of messages (collision_objects) with the collision object data for all objects in ...
std::shared_ptr< const Model > ModelConstSharedPtr
robot_state::AttachedBodyCallback current_state_attached_body_callback_
collision_detection::CollisionDetectorAllocatorPtr alloc_
boost::function< void(AttachedBody *body, bool attached)> AttachedBodyCallback
Definition: attached_body.h:50
This class maintains the representation of the environment as seen by a planning instance. The environment geometry, the robot geometry and state are maintained.
void findParent(const PlanningScene &scene)
void getCollidingLinks(std::vector< std::string > &links)
Get the names of the links that are involved in collisions for the current state. ...
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
double distanceToCollisionUnpadded(const robot_state::RobotState &robot_state, const collision_detection::AllowedCollisionMatrix &acm) const
The distance between the robot model at state robot_state to the nearest collision, ignoring self-collisions and elements that always allowed to collide, if the robot has no padding.
void propogateRobotPadding()
Copy scale and padding from active CollisionRobot to other CollisionRobots. This should be called aft...
std::map< std::string, std_msgs::ColorRGBA > ObjectColorMap
A map from object names (e.g., attached bodies, collision objects) to their colors.
void checkCollision(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res)
Check whether the current state is in collision, and if needed, updates the collision transforms of t...
collision_detection::CollisionRobotPtr crobot_unpadded_
void setAttachedBodyUpdateCallback(const robot_state::AttachedBodyCallback &callback)
Set the callback to be triggered when changes are made to the current scene state.
Maintain a sequence of waypoints and the time durations between these waypoints.
robot_state::RobotStatePtr getCurrentStateUpdated(const moveit_msgs::RobotState &update) const
Get a copy of the current state with components overwritten by the state message update.
void getCollisionDetectorNames(std::vector< std::string > &names) const
get the types of collision detector that have already been added. These are the types which can be pa...
void getPlanningSceneMsg(moveit_msgs::PlanningScene &scene) const
Construct a message (scene) with all the necessary data so that the scene can be later reconstructed ...
void checkCollision(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, robot_state::RobotState &robot_state) const
Check whether a specified state (robot_state) is in collision. This variant of the function takes a n...
void getCollidingPairs(collision_detection::CollisionResult::ContactMap &contacts)
Get the names of the links that are involved in collisions for the current state. Update the link tra...
double distanceToCollision(const robot_state::RobotState &robot_state, const collision_detection::AllowedCollisionMatrix &acm) const
The distance between the robot model at state robot_state to the nearest collision, ignoring self-collisions and elements that are allowed to collide.
void setActiveCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr &allocator, bool exclusive=false)
Set the type of collision detector to use. Calls addCollisionDetector() to add it if it has not alrea...
const std::string & getPlanningFrame() const
Get the frame in which planning is performed.
const collision_detection::CollisionRobotConstPtr & getCollisionRobotUnpadded() const
Get the active collision detector for the robot.
bool setPlanningSceneMsg(const moveit_msgs::PlanningScene &scene)
Set this instance of a planning scene to be the same as the one serialized in the scene message...
void checkCollisionUnpadded(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, const robot_state::RobotState &robot_state) const
Check whether a specified state (robot_state) is in collision, but use a collision_detection::Collisi...
This namespace includes the central class for representing planning contexts.
void checkSelfCollision(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, robot_state::RobotState &robot_state, const collision_detection::AllowedCollisionMatrix &acm) const
Check whether a specified state (robot_state) is in self collision, with respect to a given allowed c...
bool getOctomapMsg(octomap_msgs::OctomapWithPose &octomap) const
Construct a message (octomap) with the octomap data from the planning_scene.
collision_detection::AllowedCollisionMatrix & getAllowedCollisionMatrixNonConst()
Get the allowed collision matrix.
void getCollidingPairs(collision_detection::CollisionResult::ContactMap &contacts) const
Get the names of the links that are involved in collisions for the current state. ...
void checkSelfCollision(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, const robot_state::RobotState &robot_state, const collision_detection::AllowedCollisionMatrix &acm) const
Check whether a specified state (robot_state) is in self collision, with respect to a given allowed c...
void removeAllCollisionObjects()
Clear all collision objects in planning scene.
CollisionDetectorPtr active_collision_
bool isStateConstrained(const moveit_msgs::RobotState &state, const moveit_msgs::Constraints &constr, bool verbose=false) const
Check if a given state satisfies a set of constraints.
collision_detection::World::ObserverCallbackFn current_world_object_update_callback_
collision_detection::WorldDiffPtr world_diff_
void addCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr &allocator)
Add a new collision detector type.
std::map< std::string, CollisionDetectorPtr > collision_
PlanningScenePtr diff() const
Return a new child PlanningScene that uses this one as parent.
void setName(const std::string &name)
Set the name of the planning scene.
void pushDiffs(const PlanningScenePtr &scene)
If there is a parent specified for this scene, then the diffs with respect to that parent are applied...
const std_msgs::ColorRGBA & getObjectColor(const std::string &id) const
bool processCollisionObjectRemove(const moveit_msgs::CollisionObject &object)
const std::string & getActiveCollisionDetectorName() const
bool isStateValid(const moveit_msgs::RobotState &state, const std::string &group="", bool verbose=false) const
Check if a given state is valid. This means checking for collisions and feasibility.
bool usePlanningSceneMsg(const moveit_msgs::PlanningScene &scene)
Call setPlanningSceneMsg() or setPlanningSceneDiffMsg() depending on how the is_diff member of the me...
void clearDiffs()
Clear the diffs accumulated for this planning scene, with respect to the parent. This function is a n...
robot_state::TransformsPtr scene_transforms_
collision_detection::World::ObserverHandle current_world_object_update_observer_handle_
void setCurrentState(const moveit_msgs::RobotState &state)
Set the current robot state to be state. If not all joint values are specified, the previously mainta...
double distanceToCollision(const robot_state::RobotState &robot_state) const
The distance between the robot model at state robot_state to the nearest collision (ignoring self-col...
std::map< std::string, CollisionDetectorPtr >::iterator CollisionDetectorIterator
collision_detection::WorldPtr world_
static void poseMsgToEigen(const geometry_msgs::Pose &msg, Eigen::Isometry3d &out)
void getCollidingPairs(collision_detection::CollisionResult::ContactMap &contacts, robot_state::RobotState &robot_state) const
Get the names of the links that are involved in collisions for the state robot_state. Update the link transforms for robot_state if needed.
void updateCollisionBodyTransforms()
Update the transforms for the collision bodies. This call is needed before calling collision checking...
double distanceToCollisionUnpadded(robot_state::RobotState &robot_state) const
The distance between the robot model at state robot_state to the nearest collision (ignoring self-col...
void getCollidingPairs(collision_detection::CollisionResult::ContactMap &contacts, const robot_state::RobotState &robot_state) const
Get the names of the links that are involved in collisions for the state robot_state.
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:122
void updateLinkTransforms()
Update the reference frame transforms for links. This call is needed before using the transforms of l...
bool processCollisionObjectMove(const moveit_msgs::CollisionObject &object)
bool getAttachedCollisionObjectMsg(moveit_msgs::AttachedCollisionObject &attached_collision_obj, const std::string &ns) const
Construct a message (attached_collision_object) with the attached collision object data from the plan...
void checkCollisionUnpadded(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res) const
Check whether the current state is in collision, but use a collision_detection::CollisionRobot instan...
const MotionFeasibilityFn & getMotionFeasibilityPredicate() const
Get the predicate that decides whether motion segments are considered valid or invalid for reasons be...
const collision_detection::CollisionRobotConstPtr & getCollisionRobotUnpadded() const
void checkCollision(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res) const
Check whether the current state is in collision. The current state is expected to be updated...
void getCollidingLinks(std::vector< std::string > &links) const
Get the names of the links that are involved in collisions for the current state. ...
void checkCollision(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, robot_state::RobotState &robot_state, const collision_detection::AllowedCollisionMatrix &acm) const
Check whether a specified state (robot_state) is in collision, with respect to a given allowed collis...
collision_detection::CollisionRobotConstPtr crobot_const_
const collision_detection::CollisionRobotConstPtr & getCollisionRobot() const
Get the active collision detector for the robot.
double distanceToCollisionUnpadded(robot_state::RobotState &robot_state, const collision_detection::AllowedCollisionMatrix &acm) const
The distance between the robot model at state robot_state to the nearest collision, ignoring self-collisions and elements that are allowed to collide, if the robot has no padding.
const robot_state::RobotState & getCurrentState() const
Get the state at which the robot is assumed to be.
const Eigen::Isometry3d & getFrameTransform(robot_state::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...
const collision_detection::WorldConstPtr & getWorld() const
Get the representation of the world.
void checkSelfCollision(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res) const
Check whether the current state is in self collision.
PlanningSceneConstPtr parent_
boost::function< bool(const robot_state::RobotState &, const robot_state::RobotState &, bool)> MotionFeasibilityFn
This is the function signature for additional feasibility checks to be imposed on motions segments be...
const collision_detection::CollisionWorldConstPtr & getCollisionWorld() const
Get the active collision detector for the world.
boost::function< bool(const robot_state::RobotState &, bool)> StateFeasibilityFn
This is the function signature for additional feasibility checks to be imposed on states (in addition...
bool isStateColliding(const std::string &group="", bool verbose=false)
Check if the current state is in collision (with the environment or self collision). If a group name is specified, collision checking is done for that group only. Since the function is non-const, the current state transforms are updated before the collision check.
bool isStateColliding(robot_state::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...
double distanceToCollision(robot_state::RobotState &robot_state) const
The distance between the robot model at state robot_state to the nearest collision (ignoring self-col...
const Eigen::Isometry3d & getFrameTransform(const std::string &id) const
Get the transform corresponding to the frame id. This will be known if id is a link name...
bool setPlanningSceneDiffMsg(const moveit_msgs::PlanningScene &scene)
Apply changes to this planning scene as diffs, even if the message itself is not marked as being a di...
void checkCollisionUnpadded(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res)
Check whether the current state is in collision, but use a collision_detection::CollisionRobot instan...
collision_detection::CollisionRobotConstPtr crobot_unpadded_const_


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Tue Jan 14 2020 03:51:06