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:
91  PlanningScene(const robot_model::RobotModelConstPtr& robot_model,
92  collision_detection::WorldPtr world = collision_detection::WorldPtr(new collision_detection::World()));
93 
96  PlanningScene(const urdf::ModelInterfaceSharedPtr& urdf_model, const srdf::ModelConstSharedPtr& srdf_model,
97  collision_detection::WorldPtr world = collision_detection::WorldPtr(new collision_detection::World()));
98 
99  static const std::string OCTOMAP_NS;
100  static const std::string DEFAULT_SCENE_NAME;
101 
102  ~PlanningScene();
103 
105  const std::string& getName() const
106  {
107  return name_;
108  }
109 
111  void setName(const std::string& name)
112  {
113  name_ = name;
114  }
115 
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 kmodel_;
145  }
146 
149  {
150  // if we have an updated state, return it; otherwise, return the parent one
151  return kstate_ ? *kstate_ : 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 ftf_ ? ftf_->getTargetFrame() : parent_->getPlanningFrame();
169  }
170 
173  {
174  // if we have updated transforms, return those
175  return (ftf_ || !parent_) ? *ftf_ : parent_->getTransforms();
176  }
177 
181 
184 
189  const Eigen::Affine3d& getFrameTransform(const std::string& id) const;
190 
196  const Eigen::Affine3d& getFrameTransform(const std::string& id);
197 
203  const Eigen::Affine3d& getFrameTransform(robot_state::RobotState& state, const std::string& id) const
204  {
205  state.updateLinkTransforms();
206  return getFrameTransform(static_cast<const robot_state::RobotState&>(state), id);
207  }
208 
213  const Eigen::Affine3d& getFrameTransform(const robot_state::RobotState& state, const std::string& id) const;
214 
217  bool knowsFrameTransform(const std::string& id) const;
218 
221  bool knowsFrameTransform(const robot_state::RobotState& state, const std::string& id) const;
222 
247  void addCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr& allocator);
248 
258  void setActiveCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr& allocator,
259  bool exclusive = false);
260 
267  bool setActiveCollisionDetector(const std::string& collision_detector_name);
268 
269  const std::string& getActiveCollisionDetectorName() const
270  {
271  return active_collision_->alloc_->getName();
272  }
273 
276  void getCollisionDetectorNames(std::vector<std::string>& names) const;
277 
279  const collision_detection::WorldConstPtr& getWorld() const
280  {
281  // we always have a world representation
282  return world_const_;
283  }
284 
285  // brief Get the representation of the world
286  const collision_detection::WorldPtr& getWorldNonConst()
287  {
288  // we always have a world representation
289  return world_;
290  }
291 
293  const collision_detection::CollisionWorldConstPtr& getCollisionWorld() const
294  {
295  // we always have a world representation after configure is called.
296  return active_collision_->cworld_const_;
297  }
298 
300  const collision_detection::CollisionRobotConstPtr& getCollisionRobot() const
301  {
302  return active_collision_->getCollisionRobot();
303  }
304 
306  const collision_detection::CollisionRobotConstPtr& getCollisionRobotUnpadded() const
307  {
308  return active_collision_->getCollisionRobotUnpadded();
309  }
310 
312  const collision_detection::CollisionWorldConstPtr&
313  getCollisionWorld(const std::string& collision_detector_name) const;
314 
316  const collision_detection::CollisionRobotConstPtr&
317  getCollisionRobot(const std::string& collision_detector_name) const;
318 
321  const collision_detection::CollisionRobotConstPtr&
322  getCollisionRobotUnpadded(const std::string& collision_detector_name) const;
323 
328  const collision_detection::CollisionRobotPtr& getCollisionRobotNonConst();
329 
333  void propogateRobotPadding();
334 
337  {
338  return acm_ ? *acm_ : parent_->getAllowedCollisionMatrix();
339  }
342 
353  bool isStateColliding(const std::string& group = "", bool verbose = false);
354 
358  bool isStateColliding(const std::string& group = "", bool verbose = false) const
359  {
360  return isStateColliding(getCurrentState(), group, verbose);
361  }
362 
367  bool isStateColliding(robot_state::RobotState& state, const std::string& group = "", bool verbose = false) const
368  {
370  return isStateColliding(static_cast<const robot_state::RobotState&>(state), group, verbose);
371  }
372 
376  bool isStateColliding(const robot_state::RobotState& state, const std::string& group = "",
377  bool verbose = false) const;
378 
381  bool isStateColliding(const moveit_msgs::RobotState& state, const std::string& group = "",
382  bool verbose = false) const;
383 
387 
390  {
391  checkCollision(req, res, getCurrentState());
392  }
393 
397  robot_state::RobotState& kstate) const
398  {
400  checkCollision(req, res, static_cast<const robot_state::RobotState&>(kstate));
401  }
402 
406  const robot_state::RobotState& kstate) const;
407 
413  {
415  checkCollision(req, res, static_cast<const robot_state::RobotState&>(kstate), acm);
416  }
417 
421  const robot_state::RobotState& kstate,
423 
429 
434  {
436  }
437 
442  {
444  }
445 
451  {
453  checkCollisionUnpadded(req, res, static_cast<const robot_state::RobotState&>(kstate), getAllowedCollisionMatrix());
454  }
455 
462  {
464  checkCollisionUnpadded(req, res, static_cast<const robot_state::RobotState&>(kstate), acm);
465  }
466 
472 
475 
479  {
480  checkSelfCollision(req, res, getCurrentState());
481  }
482 
485  robot_state::RobotState& kstate) const
486  {
488  checkSelfCollision(req, res, static_cast<const robot_state::RobotState&>(kstate), getAllowedCollisionMatrix());
489  }
490 
493  const robot_state::RobotState& kstate) const
494  {
495  // do self-collision checking with the unpadded version of the robot
496  getCollisionRobotUnpadded()->checkSelfCollision(req, res, kstate, getAllowedCollisionMatrix());
497  }
498 
503  {
505  checkSelfCollision(req, res, static_cast<const robot_state::RobotState&>(kstate), acm);
506  }
507 
511  const robot_state::RobotState& kstate,
513  {
514  // do self-collision checking with the unpadded version of the robot
515  getCollisionRobotUnpadded()->checkSelfCollision(req, res, kstate, acm);
516  }
517 
519  void getCollidingLinks(std::vector<std::string>& links);
520 
522  void getCollidingLinks(std::vector<std::string>& links) const
523  {
525  }
526 
529  void getCollidingLinks(std::vector<std::string>& links, robot_state::RobotState& kstate) const
530  {
532  getCollidingLinks(links, static_cast<const robot_state::RobotState&>(kstate), getAllowedCollisionMatrix());
533  }
534 
536  void getCollidingLinks(std::vector<std::string>& links, const robot_state::RobotState& kstate) const
537  {
539  }
540 
543  void getCollidingLinks(std::vector<std::string>& links, robot_state::RobotState& kstate,
545  {
547  getCollidingLinks(links, static_cast<const robot_state::RobotState&>(kstate), acm);
548  }
549 
552  void getCollidingLinks(std::vector<std::string>& links, const robot_state::RobotState& kstate,
554 
558 
561  {
563  }
564 
567  const robot_state::RobotState& kstate) const
568  {
569  getCollidingPairs(contacts, kstate, getAllowedCollisionMatrix());
570  }
571 
575  robot_state::RobotState& kstate) const
576  {
578  getCollidingPairs(contacts, static_cast<const robot_state::RobotState&>(kstate), getAllowedCollisionMatrix());
579  }
580 
585  {
587  getCollidingPairs(contacts, static_cast<const robot_state::RobotState&>(kstate), acm);
588  }
589 
593  const robot_state::RobotState& kstate,
595 
606  {
608  return distanceToCollision(static_cast<const robot_state::RobotState&>(kstate));
609  }
610 
613  double distanceToCollision(const robot_state::RobotState& kstate) const
614  {
615  return getCollisionWorld()->distanceRobot(*getCollisionRobot(), kstate, getAllowedCollisionMatrix());
616  }
617 
621  {
623  return distanceToCollisionUnpadded(static_cast<const robot_state::RobotState&>(kstate));
624  }
625 
629  {
630  return getCollisionWorld()->distanceRobot(*getCollisionRobotUnpadded(), kstate, getAllowedCollisionMatrix());
631  }
632 
637  {
639  return distanceToCollision(static_cast<const robot_state::RobotState&>(kstate), acm);
640  }
641 
646  {
647  return getCollisionWorld()->distanceRobot(*getCollisionRobot(), kstate, acm);
648  }
649 
654  {
656  return distanceToCollisionUnpadded(static_cast<const robot_state::RobotState&>(kstate), acm);
657  }
658 
663  {
664  return getCollisionWorld()->distanceRobot(*getCollisionRobotUnpadded(), kstate, acm);
665  }
666 
670  void saveGeometryToStream(std::ostream& out) const;
671 
673  void loadGeometryFromStream(std::istream& in);
674 
676  void loadGeometryFromStream(std::istream& in, const Eigen::Affine3d& offset);
677 
682  void getPlanningSceneDiffMsg(moveit_msgs::PlanningScene& scene) const;
683 
687  void getPlanningSceneMsg(moveit_msgs::PlanningScene& scene) const;
688 
691  void getPlanningSceneMsg(moveit_msgs::PlanningScene& scene, const moveit_msgs::PlanningSceneComponents& comp) const;
692 
695  bool getCollisionObjectMsg(moveit_msgs::CollisionObject& collision_obj, const std::string& ns) const;
696 
699  void getCollisionObjectMsgs(std::vector<moveit_msgs::CollisionObject>& collision_objs) const;
700 
703  bool getAttachedCollisionObjectMsg(moveit_msgs::AttachedCollisionObject& attached_collision_obj,
704  const std::string& ns) const;
705 
708  void getAttachedCollisionObjectMsgs(std::vector<moveit_msgs::AttachedCollisionObject>& attached_collision_objs) const;
709 
711  bool getOctomapMsg(octomap_msgs::OctomapWithPose& octomap) const;
712 
714  void getObjectColorMsgs(std::vector<moveit_msgs::ObjectColor>& object_colors) const;
715 
721  bool setPlanningSceneDiffMsg(const moveit_msgs::PlanningScene& scene);
722 
725  bool setPlanningSceneMsg(const moveit_msgs::PlanningScene& scene);
726 
729  bool usePlanningSceneMsg(const moveit_msgs::PlanningScene& scene);
730 
731  bool processCollisionObjectMsg(const moveit_msgs::CollisionObject& object);
732  bool processAttachedCollisionObjectMsg(const moveit_msgs::AttachedCollisionObject& object);
733 
734  bool processPlanningSceneWorldMsg(const moveit_msgs::PlanningSceneWorld& world);
735 
736  void processOctomapMsg(const octomap_msgs::OctomapWithPose& map);
737  void processOctomapMsg(const octomap_msgs::Octomap& map);
738  void processOctomapPtr(const std::shared_ptr<const octomap::OcTree>& octree, const Eigen::Affine3d& t);
739 
744 
748  void setCurrentState(const moveit_msgs::RobotState& state);
749 
751  void setCurrentState(const robot_state::RobotState& state);
752 
755 
758 
759  bool hasObjectColor(const std::string& id) const;
760 
761  const std_msgs::ColorRGBA& getObjectColor(const std::string& id) const;
762  void setObjectColor(const std::string& id, const std_msgs::ColorRGBA& color);
763  void removeObjectColor(const std::string& id);
764  void getKnownObjectColors(ObjectColorMap& kc) const;
765 
766  bool hasObjectType(const std::string& id) const;
767 
768  const object_recognition_msgs::ObjectType& getObjectType(const std::string& id) const;
769  void setObjectType(const std::string& id, const object_recognition_msgs::ObjectType& type);
770  void removeObjectType(const std::string& id);
771  void getKnownObjectTypes(ObjectTypeMap& kc) const;
772 
775  void clearDiffs();
776 
780  void pushDiffs(const PlanningScenePtr& scene);
781 
785  void decoupleParent();
786 
790  void setStateFeasibilityPredicate(const StateFeasibilityFn& fn)
791  {
792  state_feasibility_ = fn;
793  }
794 
797  const StateFeasibilityFn& getStateFeasibilityPredicate() const
798  {
799  return state_feasibility_;
800  }
801 
804  void setMotionFeasibilityPredicate(const MotionFeasibilityFn& fn)
805  {
806  motion_feasibility_ = fn;
807  }
808 
811  const MotionFeasibilityFn& getMotionFeasibilityPredicate() const
812  {
813  return motion_feasibility_;
814  }
815 
818  bool isStateFeasible(const moveit_msgs::RobotState& state, bool verbose = false) const;
819 
822  bool isStateFeasible(const robot_state::RobotState& state, bool verbose = false) const;
823 
825  bool isStateConstrained(const moveit_msgs::RobotState& state, const moveit_msgs::Constraints& constr,
826  bool verbose = false) const;
827 
829  bool isStateConstrained(const robot_state::RobotState& state, const moveit_msgs::Constraints& constr,
830  bool verbose = false) const;
831 
833  bool isStateConstrained(const moveit_msgs::RobotState& state,
834  const kinematic_constraints::KinematicConstraintSet& constr, bool verbose = false) const;
835 
838  const kinematic_constraints::KinematicConstraintSet& constr, bool verbose = false) const;
839 
841  bool isStateValid(const moveit_msgs::RobotState& state, const std::string& group = "", bool verbose = false) const;
842 
844  bool isStateValid(const robot_state::RobotState& state, const std::string& group = "", bool verbose = false) const;
845 
848  bool isStateValid(const moveit_msgs::RobotState& state, const moveit_msgs::Constraints& constr,
849  const std::string& group = "", bool verbose = false) const;
850 
853  bool isStateValid(const robot_state::RobotState& state, const moveit_msgs::Constraints& constr,
854  const std::string& group = "", bool verbose = false) const;
855 
859  const std::string& group = "", bool verbose = false) const;
860 
862  bool isPathValid(const moveit_msgs::RobotState& start_state, const moveit_msgs::RobotTrajectory& trajectory,
863  const std::string& group = "", bool verbose = false,
864  std::vector<std::size_t>* invalid_index = NULL) const;
865 
869  bool isPathValid(const moveit_msgs::RobotState& start_state, const moveit_msgs::RobotTrajectory& trajectory,
870  const moveit_msgs::Constraints& path_constraints, const std::string& group = "",
871  bool verbose = false, std::vector<std::size_t>* invalid_index = NULL) const;
872 
876  bool isPathValid(const moveit_msgs::RobotState& start_state, const moveit_msgs::RobotTrajectory& trajectory,
877  const moveit_msgs::Constraints& path_constraints, const moveit_msgs::Constraints& goal_constraints,
878  const std::string& group = "", bool verbose = false,
879  std::vector<std::size_t>* invalid_index = NULL) const;
880 
884  bool isPathValid(const moveit_msgs::RobotState& start_state, const moveit_msgs::RobotTrajectory& trajectory,
885  const moveit_msgs::Constraints& path_constraints,
886  const std::vector<moveit_msgs::Constraints>& goal_constraints, const std::string& group = "",
887  bool verbose = false, std::vector<std::size_t>* invalid_index = NULL) const;
888 
892  bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory,
893  const moveit_msgs::Constraints& path_constraints,
894  const std::vector<moveit_msgs::Constraints>& goal_constraints, const std::string& group = "",
895  bool verbose = false, std::vector<std::size_t>* invalid_index = NULL) const;
896 
900  bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory,
901  const moveit_msgs::Constraints& path_constraints, const moveit_msgs::Constraints& goal_constraints,
902  const std::string& group = "", bool verbose = false,
903  std::vector<std::size_t>* invalid_index = NULL) const;
904 
907  bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory,
908  const moveit_msgs::Constraints& path_constraints, const std::string& group = "",
909  bool verbose = false, std::vector<std::size_t>* invalid_index = NULL) const;
910 
912  bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory, const std::string& group = "",
913  bool verbose = false, std::vector<std::size_t>* invalid_index = NULL) const;
914 
917  void getCostSources(const robot_trajectory::RobotTrajectory& trajectory, std::size_t max_costs,
918  std::set<collision_detection::CostSource>& costs, double overlap_fraction = 0.9) const;
919 
922  void getCostSources(const robot_trajectory::RobotTrajectory& trajectory, std::size_t max_costs,
923  const std::string& group_name, std::set<collision_detection::CostSource>& costs,
924  double overlap_fraction = 0.9) const;
925 
927  void getCostSources(const robot_state::RobotState& state, std::size_t max_costs,
928  std::set<collision_detection::CostSource>& costs) const;
929 
932  void getCostSources(const robot_state::RobotState& state, std::size_t max_costs, const std::string& group_name,
933  std::set<collision_detection::CostSource>& costs) const;
934 
936  void printKnownObjects(std::ostream& out) const;
937 
940  static bool isEmpty(const moveit_msgs::PlanningScene& msg);
941 
944  static bool isEmpty(const moveit_msgs::PlanningSceneWorld& msg);
945 
947  static bool isEmpty(const moveit_msgs::RobotState& msg);
948 
950  static PlanningScenePtr clone(const PlanningSceneConstPtr& scene);
951 
952 private:
953  /* Private constructor used by the diff() methods. */
954  PlanningScene(const PlanningSceneConstPtr& parent);
955 
956  /* Initialize the scene. This should only be called by the constructors.
957  * Requires a valid robot_model_ */
958  void initialize();
959 
960  /* helper function to create a RobotModel from a urdf/srdf. */
961  static robot_model::RobotModelPtr createRobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model,
962  const srdf::ModelConstSharedPtr& srdf_model);
963 
965 
966  /* \brief A set of compatible collision detectors */
968  {
969  collision_detection::CollisionDetectorAllocatorPtr alloc_;
970  collision_detection::CollisionRobotPtr crobot_unpadded_; // if NULL use parent's
971  collision_detection::CollisionRobotConstPtr crobot_unpadded_const_;
972  collision_detection::CollisionRobotPtr crobot_; // if NULL use parent's
973  collision_detection::CollisionRobotConstPtr crobot_const_;
974 
975  collision_detection::CollisionWorldPtr cworld_; // never NULL
976  collision_detection::CollisionWorldConstPtr cworld_const_;
977 
978  CollisionDetectorConstPtr parent_; // may be NULL
979 
980  const collision_detection::CollisionRobotConstPtr& getCollisionRobot() const
981  {
982  return crobot_const_ ? crobot_const_ : parent_->getCollisionRobot();
983  }
984  const collision_detection::CollisionRobotConstPtr& getCollisionRobotUnpadded() const
985  {
986  return crobot_unpadded_const_ ? crobot_unpadded_const_ : parent_->getCollisionRobotUnpadded();
987  }
988  void findParent(const PlanningScene& scene);
989  void copyPadding(const CollisionDetector& src);
990  };
991  friend struct CollisionDetector;
992 
993  typedef std::map<std::string, CollisionDetectorPtr>::iterator CollisionDetectorIterator;
994  typedef std::map<std::string, CollisionDetectorPtr>::const_iterator CollisionDetectorConstIterator;
995 
998 
999  std::string name_; // may be empty
1000 
1001  PlanningSceneConstPtr parent_; // Null unless this is a diff scene
1002 
1003  robot_model::RobotModelConstPtr kmodel_; // Never null (may point to same model as parent)
1004 
1005  robot_state::RobotStatePtr kstate_; // if NULL use parent's
1007  // bodies
1008 
1009  robot_state::TransformsPtr ftf_; // if NULL use parent's
1010 
1011  collision_detection::WorldPtr world_; // never NULL, never shared with parent/child
1012  collision_detection::WorldConstPtr world_const_; // copy of world_
1013  collision_detection::WorldDiffPtr world_diff_; // NULL unless this is a diff scene
1016 
1017  std::map<std::string, CollisionDetectorPtr> collision_; // never empty
1018  CollisionDetectorPtr active_collision_; // copy of one of the entries in collision_. Never NULL.
1019 
1020  collision_detection::AllowedCollisionMatrixPtr acm_; // if NULL use parent's
1021 
1022  StateFeasibilityFn state_feasibility_;
1023  MotionFeasibilityFn motion_feasibility_;
1024 
1025  std::unique_ptr<ObjectColorMap> object_colors_;
1026 
1027  // a map of object types
1028  std::unique_ptr<ObjectTypeMap> object_types_;
1029 };
1030 }
1031 
1032 #endif
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...
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...
const collision_detection::CollisionRobotConstPtr & getCollisionRobot() const
Get the active collision detector for the robot.
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...
double distanceToCollisionUnpadded(robot_state::RobotState &kstate, const collision_detection::AllowedCollisionMatrix &acm) const
The distance between the robot model at state kstate to the nearest collision, ignoring self-collisio...
MOVEIT_CLASS_FORWARD(PlanningScene)
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 checkCollisionUnpadded(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, robot_state::RobotState &kstate, const collision_detection::AllowedCollisionMatrix &acm) const
Check whether a specified state (kstate) is in collision, with respect to a given allowed collision m...
collision_detection::CollisionWorldConstPtr cworld_const_
void checkCollision(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, robot_state::RobotState &kstate, const collision_detection::AllowedCollisionMatrix &acm) const
Check whether a specified state (kstate) is in collision, with respect to a given allowed collision m...
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!
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...
double distanceToCollisionUnpadded(const robot_state::RobotState &kstate, const collision_detection::AllowedCollisionMatrix &acm) const
The distance between the robot model at state kstate to the nearest collision, ignoring self-collisio...
robot_state::RobotState & getCurrentStateNonConst()
Get the state at which the robot is assumed to be.
double distanceToCollision(robot_state::RobotState &kstate, const collision_detection::AllowedCollisionMatrix &acm) const
The distance between the robot model at state kstate to the nearest collision, ignoring self-collisio...
void checkCollision(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, robot_state::RobotState &kstate) const
Check whether a specified state (kstate) is in collision. This variant of the function takes a non-co...
robot_state::TransformsPtr ftf_
const collision_detection::CollisionRobotConstPtr & getCollisionRobotUnpadded() const
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)
const std::string & getName() const
Get the name of the planning scene. This is empty by default.
MotionFeasibilityFn motion_feasibility_
bool hasObjectType(const std::string &id) const
const collision_detection::AllowedCollisionMatrix & getAllowedCollisionMatrix() const
Get the allowed collision matrix.
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...
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.
void removeObjectColor(const std::string &id)
double distanceToCollisionUnpadded(const robot_state::RobotState &kstate) const
The distance between the robot model at state kstate to the nearest collision (ignoring self-collisio...
robot_state::RobotStatePtr kstate_
StateFeasibilityFn state_feasibility_
PlanningScene(const robot_model::RobotModelConstPtr &robot_model, collision_detection::WorldPtr world=collision_detection::WorldPtr(new collision_detection::World()))
construct using an existing RobotModel
void checkSelfCollision(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res)
Check whether the current state is in self collision.
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...
static const std::string DEFAULT_SCENE_NAME
Maintain a representation of the environment.
Definition: world.h:60
const Eigen::Affine3d & 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 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.
bool processAttachedCollisionObjectMsg(const moveit_msgs::AttachedCollisionObject &object)
A class that contains many different constraints, and can check RobotState *versus the full set...
robot_model::RobotModelConstPtr kmodel_
bool processPlanningSceneWorldMsg(const moveit_msgs::PlanningSceneWorld &world)
const StateFeasibilityFn & getStateFeasibilityPredicate() const
Get the predicate that decides whether states are considered valid or invalid for reasons beyond ones...
const MotionFeasibilityFn & getMotionFeasibilityPredicate() const
Get the predicate that decides whether motion segments are considered valid or invalid for reasons be...
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 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
void setObjectColor(const std::string &id, const std_msgs::ColorRGBA &color)
collision_detection::CollisionRobotPtr crobot_
static const std::string OCTOMAP_NS
bool processCollisionObjectMsg(const moveit_msgs::CollisionObject &object)
Provides an implementation of a snapshot of a transform tree that can be easily queried for transform...
Definition: transforms.h:61
const std_msgs::ColorRGBA & getObjectColor(const std::string &id) const
void copyPadding(const CollisionDetector &src)
collision_detection::AllowedCollisionMatrixPtr acm_
collision_detection::CollisionWorldPtr cworld_
static robot_model::RobotModelPtr createRobotModel(const urdf::ModelInterfaceSharedPtr &urdf_model, const srdf::ModelConstSharedPtr &srdf_model)
void setCollisionObjectUpdateCallback(const collision_detection::World::ObserverCallbackFn &callback)
Set the callback to be triggered when changes are made to the current scene world.
void printKnownObjects(std::ostream &out) const
Outputs debug information about the planning scene contents.
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)
PlanningScenePtr diff() const
Return a new child PlanningScene that uses this one as parent.
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 ...
void checkCollisionUnpadded(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, const robot_state::RobotState &kstate) const
Check whether a specified state (kstate) is in collision, but use a collision_detection::CollisionRob...
bool verbose
robot_state::Transforms & getTransformsNonConst()
Get the set of fixed transforms from known frames to the planning frame.
void getKnownObjectColors(ObjectColorMap &kc) const
void getCollidingLinks(std::vector< std::string > &links, robot_state::RobotState &kstate) const
Get the names of the links that are involved in collisions for the state kstate. Update the link tran...
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)
const robot_model::RobotModelConstPtr & getRobotModel() const
Get the kinematic model for which the planning scene is maintained.
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...
const robot_state::Transforms & getTransforms() const
Get the set of fixed transforms from known frames to the planning frame.
robot_trajectory::RobotTrajectory trajectory(rmodel,"right_arm")
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 checkSelfCollision(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, robot_state::RobotState &kstate, const collision_detection::AllowedCollisionMatrix &acm) const
Check whether a specified state (kstate) is in self collision, with respect to a given allowed collis...
const robot_state::RobotState & getCurrentState() const
Get the state at which the robot is assumed to be.
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.
void getCollidingLinks(std::vector< std::string > &links) const
Get the names of the links that are involved in collisions for the current state. ...
const Eigen::Affine3d & 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...
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...
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...
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...
double distanceToCollision(robot_state::RobotState &kstate) const
The distance between the robot model at state kstate to the nearest collision (ignoring self-collisio...
void checkSelfCollision(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res) const
Check whether the current state is in self collision.
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 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...
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.
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...
This namespace includes the central class for representing planning contexts.
void getKnownObjectTypes(ObjectTypeMap &kc) const
collision_detection::AllowedCollisionMatrix & getAllowedCollisionMatrixNonConst()
Get the allowed collision matrix.
void checkSelfCollision(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, const robot_state::RobotState &kstate) const
Check whether a specified state (kstate) is in self collision.
void getCollidingPairs(collision_detection::CollisionResult::ContactMap &contacts) const
Get the names of the links that are involved in collisions for the current state. ...
void removeAllCollisionObjects()
Clear all collision objects in planning scene.
CollisionDetectorPtr active_collision_
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_
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...
void getCollidingPairs(collision_detection::CollisionResult::ContactMap &contacts, const robot_state::RobotState &kstate) const
Get the names of the links that are involved in collisions for the state kstate.
const std::string & getActiveCollisionDetectorName() const
bool hasObjectColor(const std::string &id) const
const object_recognition_msgs::ObjectType & getObjectType(const std::string &id) const
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.
double distanceToCollision(const robot_state::RobotState &kstate) const
The distance between the robot model at state kstate to the nearest collision (ignoring self-collisio...
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.
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...
void getCollidingPairs(collision_detection::CollisionResult::ContactMap &contacts, robot_state::RobotState &kstate, const collision_detection::AllowedCollisionMatrix &acm) const
Get the names of the links that are involved in collisions for the state kstate given the allowed col...
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...
void processOctomapPtr(const std::shared_ptr< const octomap::OcTree > &octree, const Eigen::Affine3d &t)
std::map< std::string, CollisionDetectorPtr >::iterator CollisionDetectorIterator
void checkSelfCollision(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, robot_state::RobotState &kstate) const
Check whether a specified state (kstate) is in self collision.
collision_detection::WorldPtr world_
void updateCollisionBodyTransforms()
Update the transforms for the collision bodies. This call is needed before calling collision checking...
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 ...
double distanceToCollisionUnpadded(robot_state::RobotState &kstate) const
The distance between the robot model at state kstate to the nearest collision (ignoring self-collisio...
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...
void checkCollisionUnpadded(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, robot_state::RobotState &kstate) const
Check whether a specified state (kstate) is in collision, but use a collision_detection::CollisionRob...
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 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 getPlanningSceneDiffMsg(moveit_msgs::PlanningScene &scene) const
Fill the message scene with the differences between this instance of PlanningScene with respect to th...
const collision_detection::CollisionRobotConstPtr & getCollisionRobot() const
const PlanningSceneConstPtr & getParent() const
Get the parent scene (whith respect to which the diffs are maintained). This may be empty...
collision_detection::CollisionRobotConstPtr crobot_const_
void loadGeometryFromStream(std::istream &in)
Load the geometry of the planning scene from a stream.
double distanceToCollision(const robot_state::RobotState &kstate, const collision_detection::AllowedCollisionMatrix &acm) const
The distance between the robot model at state kstate to the nearest collision, ignoring self-collisio...
bool getOctomapMsg(octomap_msgs::OctomapWithPose &octomap) const
Construct a message (octomap) with the octomap data from the planning_scene.
void getCollidingLinks(std::vector< std::string > &links, robot_state::RobotState &kstate, const collision_detection::AllowedCollisionMatrix &acm) const
Get the names of the links that are involved in collisions for the state kstate given the allowed col...
PlanningSceneConstPtr parent_
void getPlanningSceneMsg(moveit_msgs::PlanningScene &scene) const
Construct a message (scene) with all the necessary data so that the scene can be later reconstructed ...
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...
void getCollidingLinks(std::vector< std::string > &links, const robot_state::RobotState &kstate) const
Get the names of the links that are involved in collisions for the state kstate.
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...
void saveGeometryToStream(std::ostream &out) const
Save the geometry of the planning scene to a stream, as plain text.
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.
void getCollidingPairs(collision_detection::CollisionResult::ContactMap &contacts, robot_state::RobotState &kstate) const
Get the names of the links that are involved in collisions for the state kstate. Update the link tran...
const collision_detection::CollisionWorldConstPtr & getCollisionWorld() const
Get the active collision detector for the world.
const collision_detection::WorldConstPtr & getWorld() const
Get the representation of the world.
void checkSelfCollision(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, const robot_state::RobotState &kstate, const collision_detection::AllowedCollisionMatrix &acm) const
Check whether a specified state (kstate) is in self collision, with respect to a given allowed collis...
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 Sun Oct 18 2020 13:16:33