planning_scene.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Ioan Sucan, Acorn Pooley */
00036 
00037 #ifndef MOVEIT_PLANNING_SCENE_PLANNING_SCENE_
00038 #define MOVEIT_PLANNING_SCENE_PLANNING_SCENE_
00039 
00040 #include <moveit/robot_model/robot_model.h>
00041 #include <moveit/robot_state/robot_state.h>
00042 #include <moveit/transforms/transforms.h>
00043 #include <moveit/collision_detection/collision_detector_allocator.h>
00044 #include <moveit/collision_detection/world_diff.h>
00045 #include <moveit/kinematic_constraints/kinematic_constraint.h>
00046 #include <moveit/kinematics_base/kinematics_base.h>
00047 #include <moveit/robot_trajectory/robot_trajectory.h>
00048 #include <moveit/macros/class_forward.h>
00049 #include <moveit/macros/deprecation.h>
00050 #include <moveit_msgs/PlanningScene.h>
00051 #include <moveit_msgs/RobotTrajectory.h>
00052 #include <moveit_msgs/Constraints.h>
00053 #include <moveit_msgs/PlanningSceneComponents.h>
00054 #include <boost/enable_shared_from_this.hpp>
00055 #include <boost/noncopyable.hpp>
00056 #include <boost/shared_ptr.hpp>
00057 #include <boost/function.hpp>
00058 #include <boost/concept_check.hpp>
00059 
00061 namespace planning_scene
00062 {
00063 MOVEIT_CLASS_FORWARD(PlanningScene);
00064 
00069 typedef boost::function<bool(const robot_state::RobotState&, bool)> StateFeasibilityFn;
00070 
00076 typedef boost::function<bool(const robot_state::RobotState&, const robot_state::RobotState&, bool)> MotionFeasibilityFn;
00077 
00079 typedef std::map<std::string, std_msgs::ColorRGBA> ObjectColorMap;
00080 
00082 typedef std::map<std::string, object_recognition_msgs::ObjectType> ObjectTypeMap;
00083 
00087 class PlanningScene : private boost::noncopyable, public boost::enable_shared_from_this<PlanningScene>
00088 {
00089 public:
00091   PlanningScene(const robot_model::RobotModelConstPtr& robot_model,
00092                 collision_detection::WorldPtr world = collision_detection::WorldPtr(new collision_detection::World()));
00093 
00096   PlanningScene(const boost::shared_ptr<const urdf::ModelInterface>& urdf_model,
00097                 const boost::shared_ptr<const srdf::Model>& srdf_model,
00098                 collision_detection::WorldPtr world = collision_detection::WorldPtr(new collision_detection::World()));
00099 
00100   static const std::string OCTOMAP_NS;
00101   static const std::string DEFAULT_SCENE_NAME;
00102 
00103   ~PlanningScene();
00104 
00106   const std::string& getName() const
00107   {
00108     return name_;
00109   }
00110 
00112   void setName(const std::string& name)
00113   {
00114     name_ = name;
00115   }
00116 
00129   PlanningScenePtr diff() const;
00130 
00133   PlanningScenePtr diff(const moveit_msgs::PlanningScene& msg) const;
00134 
00136   const PlanningSceneConstPtr& getParent() const
00137   {
00138     return parent_;
00139   }
00140 
00142   const robot_model::RobotModelConstPtr& getRobotModel() const
00143   {
00144     // the kinematic model does not change
00145     return kmodel_;
00146   }
00147 
00149   const robot_state::RobotState& getCurrentState() const
00150   {
00151     // if we have an updated state, return it; otherwise, return the parent one
00152     return kstate_ ? *kstate_ : parent_->getCurrentState();
00153   }
00155   robot_state::RobotState& getCurrentStateNonConst();
00156 
00158   robot_state::RobotStatePtr getCurrentStateUpdated(const moveit_msgs::RobotState& update) const;
00159 
00166   const std::string& getPlanningFrame() const
00167   {
00168     // if we have an updated set of transforms, return it; otherwise, return the parent one
00169     return ftf_ ? ftf_->getTargetFrame() : parent_->getPlanningFrame();
00170   }
00171 
00173   const robot_state::Transforms& getTransforms() const
00174   {
00175     // if we have updated transforms, return those
00176     return (ftf_ || !parent_) ? *ftf_ : parent_->getTransforms();
00177   }
00178 
00181   const robot_state::Transforms& getTransforms();
00182 
00184   robot_state::Transforms& getTransformsNonConst();
00185 
00190   const Eigen::Affine3d& getFrameTransform(const std::string& id) const;
00191 
00197   const Eigen::Affine3d& getFrameTransform(const std::string& id);
00198 
00204   const Eigen::Affine3d& getFrameTransform(robot_state::RobotState& state, const std::string& id) const
00205   {
00206     state.updateLinkTransforms();
00207     return getFrameTransform(static_cast<const robot_state::RobotState&>(state), id);
00208   }
00209 
00214   const Eigen::Affine3d& getFrameTransform(const robot_state::RobotState& state, const std::string& id) const;
00215 
00218   bool knowsFrameTransform(const std::string& id) const;
00219 
00222   bool knowsFrameTransform(const robot_state::RobotState& state, const std::string& id) const;
00223 
00248   void addCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr& allocator);
00249 
00259   void setActiveCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr& allocator,
00260                                   bool exclusive = false);
00261 
00268   bool setActiveCollisionDetector(const std::string& collision_detector_name);
00269 
00270   const std::string& getActiveCollisionDetectorName() const
00271   {
00272     return active_collision_->alloc_->getName();
00273   }
00274 
00277   void getCollisionDetectorNames(std::vector<std::string>& names) const;
00278 
00280   const collision_detection::WorldConstPtr& getWorld() const
00281   {
00282     // we always have a world representation
00283     return world_const_;
00284   }
00285 
00286   // brief Get the representation of the world
00287   const collision_detection::WorldPtr& getWorldNonConst()
00288   {
00289     // we always have a world representation
00290     return world_;
00291   }
00292 
00294   const collision_detection::CollisionWorldConstPtr& getCollisionWorld() const
00295   {
00296     // we always have a world representation after configure is called.
00297     return active_collision_->cworld_const_;
00298   }
00299 
00301   const collision_detection::CollisionRobotConstPtr& getCollisionRobot() const
00302   {
00303     return active_collision_->getCollisionRobot();
00304   }
00305 
00307   const collision_detection::CollisionRobotConstPtr& getCollisionRobotUnpadded() const
00308   {
00309     return active_collision_->getCollisionRobotUnpadded();
00310   }
00311 
00313   const collision_detection::CollisionWorldConstPtr&
00314   getCollisionWorld(const std::string& collision_detector_name) const;
00315 
00317   const collision_detection::CollisionRobotConstPtr&
00318   getCollisionRobot(const std::string& collision_detector_name) const;
00319 
00322   const collision_detection::CollisionRobotConstPtr&
00323   getCollisionRobotUnpadded(const std::string& collision_detector_name) const;
00324 
00329   const collision_detection::CollisionRobotPtr& getCollisionRobotNonConst();
00330 
00334   void propogateRobotPadding();
00335 
00337   const collision_detection::AllowedCollisionMatrix& getAllowedCollisionMatrix() const
00338   {
00339     return acm_ ? *acm_ : parent_->getAllowedCollisionMatrix();
00340   }
00342   collision_detection::AllowedCollisionMatrix& getAllowedCollisionMatrixNonConst();
00343 
00354   bool isStateColliding(const std::string& group = "", bool verbose = false);
00355 
00359   bool isStateColliding(const std::string& group = "", bool verbose = false) const
00360   {
00361     return isStateColliding(getCurrentState(), group, verbose);
00362   }
00363 
00368   bool isStateColliding(robot_state::RobotState& state, const std::string& group = "", bool verbose = false) const
00369   {
00370     state.updateCollisionBodyTransforms();
00371     return isStateColliding(static_cast<const robot_state::RobotState&>(state), group, verbose);
00372   }
00373 
00377   bool isStateColliding(const robot_state::RobotState& state, const std::string& group = "",
00378                         bool verbose = false) const;
00379 
00382   bool isStateColliding(const moveit_msgs::RobotState& state, const std::string& group = "",
00383                         bool verbose = false) const;
00384 
00387   void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res);
00388 
00390   void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res) const
00391   {
00392     checkCollision(req, res, getCurrentState());
00393   }
00394 
00397   void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
00398                       robot_state::RobotState& kstate) const
00399   {
00400     kstate.updateCollisionBodyTransforms();
00401     checkCollision(req, res, static_cast<const robot_state::RobotState&>(kstate));
00402   }
00403 
00406   void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
00407                       const robot_state::RobotState& kstate) const;
00408 
00412   void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
00413                       robot_state::RobotState& kstate, const collision_detection::AllowedCollisionMatrix& acm) const
00414   {
00415     kstate.updateCollisionBodyTransforms();
00416     checkCollision(req, res, static_cast<const robot_state::RobotState&>(kstate), acm);
00417   }
00418 
00421   void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
00422                       const robot_state::RobotState& kstate,
00423                       const collision_detection::AllowedCollisionMatrix& acm) const;
00424 
00428   void checkCollisionUnpadded(const collision_detection::CollisionRequest& req,
00429                               collision_detection::CollisionResult& res);
00430 
00433   void checkCollisionUnpadded(const collision_detection::CollisionRequest& req,
00434                               collision_detection::CollisionResult& res) const
00435   {
00436     checkCollisionUnpadded(req, res, getCurrentState(), getAllowedCollisionMatrix());
00437   }
00438 
00441   void checkCollisionUnpadded(const collision_detection::CollisionRequest& req,
00442                               collision_detection::CollisionResult& res, const robot_state::RobotState& kstate) const
00443   {
00444     checkCollisionUnpadded(req, res, kstate, getAllowedCollisionMatrix());
00445   }
00446 
00450   void checkCollisionUnpadded(const collision_detection::CollisionRequest& req,
00451                               collision_detection::CollisionResult& res, robot_state::RobotState& kstate) const
00452   {
00453     kstate.updateCollisionBodyTransforms();
00454     checkCollisionUnpadded(req, res, static_cast<const robot_state::RobotState&>(kstate), getAllowedCollisionMatrix());
00455   }
00456 
00460   void checkCollisionUnpadded(const collision_detection::CollisionRequest& req,
00461                               collision_detection::CollisionResult& res, robot_state::RobotState& kstate,
00462                               const collision_detection::AllowedCollisionMatrix& acm) const
00463   {
00464     kstate.updateCollisionBodyTransforms();
00465     checkCollisionUnpadded(req, res, static_cast<const robot_state::RobotState&>(kstate), acm);
00466   }
00467 
00470   void checkCollisionUnpadded(const collision_detection::CollisionRequest& req,
00471                               collision_detection::CollisionResult& res, const robot_state::RobotState& kstate,
00472                               const collision_detection::AllowedCollisionMatrix& acm) const;
00473 
00475   void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res);
00476 
00478   void checkSelfCollision(const collision_detection::CollisionRequest& req,
00479                           collision_detection::CollisionResult& res) const
00480   {
00481     checkSelfCollision(req, res, getCurrentState());
00482   }
00483 
00485   void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
00486                           robot_state::RobotState& kstate) const
00487   {
00488     kstate.updateCollisionBodyTransforms();
00489     checkSelfCollision(req, res, static_cast<const robot_state::RobotState&>(kstate), getAllowedCollisionMatrix());
00490   }
00491 
00493   void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
00494                           const robot_state::RobotState& kstate) const
00495   {
00496     // do self-collision checking with the unpadded version of the robot
00497     getCollisionRobotUnpadded()->checkSelfCollision(req, res, kstate, getAllowedCollisionMatrix());
00498   }
00499 
00502   void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
00503                           robot_state::RobotState& kstate, const collision_detection::AllowedCollisionMatrix& acm) const
00504   {
00505     kstate.updateCollisionBodyTransforms();
00506     checkSelfCollision(req, res, static_cast<const robot_state::RobotState&>(kstate), acm);
00507   }
00508 
00511   void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
00512                           const robot_state::RobotState& kstate,
00513                           const collision_detection::AllowedCollisionMatrix& acm) const
00514   {
00515     // do self-collision checking with the unpadded version of the robot
00516     getCollisionRobotUnpadded()->checkSelfCollision(req, res, kstate, acm);
00517   }
00518 
00520   void getCollidingLinks(std::vector<std::string>& links);
00521 
00523   void getCollidingLinks(std::vector<std::string>& links) const
00524   {
00525     getCollidingLinks(links, getCurrentState(), getAllowedCollisionMatrix());
00526   }
00527 
00530   void getCollidingLinks(std::vector<std::string>& links, robot_state::RobotState& kstate) const
00531   {
00532     kstate.updateCollisionBodyTransforms();
00533     getCollidingLinks(links, static_cast<const robot_state::RobotState&>(kstate), getAllowedCollisionMatrix());
00534   }
00535 
00537   void getCollidingLinks(std::vector<std::string>& links, const robot_state::RobotState& kstate) const
00538   {
00539     getCollidingLinks(links, kstate, getAllowedCollisionMatrix());
00540   }
00541 
00544   void getCollidingLinks(std::vector<std::string>& links, robot_state::RobotState& kstate,
00545                          const collision_detection::AllowedCollisionMatrix& acm) const
00546   {
00547     kstate.updateCollisionBodyTransforms();
00548     getCollidingLinks(links, static_cast<const robot_state::RobotState&>(kstate), acm);
00549   }
00550 
00553   void getCollidingLinks(std::vector<std::string>& links, const robot_state::RobotState& kstate,
00554                          const collision_detection::AllowedCollisionMatrix& acm) const;
00555 
00558   void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts);
00559 
00561   void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts) const
00562   {
00563     getCollidingPairs(contacts, getCurrentState(), getAllowedCollisionMatrix());
00564   }
00565 
00567   void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts,
00568                          const robot_state::RobotState& kstate) const
00569   {
00570     getCollidingPairs(contacts, kstate, getAllowedCollisionMatrix());
00571   }
00572 
00575   void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts,
00576                          robot_state::RobotState& kstate) const
00577   {
00578     kstate.updateCollisionBodyTransforms();
00579     getCollidingPairs(contacts, static_cast<const robot_state::RobotState&>(kstate), getAllowedCollisionMatrix());
00580   }
00581 
00584   void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts, robot_state::RobotState& kstate,
00585                          const collision_detection::AllowedCollisionMatrix& acm) const
00586   {
00587     kstate.updateCollisionBodyTransforms();
00588     getCollidingPairs(contacts, static_cast<const robot_state::RobotState&>(kstate), acm);
00589   }
00590 
00593   void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts,
00594                          const robot_state::RobotState& kstate,
00595                          const collision_detection::AllowedCollisionMatrix& acm) const;
00596 
00606   double distanceToCollision(robot_state::RobotState& kstate) const
00607   {
00608     kstate.updateCollisionBodyTransforms();
00609     return distanceToCollision(static_cast<const robot_state::RobotState&>(kstate));
00610   }
00611 
00614   double distanceToCollision(const robot_state::RobotState& kstate) const
00615   {
00616     return getCollisionWorld()->distanceRobot(*getCollisionRobot(), kstate, getAllowedCollisionMatrix());
00617   }
00618 
00621   double distanceToCollisionUnpadded(robot_state::RobotState& kstate) const
00622   {
00623     kstate.updateCollisionBodyTransforms();
00624     return distanceToCollisionUnpadded(static_cast<const robot_state::RobotState&>(kstate));
00625   }
00626 
00629   double distanceToCollisionUnpadded(const robot_state::RobotState& kstate) const
00630   {
00631     return getCollisionWorld()->distanceRobot(*getCollisionRobotUnpadded(), kstate, getAllowedCollisionMatrix());
00632   }
00633 
00636   double distanceToCollision(robot_state::RobotState& kstate,
00637                              const collision_detection::AllowedCollisionMatrix& acm) const
00638   {
00639     kstate.updateCollisionBodyTransforms();
00640     return distanceToCollision(static_cast<const robot_state::RobotState&>(kstate), acm);
00641   }
00642 
00645   double distanceToCollision(const robot_state::RobotState& kstate,
00646                              const collision_detection::AllowedCollisionMatrix& acm) const
00647   {
00648     return getCollisionWorld()->distanceRobot(*getCollisionRobot(), kstate, acm);
00649   }
00650 
00653   double distanceToCollisionUnpadded(robot_state::RobotState& kstate,
00654                                      const collision_detection::AllowedCollisionMatrix& acm) const
00655   {
00656     kstate.updateCollisionBodyTransforms();
00657     return distanceToCollisionUnpadded(static_cast<const robot_state::RobotState&>(kstate), acm);
00658   }
00659 
00662   double distanceToCollisionUnpadded(const robot_state::RobotState& kstate,
00663                                      const collision_detection::AllowedCollisionMatrix& acm) const
00664   {
00665     return getCollisionWorld()->distanceRobot(*getCollisionRobotUnpadded(), kstate, acm);
00666   }
00667 
00671   void saveGeometryToStream(std::ostream& out) const;
00672 
00674   void loadGeometryFromStream(std::istream& in);
00675 
00677   void loadGeometryFromStream(std::istream& in, const Eigen::Affine3d& offset);
00678 
00683   void getPlanningSceneDiffMsg(moveit_msgs::PlanningScene& scene) const;
00684 
00688   void getPlanningSceneMsg(moveit_msgs::PlanningScene& scene) const;
00689 
00692   void getPlanningSceneMsg(moveit_msgs::PlanningScene& scene, const moveit_msgs::PlanningSceneComponents& comp) const;
00693 
00699   bool setPlanningSceneDiffMsg(const moveit_msgs::PlanningScene& scene);
00700 
00703   bool setPlanningSceneMsg(const moveit_msgs::PlanningScene& scene);
00704 
00707   bool usePlanningSceneMsg(const moveit_msgs::PlanningScene& scene);
00708 
00709   bool processCollisionObjectMsg(const moveit_msgs::CollisionObject& object);
00710   bool processAttachedCollisionObjectMsg(const moveit_msgs::AttachedCollisionObject& object);
00711 
00712   bool processPlanningSceneWorldMsg(const moveit_msgs::PlanningSceneWorld& world);
00713 
00714   void processOctomapMsg(const octomap_msgs::OctomapWithPose& map);
00715   void processOctomapMsg(const octomap_msgs::Octomap& map);
00716   void processOctomapPtr(const boost::shared_ptr<const octomap::OcTree>& octree, const Eigen::Affine3d& t);
00717 
00721   void removeAllCollisionObjects();
00722 
00726   void setCurrentState(const moveit_msgs::RobotState& state);
00727 
00729   void setCurrentState(const robot_state::RobotState& state);
00730 
00732   void setAttachedBodyUpdateCallback(const robot_state::AttachedBodyCallback& callback);
00733 
00735   void setCollisionObjectUpdateCallback(const collision_detection::World::ObserverCallbackFn& callback);
00736 
00737   bool hasObjectColor(const std::string& id) const;
00738 
00739   const std_msgs::ColorRGBA& getObjectColor(const std::string& id) const;
00740   void setObjectColor(const std::string& id, const std_msgs::ColorRGBA& color);
00741   void removeObjectColor(const std::string& id);
00742   void getKnownObjectColors(ObjectColorMap& kc) const;
00743 
00744   bool hasObjectType(const std::string& id) const;
00745 
00746   const object_recognition_msgs::ObjectType& getObjectType(const std::string& id) const;
00747   void setObjectType(const std::string& id, const object_recognition_msgs::ObjectType& type);
00748   void removeObjectType(const std::string& id);
00749   void getKnownObjectTypes(ObjectTypeMap& kc) const;
00750 
00753   void clearDiffs();
00754 
00758   void pushDiffs(const PlanningScenePtr& scene);
00759 
00763   void decoupleParent();
00764 
00768   void setStateFeasibilityPredicate(const StateFeasibilityFn& fn)
00769   {
00770     state_feasibility_ = fn;
00771   }
00772 
00775   const StateFeasibilityFn& getStateFeasibilityPredicate() const
00776   {
00777     return state_feasibility_;
00778   }
00779 
00782   void setMotionFeasibilityPredicate(const MotionFeasibilityFn& fn)
00783   {
00784     motion_feasibility_ = fn;
00785   }
00786 
00789   const MotionFeasibilityFn& getMotionFeasibilityPredicate() const
00790   {
00791     return motion_feasibility_;
00792   }
00793 
00796   bool isStateFeasible(const moveit_msgs::RobotState& state, bool verbose = false) const;
00797 
00800   bool isStateFeasible(const robot_state::RobotState& state, bool verbose = false) const;
00801 
00803   bool isStateConstrained(const moveit_msgs::RobotState& state, const moveit_msgs::Constraints& constr,
00804                           bool verbose = false) const;
00805 
00807   bool isStateConstrained(const robot_state::RobotState& state, const moveit_msgs::Constraints& constr,
00808                           bool verbose = false) const;
00809 
00811   bool isStateConstrained(const moveit_msgs::RobotState& state,
00812                           const kinematic_constraints::KinematicConstraintSet& constr, bool verbose = false) const;
00813 
00815   bool isStateConstrained(const robot_state::RobotState& state,
00816                           const kinematic_constraints::KinematicConstraintSet& constr, bool verbose = false) const;
00817 
00819   bool isStateValid(const moveit_msgs::RobotState& state, const std::string& group = "", bool verbose = false) const;
00820 
00822   bool isStateValid(const robot_state::RobotState& state, const std::string& group = "", bool verbose = false) const;
00823 
00826   bool isStateValid(const moveit_msgs::RobotState& state, const moveit_msgs::Constraints& constr,
00827                     const std::string& group = "", bool verbose = false) const;
00828 
00831   bool isStateValid(const robot_state::RobotState& state, const moveit_msgs::Constraints& constr,
00832                     const std::string& group = "", bool verbose = false) const;
00833 
00836   bool isStateValid(const robot_state::RobotState& state, const kinematic_constraints::KinematicConstraintSet& constr,
00837                     const std::string& group = "", bool verbose = false) const;
00838 
00840   bool isPathValid(const moveit_msgs::RobotState& start_state, const moveit_msgs::RobotTrajectory& trajectory,
00841                    const std::string& group = "", bool verbose = false,
00842                    std::vector<std::size_t>* invalid_index = NULL) const;
00843 
00847   bool isPathValid(const moveit_msgs::RobotState& start_state, const moveit_msgs::RobotTrajectory& trajectory,
00848                    const moveit_msgs::Constraints& path_constraints, const std::string& group = "",
00849                    bool verbose = false, std::vector<std::size_t>* invalid_index = NULL) const;
00850 
00854   bool isPathValid(const moveit_msgs::RobotState& start_state, const moveit_msgs::RobotTrajectory& trajectory,
00855                    const moveit_msgs::Constraints& path_constraints, const moveit_msgs::Constraints& goal_constraints,
00856                    const std::string& group = "", bool verbose = false,
00857                    std::vector<std::size_t>* invalid_index = NULL) const;
00858 
00862   bool isPathValid(const moveit_msgs::RobotState& start_state, const moveit_msgs::RobotTrajectory& trajectory,
00863                    const moveit_msgs::Constraints& path_constraints,
00864                    const std::vector<moveit_msgs::Constraints>& goal_constraints, const std::string& group = "",
00865                    bool verbose = false, std::vector<std::size_t>* invalid_index = NULL) const;
00866 
00870   bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory,
00871                    const moveit_msgs::Constraints& path_constraints,
00872                    const std::vector<moveit_msgs::Constraints>& goal_constraints, const std::string& group = "",
00873                    bool verbose = false, std::vector<std::size_t>* invalid_index = NULL) const;
00874 
00878   bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory,
00879                    const moveit_msgs::Constraints& path_constraints, const moveit_msgs::Constraints& goal_constraints,
00880                    const std::string& group = "", bool verbose = false,
00881                    std::vector<std::size_t>* invalid_index = NULL) const;
00882 
00885   bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory,
00886                    const moveit_msgs::Constraints& path_constraints, const std::string& group = "",
00887                    bool verbose = false, std::vector<std::size_t>* invalid_index = NULL) const;
00888 
00890   bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory, const std::string& group = "",
00891                    bool verbose = false, std::vector<std::size_t>* invalid_index = NULL) const;
00892 
00895   void getCostSources(const robot_trajectory::RobotTrajectory& trajectory, std::size_t max_costs,
00896                       std::set<collision_detection::CostSource>& costs, double overlap_fraction = 0.9) const;
00897 
00900   void getCostSources(const robot_trajectory::RobotTrajectory& trajectory, std::size_t max_costs,
00901                       const std::string& group_name, std::set<collision_detection::CostSource>& costs,
00902                       double overlap_fraction = 0.9) const;
00903 
00905   void getCostSources(const robot_state::RobotState& state, std::size_t max_costs,
00906                       std::set<collision_detection::CostSource>& costs) const;
00907 
00910   void getCostSources(const robot_state::RobotState& state, std::size_t max_costs, const std::string& group_name,
00911                       std::set<collision_detection::CostSource>& costs) const;
00912 
00914   void printKnownObjects(std::ostream& out) const;
00915 
00918   static bool isEmpty(const moveit_msgs::PlanningScene& msg);
00919 
00922   static bool isEmpty(const moveit_msgs::PlanningSceneWorld& msg);
00923 
00925   static bool isEmpty(const moveit_msgs::RobotState& msg);
00926 
00928   static PlanningScenePtr clone(const PlanningSceneConstPtr& scene);
00929 
00930 private:
00931   /* Private constructor used by the diff() methods. */
00932   PlanningScene(const PlanningSceneConstPtr& parent);
00933 
00934   /* Initialize the scene.  This should only be called by the constructors.
00935    * Requires a valid robot_model_ */
00936   void initialize();
00937 
00938   /* helper function to create a RobotModel from a urdf/srdf. */
00939   static robot_model::RobotModelPtr createRobotModel(const boost::shared_ptr<const urdf::ModelInterface>& urdf_model,
00940                                                      const boost::shared_ptr<const srdf::Model>& srdf_model);
00941 
00942   void getPlanningSceneMsgCollisionObject(moveit_msgs::PlanningScene& scene, const std::string& ns) const;
00943   void getPlanningSceneMsgCollisionObjects(moveit_msgs::PlanningScene& scene) const;
00944   void getPlanningSceneMsgOctomap(moveit_msgs::PlanningScene& scene) const;
00945   void getPlanningSceneMsgObjectColors(moveit_msgs::PlanningScene& scene_msg) const;
00946 
00947   MOVEIT_CLASS_FORWARD(CollisionDetector);
00948 
00949   /* \brief A set of compatible collision detectors */
00950   struct CollisionDetector
00951   {
00952     collision_detection::CollisionDetectorAllocatorPtr alloc_;
00953     collision_detection::CollisionRobotPtr crobot_unpadded_;  // if NULL use parent's
00954     collision_detection::CollisionRobotConstPtr crobot_unpadded_const_;
00955     collision_detection::CollisionRobotPtr crobot_;  // if NULL use parent's
00956     collision_detection::CollisionRobotConstPtr crobot_const_;
00957 
00958     collision_detection::CollisionWorldPtr cworld_;  // never NULL
00959     collision_detection::CollisionWorldConstPtr cworld_const_;
00960 
00961     CollisionDetectorConstPtr parent_;  // may be NULL
00962 
00963     const collision_detection::CollisionRobotConstPtr& getCollisionRobot() const
00964     {
00965       return crobot_const_ ? crobot_const_ : parent_->getCollisionRobot();
00966     }
00967     const collision_detection::CollisionRobotConstPtr& getCollisionRobotUnpadded() const
00968     {
00969       return crobot_unpadded_const_ ? crobot_unpadded_const_ : parent_->getCollisionRobotUnpadded();
00970     }
00971     void findParent(const PlanningScene& scene);
00972     void copyPadding(const CollisionDetector& src);
00973   };
00974   friend struct CollisionDetector;
00975 
00976   typedef std::map<std::string, CollisionDetectorPtr>::iterator CollisionDetectorIterator;
00977   typedef std::map<std::string, CollisionDetectorPtr>::const_iterator CollisionDetectorConstIterator;
00978 
00979   void allocateCollisionDetectors();
00980   void allocateCollisionDetectors(CollisionDetector& detector);
00981 
00982   std::string name_;  // may be empty
00983 
00984   PlanningSceneConstPtr parent_;  // Null unless this is a diff scene
00985 
00986   robot_model::RobotModelConstPtr kmodel_;  // Never null (may point to same model as parent)
00987 
00988   robot_state::RobotStatePtr kstate_;                                       // if NULL use parent's
00989   robot_state::AttachedBodyCallback current_state_attached_body_callback_;  // called when changes are made to attached
00990                                                                             // bodies
00991 
00992   robot_state::TransformsPtr ftf_;  // if NULL use parent's
00993 
00994   collision_detection::WorldPtr world_;             // never NULL, never shared with parent/child
00995   collision_detection::WorldConstPtr world_const_;  // copy of world_
00996   collision_detection::WorldDiffPtr world_diff_;    // NULL unless this is a diff scene
00997   collision_detection::World::ObserverCallbackFn current_world_object_update_callback_;
00998   collision_detection::World::ObserverHandle current_world_object_update_observer_handle_;
00999 
01000   std::map<std::string, CollisionDetectorPtr> collision_;  // never empty
01001   CollisionDetectorPtr active_collision_;                  // copy of one of the entries in collision_.  Never NULL.
01002 
01003   collision_detection::AllowedCollisionMatrixPtr acm_;  // if NULL use parent's
01004 
01005   StateFeasibilityFn state_feasibility_;
01006   MotionFeasibilityFn motion_feasibility_;
01007 
01008   boost::scoped_ptr<ObjectColorMap> object_colors_;
01009 
01010   // a map of object types
01011   boost::scoped_ptr<ObjectTypeMap> object_types_;
01012 };
01013 }
01014 
01015 #endif


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jun 19 2019 19:23:49