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 
143 namespace planning_scene
144 {
145 MOVEIT_CLASS_FORWARD(PlanningScene); // Defines PlanningScenePtr, ConstPtr, WeakPtr... etc
146 
151 typedef boost::function<bool(const moveit::core::RobotState&, bool)> StateFeasibilityFn;
152 
158 using MotionFeasibilityFn =
159  boost::function<bool(const moveit::core::RobotState&, const moveit::core::RobotState&, bool)>;
160 
162 using ObjectColorMap = std::map<std::string, std_msgs::ColorRGBA>;
163 
165 using ObjectTypeMap = std::map<std::string, object_recognition_msgs::ObjectType>;
166 
170 class PlanningScene : private boost::noncopyable, public std::enable_shared_from_this<PlanningScene>
171 {
172 public:
174  PlanningScene(const moveit::core::RobotModelConstPtr& robot_model,
175  const collision_detection::WorldPtr& world = std::make_shared<collision_detection::World>());
176 
179  PlanningScene(const urdf::ModelInterfaceSharedPtr& urdf_model, const srdf::ModelConstSharedPtr& srdf_model,
180  const collision_detection::WorldPtr& world = std::make_shared<collision_detection::World>());
181 
182  static MOVEIT_PLANNING_SCENE_EXPORT const std::string OCTOMAP_NS;
183  static MOVEIT_PLANNING_SCENE_EXPORT const std::string DEFAULT_SCENE_NAME;
184 
185  ~PlanningScene();
186 
188  const std::string& getName() const
189  {
190  return name_;
191  }
192 
194  void setName(const std::string& name)
195  {
196  name_ = name;
197  }
198 
209  PlanningScenePtr diff() const;
210 
213  PlanningScenePtr diff(const moveit_msgs::PlanningScene& msg) const;
214 
216  const PlanningSceneConstPtr& getParent() const
217  {
218  return parent_;
219  }
220 
222  const moveit::core::RobotModelConstPtr& getRobotModel() const
223  {
224  // the kinematic model does not change
225  return robot_model_;
226  }
227 
230  {
231  // if we have an updated state, return it; otherwise, return the parent one
232  return robot_state_ ? *robot_state_ : parent_->getCurrentState();
233  }
236 
238  moveit::core::RobotStatePtr getCurrentStateUpdated(const moveit_msgs::RobotState& update) const;
239 
246  const std::string& getPlanningFrame() const
247  {
248  // if we have an updated set of transforms, return it; otherwise, return the parent one
249  return scene_transforms_ ? scene_transforms_->getTargetFrame() : parent_->getPlanningFrame();
250  }
251 
254  {
255  if (scene_transforms_ || !parent_)
256  {
257  return *scene_transforms_;
258  }
259 
260  // if this planning scene is a child of another, and doesn't have its own custom transforms
261  return parent_->getTransforms();
262  }
263 
267 
270 
275  const Eigen::Isometry3d& getFrameTransform(const std::string& id) const;
276 
282  const Eigen::Isometry3d& getFrameTransform(const std::string& id);
283 
289  const Eigen::Isometry3d& getFrameTransform(moveit::core::RobotState& state, const std::string& id) const
290  {
291  state.updateLinkTransforms();
292  return getFrameTransform(static_cast<const moveit::core::RobotState&>(state), id);
293  }
294 
299  const Eigen::Isometry3d& getFrameTransform(const moveit::core::RobotState& state, const std::string& id) const;
300 
303  bool knowsFrameTransform(const std::string& id) const;
304 
307  bool knowsFrameTransform(const moveit::core::RobotState& state, const std::string& id) const;
308 
333  void addCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr& allocator);
334 
344  void setActiveCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr& allocator,
345  bool exclusive = false);
346 
353  bool setActiveCollisionDetector(const std::string& collision_detector_name);
354 
355  const std::string& getActiveCollisionDetectorName() const
356  {
357  return active_collision_->alloc_->getName();
358  }
359 
362  void getCollisionDetectorNames(std::vector<std::string>& names) const;
363 
365  const collision_detection::WorldConstPtr& getWorld() const
366  {
367  // we always have a world representation
368  return world_const_;
369  }
370 
371  // brief Get the representation of the world
372  const collision_detection::WorldPtr& getWorldNonConst()
373  {
374  // we always have a world representation
375  return world_;
376  }
377 
379  const collision_detection::CollisionEnvConstPtr& getCollisionEnv() const
380  {
381  return active_collision_->getCollisionEnv();
382  }
383 
385  const collision_detection::CollisionEnvConstPtr& getCollisionEnvUnpadded() const
386  {
387  return active_collision_->getCollisionEnvUnpadded();
388  }
389 
391  const collision_detection::CollisionEnvConstPtr& getCollisionEnv(const std::string& collision_detector_name) const;
392 
395  const collision_detection::CollisionEnvConstPtr&
396  getCollisionEnvUnpadded(const std::string& collision_detector_name) const;
397 
402  const collision_detection::CollisionEnvPtr& getCollisionEnvNonConst();
403 
407  void propogateRobotPadding();
408 
411  {
412  return acm_ ? *acm_ : parent_->getAllowedCollisionMatrix();
413  }
416 
427  bool isStateColliding(const std::string& group = "", bool verbose = false);
428 
432  bool isStateColliding(const std::string& group = "", bool verbose = false) const
433  {
434  return isStateColliding(getCurrentState(), group, verbose);
435  }
436 
441  bool isStateColliding(moveit::core::RobotState& state, const std::string& group = "", bool verbose = false) const
442  {
444  return isStateColliding(static_cast<const moveit::core::RobotState&>(state), group, verbose);
445  }
446 
450  bool isStateColliding(const moveit::core::RobotState& state, const std::string& group = "",
451  bool verbose = false) const;
452 
455  bool isStateColliding(const moveit_msgs::RobotState& state, const std::string& group = "", bool verbose = false) const;
456 
460 
463  {
465  }
466 
471  {
472  robot_state.updateCollisionBodyTransforms();
473  checkCollision(req, res, static_cast<const moveit::core::RobotState&>(robot_state));
474  }
475 
481 
488  {
489  robot_state.updateCollisionBodyTransforms();
490  checkCollision(req, res, static_cast<const moveit::core::RobotState&>(robot_state), acm);
491  }
492 
498 
504 
509  {
511  }
512 
518  {
520  }
521 
527  {
528  robot_state.updateCollisionBodyTransforms();
529  checkCollisionUnpadded(req, res, static_cast<const moveit::core::RobotState&>(robot_state),
531  }
532 
539  {
540  robot_state.updateCollisionBodyTransforms();
541  checkCollisionUnpadded(req, res, static_cast<const moveit::core::RobotState&>(robot_state), acm);
542  }
543 
549 
552 
556  {
558  }
559 
563  {
564  robot_state.updateCollisionBodyTransforms();
566  }
567 
571  {
572  // do self-collision checking with the unpadded version of the robot
573  getCollisionEnvUnpadded()->checkSelfCollision(req, res, robot_state, getAllowedCollisionMatrix());
574  }
575 
581  {
582  robot_state.updateCollisionBodyTransforms();
583  checkSelfCollision(req, res, static_cast<const moveit::core::RobotState&>(robot_state), acm);
584  }
585 
591  {
592  // do self-collision checking with the unpadded version of the robot
593  getCollisionEnvUnpadded()->checkSelfCollision(req, res, robot_state, acm);
594  }
595 
597  void getCollidingLinks(std::vector<std::string>& links);
598 
600  void getCollidingLinks(std::vector<std::string>& links) const
601  {
603  }
604 
607  void getCollidingLinks(std::vector<std::string>& links, moveit::core::RobotState& robot_state) const
608  {
609  robot_state.updateCollisionBodyTransforms();
611  }
612 
614  void getCollidingLinks(std::vector<std::string>& links, const moveit::core::RobotState& robot_state) const
615  {
617  }
618 
621  void getCollidingLinks(std::vector<std::string>& links, moveit::core::RobotState& robot_state,
623  {
624  robot_state.updateCollisionBodyTransforms();
625  getCollidingLinks(links, static_cast<const moveit::core::RobotState&>(robot_state), acm);
626  }
627 
630  void getCollidingLinks(std::vector<std::string>& links, const moveit::core::RobotState& robot_state,
632 
636 
639  {
641  }
642 
646  const moveit::core::RobotState& robot_state, const std::string& group_name = "") const
647  {
648  getCollidingPairs(contacts, robot_state, getAllowedCollisionMatrix(), group_name);
649  }
650 
655  moveit::core::RobotState& robot_state, const std::string& group_name = "") const
656  {
657  robot_state.updateCollisionBodyTransforms();
659  group_name);
660  }
661 
667  const std::string& group_name = "") const
668  {
669  robot_state.updateCollisionBodyTransforms();
670  getCollidingPairs(contacts, static_cast<const moveit::core::RobotState&>(robot_state), acm, group_name);
671  }
672 
678  const std::string& group_name = "") const;
679 
691  {
692  robot_state.updateCollisionBodyTransforms();
693  return distanceToCollision(static_cast<const moveit::core::RobotState&>(robot_state));
694  }
695 
700  {
701  return getCollisionEnv()->distanceRobot(robot_state, getAllowedCollisionMatrix());
702  }
703 
707  {
708  robot_state.updateCollisionBodyTransforms();
710  }
711 
715  {
717  }
718 
724  {
725  robot_state.updateCollisionBodyTransforms();
726  return distanceToCollision(static_cast<const moveit::core::RobotState&>(robot_state), acm);
727  }
728 
734  {
735  return getCollisionEnv()->distanceRobot(robot_state, acm);
736  }
737 
743  {
744  robot_state.updateCollisionBodyTransforms();
745  return distanceToCollisionUnpadded(static_cast<const moveit::core::RobotState&>(robot_state), acm);
746  }
747 
753  {
754  return getCollisionEnvUnpadded()->distanceRobot(robot_state, acm);
755  }
756 
763  void saveGeometryToStream(std::ostream& out) const;
764 
766  bool loadGeometryFromStream(std::istream& in);
767 
769  bool loadGeometryFromStream(std::istream& in, const Eigen::Isometry3d& offset);
770 
775  void getPlanningSceneDiffMsg(moveit_msgs::PlanningScene& scene) const;
776 
780  void getPlanningSceneMsg(moveit_msgs::PlanningScene& scene) const;
781 
784  void getPlanningSceneMsg(moveit_msgs::PlanningScene& scene, const moveit_msgs::PlanningSceneComponents& comp) const;
785 
788  bool getCollisionObjectMsg(moveit_msgs::CollisionObject& collision_obj, const std::string& ns) const;
789 
792  void getCollisionObjectMsgs(std::vector<moveit_msgs::CollisionObject>& collision_objs) const;
793 
796  bool getAttachedCollisionObjectMsg(moveit_msgs::AttachedCollisionObject& attached_collision_obj,
797  const std::string& ns) const;
798 
801  void getAttachedCollisionObjectMsgs(std::vector<moveit_msgs::AttachedCollisionObject>& attached_collision_objs) const;
802 
804  bool getOctomapMsg(octomap_msgs::OctomapWithPose& octomap) const;
805 
807  void getObjectColorMsgs(std::vector<moveit_msgs::ObjectColor>& object_colors) const;
808 
814  bool setPlanningSceneDiffMsg(const moveit_msgs::PlanningScene& scene);
815 
818  bool setPlanningSceneMsg(const moveit_msgs::PlanningScene& scene);
819 
822  bool usePlanningSceneMsg(const moveit_msgs::PlanningScene& scene);
823 
828  bool shapesAndPosesFromCollisionObjectMessage(const moveit_msgs::CollisionObject& object,
829  Eigen::Isometry3d& object_pose_in_header_frame,
830  std::vector<shapes::ShapeConstPtr>& shapes,
831  EigenSTL::vector_Isometry3d& shape_poses);
832 
833  bool processCollisionObjectMsg(const moveit_msgs::CollisionObject& object);
834  bool processAttachedCollisionObjectMsg(const moveit_msgs::AttachedCollisionObject& object);
835 
836  bool processPlanningSceneWorldMsg(const moveit_msgs::PlanningSceneWorld& world);
837 
838  void processOctomapMsg(const octomap_msgs::OctomapWithPose& map);
839  void processOctomapMsg(const octomap_msgs::Octomap& map);
840  void processOctomapPtr(const std::shared_ptr<const octomap::OcTree>& octree, const Eigen::Isometry3d& t);
841 
846 
850  void setCurrentState(const moveit_msgs::RobotState& state);
851 
853  void setCurrentState(const moveit::core::RobotState& state);
854 
857 
860 
861  bool hasObjectColor(const std::string& id) const;
862 
863  const std_msgs::ColorRGBA& getObjectColor(const std::string& id) const;
864  void setObjectColor(const std::string& id, const std_msgs::ColorRGBA& color);
865  void removeObjectColor(const std::string& id);
866  void getKnownObjectColors(ObjectColorMap& kc) const;
867 
868  bool hasObjectType(const std::string& id) const;
869 
870  const object_recognition_msgs::ObjectType& getObjectType(const std::string& id) const;
871  void setObjectType(const std::string& id, const object_recognition_msgs::ObjectType& type);
872  void removeObjectType(const std::string& id);
873  void getKnownObjectTypes(ObjectTypeMap& kc) const;
874 
879  void clearDiffs();
880 
884  void pushDiffs(const PlanningScenePtr& scene);
885 
889  void decoupleParent();
890 
895  {
896  state_feasibility_ = fn;
897  }
898 
902  {
903  return state_feasibility_;
904  }
905 
909  {
910  motion_feasibility_ = fn;
911  }
912 
916  {
917  return motion_feasibility_;
918  }
919 
922  bool isStateFeasible(const moveit_msgs::RobotState& state, bool verbose = false) const;
923 
926  bool isStateFeasible(const moveit::core::RobotState& state, bool verbose = false) const;
927 
929  bool isStateConstrained(const moveit_msgs::RobotState& state, const moveit_msgs::Constraints& constr,
930  bool verbose = false) const;
931 
933  bool isStateConstrained(const moveit::core::RobotState& state, const moveit_msgs::Constraints& constr,
934  bool verbose = false) const;
935 
937  bool isStateConstrained(const moveit_msgs::RobotState& state,
938  const kinematic_constraints::KinematicConstraintSet& constr, bool verbose = false) const;
939 
942  const kinematic_constraints::KinematicConstraintSet& constr, bool verbose = false) const;
943 
945  bool isStateValid(const moveit_msgs::RobotState& state, const std::string& group = "", bool verbose = false) const;
946 
948  bool isStateValid(const moveit::core::RobotState& state, const std::string& group = "", bool verbose = false) const;
949 
952  bool isStateValid(const moveit_msgs::RobotState& state, const moveit_msgs::Constraints& constr,
953  const std::string& group = "", bool verbose = false) const;
954 
957  bool isStateValid(const moveit::core::RobotState& state, const moveit_msgs::Constraints& constr,
958  const std::string& group = "", bool verbose = false) const;
959 
963  const std::string& group = "", bool verbose = false) const;
964 
966  bool isPathValid(const moveit_msgs::RobotState& start_state, const moveit_msgs::RobotTrajectory& trajectory,
967  const std::string& group = "", bool verbose = false,
968  std::vector<std::size_t>* invalid_index = nullptr) const;
969 
973  bool isPathValid(const moveit_msgs::RobotState& start_state, const moveit_msgs::RobotTrajectory& trajectory,
974  const moveit_msgs::Constraints& path_constraints, const std::string& group = "",
975  bool verbose = false, std::vector<std::size_t>* invalid_index = nullptr) const;
976 
980  bool isPathValid(const moveit_msgs::RobotState& start_state, const moveit_msgs::RobotTrajectory& trajectory,
981  const moveit_msgs::Constraints& path_constraints, const moveit_msgs::Constraints& goal_constraints,
982  const std::string& group = "", bool verbose = false,
983  std::vector<std::size_t>* invalid_index = nullptr) const;
984 
988  bool isPathValid(const moveit_msgs::RobotState& start_state, const moveit_msgs::RobotTrajectory& trajectory,
989  const moveit_msgs::Constraints& path_constraints,
990  const std::vector<moveit_msgs::Constraints>& goal_constraints, const std::string& group = "",
991  bool verbose = false, std::vector<std::size_t>* invalid_index = nullptr) const;
992 
996  bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory,
997  const moveit_msgs::Constraints& path_constraints,
998  const std::vector<moveit_msgs::Constraints>& goal_constraints, const std::string& group = "",
999  bool verbose = false, std::vector<std::size_t>* invalid_index = nullptr) const;
1000 
1004  bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory,
1005  const moveit_msgs::Constraints& path_constraints, const moveit_msgs::Constraints& goal_constraints,
1006  const std::string& group = "", bool verbose = false,
1007  std::vector<std::size_t>* invalid_index = nullptr) const;
1008 
1011  bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory,
1012  const moveit_msgs::Constraints& path_constraints, const std::string& group = "",
1013  bool verbose = false, std::vector<std::size_t>* invalid_index = nullptr) const;
1014 
1016  bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory, const std::string& group = "",
1017  bool verbose = false, std::vector<std::size_t>* invalid_index = nullptr) const;
1018 
1021  void getCostSources(const robot_trajectory::RobotTrajectory& trajectory, std::size_t max_costs,
1022  std::set<collision_detection::CostSource>& costs, double overlap_fraction = 0.9) const;
1023 
1026  void getCostSources(const robot_trajectory::RobotTrajectory& trajectory, std::size_t max_costs,
1027  const std::string& group_name, std::set<collision_detection::CostSource>& costs,
1028  double overlap_fraction = 0.9) const;
1029 
1031  void getCostSources(const moveit::core::RobotState& state, std::size_t max_costs,
1032  std::set<collision_detection::CostSource>& costs) const;
1033 
1036  void getCostSources(const moveit::core::RobotState& state, std::size_t max_costs, const std::string& group_name,
1037  std::set<collision_detection::CostSource>& costs) const;
1038 
1040  void printKnownObjects(std::ostream& out = std::cout) const;
1041 
1044  [[deprecated("Use moveit/utils/message_checks.h instead")]] static bool isEmpty(const moveit_msgs::PlanningScene& msg);
1045 
1048  [[deprecated("Use moveit/utils/message_checks.h instead")]] static bool
1049  isEmpty(const moveit_msgs::PlanningSceneWorld& msg);
1050 
1052  [[deprecated("Use moveit/utils/message_checks.h instead")]] static bool isEmpty(const moveit_msgs::RobotState& msg);
1053 
1055  static PlanningScenePtr clone(const PlanningSceneConstPtr& scene);
1056 
1057 private:
1058  /* Private constructor used by the diff() methods. */
1059  PlanningScene(const PlanningSceneConstPtr& parent);
1060 
1061  /* Initialize the scene. This should only be called by the constructors.
1062  * Requires a valid robot_model_ */
1063  void initialize();
1064 
1065  /* helper function to create a RobotModel from a urdf/srdf. */
1066  static moveit::core::RobotModelPtr createRobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model,
1067  const srdf::ModelConstSharedPtr& srdf_model);
1068 
1069  /* Helper functions for processing collision objects */
1070  bool processCollisionObjectAdd(const moveit_msgs::CollisionObject& object);
1071  bool processCollisionObjectRemove(const moveit_msgs::CollisionObject& object);
1072  bool processCollisionObjectMove(const moveit_msgs::CollisionObject& object);
1073 
1074  /* For exporting and importing the planning scene */
1075  bool readPoseFromText(std::istream& in, Eigen::Isometry3d& pose) const;
1076  void writePoseToText(std::ostream& out, const Eigen::Isometry3d& pose) const;
1077 
1079  static void poseMsgToEigen(const geometry_msgs::Pose& msg, Eigen::Isometry3d& out);
1080 
1082 
1083  /* \brief A set of compatible collision detectors */
1084  struct CollisionDetector
1085  {
1086  collision_detection::CollisionDetectorAllocatorPtr alloc_;
1087  collision_detection::CollisionEnvPtr cenv_; // never NULL
1088  collision_detection::CollisionEnvConstPtr cenv_const_;
1089 
1090  collision_detection::CollisionEnvPtr cenv_unpadded_;
1091  collision_detection::CollisionEnvConstPtr cenv_unpadded_const_;
1092 
1093  CollisionDetectorConstPtr parent_; // may be NULL
1094 
1095  const collision_detection::CollisionEnvConstPtr& getCollisionEnv() const
1096  {
1097  return cenv_const_ ? cenv_const_ : parent_->getCollisionEnv();
1098  }
1099  const collision_detection::CollisionEnvConstPtr& getCollisionEnvUnpadded() const
1100  {
1101  return cenv_unpadded_const_ ? cenv_unpadded_const_ : parent_->getCollisionEnvUnpadded();
1102  }
1103  void findParent(const PlanningScene& scene);
1104  void copyPadding(const CollisionDetector& src);
1105  };
1106  friend struct CollisionDetector;
1107 
1108  using CollisionDetectorIterator = std::map<std::string, CollisionDetectorPtr>::iterator;
1109  using CollisionDetectorConstIterator = std::map<std::string, CollisionDetectorPtr>::const_iterator;
1110 
1113 
1114  std::string name_; // may be empty
1115 
1116  PlanningSceneConstPtr parent_; // Null unless this is a diff scene
1117 
1118  moveit::core::RobotModelConstPtr robot_model_; // Never null (may point to same model as parent)
1120  moveit::core::RobotStatePtr robot_state_; // if NULL use parent's
1121 
1122  // Called when changes are made to attached bodies
1124 
1125  // This variable is not necessarily used by child planning scenes
1126  // This Transforms class is actually a SceneTransforms class
1127  moveit::core::TransformsPtr scene_transforms_; // if NULL use parent's
1128 
1129  collision_detection::WorldPtr world_; // never NULL, never shared with parent/child
1130  collision_detection::WorldConstPtr world_const_; // copy of world_
1131  collision_detection::WorldDiffPtr world_diff_; // NULL unless this is a diff scene
1134 
1135  std::map<std::string, CollisionDetectorPtr> collision_; // never empty
1136  CollisionDetectorPtr active_collision_; // copy of one of the entries in collision_. Never NULL.
1137 
1138  collision_detection::AllowedCollisionMatrixPtr acm_; // if NULL use parent's
1139 
1142 
1143  std::unique_ptr<ObjectColorMap> object_colors_;
1144 
1145  // a map of object types
1146  std::unique_ptr<ObjectTypeMap> object_types_;
1147 };
1148 } // namespace planning_scene
planning_scene::PlanningScene::getAllowedCollisionMatrixNonConst
collision_detection::AllowedCollisionMatrix & getAllowedCollisionMatrixNonConst()
Get the allowed collision matrix.
Definition: planning_scene.cpp:643
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:261
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:947
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:387
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:940
planning_scene::PlanningScene::printKnownObjects
void printKnownObjects(std::ostream &out=std::cout) const
Outputs debug information about the planning scene contents.
Definition: planning_scene.cpp:2423
planning_scene::PlanningScene::world_diff_
collision_detection::WorldDiffPtr world_diff_
Definition: planning_scene.h:1163
shapes
EigenSTL::vector_Isometry3d
std::vector< Eigen::Isometry3d, Eigen::aligned_allocator< Eigen::Isometry3d > > vector_Isometry3d
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:722
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:254
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:897
planning_scene::PlanningScene::getWorld
const collision_detection::WorldConstPtr & getWorld() const
Get the representation of the world.
Definition: planning_scene.h:397
planning_scene::PlanningScene::hasObjectColor
bool hasObjectColor(const std::string &id) const
Definition: planning_scene.cpp:2071
planning_scene::PlanningScene::current_world_object_update_callback_
collision_detection::World::ObserverCallbackFn current_world_object_update_callback_
Definition: planning_scene.h:1164
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:840
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:1511
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:437
planning_scene::PlanningScene::CollisionDetector
Definition: planning_scene.h:1116
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:285
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:2166
planning_scene::PlanningScene::getAllowedCollisionMatrix
const collision_detection::AllowedCollisionMatrix & getAllowedCollisionMatrix() const
Get the allowed collision matrix.
Definition: planning_scene.h:442
robot_trajectory.h
planning_scene::PlanningScene::getObjectType
const object_recognition_msgs::ObjectType & getObjectType(const std::string &id) const
Definition: planning_scene.cpp:2034
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:2364
planning_scene::PlanningScene::CollisionDetector::copyPadding
void copyPadding(const CollisionDetector &src)
Definition: planning_scene.cpp:258
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:191
planning_scene::PlanningScene::writePoseToText
void writePoseToText(std::ostream &out, const Eigen::Isometry3d &pose) const
Definition: planning_scene.cpp:1192
planning_scene::PlanningScene::parent_
PlanningSceneConstPtr parent_
Definition: planning_scene.h:1148
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:1237
collision_detection::World::ObserverHandle
Definition: world.h:290
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:1199
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:359
planning_scene::PlanningScene::getObjectColor
const std_msgs::ColorRGBA & getObjectColor(const std::string &id) const
Definition: planning_scene.cpp:2081
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:620
planning_scene::PlanningScene::getCollidingPairs
void getCollidingPairs(collision_detection::CollisionResult::ContactMap &contacts) const
Get the names of the links that are involved in collisions for the current state.
Definition: planning_scene.h:670
planning_scene::PlanningScene::active_collision_
CollisionDetectorPtr active_collision_
Definition: planning_scene.h:1168
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:264
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:197
planning_scene::PlanningScene::processOctomapPtr
void processOctomapPtr(const std::shared_ptr< const octomap::OcTree > &octree, const Eigen::Isometry3d &t)
Definition: planning_scene.cpp:1478
collision_detection::CollisionRequest
Representation of a collision checking request.
Definition: include/moveit/collision_detection/collision_common.h:179
planning_scene::PlanningScene::getCurrentStateNonConst
moveit::core::RobotState & getCurrentStateNonConst()
Get the state at which the robot is assumed to be.
Definition: planning_scene.cpp:609
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:803
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:397
planning_scene::PlanningScene::collision_
std::map< std::string, CollisionDetectorPtr > collision_
Definition: planning_scene.h:1167
planning_scene::PlanningScene::CollisionDetectorIterator
std::map< std::string, CollisionDetectorPtr >::iterator CollisionDetectorIterator
Definition: planning_scene.h:1140
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:912
planning_scene::PlanningScene::CollisionDetectorConstIterator
std::map< std::string, CollisionDetectorPtr >::const_iterator CollisionDetectorConstIterator
Definition: planning_scene.h:1141
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:2206
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:556
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:868
planning_scene::PlanningScene::CollisionDetector::parent_
CollisionDetectorConstPtr parent_
Definition: planning_scene.h:1125
planning_scene::PlanningScene::current_world_object_update_observer_handle_
collision_detection::World::ObserverHandle current_world_object_update_observer_handle_
Definition: planning_scene.h:1165
planning_scene::PlanningScene::DEFAULT_SCENE_NAME
static const MOVEIT_PLANNING_SCENE_EXPORT std::string DEFAULT_SCENE_NAME
Definition: planning_scene.h:215
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:2007
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:933
collision_detection::AllowedCollisionMatrix
Definition of a structure for the allowed collision matrix.
Definition: collision_matrix.h:112
planning_scene::PlanningScene::hasObjectType
bool hasObjectType(const std::string &id) const
Definition: planning_scene.cpp:2024
planning_scene::PlanningScene::getKnownObjectColors
void getKnownObjectColors(ObjectColorMap &kc) const
Definition: planning_scene.cpp:2095
planning_scene::PlanningScene::robot_state_
moveit::core::RobotStatePtr robot_state_
Definition: planning_scene.h:1152
planning_scene::PlanningScene::CollisionDetector::getCollisionEnv
const collision_detection::CollisionEnvConstPtr & getCollisionEnv() const
Definition: planning_scene.h:1127
planning_scene::PlanningScene::CollisionDetector::alloc_
collision_detection::CollisionDetectorAllocatorPtr alloc_
Definition: planning_scene.h:1118
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:2130
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:283
planning_scene::MOVEIT_CLASS_FORWARD
MOVEIT_CLASS_FORWARD(PlanningScene)
collision_detection::World::ObserverCallbackFn
boost::function< void(const ObjectConstPtr &, Action)> ObserverCallbackFn
Definition: world.h:305
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:494
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:532
planning_scene::PlanningScene::getActiveCollisionDetectorName
const std::string & getActiveCollisionDetectorName() const
Definition: planning_scene.h:387
collision_detection::CollisionResult
Representation of a collision checking result.
Definition: include/moveit/collision_detection/collision_common.h:382
planning_scene::PlanningScene::CollisionDetector::getCollisionEnvUnpadded
const collision_detection::CollisionEnvConstPtr & getCollisionEnvUnpadded() const
Definition: planning_scene.h:1131
collision_env.h
planning_scene::PlanningScene::world_
collision_detection::WorldPtr world_
Definition: planning_scene.h:1161
planning_scene::PlanningScene::CollisionDetector::cenv_unpadded_
collision_detection::CollisionEnvPtr cenv_unpadded_
Definition: planning_scene.h:1122
planning_scene::PlanningScene::removeObjectType
void removeObjectType(const std::string &id)
Definition: planning_scene.cpp:2055
planning_scene::PlanningScene::setObjectType
void setObjectType(const std::string &id, const object_recognition_msgs::ObjectType &type)
Definition: planning_scene.cpp:2048
planning_scene::PlanningScene::removeAllCollisionObjects
void removeAllCollisionObjects()
Clear all collision objects in planning scene.
Definition: planning_scene.cpp:1443
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:2241
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:738
planning_scene::PlanningScene::setName
void setName(const std::string &name)
Set the name of the planning scene.
Definition: planning_scene.h:226
planning_scene::PlanningScene::~PlanningScene
~PlanningScene()
Definition: planning_scene.cpp:170
planning_scene::PlanningScene::state_feasibility_
StateFeasibilityFn state_feasibility_
Definition: planning_scene.h:1172
planning_scene::PlanningScene::shapesAndPosesFromCollisionObjectMessage
bool shapesAndPosesFromCollisionObjectMessage(const moveit_msgs::CollisionObject &object, Eigen::Isometry3d &object_pose_in_header_frame, std::vector< shapes::ShapeConstPtr > &shapes, EigenSTL::vector_Isometry3d &shape_poses)
Takes the object message and returns the object pose, shapes and shape poses. If the object pose is e...
Definition: planning_scene.cpp:1766
planning_scene::PlanningScene::world_const_
collision_detection::WorldConstPtr world_const_
Definition: planning_scene.h:1162
planning_scene::PlanningScene::getKnownObjectTypes
void getKnownObjectTypes(ObjectTypeMap &kc) const
Definition: planning_scene.cpp:2061
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:671
kinematic_constraints::KinematicConstraintSet
A class that contains many different constraints, and can check RobotState *versus the full set....
Definition: kinematic_constraint.h:923
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:194
planning_scene::PlanningScene::readPoseFromText
bool readPoseFromText(std::istream &in, Eigen::Isometry3d &pose) const
Definition: planning_scene.cpp:1175
planning_scene::PlanningScene::object_types_
std::unique_ptr< ObjectTypeMap > object_types_
Definition: planning_scene.h:1178
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:183
planning_scene::PlanningScene::object_colors_
std::unique_ptr< ObjectColorMap > object_colors_
Definition: planning_scene.h:1175
planning_scene::PlanningScene::getCollisionEnvUnpadded
const collision_detection::CollisionEnvConstPtr & getCollisionEnvUnpadded() const
Get the active collision detector for the robot.
Definition: planning_scene.h:417
planning_scene::PlanningScene::processOctomapMsg
void processOctomapMsg(const octomap_msgs::OctomapWithPose &map)
Definition: planning_scene.cpp:1455
planning_scene::PlanningScene::processCollisionObjectMove
bool processCollisionObjectMove(const moveit_msgs::CollisionObject &object)
Definition: planning_scene.cpp:1914
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:627
planning_scene::PlanningScene::acm_
collision_detection::AllowedCollisionMatrixPtr acm_
Definition: planning_scene.h:1170
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:220
planning_scene::PlanningScene::removeObjectColor
void removeObjectColor(const std::string &id)
Definition: planning_scene.cpp:2117
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:770
planning_scene::PlanningScene::CollisionDetector::cenv_const_
collision_detection::CollisionEnvConstPtr cenv_const_
Definition: planning_scene.h:1120
planning_scene::PlanningScene::loadGeometryFromStream
bool loadGeometryFromStream(std::istream &in)
Load the geometry of the planning scene from a stream.
Definition: planning_scene.cpp:1041
planning_scene::PlanningScene::robot_model_
moveit::core::RobotModelConstPtr robot_model_
Definition: planning_scene.h:1150
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:579
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:238
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:1297
planning_scene::PlanningScene::getPlanningFrame
const std::string & getPlanningFrame() const
Get the frame in which planning is performed.
Definition: planning_scene.h:278
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:1389
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:742
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:1351
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:2148
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:852
planning_scene::PlanningScene::CollisionDetector::findParent
void findParent(const PlanningScene &scene)
Definition: planning_scene.cpp:273
planning_scene::PlanningScene::CollisionDetector::cenv_
collision_detection::CollisionEnvPtr cenv_
Definition: planning_scene.h:1119
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:248
planning_scene::PlanningScene::motion_feasibility_
MotionFeasibilityFn motion_feasibility_
Definition: planning_scene.h:1173
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:604
planning_scene::PlanningScene::OCTOMAP_NS
static const MOVEIT_PLANNING_SCENE_EXPORT std::string OCTOMAP_NS
Definition: planning_scene.h:214
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:510
planning_scene::PlanningScene::scene_transforms_
moveit::core::TransformsPtr scene_transforms_
Definition: planning_scene.h:1159
planning_scene::PlanningScene::processPlanningSceneWorldMsg
bool processPlanningSceneWorldMsg(const moveit_msgs::PlanningSceneWorld &world)
Definition: planning_scene.cpp:1380
planning_scene::PlanningScene::current_state_attached_body_callback_
moveit::core::AttachedBodyCallback current_state_attached_body_callback_
Definition: planning_scene.h:1155
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:1976
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:876
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:634
planning_scene::PlanningScene::CollisionDetector::cenv_unpadded_const_
collision_detection::CollisionEnvConstPtr cenv_unpadded_const_
Definition: planning_scene.h:1123
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:657
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:926
planning_scene::PlanningScene::name_
std::string name_
Definition: planning_scene.h:1146
planning_scene
This namespace includes the central class for representing planning scenes.
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:999
planning_scene::PlanningScene::getWorldNonConst
const collision_detection::WorldPtr & getWorldNonConst()
Definition: planning_scene.h:404
planning_scene::PlanningScene::getCollisionEnv
const collision_detection::CollisionEnvConstPtr & getCollisionEnv() const
Get the active collision environment.
Definition: planning_scene.h:411
planning_scene::PlanningScene::processCollisionObjectRemove
bool processCollisionObjectRemove(const moveit_msgs::CollisionObject &object)
Definition: planning_scene.cpp:1893
planning_scene::PlanningScene::processCollisionObjectMsg
bool processCollisionObjectMsg(const moveit_msgs::CollisionObject &object)
Definition: planning_scene.cpp:1725
robot_state.h
planning_scene::PlanningScene::CollisionDetector
friend struct CollisionDetector
Definition: planning_scene.h:1138
planning_scene::PlanningScene::createRobotModel
static moveit::core::RobotModelPtr createRobotModel(const urdf::ModelInterfaceSharedPtr &urdf_model, const srdf::ModelConstSharedPtr &srdf_model)
Definition: planning_scene.cpp:192
planning_scene::PlanningScene::diff
PlanningScenePtr diff() const
Return a new child PlanningScene that uses this one as parent.
Definition: planning_scene.cpp:246
collision_detector_allocator.h
planning_scene::PlanningScene::processCollisionObjectAdd
bool processCollisionObjectAdd(const moveit_msgs::CollisionObject &object)
Definition: planning_scene.cpp:1849
planning_scene::PlanningScene::poseMsgToEigen
static void poseMsgToEigen(const geometry_msgs::Pose &msg, Eigen::Isometry3d &out)
Definition: planning_scene.cpp:1750
verbose
bool verbose
planning_scene::PlanningScene::setObjectColor
void setObjectColor(const std::string &id, const std_msgs::ColorRGBA &color)
Definition: planning_scene.cpp:2105
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:316


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sat Dec 21 2024 03:23:42