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); // Defines PlanningScenePtr, ConstPtr, WeakPtr... etc
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& getCollisionWorld(const std::string& collision_detector_name) const;
317 
319  const collision_detection::CollisionRobotConstPtr& getCollisionRobot(const std::string& collision_detector_name) const;
320 
323  const collision_detection::CollisionRobotConstPtr&
324  getCollisionRobotUnpadded(const std::string& collision_detector_name) const;
325 
330  const collision_detection::CollisionRobotPtr& getCollisionRobotNonConst();
331 
335  void propogateRobotPadding();
336 
339  {
340  return acm_ ? *acm_ : parent_->getAllowedCollisionMatrix();
341  }
344 
355  bool isStateColliding(const std::string& group = "", bool verbose = false);
356 
360  bool isStateColliding(const std::string& group = "", bool verbose = false) const
361  {
362  return isStateColliding(getCurrentState(), group, verbose);
363  }
364 
369  bool isStateColliding(robot_state::RobotState& state, const std::string& group = "", bool verbose = false) const
370  {
372  return isStateColliding(static_cast<const robot_state::RobotState&>(state), group, verbose);
373  }
374 
378  bool isStateColliding(const robot_state::RobotState& state, const std::string& group = "", bool verbose = false) const;
379 
382  bool isStateColliding(const moveit_msgs::RobotState& state, const std::string& group = "", bool verbose = false) const;
383 
387 
390  {
391  checkCollision(req, res, getCurrentState());
392  }
393 
398  {
399  robot_state.updateCollisionBodyTransforms();
400  checkCollision(req, res, static_cast<const robot_state::RobotState&>(robot_state));
401  }
402 
407  const robot_state::RobotState& robot_state) const;
408 
413  robot_state::RobotState& robot_state,
415  {
416  robot_state.updateCollisionBodyTransforms();
417  checkCollision(req, res, static_cast<const robot_state::RobotState&>(robot_state), acm);
418  }
419 
423  const robot_state::RobotState& robot_state,
425 
431 
436  {
438  }
439 
444  const robot_state::RobotState& robot_state) const
445  {
446  checkCollisionUnpadded(req, res, robot_state, getAllowedCollisionMatrix());
447  }
448 
454  {
455  robot_state.updateCollisionBodyTransforms();
456  checkCollisionUnpadded(req, res, static_cast<const robot_state::RobotState&>(robot_state),
458  }
459 
466  {
467  robot_state.updateCollisionBodyTransforms();
468  checkCollisionUnpadded(req, res, static_cast<const robot_state::RobotState&>(robot_state), acm);
469  }
470 
476 
479 
483  {
484  checkSelfCollision(req, res, getCurrentState());
485  }
486 
489  robot_state::RobotState& robot_state) const
490  {
491  robot_state.updateCollisionBodyTransforms();
492  checkSelfCollision(req, res, static_cast<const robot_state::RobotState&>(robot_state), getAllowedCollisionMatrix());
493  }
494 
497  const robot_state::RobotState& robot_state) const
498  {
499  // do self-collision checking with the unpadded version of the robot
500  getCollisionRobotUnpadded()->checkSelfCollision(req, res, robot_state, getAllowedCollisionMatrix());
501  }
502 
506  robot_state::RobotState& robot_state,
508  {
509  robot_state.updateCollisionBodyTransforms();
510  checkSelfCollision(req, res, static_cast<const robot_state::RobotState&>(robot_state), acm);
511  }
512 
516  const robot_state::RobotState& robot_state,
518  {
519  // do self-collision checking with the unpadded version of the robot
520  getCollisionRobotUnpadded()->checkSelfCollision(req, res, robot_state, acm);
521  }
522 
524  void getCollidingLinks(std::vector<std::string>& links);
525 
527  void getCollidingLinks(std::vector<std::string>& links) const
528  {
530  }
531 
534  void getCollidingLinks(std::vector<std::string>& links, robot_state::RobotState& robot_state) const
535  {
536  robot_state.updateCollisionBodyTransforms();
537  getCollidingLinks(links, static_cast<const robot_state::RobotState&>(robot_state), getAllowedCollisionMatrix());
538  }
539 
541  void getCollidingLinks(std::vector<std::string>& links, const robot_state::RobotState& robot_state) const
542  {
543  getCollidingLinks(links, robot_state, getAllowedCollisionMatrix());
544  }
545 
548  void getCollidingLinks(std::vector<std::string>& links, robot_state::RobotState& robot_state,
550  {
551  robot_state.updateCollisionBodyTransforms();
552  getCollidingLinks(links, static_cast<const robot_state::RobotState&>(robot_state), acm);
553  }
554 
557  void getCollidingLinks(std::vector<std::string>& links, const robot_state::RobotState& robot_state,
559 
563 
566  {
568  }
569 
572  const robot_state::RobotState& robot_state) const
573  {
574  getCollidingPairs(contacts, robot_state, getAllowedCollisionMatrix());
575  }
576 
580  robot_state::RobotState& robot_state) const
581  {
582  robot_state.updateCollisionBodyTransforms();
583  getCollidingPairs(contacts, static_cast<const robot_state::RobotState&>(robot_state), getAllowedCollisionMatrix());
584  }
585 
589  robot_state::RobotState& robot_state,
591  {
592  robot_state.updateCollisionBodyTransforms();
593  getCollidingPairs(contacts, static_cast<const robot_state::RobotState&>(robot_state), acm);
594  }
595 
599  const robot_state::RobotState& robot_state,
601 
612  double distanceToCollision(robot_state::RobotState& robot_state) const
613  {
614  robot_state.updateCollisionBodyTransforms();
615  return distanceToCollision(static_cast<const robot_state::RobotState&>(robot_state));
616  }
617 
621  double distanceToCollision(const robot_state::RobotState& robot_state) const
622  {
623  return getCollisionWorld()->distanceRobot(*getCollisionRobot(), robot_state, getAllowedCollisionMatrix());
624  }
625 
629  {
630  robot_state.updateCollisionBodyTransforms();
631  return distanceToCollisionUnpadded(static_cast<const robot_state::RobotState&>(robot_state));
632  }
633 
636  double distanceToCollisionUnpadded(const robot_state::RobotState& robot_state) const
637  {
638  return getCollisionWorld()->distanceRobot(*getCollisionRobotUnpadded(), robot_state, getAllowedCollisionMatrix());
639  }
640 
646  {
647  robot_state.updateCollisionBodyTransforms();
648  return distanceToCollision(static_cast<const robot_state::RobotState&>(robot_state), acm);
649  }
650 
654  double distanceToCollision(const robot_state::RobotState& robot_state,
656  {
657  return getCollisionWorld()->distanceRobot(*getCollisionRobot(), robot_state, acm);
658  }
659 
665  {
666  robot_state.updateCollisionBodyTransforms();
667  return distanceToCollisionUnpadded(static_cast<const robot_state::RobotState&>(robot_state), acm);
668  }
669 
675  {
676  return getCollisionWorld()->distanceRobot(*getCollisionRobotUnpadded(), robot_state, acm);
677  }
678 
682  void saveGeometryToStream(std::ostream& out) const;
683 
685  bool loadGeometryFromStream(std::istream& in);
686 
688  bool loadGeometryFromStream(std::istream& in, const Eigen::Isometry3d& offset);
689 
694  void getPlanningSceneDiffMsg(moveit_msgs::PlanningScene& scene) const;
695 
699  void getPlanningSceneMsg(moveit_msgs::PlanningScene& scene) const;
700 
703  void getPlanningSceneMsg(moveit_msgs::PlanningScene& scene, const moveit_msgs::PlanningSceneComponents& comp) const;
704 
707  bool getCollisionObjectMsg(moveit_msgs::CollisionObject& collision_obj, const std::string& ns) const;
708 
711  void getCollisionObjectMsgs(std::vector<moveit_msgs::CollisionObject>& collision_objs) const;
712 
715  bool getAttachedCollisionObjectMsg(moveit_msgs::AttachedCollisionObject& attached_collision_obj,
716  const std::string& ns) const;
717 
720  void getAttachedCollisionObjectMsgs(std::vector<moveit_msgs::AttachedCollisionObject>& attached_collision_objs) const;
721 
723  bool getOctomapMsg(octomap_msgs::OctomapWithPose& octomap) const;
724 
726  void getObjectColorMsgs(std::vector<moveit_msgs::ObjectColor>& object_colors) const;
727 
733  bool setPlanningSceneDiffMsg(const moveit_msgs::PlanningScene& scene);
734 
737  bool setPlanningSceneMsg(const moveit_msgs::PlanningScene& scene);
738 
741  bool usePlanningSceneMsg(const moveit_msgs::PlanningScene& scene);
742 
743  bool processCollisionObjectMsg(const moveit_msgs::CollisionObject& object);
744  bool processAttachedCollisionObjectMsg(const moveit_msgs::AttachedCollisionObject& object);
745 
746  bool processPlanningSceneWorldMsg(const moveit_msgs::PlanningSceneWorld& world);
747 
748  void processOctomapMsg(const octomap_msgs::OctomapWithPose& map);
749  void processOctomapMsg(const octomap_msgs::Octomap& map);
750  void processOctomapPtr(const std::shared_ptr<const octomap::OcTree>& octree, const Eigen::Isometry3d& t);
751 
756 
760  void setCurrentState(const moveit_msgs::RobotState& state);
761 
763  void setCurrentState(const robot_state::RobotState& state);
764 
767 
770 
771  bool hasObjectColor(const std::string& id) const;
772 
773  const std_msgs::ColorRGBA& getObjectColor(const std::string& id) const;
774  void setObjectColor(const std::string& id, const std_msgs::ColorRGBA& color);
775  void removeObjectColor(const std::string& id);
776  void getKnownObjectColors(ObjectColorMap& kc) const;
777 
778  bool hasObjectType(const std::string& id) const;
779 
780  const object_recognition_msgs::ObjectType& getObjectType(const std::string& id) const;
781  void setObjectType(const std::string& id, const object_recognition_msgs::ObjectType& type);
782  void removeObjectType(const std::string& id);
783  void getKnownObjectTypes(ObjectTypeMap& kc) const;
784 
787  void clearDiffs();
788 
792  void pushDiffs(const PlanningScenePtr& scene);
793 
797  void decoupleParent();
798 
802  void setStateFeasibilityPredicate(const StateFeasibilityFn& fn)
803  {
804  state_feasibility_ = fn;
805  }
806 
809  const StateFeasibilityFn& getStateFeasibilityPredicate() const
810  {
811  return state_feasibility_;
812  }
813 
816  void setMotionFeasibilityPredicate(const MotionFeasibilityFn& fn)
817  {
818  motion_feasibility_ = fn;
819  }
820 
823  const MotionFeasibilityFn& getMotionFeasibilityPredicate() const
824  {
825  return motion_feasibility_;
826  }
827 
830  bool isStateFeasible(const moveit_msgs::RobotState& state, bool verbose = false) const;
831 
834  bool isStateFeasible(const robot_state::RobotState& state, bool verbose = false) const;
835 
837  bool isStateConstrained(const moveit_msgs::RobotState& state, const moveit_msgs::Constraints& constr,
838  bool verbose = false) const;
839 
841  bool isStateConstrained(const robot_state::RobotState& state, const moveit_msgs::Constraints& constr,
842  bool verbose = false) const;
843 
845  bool isStateConstrained(const moveit_msgs::RobotState& state,
846  const kinematic_constraints::KinematicConstraintSet& constr, bool verbose = false) const;
847 
850  const kinematic_constraints::KinematicConstraintSet& constr, bool verbose = false) const;
851 
853  bool isStateValid(const moveit_msgs::RobotState& state, const std::string& group = "", bool verbose = false) const;
854 
856  bool isStateValid(const robot_state::RobotState& state, const std::string& group = "", bool verbose = false) const;
857 
860  bool isStateValid(const moveit_msgs::RobotState& state, const moveit_msgs::Constraints& constr,
861  const std::string& group = "", bool verbose = false) const;
862 
865  bool isStateValid(const robot_state::RobotState& state, const moveit_msgs::Constraints& constr,
866  const std::string& group = "", bool verbose = false) const;
867 
871  const std::string& group = "", bool verbose = false) const;
872 
874  bool isPathValid(const moveit_msgs::RobotState& start_state, const moveit_msgs::RobotTrajectory& trajectory,
875  const std::string& group = "", bool verbose = false,
876  std::vector<std::size_t>* invalid_index = NULL) const;
877 
881  bool isPathValid(const moveit_msgs::RobotState& start_state, const moveit_msgs::RobotTrajectory& trajectory,
882  const moveit_msgs::Constraints& path_constraints, const std::string& group = "",
883  bool verbose = false, std::vector<std::size_t>* invalid_index = NULL) const;
884 
888  bool isPathValid(const moveit_msgs::RobotState& start_state, const moveit_msgs::RobotTrajectory& trajectory,
889  const moveit_msgs::Constraints& path_constraints, const moveit_msgs::Constraints& goal_constraints,
890  const std::string& group = "", bool verbose = false,
891  std::vector<std::size_t>* invalid_index = NULL) const;
892 
896  bool isPathValid(const moveit_msgs::RobotState& start_state, const moveit_msgs::RobotTrajectory& trajectory,
897  const moveit_msgs::Constraints& path_constraints,
898  const std::vector<moveit_msgs::Constraints>& goal_constraints, const std::string& group = "",
899  bool verbose = false, std::vector<std::size_t>* invalid_index = NULL) const;
900 
904  bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory,
905  const moveit_msgs::Constraints& path_constraints,
906  const std::vector<moveit_msgs::Constraints>& goal_constraints, const std::string& group = "",
907  bool verbose = false, std::vector<std::size_t>* invalid_index = NULL) const;
908 
912  bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory,
913  const moveit_msgs::Constraints& path_constraints, const moveit_msgs::Constraints& goal_constraints,
914  const std::string& group = "", bool verbose = false,
915  std::vector<std::size_t>* invalid_index = NULL) const;
916 
919  bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory,
920  const moveit_msgs::Constraints& path_constraints, const std::string& group = "",
921  bool verbose = false, std::vector<std::size_t>* invalid_index = NULL) const;
922 
924  bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory, const std::string& group = "",
925  bool verbose = false, std::vector<std::size_t>* invalid_index = NULL) const;
926 
929  void getCostSources(const robot_trajectory::RobotTrajectory& trajectory, std::size_t max_costs,
930  std::set<collision_detection::CostSource>& costs, double overlap_fraction = 0.9) const;
931 
934  void getCostSources(const robot_trajectory::RobotTrajectory& trajectory, std::size_t max_costs,
935  const std::string& group_name, std::set<collision_detection::CostSource>& costs,
936  double overlap_fraction = 0.9) const;
937 
939  void getCostSources(const robot_state::RobotState& state, std::size_t max_costs,
940  std::set<collision_detection::CostSource>& costs) const;
941 
944  void getCostSources(const robot_state::RobotState& state, std::size_t max_costs, const std::string& group_name,
945  std::set<collision_detection::CostSource>& costs) const;
946 
948  void printKnownObjects(std::ostream& out = std::cout) const;
949 
952  static bool isEmpty(const moveit_msgs::PlanningScene& msg);
953 
956  static bool isEmpty(const moveit_msgs::PlanningSceneWorld& msg);
957 
959  static bool isEmpty(const moveit_msgs::RobotState& msg);
960 
962  static PlanningScenePtr clone(const PlanningSceneConstPtr& scene);
963 
964 private:
965  /* Private constructor used by the diff() methods. */
966  PlanningScene(const PlanningSceneConstPtr& parent);
967 
968  /* Initialize the scene. This should only be called by the constructors.
969  * Requires a valid robot_model_ */
970  void initialize();
971 
972  /* helper function to create a RobotModel from a urdf/srdf. */
973  static robot_model::RobotModelPtr createRobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model,
974  const srdf::ModelConstSharedPtr& srdf_model);
975 
976  /* Helper functions for processing collision objects */
977  bool processCollisionObjectAdd(const moveit_msgs::CollisionObject& object);
978  bool processCollisionObjectRemove(const moveit_msgs::CollisionObject& object);
979  bool processCollisionObjectMove(const moveit_msgs::CollisionObject& object);
980 
982  static void poseMsgToEigen(const geometry_msgs::Pose& msg, Eigen::Isometry3d& out);
983 
985 
986  /* \brief A set of compatible collision detectors */
988  {
989  collision_detection::CollisionDetectorAllocatorPtr alloc_;
990  collision_detection::CollisionRobotPtr crobot_unpadded_; // if NULL use parent's
991  collision_detection::CollisionRobotConstPtr crobot_unpadded_const_;
992  collision_detection::CollisionRobotPtr crobot_; // if NULL use parent's
993  collision_detection::CollisionRobotConstPtr crobot_const_;
994 
995  collision_detection::CollisionWorldPtr cworld_; // never NULL
996  collision_detection::CollisionWorldConstPtr cworld_const_;
997 
998  CollisionDetectorConstPtr parent_; // may be NULL
999 
1000  const collision_detection::CollisionRobotConstPtr& getCollisionRobot() const
1001  {
1002  return crobot_const_ ? crobot_const_ : parent_->getCollisionRobot();
1003  }
1004  const collision_detection::CollisionRobotConstPtr& getCollisionRobotUnpadded() const
1005  {
1006  return crobot_unpadded_const_ ? crobot_unpadded_const_ : parent_->getCollisionRobotUnpadded();
1007  }
1008  void findParent(const PlanningScene& scene);
1009  void copyPadding(const CollisionDetector& src);
1010  };
1011  friend struct CollisionDetector;
1012 
1013  typedef std::map<std::string, CollisionDetectorPtr>::iterator CollisionDetectorIterator;
1014  typedef std::map<std::string, CollisionDetectorPtr>::const_iterator CollisionDetectorConstIterator;
1015 
1018 
1019  std::string name_; // may be empty
1020 
1021  PlanningSceneConstPtr parent_; // Null unless this is a diff scene
1022 
1023  robot_model::RobotModelConstPtr robot_model_; // Never null (may point to same model as parent)
1024 
1025  robot_state::RobotStatePtr robot_state_; // if NULL use parent's
1026 
1027  // Called when changes are made to attached bodies
1029 
1030  // This variable is not necessarily used by child planning scenes
1031  // This Transforms class is actually a SceneTransforms class
1032  robot_state::TransformsPtr scene_transforms_; // if NULL use parent's
1033 
1034  collision_detection::WorldPtr world_; // never NULL, never shared with parent/child
1035  collision_detection::WorldConstPtr world_const_; // copy of world_
1036  collision_detection::WorldDiffPtr world_diff_; // NULL unless this is a diff scene
1039 
1040  std::map<std::string, CollisionDetectorPtr> collision_; // never empty
1041  CollisionDetectorPtr active_collision_; // copy of one of the entries in collision_. Never NULL.
1042 
1043  collision_detection::AllowedCollisionMatrixPtr acm_; // if NULL use parent's
1044 
1045  StateFeasibilityFn state_feasibility_;
1046  MotionFeasibilityFn motion_feasibility_;
1047 
1048  std::unique_ptr<ObjectColorMap> object_colors_;
1049 
1050  // a map of object types
1051  std::unique_ptr<ObjectTypeMap> object_types_;
1052 };
1053 } // namespace planning_scene
1054 
1055 #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:59
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:60
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...
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 Mon Nov 23 2020 03:52:30