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()
std::unique_ptr< ObjectColorMap > object_colors_
std::map< std::pair< std::string, std::string >, std::vector< Contact > > ContactMap
boost::function< void(const ObjectConstPtr &, Action)> ObserverCallbackFn
Definition: world.h:229
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.
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...
MOVEIT_CLASS_FORWARD(CollisionDetector)
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. ...
static PlanningScenePtr clone(const PlanningSceneConstPtr &scene)
Clone a planning scene. Even if the scene scene depends on a parent, the cloned scene will not...
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
PlanningScenePtr diff() const
Return a new child PlanningScene that uses this one as parent.
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:82
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 Jan 21 2018 03:54:29