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 
50 #include <moveit_msgs/PlanningScene.h>
51 #include <moveit_msgs/RobotTrajectory.h>
52 #include <moveit_msgs/Constraints.h>
53 #include <moveit_msgs/PlanningSceneComponents.h>
54 #include <octomap_msgs/OctomapWithPose.h>
55 #include <boost/noncopyable.hpp>
56 #include <boost/function.hpp>
57 #include <boost/concept_check.hpp>
58 #include <memory>
59 
61 namespace planning_scene
62 {
63 MOVEIT_CLASS_FORWARD(PlanningScene);
64 
69 typedef boost::function<bool(const robot_state::RobotState&, bool)> StateFeasibilityFn;
70 
76 typedef boost::function<bool(const robot_state::RobotState&, const robot_state::RobotState&, bool)> MotionFeasibilityFn;
77 
79 typedef std::map<std::string, std_msgs::ColorRGBA> ObjectColorMap;
80 
82 typedef std::map<std::string, object_recognition_msgs::ObjectType> ObjectTypeMap;
83 
87 class PlanningScene : private boost::noncopyable, public std::enable_shared_from_this<PlanningScene>
88 {
89 public:
92  const robot_model::RobotModelConstPtr& robot_model,
93  const collision_detection::WorldPtr& world = collision_detection::WorldPtr(new collision_detection::World()));
94 
98  const urdf::ModelInterfaceSharedPtr& urdf_model, const srdf::ModelConstSharedPtr& srdf_model,
99  const collision_detection::WorldPtr& world = collision_detection::WorldPtr(new collision_detection::World()));
100 
101  static const std::string OCTOMAP_NS;
102  static const std::string DEFAULT_SCENE_NAME;
103 
104  ~PlanningScene();
105 
107  const std::string& getName() const
108  {
109  return name_;
110  }
111 
113  void setName(const std::string& name)
114  {
115  name_ = name;
116  }
117 
128  PlanningScenePtr diff() const;
129 
132  PlanningScenePtr diff(const moveit_msgs::PlanningScene& msg) const;
133 
135  const PlanningSceneConstPtr& getParent() const
136  {
137  return parent_;
138  }
139 
141  const robot_model::RobotModelConstPtr& getRobotModel() const
142  {
143  // the kinematic model does not change
144  return robot_model_;
145  }
146 
149  {
150  // if we have an updated state, return it; otherwise, return the parent one
151  return robot_state_ ? *robot_state_ : parent_->getCurrentState();
152  }
155 
157  robot_state::RobotStatePtr getCurrentStateUpdated(const moveit_msgs::RobotState& update) const;
158 
165  const std::string& getPlanningFrame() const
166  {
167  // if we have an updated set of transforms, return it; otherwise, return the parent one
168  return scene_transforms_ ? scene_transforms_->getTargetFrame() : parent_->getPlanningFrame();
169  }
170 
173  {
174  if (scene_transforms_ || !parent_)
175  {
176  return *scene_transforms_;
177  }
178 
179  // if this planning scene is a child of another, and doesn't have its own custom transforms
180  return parent_->getTransforms();
181  }
182 
186 
189 
194  const Eigen::Isometry3d& getFrameTransform(const std::string& id) const;
195 
201  const Eigen::Isometry3d& getFrameTransform(const std::string& id);
202 
208  const Eigen::Isometry3d& getFrameTransform(robot_state::RobotState& state, const std::string& id) const
209  {
210  state.updateLinkTransforms();
211  return getFrameTransform(static_cast<const robot_state::RobotState&>(state), id);
212  }
213 
218  const Eigen::Isometry3d& getFrameTransform(const robot_state::RobotState& state, const std::string& id) const;
219 
222  bool knowsFrameTransform(const std::string& id) const;
223 
226  bool knowsFrameTransform(const robot_state::RobotState& state, const std::string& id) const;
227 
252  void addCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr& allocator);
253 
263  void setActiveCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr& allocator,
264  bool exclusive = false);
265 
272  bool setActiveCollisionDetector(const std::string& collision_detector_name);
273 
274  const std::string& getActiveCollisionDetectorName() const
275  {
276  return active_collision_->alloc_->getName();
277  }
278 
281  void getCollisionDetectorNames(std::vector<std::string>& names) const;
282 
284  const collision_detection::WorldConstPtr& getWorld() const
285  {
286  // we always have a world representation
287  return world_const_;
288  }
289 
290  // brief Get the representation of the world
291  const collision_detection::WorldPtr& getWorldNonConst()
292  {
293  // we always have a world representation
294  return world_;
295  }
296 
298  const collision_detection::CollisionWorldConstPtr& getCollisionWorld() const
299  {
300  // we always have a world representation after configure is called.
301  return active_collision_->cworld_const_;
302  }
303 
305  const collision_detection::CollisionRobotConstPtr& getCollisionRobot() const
306  {
307  return active_collision_->getCollisionRobot();
308  }
309 
311  const collision_detection::CollisionRobotConstPtr& getCollisionRobotUnpadded() const
312  {
313  return active_collision_->getCollisionRobotUnpadded();
314  }
315 
317  const collision_detection::CollisionWorldConstPtr&
318  getCollisionWorld(const std::string& collision_detector_name) const;
319 
321  const collision_detection::CollisionRobotConstPtr&
322  getCollisionRobot(const std::string& collision_detector_name) const;
323 
326  const collision_detection::CollisionRobotConstPtr&
327  getCollisionRobotUnpadded(const std::string& collision_detector_name) const;
328 
333  const collision_detection::CollisionRobotPtr& getCollisionRobotNonConst();
334 
338  void propogateRobotPadding();
339 
342  {
343  return acm_ ? *acm_ : parent_->getAllowedCollisionMatrix();
344  }
347 
358  bool isStateColliding(const std::string& group = "", bool verbose = false);
359 
363  bool isStateColliding(const std::string& group = "", bool verbose = false) const
364  {
365  return isStateColliding(getCurrentState(), group, verbose);
366  }
367 
372  bool isStateColliding(robot_state::RobotState& state, const std::string& group = "", bool verbose = false) const
373  {
375  return isStateColliding(static_cast<const robot_state::RobotState&>(state), group, verbose);
376  }
377 
381  bool isStateColliding(const robot_state::RobotState& state, const std::string& group = "",
382  bool verbose = false) const;
383 
386  bool isStateColliding(const moveit_msgs::RobotState& state, const std::string& group = "",
387  bool verbose = false) const;
388 
392 
395  {
396  checkCollision(req, res, getCurrentState());
397  }
398 
403  {
404  robot_state.updateCollisionBodyTransforms();
405  checkCollision(req, res, static_cast<const robot_state::RobotState&>(robot_state));
406  }
407 
412  const robot_state::RobotState& robot_state) const;
413 
418  robot_state::RobotState& robot_state,
420  {
421  robot_state.updateCollisionBodyTransforms();
422  checkCollision(req, res, static_cast<const robot_state::RobotState&>(robot_state), acm);
423  }
424 
428  const robot_state::RobotState& robot_state,
430 
436 
441  {
443  }
444 
449  const robot_state::RobotState& robot_state) const
450  {
451  checkCollisionUnpadded(req, res, robot_state, getAllowedCollisionMatrix());
452  }
453 
459  {
460  robot_state.updateCollisionBodyTransforms();
461  checkCollisionUnpadded(req, res, static_cast<const robot_state::RobotState&>(robot_state),
463  }
464 
471  {
472  robot_state.updateCollisionBodyTransforms();
473  checkCollisionUnpadded(req, res, static_cast<const robot_state::RobotState&>(robot_state), acm);
474  }
475 
481 
484 
488  {
489  checkSelfCollision(req, res, getCurrentState());
490  }
491 
494  robot_state::RobotState& robot_state) const
495  {
496  robot_state.updateCollisionBodyTransforms();
497  checkSelfCollision(req, res, static_cast<const robot_state::RobotState&>(robot_state), getAllowedCollisionMatrix());
498  }
499 
502  const robot_state::RobotState& robot_state) const
503  {
504  // do self-collision checking with the unpadded version of the robot
505  getCollisionRobotUnpadded()->checkSelfCollision(req, res, robot_state, getAllowedCollisionMatrix());
506  }
507 
511  robot_state::RobotState& robot_state,
513  {
514  robot_state.updateCollisionBodyTransforms();
515  checkSelfCollision(req, res, static_cast<const robot_state::RobotState&>(robot_state), acm);
516  }
517 
521  const robot_state::RobotState& robot_state,
523  {
524  // do self-collision checking with the unpadded version of the robot
525  getCollisionRobotUnpadded()->checkSelfCollision(req, res, robot_state, acm);
526  }
527 
529  void getCollidingLinks(std::vector<std::string>& links);
530 
532  void getCollidingLinks(std::vector<std::string>& links) const
533  {
535  }
536 
539  void getCollidingLinks(std::vector<std::string>& links, robot_state::RobotState& robot_state) const
540  {
541  robot_state.updateCollisionBodyTransforms();
542  getCollidingLinks(links, static_cast<const robot_state::RobotState&>(robot_state), getAllowedCollisionMatrix());
543  }
544 
546  void getCollidingLinks(std::vector<std::string>& links, const robot_state::RobotState& robot_state) const
547  {
548  getCollidingLinks(links, robot_state, getAllowedCollisionMatrix());
549  }
550 
553  void getCollidingLinks(std::vector<std::string>& links, robot_state::RobotState& robot_state,
555  {
556  robot_state.updateCollisionBodyTransforms();
557  getCollidingLinks(links, static_cast<const robot_state::RobotState&>(robot_state), acm);
558  }
559 
562  void getCollidingLinks(std::vector<std::string>& links, const robot_state::RobotState& robot_state,
564 
568 
571  {
573  }
574 
577  const robot_state::RobotState& robot_state) const
578  {
579  getCollidingPairs(contacts, robot_state, getAllowedCollisionMatrix());
580  }
581 
585  robot_state::RobotState& robot_state) const
586  {
587  robot_state.updateCollisionBodyTransforms();
588  getCollidingPairs(contacts, static_cast<const robot_state::RobotState&>(robot_state), getAllowedCollisionMatrix());
589  }
590 
594  robot_state::RobotState& robot_state,
596  {
597  robot_state.updateCollisionBodyTransforms();
598  getCollidingPairs(contacts, static_cast<const robot_state::RobotState&>(robot_state), acm);
599  }
600 
604  const robot_state::RobotState& robot_state,
606 
617  double distanceToCollision(robot_state::RobotState& robot_state) const
618  {
619  robot_state.updateCollisionBodyTransforms();
620  return distanceToCollision(static_cast<const robot_state::RobotState&>(robot_state));
621  }
622 
626  double distanceToCollision(const robot_state::RobotState& robot_state) const
627  {
628  return getCollisionWorld()->distanceRobot(*getCollisionRobot(), robot_state, getAllowedCollisionMatrix());
629  }
630 
634  {
635  robot_state.updateCollisionBodyTransforms();
636  return distanceToCollisionUnpadded(static_cast<const robot_state::RobotState&>(robot_state));
637  }
638 
641  double distanceToCollisionUnpadded(const robot_state::RobotState& robot_state) const
642  {
643  return getCollisionWorld()->distanceRobot(*getCollisionRobotUnpadded(), robot_state, getAllowedCollisionMatrix());
644  }
645 
651  {
652  robot_state.updateCollisionBodyTransforms();
653  return distanceToCollision(static_cast<const robot_state::RobotState&>(robot_state), acm);
654  }
655 
659  double distanceToCollision(const robot_state::RobotState& robot_state,
661  {
662  return getCollisionWorld()->distanceRobot(*getCollisionRobot(), robot_state, acm);
663  }
664 
670  {
671  robot_state.updateCollisionBodyTransforms();
672  return distanceToCollisionUnpadded(static_cast<const robot_state::RobotState&>(robot_state), acm);
673  }
674 
680  {
681  return getCollisionWorld()->distanceRobot(*getCollisionRobotUnpadded(), robot_state, acm);
682  }
683 
687  void saveGeometryToStream(std::ostream& out) const;
688 
690  bool loadGeometryFromStream(std::istream& in);
691 
693  bool loadGeometryFromStream(std::istream& in, const Eigen::Isometry3d& offset);
694 
699  void getPlanningSceneDiffMsg(moveit_msgs::PlanningScene& scene) const;
700 
704  void getPlanningSceneMsg(moveit_msgs::PlanningScene& scene) const;
705 
708  void getPlanningSceneMsg(moveit_msgs::PlanningScene& scene, const moveit_msgs::PlanningSceneComponents& comp) const;
709 
712  bool getCollisionObjectMsg(moveit_msgs::CollisionObject& collision_obj, const std::string& ns) const;
713 
716  void getCollisionObjectMsgs(std::vector<moveit_msgs::CollisionObject>& collision_objs) const;
717 
720  bool getAttachedCollisionObjectMsg(moveit_msgs::AttachedCollisionObject& attached_collision_obj,
721  const std::string& ns) const;
722 
725  void getAttachedCollisionObjectMsgs(std::vector<moveit_msgs::AttachedCollisionObject>& attached_collision_objs) const;
726 
728  bool getOctomapMsg(octomap_msgs::OctomapWithPose& octomap) const;
729 
731  void getObjectColorMsgs(std::vector<moveit_msgs::ObjectColor>& object_colors) const;
732 
738  bool setPlanningSceneDiffMsg(const moveit_msgs::PlanningScene& scene);
739 
742  bool setPlanningSceneMsg(const moveit_msgs::PlanningScene& scene);
743 
746  bool usePlanningSceneMsg(const moveit_msgs::PlanningScene& scene);
747 
748  bool processCollisionObjectMsg(const moveit_msgs::CollisionObject& object);
749  bool processAttachedCollisionObjectMsg(const moveit_msgs::AttachedCollisionObject& object);
750 
751  bool processPlanningSceneWorldMsg(const moveit_msgs::PlanningSceneWorld& world);
752 
753  void processOctomapMsg(const octomap_msgs::OctomapWithPose& map);
754  void processOctomapMsg(const octomap_msgs::Octomap& map);
755  void processOctomapPtr(const std::shared_ptr<const octomap::OcTree>& octree, const Eigen::Isometry3d& t);
756 
761 
765  void setCurrentState(const moveit_msgs::RobotState& state);
766 
768  void setCurrentState(const robot_state::RobotState& state);
769 
772 
775 
776  bool hasObjectColor(const std::string& id) const;
777 
778  const std_msgs::ColorRGBA& getObjectColor(const std::string& id) const;
779  void setObjectColor(const std::string& id, const std_msgs::ColorRGBA& color);
780  void removeObjectColor(const std::string& id);
781  void getKnownObjectColors(ObjectColorMap& kc) const;
782 
783  bool hasObjectType(const std::string& id) const;
784 
785  const object_recognition_msgs::ObjectType& getObjectType(const std::string& id) const;
786  void setObjectType(const std::string& id, const object_recognition_msgs::ObjectType& type);
787  void removeObjectType(const std::string& id);
788  void getKnownObjectTypes(ObjectTypeMap& kc) const;
789 
792  void clearDiffs();
793 
797  void pushDiffs(const PlanningScenePtr& scene);
798 
802  void decoupleParent();
803 
807  void setStateFeasibilityPredicate(const StateFeasibilityFn& fn)
808  {
809  state_feasibility_ = fn;
810  }
811 
814  const StateFeasibilityFn& getStateFeasibilityPredicate() const
815  {
816  return state_feasibility_;
817  }
818 
821  void setMotionFeasibilityPredicate(const MotionFeasibilityFn& fn)
822  {
823  motion_feasibility_ = fn;
824  }
825 
828  const MotionFeasibilityFn& getMotionFeasibilityPredicate() const
829  {
830  return motion_feasibility_;
831  }
832 
835  bool isStateFeasible(const moveit_msgs::RobotState& state, bool verbose = false) const;
836 
839  bool isStateFeasible(const robot_state::RobotState& state, bool verbose = false) const;
840 
842  bool isStateConstrained(const moveit_msgs::RobotState& state, const moveit_msgs::Constraints& constr,
843  bool verbose = false) const;
844 
846  bool isStateConstrained(const robot_state::RobotState& state, const moveit_msgs::Constraints& constr,
847  bool verbose = false) const;
848 
850  bool isStateConstrained(const moveit_msgs::RobotState& state,
851  const kinematic_constraints::KinematicConstraintSet& constr, bool verbose = false) const;
852 
855  const kinematic_constraints::KinematicConstraintSet& constr, bool verbose = false) const;
856 
858  bool isStateValid(const moveit_msgs::RobotState& state, const std::string& group = "", bool verbose = false) const;
859 
861  bool isStateValid(const robot_state::RobotState& state, const std::string& group = "", bool verbose = false) const;
862 
865  bool isStateValid(const moveit_msgs::RobotState& state, const moveit_msgs::Constraints& constr,
866  const std::string& group = "", bool verbose = false) const;
867 
870  bool isStateValid(const robot_state::RobotState& state, const moveit_msgs::Constraints& constr,
871  const std::string& group = "", bool verbose = false) const;
872 
876  const std::string& group = "", bool verbose = false) const;
877 
879  bool isPathValid(const moveit_msgs::RobotState& start_state, const moveit_msgs::RobotTrajectory& trajectory,
880  const std::string& group = "", bool verbose = false,
881  std::vector<std::size_t>* invalid_index = NULL) const;
882 
886  bool isPathValid(const moveit_msgs::RobotState& start_state, const moveit_msgs::RobotTrajectory& trajectory,
887  const moveit_msgs::Constraints& path_constraints, const std::string& group = "",
888  bool verbose = false, std::vector<std::size_t>* invalid_index = NULL) const;
889 
893  bool isPathValid(const moveit_msgs::RobotState& start_state, const moveit_msgs::RobotTrajectory& trajectory,
894  const moveit_msgs::Constraints& path_constraints, const moveit_msgs::Constraints& goal_constraints,
895  const std::string& group = "", bool verbose = false,
896  std::vector<std::size_t>* invalid_index = NULL) const;
897 
901  bool isPathValid(const moveit_msgs::RobotState& start_state, const moveit_msgs::RobotTrajectory& trajectory,
902  const moveit_msgs::Constraints& path_constraints,
903  const std::vector<moveit_msgs::Constraints>& goal_constraints, const std::string& group = "",
904  bool verbose = false, std::vector<std::size_t>* invalid_index = NULL) const;
905 
909  bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory,
910  const moveit_msgs::Constraints& path_constraints,
911  const std::vector<moveit_msgs::Constraints>& goal_constraints, const std::string& group = "",
912  bool verbose = false, std::vector<std::size_t>* invalid_index = NULL) const;
913 
917  bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory,
918  const moveit_msgs::Constraints& path_constraints, const moveit_msgs::Constraints& goal_constraints,
919  const std::string& group = "", bool verbose = false,
920  std::vector<std::size_t>* invalid_index = NULL) const;
921 
924  bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory,
925  const moveit_msgs::Constraints& path_constraints, const std::string& group = "",
926  bool verbose = false, std::vector<std::size_t>* invalid_index = NULL) const;
927 
929  bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory, const std::string& group = "",
930  bool verbose = false, std::vector<std::size_t>* invalid_index = NULL) const;
931 
934  void getCostSources(const robot_trajectory::RobotTrajectory& trajectory, std::size_t max_costs,
935  std::set<collision_detection::CostSource>& costs, double overlap_fraction = 0.9) const;
936 
939  void getCostSources(const robot_trajectory::RobotTrajectory& trajectory, std::size_t max_costs,
940  const std::string& group_name, std::set<collision_detection::CostSource>& costs,
941  double overlap_fraction = 0.9) const;
942 
944  void getCostSources(const robot_state::RobotState& state, std::size_t max_costs,
945  std::set<collision_detection::CostSource>& costs) const;
946 
949  void getCostSources(const robot_state::RobotState& state, std::size_t max_costs, const std::string& group_name,
950  std::set<collision_detection::CostSource>& costs) const;
951 
953  void printKnownObjects(std::ostream& out = std::cout) const;
954 
957  static bool isEmpty(const moveit_msgs::PlanningScene& msg);
958 
961  static bool isEmpty(const moveit_msgs::PlanningSceneWorld& msg);
962 
964  static bool isEmpty(const moveit_msgs::RobotState& msg);
965 
967  static PlanningScenePtr clone(const PlanningSceneConstPtr& scene);
968 
969 private:
970  /* Private constructor used by the diff() methods. */
971  PlanningScene(const PlanningSceneConstPtr& parent);
972 
973  /* Initialize the scene. This should only be called by the constructors.
974  * Requires a valid robot_model_ */
975  void initialize();
976 
977  /* helper function to create a RobotModel from a urdf/srdf. */
978  static robot_model::RobotModelPtr createRobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model,
979  const srdf::ModelConstSharedPtr& srdf_model);
980 
981  /* Helper functions for processing collision objects */
982  bool processCollisionObjectAdd(const moveit_msgs::CollisionObject& object);
983  bool processCollisionObjectRemove(const moveit_msgs::CollisionObject& object);
984  bool processCollisionObjectMove(const moveit_msgs::CollisionObject& object);
985 
987  static void poseMsgToEigen(const geometry_msgs::Pose& msg, Eigen::Isometry3d& out);
988 
990 
991  /* \brief A set of compatible collision detectors */
993  {
994  collision_detection::CollisionDetectorAllocatorPtr alloc_;
995  collision_detection::CollisionRobotPtr crobot_unpadded_; // if NULL use parent's
996  collision_detection::CollisionRobotConstPtr crobot_unpadded_const_;
997  collision_detection::CollisionRobotPtr crobot_; // if NULL use parent's
998  collision_detection::CollisionRobotConstPtr crobot_const_;
999 
1000  collision_detection::CollisionWorldPtr cworld_; // never NULL
1001  collision_detection::CollisionWorldConstPtr cworld_const_;
1002 
1003  CollisionDetectorConstPtr parent_; // may be NULL
1004 
1005  const collision_detection::CollisionRobotConstPtr& getCollisionRobot() const
1006  {
1007  return crobot_const_ ? crobot_const_ : parent_->getCollisionRobot();
1008  }
1009  const collision_detection::CollisionRobotConstPtr& getCollisionRobotUnpadded() const
1010  {
1011  return crobot_unpadded_const_ ? crobot_unpadded_const_ : parent_->getCollisionRobotUnpadded();
1012  }
1013  void findParent(const PlanningScene& scene);
1014  void copyPadding(const CollisionDetector& src);
1015  };
1016  friend struct CollisionDetector;
1017 
1018  typedef std::map<std::string, CollisionDetectorPtr>::iterator CollisionDetectorIterator;
1019  typedef std::map<std::string, CollisionDetectorPtr>::const_iterator CollisionDetectorConstIterator;
1020 
1023 
1024  std::string name_; // may be empty
1025 
1026  PlanningSceneConstPtr parent_; // Null unless this is a diff scene
1027 
1028  robot_model::RobotModelConstPtr robot_model_; // Never null (may point to same model as parent)
1029 
1030  robot_state::RobotStatePtr robot_state_; // if NULL use parent's
1031 
1032  // Called when changes are made to attached bodies
1034 
1035  // This variable is not necessarily used by child planning scenes
1036  // This Transforms class is actually a SceneTransform class
1037  robot_state::TransformsPtr scene_transforms_; // if NULL use parent's
1038 
1039  collision_detection::WorldPtr world_; // never NULL, never shared with parent/child
1040  collision_detection::WorldConstPtr world_const_; // copy of world_
1041  collision_detection::WorldDiffPtr world_diff_; // NULL unless this is a diff scene
1044 
1045  std::map<std::string, CollisionDetectorPtr> collision_; // never empty
1046  CollisionDetectorPtr active_collision_; // copy of one of the entries in collision_. Never NULL.
1047 
1048  collision_detection::AllowedCollisionMatrixPtr acm_; // if NULL use parent's
1049 
1050  StateFeasibilityFn state_feasibility_;
1051  MotionFeasibilityFn motion_feasibility_;
1052 
1053  std::unique_ptr<ObjectColorMap> object_colors_;
1054 
1055  // a map of object types
1056  std::unique_ptr<ObjectTypeMap> object_types_;
1057 };
1058 }
1059 
1060 #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:123
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 Mon Nov 11 2019 04:01:09