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 #pragma once
38 
49 #include <moveit_msgs/PlanningScene.h>
50 #include <moveit_msgs/RobotTrajectory.h>
51 #include <moveit_msgs/Constraints.h>
52 #include <moveit_msgs/PlanningSceneComponents.h>
53 #include <octomap_msgs/OctomapWithPose.h>
54 #include <boost/noncopyable.hpp>
55 #include <boost/function.hpp>
56 #include <boost/concept_check.hpp>
57 #include <memory>
58 
59 // Import/export for windows dll's and visibility for gcc shared libraries.
60 #include <moveit/moveit_planning_scene_export.h>
61 
63 namespace planning_scene
64 {
65 MOVEIT_CLASS_FORWARD(PlanningScene); // Defines PlanningScenePtr, ConstPtr, WeakPtr... etc
66 
71 typedef boost::function<bool(const moveit::core::RobotState&, bool)> StateFeasibilityFn;
72 
78 using MotionFeasibilityFn =
79  boost::function<bool(const moveit::core::RobotState&, const moveit::core::RobotState&, bool)>;
80 
82 using ObjectColorMap = std::map<std::string, std_msgs::ColorRGBA>;
83 
85 using ObjectTypeMap = std::map<std::string, object_recognition_msgs::ObjectType>;
86 
90 class PlanningScene : private boost::noncopyable, public std::enable_shared_from_this<PlanningScene>
91 {
92 public:
94  PlanningScene(const moveit::core::RobotModelConstPtr& robot_model,
95  const collision_detection::WorldPtr& world = std::make_shared<collision_detection::World>());
96 
99  PlanningScene(const urdf::ModelInterfaceSharedPtr& urdf_model, const srdf::ModelConstSharedPtr& srdf_model,
100  const collision_detection::WorldPtr& world = std::make_shared<collision_detection::World>());
101 
102  static MOVEIT_PLANNING_SCENE_EXPORT const std::string OCTOMAP_NS;
103  static MOVEIT_PLANNING_SCENE_EXPORT const std::string DEFAULT_SCENE_NAME;
104 
105  ~PlanningScene();
106 
108  const std::string& getName() const
109  {
110  return name_;
111  }
112 
114  void setName(const std::string& name)
115  {
116  name_ = name;
117  }
118 
129  PlanningScenePtr diff() const;
130 
133  PlanningScenePtr diff(const moveit_msgs::PlanningScene& msg) const;
134 
136  const PlanningSceneConstPtr& getParent() const
137  {
138  return parent_;
139  }
140 
142  const moveit::core::RobotModelConstPtr& getRobotModel() const
143  {
144  // the kinematic model does not change
145  return robot_model_;
146  }
147 
150  {
151  // if we have an updated state, return it; otherwise, return the parent one
152  return robot_state_ ? *robot_state_ : parent_->getCurrentState();
153  }
156 
158  moveit::core::RobotStatePtr getCurrentStateUpdated(const moveit_msgs::RobotState& update) const;
159 
166  const std::string& getPlanningFrame() const
167  {
168  // if we have an updated set of transforms, return it; otherwise, return the parent one
169  return scene_transforms_ ? scene_transforms_->getTargetFrame() : parent_->getPlanningFrame();
170  }
171 
174  {
175  if (scene_transforms_ || !parent_)
176  {
177  return *scene_transforms_;
178  }
179 
180  // if this planning scene is a child of another, and doesn't have its own custom transforms
181  return parent_->getTransforms();
182  }
183 
187 
190 
195  const Eigen::Isometry3d& getFrameTransform(const std::string& id) const;
196 
202  const Eigen::Isometry3d& getFrameTransform(const std::string& id);
203 
209  const Eigen::Isometry3d& getFrameTransform(moveit::core::RobotState& state, const std::string& id) const
210  {
211  state.updateLinkTransforms();
212  return getFrameTransform(static_cast<const moveit::core::RobotState&>(state), id);
213  }
214 
219  const Eigen::Isometry3d& getFrameTransform(const moveit::core::RobotState& state, const std::string& id) const;
220 
223  bool knowsFrameTransform(const std::string& id) const;
224 
227  bool knowsFrameTransform(const moveit::core::RobotState& state, const std::string& id) const;
228 
253  void addCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr& allocator);
254 
264  void setActiveCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr& allocator,
265  bool exclusive = false);
266 
273  bool setActiveCollisionDetector(const std::string& collision_detector_name);
274 
275  const std::string& getActiveCollisionDetectorName() const
276  {
277  return active_collision_->alloc_->getName();
278  }
279 
282  void getCollisionDetectorNames(std::vector<std::string>& names) const;
283 
285  const collision_detection::WorldConstPtr& getWorld() const
286  {
287  // we always have a world representation
288  return world_const_;
289  }
290 
291  // brief Get the representation of the world
292  const collision_detection::WorldPtr& getWorldNonConst()
293  {
294  // we always have a world representation
295  return world_;
296  }
297 
299  const collision_detection::CollisionEnvConstPtr& getCollisionEnv() const
300  {
301  return active_collision_->getCollisionEnv();
302  }
303 
305  const collision_detection::CollisionEnvConstPtr& getCollisionEnvUnpadded() const
306  {
307  return active_collision_->getCollisionEnvUnpadded();
308  }
309 
311  const collision_detection::CollisionEnvConstPtr& getCollisionEnv(const std::string& collision_detector_name) const;
312 
315  const collision_detection::CollisionEnvConstPtr&
316  getCollisionEnvUnpadded(const std::string& collision_detector_name) const;
317 
322  const collision_detection::CollisionEnvPtr& getCollisionEnvNonConst();
323 
327  void propogateRobotPadding();
328 
331  {
332  return acm_ ? *acm_ : parent_->getAllowedCollisionMatrix();
333  }
336 
347  bool isStateColliding(const std::string& group = "", bool verbose = false);
348 
352  bool isStateColliding(const std::string& group = "", bool verbose = false) const
353  {
354  return isStateColliding(getCurrentState(), group, verbose);
355  }
356 
361  bool isStateColliding(moveit::core::RobotState& state, const std::string& group = "", bool verbose = false) const
362  {
364  return isStateColliding(static_cast<const moveit::core::RobotState&>(state), group, verbose);
365  }
366 
370  bool isStateColliding(const moveit::core::RobotState& state, const std::string& group = "",
371  bool verbose = false) const;
372 
375  bool isStateColliding(const moveit_msgs::RobotState& state, const std::string& group = "", bool verbose = false) const;
376 
380 
383  {
385  }
386 
391  {
392  robot_state.updateCollisionBodyTransforms();
393  checkCollision(req, res, static_cast<const moveit::core::RobotState&>(robot_state));
394  }
395 
401 
408  {
409  robot_state.updateCollisionBodyTransforms();
410  checkCollision(req, res, static_cast<const moveit::core::RobotState&>(robot_state), acm);
411  }
412 
418 
424 
429  {
431  }
432 
438  {
440  }
441 
447  {
448  robot_state.updateCollisionBodyTransforms();
449  checkCollisionUnpadded(req, res, static_cast<const moveit::core::RobotState&>(robot_state),
451  }
452 
459  {
460  robot_state.updateCollisionBodyTransforms();
461  checkCollisionUnpadded(req, res, static_cast<const moveit::core::RobotState&>(robot_state), acm);
462  }
463 
469 
472 
476  {
478  }
479 
483  {
484  robot_state.updateCollisionBodyTransforms();
486  }
487 
491  {
492  // do self-collision checking with the unpadded version of the robot
493  getCollisionEnvUnpadded()->checkSelfCollision(req, res, robot_state, getAllowedCollisionMatrix());
494  }
495 
501  {
502  robot_state.updateCollisionBodyTransforms();
503  checkSelfCollision(req, res, static_cast<const moveit::core::RobotState&>(robot_state), acm);
504  }
505 
511  {
512  // do self-collision checking with the unpadded version of the robot
513  getCollisionEnvUnpadded()->checkSelfCollision(req, res, robot_state, acm);
514  }
515 
517  void getCollidingLinks(std::vector<std::string>& links);
518 
520  void getCollidingLinks(std::vector<std::string>& links) const
521  {
523  }
524 
527  void getCollidingLinks(std::vector<std::string>& links, moveit::core::RobotState& robot_state) const
528  {
529  robot_state.updateCollisionBodyTransforms();
531  }
532 
534  void getCollidingLinks(std::vector<std::string>& links, const moveit::core::RobotState& robot_state) const
535  {
537  }
538 
541  void getCollidingLinks(std::vector<std::string>& links, moveit::core::RobotState& robot_state,
543  {
544  robot_state.updateCollisionBodyTransforms();
545  getCollidingLinks(links, static_cast<const moveit::core::RobotState&>(robot_state), acm);
546  }
547 
550  void getCollidingLinks(std::vector<std::string>& links, const moveit::core::RobotState& robot_state,
552 
556 
559  {
561  }
562 
566  {
568  }
569 
574  {
575  robot_state.updateCollisionBodyTransforms();
577  }
578 
584  {
585  robot_state.updateCollisionBodyTransforms();
586  getCollidingPairs(contacts, static_cast<const moveit::core::RobotState&>(robot_state), acm);
587  }
588 
594 
606  {
607  robot_state.updateCollisionBodyTransforms();
608  return distanceToCollision(static_cast<const moveit::core::RobotState&>(robot_state));
609  }
610 
615  {
616  return getCollisionEnv()->distanceRobot(robot_state, getAllowedCollisionMatrix());
617  }
618 
622  {
623  robot_state.updateCollisionBodyTransforms();
625  }
626 
630  {
632  }
633 
639  {
640  robot_state.updateCollisionBodyTransforms();
641  return distanceToCollision(static_cast<const moveit::core::RobotState&>(robot_state), acm);
642  }
643 
649  {
650  return getCollisionEnv()->distanceRobot(robot_state, acm);
651  }
652 
658  {
659  robot_state.updateCollisionBodyTransforms();
660  return distanceToCollisionUnpadded(static_cast<const moveit::core::RobotState&>(robot_state), acm);
661  }
662 
668  {
669  return getCollisionEnvUnpadded()->distanceRobot(robot_state, acm);
670  }
671 
675  void saveGeometryToStream(std::ostream& out) const;
676 
678  bool loadGeometryFromStream(std::istream& in);
679 
681  bool loadGeometryFromStream(std::istream& in, const Eigen::Isometry3d& offset);
682 
687  void getPlanningSceneDiffMsg(moveit_msgs::PlanningScene& scene) const;
688 
692  void getPlanningSceneMsg(moveit_msgs::PlanningScene& scene) const;
693 
696  void getPlanningSceneMsg(moveit_msgs::PlanningScene& scene, const moveit_msgs::PlanningSceneComponents& comp) const;
697 
700  bool getCollisionObjectMsg(moveit_msgs::CollisionObject& collision_obj, const std::string& ns) const;
701 
704  void getCollisionObjectMsgs(std::vector<moveit_msgs::CollisionObject>& collision_objs) const;
705 
708  bool getAttachedCollisionObjectMsg(moveit_msgs::AttachedCollisionObject& attached_collision_obj,
709  const std::string& ns) const;
710 
713  void getAttachedCollisionObjectMsgs(std::vector<moveit_msgs::AttachedCollisionObject>& attached_collision_objs) const;
714 
716  bool getOctomapMsg(octomap_msgs::OctomapWithPose& octomap) const;
717 
719  void getObjectColorMsgs(std::vector<moveit_msgs::ObjectColor>& object_colors) const;
720 
726  bool setPlanningSceneDiffMsg(const moveit_msgs::PlanningScene& scene);
727 
730  bool setPlanningSceneMsg(const moveit_msgs::PlanningScene& scene);
731 
734  bool usePlanningSceneMsg(const moveit_msgs::PlanningScene& scene);
735 
736  bool processCollisionObjectMsg(const moveit_msgs::CollisionObject& object);
737  bool processAttachedCollisionObjectMsg(const moveit_msgs::AttachedCollisionObject& object);
738 
739  bool processPlanningSceneWorldMsg(const moveit_msgs::PlanningSceneWorld& world);
740 
741  void processOctomapMsg(const octomap_msgs::OctomapWithPose& map);
742  void processOctomapMsg(const octomap_msgs::Octomap& map);
743  void processOctomapPtr(const std::shared_ptr<const octomap::OcTree>& octree, const Eigen::Isometry3d& t);
744 
749 
753  void setCurrentState(const moveit_msgs::RobotState& state);
754 
756  void setCurrentState(const moveit::core::RobotState& state);
757 
760 
763 
764  bool hasObjectColor(const std::string& id) const;
765 
766  const std_msgs::ColorRGBA& getObjectColor(const std::string& id) const;
767  void setObjectColor(const std::string& id, const std_msgs::ColorRGBA& color);
768  void removeObjectColor(const std::string& id);
769  void getKnownObjectColors(ObjectColorMap& kc) const;
770 
771  bool hasObjectType(const std::string& id) const;
772 
773  const object_recognition_msgs::ObjectType& getObjectType(const std::string& id) const;
774  void setObjectType(const std::string& id, const object_recognition_msgs::ObjectType& type);
775  void removeObjectType(const std::string& id);
776  void getKnownObjectTypes(ObjectTypeMap& kc) const;
777 
782  void clearDiffs();
783 
787  void pushDiffs(const PlanningScenePtr& scene);
788 
792  void decoupleParent();
793 
798  {
799  state_feasibility_ = fn;
800  }
801 
805  {
806  return state_feasibility_;
807  }
808 
812  {
813  motion_feasibility_ = fn;
814  }
815 
819  {
820  return motion_feasibility_;
821  }
822 
825  bool isStateFeasible(const moveit_msgs::RobotState& state, bool verbose = false) const;
826 
829  bool isStateFeasible(const moveit::core::RobotState& state, bool verbose = false) const;
830 
832  bool isStateConstrained(const moveit_msgs::RobotState& state, const moveit_msgs::Constraints& constr,
833  bool verbose = false) const;
834 
836  bool isStateConstrained(const moveit::core::RobotState& state, const moveit_msgs::Constraints& constr,
837  bool verbose = false) const;
838 
840  bool isStateConstrained(const moveit_msgs::RobotState& state,
841  const kinematic_constraints::KinematicConstraintSet& constr, bool verbose = false) const;
842 
845  const kinematic_constraints::KinematicConstraintSet& constr, bool verbose = false) const;
846 
848  bool isStateValid(const moveit_msgs::RobotState& state, const std::string& group = "", bool verbose = false) const;
849 
851  bool isStateValid(const moveit::core::RobotState& state, const std::string& group = "", bool verbose = false) const;
852 
855  bool isStateValid(const moveit_msgs::RobotState& state, const moveit_msgs::Constraints& constr,
856  const std::string& group = "", bool verbose = false) const;
857 
860  bool isStateValid(const moveit::core::RobotState& state, const moveit_msgs::Constraints& constr,
861  const std::string& group = "", bool verbose = false) const;
862 
866  const std::string& group = "", bool verbose = false) const;
867 
869  bool isPathValid(const moveit_msgs::RobotState& start_state, const moveit_msgs::RobotTrajectory& trajectory,
870  const std::string& group = "", bool verbose = false,
871  std::vector<std::size_t>* invalid_index = nullptr) const;
872 
876  bool isPathValid(const moveit_msgs::RobotState& start_state, const moveit_msgs::RobotTrajectory& trajectory,
877  const moveit_msgs::Constraints& path_constraints, const std::string& group = "",
878  bool verbose = false, std::vector<std::size_t>* invalid_index = nullptr) const;
879 
883  bool isPathValid(const moveit_msgs::RobotState& start_state, const moveit_msgs::RobotTrajectory& trajectory,
884  const moveit_msgs::Constraints& path_constraints, const moveit_msgs::Constraints& goal_constraints,
885  const std::string& group = "", bool verbose = false,
886  std::vector<std::size_t>* invalid_index = nullptr) const;
887 
891  bool isPathValid(const moveit_msgs::RobotState& start_state, const moveit_msgs::RobotTrajectory& trajectory,
892  const moveit_msgs::Constraints& path_constraints,
893  const std::vector<moveit_msgs::Constraints>& goal_constraints, const std::string& group = "",
894  bool verbose = false, std::vector<std::size_t>* invalid_index = nullptr) const;
895 
899  bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory,
900  const moveit_msgs::Constraints& path_constraints,
901  const std::vector<moveit_msgs::Constraints>& goal_constraints, const std::string& group = "",
902  bool verbose = false, std::vector<std::size_t>* invalid_index = nullptr) const;
903 
907  bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory,
908  const moveit_msgs::Constraints& path_constraints, const moveit_msgs::Constraints& goal_constraints,
909  const std::string& group = "", bool verbose = false,
910  std::vector<std::size_t>* invalid_index = nullptr) const;
911 
914  bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory,
915  const moveit_msgs::Constraints& path_constraints, const std::string& group = "",
916  bool verbose = false, std::vector<std::size_t>* invalid_index = nullptr) const;
917 
919  bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory, const std::string& group = "",
920  bool verbose = false, std::vector<std::size_t>* invalid_index = nullptr) const;
921 
924  void getCostSources(const robot_trajectory::RobotTrajectory& trajectory, std::size_t max_costs,
925  std::set<collision_detection::CostSource>& costs, double overlap_fraction = 0.9) const;
926 
929  void getCostSources(const robot_trajectory::RobotTrajectory& trajectory, std::size_t max_costs,
930  const std::string& group_name, std::set<collision_detection::CostSource>& costs,
931  double overlap_fraction = 0.9) const;
932 
934  void getCostSources(const moveit::core::RobotState& state, std::size_t max_costs,
935  std::set<collision_detection::CostSource>& costs) const;
936 
939  void getCostSources(const moveit::core::RobotState& state, std::size_t max_costs, const std::string& group_name,
940  std::set<collision_detection::CostSource>& costs) const;
941 
943  void printKnownObjects(std::ostream& out = std::cout) const;
944 
947  [[deprecated("Use moveit/utils/message_checks.h instead")]] static bool isEmpty(const moveit_msgs::PlanningScene& msg);
948 
951  [[deprecated("Use moveit/utils/message_checks.h instead")]] static bool
952  isEmpty(const moveit_msgs::PlanningSceneWorld& msg);
953 
955  [[deprecated("Use moveit/utils/message_checks.h instead")]] static bool isEmpty(const moveit_msgs::RobotState& msg);
956 
958  static PlanningScenePtr clone(const PlanningSceneConstPtr& scene);
959 
960 private:
961  /* Private constructor used by the diff() methods. */
962  PlanningScene(const PlanningSceneConstPtr& parent);
963 
964  /* Initialize the scene. This should only be called by the constructors.
965  * Requires a valid robot_model_ */
966  void initialize();
967 
968  /* helper function to create a RobotModel from a urdf/srdf. */
969  static moveit::core::RobotModelPtr createRobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model,
970  const srdf::ModelConstSharedPtr& srdf_model);
971 
972  /* Helper functions for processing collision objects */
973  bool processCollisionObjectAdd(const moveit_msgs::CollisionObject& object);
974  bool processCollisionObjectRemove(const moveit_msgs::CollisionObject& object);
975  bool processCollisionObjectMove(const moveit_msgs::CollisionObject& object);
976 
978  static void poseMsgToEigen(const geometry_msgs::Pose& msg, Eigen::Isometry3d& out);
979 
981 
982  /* \brief A set of compatible collision detectors */
983  struct CollisionDetector
984  {
985  collision_detection::CollisionDetectorAllocatorPtr alloc_;
986  collision_detection::CollisionEnvPtr cenv_; // never NULL
987  collision_detection::CollisionEnvConstPtr cenv_const_;
988 
989  collision_detection::CollisionEnvPtr cenv_unpadded_;
990  collision_detection::CollisionEnvConstPtr cenv_unpadded_const_;
991 
992  CollisionDetectorConstPtr parent_; // may be NULL
993 
994  const collision_detection::CollisionEnvConstPtr& getCollisionEnv() const
995  {
996  return cenv_const_ ? cenv_const_ : parent_->getCollisionEnv();
997  }
998  const collision_detection::CollisionEnvConstPtr& getCollisionEnvUnpadded() const
999  {
1000  return cenv_unpadded_const_ ? cenv_unpadded_const_ : parent_->getCollisionEnvUnpadded();
1001  }
1002  void findParent(const PlanningScene& scene);
1003  void copyPadding(const CollisionDetector& src);
1004  };
1005  friend struct CollisionDetector;
1006 
1007  using CollisionDetectorIterator = std::map<std::string, CollisionDetectorPtr>::iterator;
1008  using CollisionDetectorConstIterator = std::map<std::string, CollisionDetectorPtr>::const_iterator;
1009 
1012 
1013  std::string name_; // may be empty
1014 
1015  PlanningSceneConstPtr parent_; // Null unless this is a diff scene
1016 
1017  moveit::core::RobotModelConstPtr robot_model_; // Never null (may point to same model as parent)
1019  moveit::core::RobotStatePtr robot_state_; // if NULL use parent's
1020 
1021  // Called when changes are made to attached bodies
1023 
1024  // This variable is not necessarily used by child planning scenes
1025  // This Transforms class is actually a SceneTransforms class
1026  moveit::core::TransformsPtr scene_transforms_; // if NULL use parent's
1027 
1028  collision_detection::WorldPtr world_; // never NULL, never shared with parent/child
1029  collision_detection::WorldConstPtr world_const_; // copy of world_
1030  collision_detection::WorldDiffPtr world_diff_; // NULL unless this is a diff scene
1033 
1034  std::map<std::string, CollisionDetectorPtr> collision_; // never empty
1035  CollisionDetectorPtr active_collision_; // copy of one of the entries in collision_. Never NULL.
1036 
1037  collision_detection::AllowedCollisionMatrixPtr acm_; // if NULL use parent's
1038 
1041 
1042  std::unique_ptr<ObjectColorMap> object_colors_;
1043 
1044  // a map of object types
1045  std::unique_ptr<ObjectTypeMap> object_types_;
1046 };
1047 } // namespace planning_scene
planning_scene::PlanningScene::getAllowedCollisionMatrixNonConst
collision_detection::AllowedCollisionMatrix & getAllowedCollisionMatrixNonConst()
Get the allowed collision matrix.
Definition: planning_scene.cpp:649
planning_scene::PlanningScene::getCurrentState
const moveit::core::RobotState & getCurrentState() const
Get the state at which the robot is assumed to be.
Definition: planning_scene.h:181
moveit::core
Core components of MoveIt.
Definition: kinematics_base.h:83
planning_scene::PlanningScene::getMotionFeasibilityPredicate
const MotionFeasibilityFn & getMotionFeasibilityPredicate() const
Get the predicate that decides whether motion segments are considered valid or invalid for reasons be...
Definition: planning_scene.h:850
planning_scene::PlanningScene::isEmpty
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,...
Definition: planning_scene.cpp:131
collision_detection::CollisionResult::ContactMap
std::map< std::pair< std::string, std::string >, std::vector< Contact > > ContactMap
Definition: include/moveit/collision_detection/collision_common.h:182
planning_scene::PlanningScene::setMotionFeasibilityPredicate
void setMotionFeasibilityPredicate(const MotionFeasibilityFn &fn)
Specify a predicate that decides whether motion segments are considered valid or invalid for reasons ...
Definition: planning_scene.h:843
planning_scene::PlanningScene::printKnownObjects
void printKnownObjects(std::ostream &out=std::cout) const
Outputs debug information about the planning scene contents.
Definition: planning_scene.cpp:2348
planning_scene::PlanningScene::world_diff_
collision_detection::WorldDiffPtr world_diff_
Definition: planning_scene.h:1062
kinematic_constraint.h
planning_scene::PlanningScene::distanceToCollision
double distanceToCollision(moveit::core::RobotState &robot_state) const
The distance between the robot model at state robot_state to the nearest collision (ignoring self-col...
Definition: planning_scene.h:637
planning_scene::PlanningScene::PlanningScene
PlanningScene(const moveit::core::RobotModelConstPtr &robot_model, const collision_detection::WorldPtr &world=std::make_shared< collision_detection::World >())
construct using an existing RobotModel
Definition: planning_scene.cpp:146
planning_scene::PlanningScene::getRobotModel
const moveit::core::RobotModelConstPtr & getRobotModel() const
Get the kinematic model for which the planning scene is maintained.
Definition: planning_scene.h:174
planning_scene::PlanningScene::getObjectColorMsgs
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...
Definition: planning_scene.cpp:876
planning_scene::PlanningScene::getWorld
const collision_detection::WorldConstPtr & getWorld() const
Get the representation of the world.
Definition: planning_scene.h:317
planning_scene::PlanningScene::hasObjectColor
bool hasObjectColor(const std::string &id) const
Definition: planning_scene.cpp:1996
planning_scene::PlanningScene::current_world_object_update_callback_
collision_detection::World::ObserverCallbackFn current_world_object_update_callback_
Definition: planning_scene.h:1063
planning_scene::PlanningScene::getCollisionObjectMsgs
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 ...
Definition: planning_scene.cpp:819
planning_scene::PlanningScene::initialize
void initialize()
Definition: planning_scene.cpp:176
planning_scene::PlanningScene::processAttachedCollisionObjectMsg
bool processAttachedCollisionObjectMsg(const moveit_msgs::AttachedCollisionObject &object)
Definition: planning_scene.cpp:1412
world_diff.h
planning_scene::PlanningScene::pushDiffs
void pushDiffs(const PlanningScenePtr &scene)
If there is a parent specified for this scene, then the diffs with respect to that parent are applied...
Definition: planning_scene.cpp:445
planning_scene::PlanningScene::CollisionDetector
Definition: planning_scene.h:1015
planning_scene::PlanningScene::MOVEIT_STRUCT_FORWARD
MOVEIT_STRUCT_FORWARD(CollisionDetector)
planning_scene::PlanningScene::getTransforms
const moveit::core::Transforms & getTransforms() const
Get the set of fixed transforms from known frames to the planning frame.
Definition: planning_scene.h:205
planning_scene::PlanningScene::isStateConstrained
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.
Definition: planning_scene.cpp:2091
planning_scene::PlanningScene::getAllowedCollisionMatrix
const collision_detection::AllowedCollisionMatrix & getAllowedCollisionMatrix() const
Get the allowed collision matrix.
Definition: planning_scene.h:362
robot_trajectory.h
planning_scene::PlanningScene::getObjectType
const object_recognition_msgs::ObjectType & getObjectType(const std::string &id) const
Definition: planning_scene.cpp:1959
planning_scene::PlanningScene::getCostSources
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...
Definition: planning_scene.cpp:2289
planning_scene::PlanningScene::CollisionDetector::copyPadding
void copyPadding(const CollisionDetector &src)
Definition: planning_scene.cpp:266
planning_scene::MotionFeasibilityFn
boost::function< bool(const moveit::core::RobotState &, const moveit::core::RobotState &, bool)> MotionFeasibilityFn
This is the function signature for additional feasibility checks to be imposed on motions segments be...
Definition: planning_scene.h:111
planning_scene::PlanningScene::parent_
PlanningSceneConstPtr parent_
Definition: planning_scene.h:1047
kinematics_base.h
planning_scene::PlanningScene::decoupleParent
void decoupleParent()
Make sure that all the data maintained in this scene is local. All unmodified data is copied from the...
Definition: planning_scene.cpp:1139
collision_detection::World::ObserverHandle
Definition: world.h:242
planning_scene::PlanningScene::setCurrentState
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...
Definition: planning_scene.cpp:1101
class_forward.h
moveit::core::RobotState
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:155
planning_scene::PlanningScene::getCollisionDetectorNames
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...
Definition: planning_scene.cpp:367
planning_scene::PlanningScene::getObjectColor
const std_msgs::ColorRGBA & getObjectColor(const std::string &id) const
Definition: planning_scene.cpp:2006
planning_scene::PlanningScene::getCurrentStateUpdated
moveit::core::RobotStatePtr getCurrentStateUpdated(const moveit_msgs::RobotState &update) const
Get a copy of the current state with components overwritten by the state message update.
Definition: planning_scene.cpp:626
planning_scene::PlanningScene::active_collision_
CollisionDetectorPtr active_collision_
Definition: planning_scene.h:1067
robot_trajectory::RobotTrajectory
Maintain a sequence of waypoints and the time durations between these waypoints.
Definition: robot_trajectory.h:84
planning_scene::PlanningScene::propogateRobotPadding
void propogateRobotPadding()
Copy scale and padding from active CollisionRobot to other CollisionRobots. This should be called aft...
Definition: planning_scene.cpp:272
planning_scene::ObjectTypeMap
std::map< std::string, object_recognition_msgs::ObjectType > ObjectTypeMap
A map from object names (e.g., attached bodies, collision objects) to their types.
Definition: planning_scene.h:117
planning_scene::PlanningScene::processOctomapPtr
void processOctomapPtr(const std::shared_ptr< const octomap::OcTree > &octree, const Eigen::Isometry3d &t)
Definition: planning_scene.cpp:1379
collision_detection::CollisionRequest
Representation of a collision checking request.
Definition: include/moveit/collision_detection/collision_common.h:216
planning_scene::PlanningScene::getCurrentStateNonConst
moveit::core::RobotState & getCurrentStateNonConst()
Get the state at which the robot is assumed to be.
Definition: planning_scene.cpp:615
robot_model.h
planning_scene::PlanningScene::getCollisionObjectMsg
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...
Definition: planning_scene.cpp:783
planning_scene::PlanningScene::clearDiffs
void clearDiffs()
Clear the diffs accumulated for this planning scene, with respect to: the parent PlanningScene (if it...
Definition: planning_scene.cpp:405
planning_scene::PlanningScene::collision_
std::map< std::string, CollisionDetectorPtr > collision_
Definition: planning_scene.h:1066
planning_scene::PlanningScene::CollisionDetectorIterator
std::map< std::string, CollisionDetectorPtr >::iterator CollisionDetectorIterator
Definition: planning_scene.h:1039
planning_scene::PlanningScene::getPlanningSceneMsg
void getPlanningSceneMsg(moveit_msgs::PlanningScene &scene) const
Construct a message (scene) with all the necessary data so that the scene can be later reconstructed ...
Definition: planning_scene.cpp:891
planning_scene::PlanningScene::CollisionDetectorConstIterator
std::map< std::string, CollisionDetectorPtr >::const_iterator CollisionDetectorConstIterator
Definition: planning_scene.h:1040
planning_scene::PlanningScene::isStateValid
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.
Definition: planning_scene.cpp:2131
planning_scene::PlanningScene::getCollidingPairs
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...
Definition: planning_scene.cpp:564
transforms.h
planning_scene::PlanningScene::getAttachedCollisionObjectMsgs
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...
Definition: planning_scene.cpp:847
planning_scene::PlanningScene::CollisionDetector::parent_
CollisionDetectorConstPtr parent_
Definition: planning_scene.h:1024
planning_scene::PlanningScene::current_world_object_update_observer_handle_
collision_detection::World::ObserverHandle current_world_object_update_observer_handle_
Definition: planning_scene.h:1064
planning_scene::PlanningScene::DEFAULT_SCENE_NAME
static const MOVEIT_PLANNING_SCENE_EXPORT std::string DEFAULT_SCENE_NAME
Definition: planning_scene.h:135
planning_scene::PlanningScene::knowsFrameTransform
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,...
Definition: planning_scene.cpp:1932
planning_scene::PlanningScene::getStateFeasibilityPredicate
const StateFeasibilityFn & getStateFeasibilityPredicate() const
Get the predicate that decides whether states are considered valid or invalid for reasons beyond ones...
Definition: planning_scene.h:836
collision_detection::AllowedCollisionMatrix
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
Definition: collision_matrix.h:112
planning_scene::PlanningScene::hasObjectType
bool hasObjectType(const std::string &id) const
Definition: planning_scene.cpp:1949
planning_scene::PlanningScene::getKnownObjectColors
void getKnownObjectColors(ObjectColorMap &kc) const
Definition: planning_scene.cpp:2020
planning_scene::PlanningScene::robot_state_
moveit::core::RobotStatePtr robot_state_
Definition: planning_scene.h:1051
planning_scene::PlanningScene::CollisionDetector::getCollisionEnv
const collision_detection::CollisionEnvConstPtr & getCollisionEnv() const
Definition: planning_scene.h:1026
planning_scene::PlanningScene::CollisionDetector::alloc_
collision_detection::CollisionDetectorAllocatorPtr alloc_
Definition: planning_scene.h:1017
planning_scene::PlanningScene::isStateColliding
bool isStateColliding(const std::string &group="", bool verbose=false)
Check if the current state is in collision (with the environment or self collision)....
Definition: planning_scene.cpp:2055
planning_scene::PlanningScene::allocateCollisionDetectors
void allocateCollisionDetectors()
name
std::string name
planning_scene::PlanningScene::addCollisionDetector
void addCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr &allocator)
Add a new collision detector type.
Definition: planning_scene.cpp:291
planning_scene::MOVEIT_CLASS_FORWARD
MOVEIT_CLASS_FORWARD(PlanningScene)
collision_detection::World::ObserverCallbackFn
boost::function< void(const ObjectConstPtr &, Action)> ObserverCallbackFn
Definition: world.h:257
planning_scene::PlanningScene::checkCollision
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...
Definition: planning_scene.cpp:502
planning_scene::PlanningScene::checkCollisionUnpadded
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...
Definition: planning_scene.cpp:540
planning_scene::PlanningScene::getActiveCollisionDetectorName
const std::string & getActiveCollisionDetectorName() const
Definition: planning_scene.h:307
collision_detection::CollisionResult
Representation of a collision checking result.
Definition: include/moveit/collision_detection/collision_common.h:177
planning_scene::PlanningScene::CollisionDetector::getCollisionEnvUnpadded
const collision_detection::CollisionEnvConstPtr & getCollisionEnvUnpadded() const
Definition: planning_scene.h:1030
collision_env.h
planning_scene::PlanningScene::world_
collision_detection::WorldPtr world_
Definition: planning_scene.h:1060
planning_scene::PlanningScene::CollisionDetector::cenv_unpadded_
collision_detection::CollisionEnvPtr cenv_unpadded_
Definition: planning_scene.h:1021
planning_scene::PlanningScene::removeObjectType
void removeObjectType(const std::string &id)
Definition: planning_scene.cpp:1980
planning_scene::PlanningScene::setObjectType
void setObjectType(const std::string &id, const object_recognition_msgs::ObjectType &type)
Definition: planning_scene.cpp:1973
planning_scene::PlanningScene::removeAllCollisionObjects
void removeAllCollisionObjects()
Clear all collision objects in planning scene.
Definition: planning_scene.cpp:1344
planning_scene::PlanningScene::isPathValid
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=nullptr) const
Check if a given path is valid. Each state is checked for validity (collision avoidance and feasibili...
Definition: planning_scene.cpp:2166
srdf::ModelConstSharedPtr
std::shared_ptr< const Model > ModelConstSharedPtr
planning_scene::PlanningScene::distanceToCollisionUnpadded
double distanceToCollisionUnpadded(moveit::core::RobotState &robot_state) const
The distance between the robot model at state robot_state to the nearest collision (ignoring self-col...
Definition: planning_scene.h:653
planning_scene::PlanningScene::setName
void setName(const std::string &name)
Set the name of the planning scene.
Definition: planning_scene.h:146
planning_scene::PlanningScene::~PlanningScene
~PlanningScene()
Definition: planning_scene.cpp:170
planning_scene::PlanningScene::state_feasibility_
StateFeasibilityFn state_feasibility_
Definition: planning_scene.h:1071
planning_scene::PlanningScene::world_const_
collision_detection::WorldConstPtr world_const_
Definition: planning_scene.h:1061
planning_scene::PlanningScene::getKnownObjectTypes
void getKnownObjectTypes(ObjectTypeMap &kc) const
Definition: planning_scene.cpp:1986
moveit::core::AttachedBodyCallback
boost::function< void(AttachedBody *body, bool attached)> AttachedBodyCallback
Definition: attached_body.h:115
planning_scene::PlanningScene::getPlanningSceneDiffMsg
void getPlanningSceneDiffMsg(moveit_msgs::PlanningScene &scene) const
Fill the message scene with the differences between this instance of PlanningScene with respect to th...
Definition: planning_scene.cpp:677
kinematic_constraints::KinematicConstraintSet
A class that contains many different constraints, and can check RobotState *versus the full set....
Definition: kinematic_constraint.h:903
planning_scene::ObjectColorMap
std::map< std::string, std_msgs::ColorRGBA > ObjectColorMap
A map from object names (e.g., attached bodies, collision objects) to their colors.
Definition: planning_scene.h:114
planning_scene::PlanningScene::object_types_
std::unique_ptr< ObjectTypeMap > object_types_
Definition: planning_scene.h:1077
planning_scene::StateFeasibilityFn
boost::function< bool(const moveit::core::RobotState &, bool)> StateFeasibilityFn
This is the function signature for additional feasibility checks to be imposed on states (in addition...
Definition: planning_scene.h:103
planning_scene::PlanningScene::object_colors_
std::unique_ptr< ObjectColorMap > object_colors_
Definition: planning_scene.h:1074
planning_scene::PlanningScene::getCollisionEnvUnpadded
const collision_detection::CollisionEnvConstPtr & getCollisionEnvUnpadded() const
Get the active collision detector for the robot.
Definition: planning_scene.h:337
planning_scene::PlanningScene::processOctomapMsg
void processOctomapMsg(const octomap_msgs::OctomapWithPose &map)
Definition: planning_scene.cpp:1356
planning_scene::PlanningScene::processCollisionObjectMove
bool processCollisionObjectMove(const moveit_msgs::CollisionObject &object)
Definition: planning_scene.cpp:1848
planning_scene::PlanningScene::setAttachedBodyUpdateCallback
void setAttachedBodyUpdateCallback(const moveit::core::AttachedBodyCallback &callback)
Set the callback to be triggered when changes are made to the current scene state.
Definition: planning_scene.cpp:633
planning_scene::PlanningScene::acm_
collision_detection::AllowedCollisionMatrixPtr acm_
Definition: planning_scene.h:1069
planning_scene::PlanningScene::getName
const std::string & getName() const
Get the name of the planning scene. This is empty by default.
Definition: planning_scene.h:140
planning_scene::PlanningScene::removeObjectColor
void removeObjectColor(const std::string &id)
Definition: planning_scene.cpp:2042
moveit::core::RobotState::updateLinkTransforms
void updateLinkTransforms()
Update the reference frame transforms for links. This call is needed before using the transforms of l...
Definition: robot_state.cpp:744
planning_scene::PlanningScene::CollisionDetector::cenv_const_
collision_detection::CollisionEnvConstPtr cenv_const_
Definition: planning_scene.h:1019
planning_scene::PlanningScene::loadGeometryFromStream
bool loadGeometryFromStream(std::istream &in)
Load the geometry of the planning scene from a stream.
Definition: planning_scene.cpp:1011
planning_scene::PlanningScene::robot_model_
moveit::core::RobotModelConstPtr robot_model_
Definition: planning_scene.h:1049
planning_scene::PlanningScene::getCollidingLinks
void getCollidingLinks(std::vector< std::string > &links)
Get the names of the links that are involved in collisions for the current state.
Definition: planning_scene.cpp:585
planning_scene::PlanningScene::clone
static PlanningScenePtr clone(const PlanningSceneConstPtr &scene)
Clone a planning scene. Even if the scene scene depends on a parent, the cloned scene will not.
Definition: planning_scene.cpp:246
planning_scene::PlanningScene::setPlanningSceneDiffMsg
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...
Definition: planning_scene.cpp:1199
planning_scene::PlanningScene::getPlanningFrame
const std::string & getPlanningFrame() const
Get the frame in which planning is performed.
Definition: planning_scene.h:198
planning_scene::PlanningScene::usePlanningSceneMsg
bool usePlanningSceneMsg(const moveit_msgs::PlanningScene &scene)
Call setPlanningSceneMsg() or setPlanningSceneDiffMsg() depending on how the is_diff member of the me...
Definition: planning_scene.cpp:1290
moveit::core::RobotState::updateCollisionBodyTransforms
void updateCollisionBodyTransforms()
Update the transforms for the collision bodies. This call is needed before calling collision checking...
Definition: robot_state.cpp:716
planning_scene::PlanningScene::setPlanningSceneMsg
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,...
Definition: planning_scene.cpp:1253
planning_scene::PlanningScene::isStateFeasible
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...
Definition: planning_scene.cpp:2073
moveit::core::Transforms
Provides an implementation of a snapshot of a transform tree that can be easily queried for transform...
Definition: transforms.h:122
planning_scene::PlanningScene::getAttachedCollisionObjectMsg
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...
Definition: planning_scene.cpp:831
planning_scene::PlanningScene::CollisionDetector::findParent
void findParent(const PlanningScene &scene)
Definition: planning_scene.cpp:281
planning_scene::PlanningScene::CollisionDetector::cenv_
collision_detection::CollisionEnvPtr cenv_
Definition: planning_scene.h:1018
planning_scene::PlanningScene::getParent
const PlanningSceneConstPtr & getParent() const
Get the parent scene (whith respect to which the diffs are maintained). This may be empty.
Definition: planning_scene.h:168
planning_scene::PlanningScene::motion_feasibility_
MotionFeasibilityFn motion_feasibility_
Definition: planning_scene.h:1072
planning_scene::PlanningScene::getCollisionEnvNonConst
const collision_detection::CollisionEnvPtr & getCollisionEnvNonConst()
Get the representation of the collision robot This can be used to set padding and link scale on the a...
Definition: planning_scene.cpp:610
planning_scene::PlanningScene::OCTOMAP_NS
static const MOVEIT_PLANNING_SCENE_EXPORT std::string OCTOMAP_NS
Definition: planning_scene.h:134
planning_scene::PlanningScene::checkSelfCollision
void checkSelfCollision(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res)
Check whether the current state is in self collision.
Definition: planning_scene.cpp:518
planning_scene::PlanningScene::scene_transforms_
moveit::core::TransformsPtr scene_transforms_
Definition: planning_scene.h:1058
planning_scene::PlanningScene::processPlanningSceneWorldMsg
bool processPlanningSceneWorldMsg(const moveit_msgs::PlanningSceneWorld &world)
Definition: planning_scene.cpp:1281
planning_scene::PlanningScene::current_state_attached_body_callback_
moveit::core::AttachedBodyCallback current_state_attached_body_callback_
Definition: planning_scene.h:1054
planning_scene::PlanningScene::getFrameTransform
const Eigen::Isometry3d & getFrameTransform(const std::string &id) const
Get the transform corresponding to the frame id. This will be known if id is a link name,...
Definition: planning_scene.cpp:1901
octomap
planning_scene::PlanningScene::getOctomapMsg
bool getOctomapMsg(octomap_msgs::OctomapWithPose &octomap) const
Construct a message (octomap) with the octomap data from the planning_scene.
Definition: planning_scene.cpp:855
planning_scene::PlanningScene::setCollisionObjectUpdateCallback
void setCollisionObjectUpdateCallback(const collision_detection::World::ObserverCallbackFn &callback)
Set the callback to be triggered when changes are made to the current scene world.
Definition: planning_scene.cpp:640
planning_scene::PlanningScene::CollisionDetector::cenv_unpadded_const_
collision_detection::CollisionEnvConstPtr cenv_unpadded_const_
Definition: planning_scene.h:1022
planning_scene::PlanningScene::getTransformsNonConst
moveit::core::Transforms & getTransformsNonConst()
Get the set of fixed transforms from known frames to the planning frame.
Definition: planning_scene.cpp:663
planning_scene::PlanningScene::setStateFeasibilityPredicate
void setStateFeasibilityPredicate(const StateFeasibilityFn &fn)
Specify a predicate that decides whether states are considered valid or invalid for reasons beyond on...
Definition: planning_scene.h:829
planning_scene::PlanningScene::name_
std::string name_
Definition: planning_scene.h:1045
planning_scene
This namespace includes the central class for representing planning contexts.
Definition: planning_interface.h:45
planning_scene::PlanningScene::saveGeometryToStream
void saveGeometryToStream(std::ostream &out) const
Save the geometry of the planning scene to a stream, as plain text.
Definition: planning_scene.cpp:978
planning_scene::PlanningScene::getWorldNonConst
const collision_detection::WorldPtr & getWorldNonConst()
Definition: planning_scene.h:324
planning_scene::PlanningScene::getCollisionEnv
const collision_detection::CollisionEnvConstPtr & getCollisionEnv() const
Get the active collision environment.
Definition: planning_scene.h:331
planning_scene::PlanningScene::processCollisionObjectRemove
bool processCollisionObjectRemove(const moveit_msgs::CollisionObject &object)
Definition: planning_scene.cpp:1833
planning_scene::PlanningScene::processCollisionObjectMsg
bool processCollisionObjectMsg(const moveit_msgs::CollisionObject &object)
Definition: planning_scene.cpp:1707
robot_state.h
planning_scene::PlanningScene::CollisionDetector
friend struct CollisionDetector
Definition: planning_scene.h:1037
planning_scene::PlanningScene::createRobotModel
static moveit::core::RobotModelPtr createRobotModel(const urdf::ModelInterfaceSharedPtr &urdf_model, const srdf::ModelConstSharedPtr &srdf_model)
Definition: planning_scene.cpp:200
planning_scene::PlanningScene::diff
PlanningScenePtr diff() const
Return a new child PlanningScene that uses this one as parent.
Definition: planning_scene.cpp:254
collision_detector_allocator.h
planning_scene::PlanningScene::processCollisionObjectAdd
bool processCollisionObjectAdd(const moveit_msgs::CollisionObject &object)
Definition: planning_scene.cpp:1748
planning_scene::PlanningScene::poseMsgToEigen
static void poseMsgToEigen(const geometry_msgs::Pose &msg, Eigen::Isometry3d &out)
Definition: planning_scene.cpp:1732
verbose
bool verbose
planning_scene::PlanningScene::setObjectColor
void setObjectColor(const std::string &id, const std_msgs::ColorRGBA &color)
Definition: planning_scene.cpp:2030
planning_scene::PlanningScene::setActiveCollisionDetector
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...
Definition: planning_scene.cpp:324


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jul 21 2021 02:23:41