00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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/deprecation.h>
00049 #include <moveit_msgs/PlanningScene.h>
00050 #include <moveit_msgs/RobotTrajectory.h>
00051 #include <moveit_msgs/Constraints.h>
00052 #include <moveit_msgs/PlanningSceneComponents.h>
00053 #include <boost/enable_shared_from_this.hpp>
00054 #include <boost/noncopyable.hpp>
00055 #include <boost/shared_ptr.hpp>
00056 #include <boost/function.hpp>
00057 #include <boost/concept_check.hpp>
00058
00060 namespace planning_scene
00061 {
00062
00063 class PlanningScene;
00064 typedef boost::shared_ptr<PlanningScene> PlanningScenePtr;
00065 typedef boost::shared_ptr<const PlanningScene> PlanningSceneConstPtr;
00066
00069 typedef boost::function<bool(const robot_state::RobotState&, bool)> StateFeasibilityFn;
00070
00074 typedef boost::function<bool(const robot_state::RobotState&, const robot_state::RobotState&, bool)> MotionFeasibilityFn;
00075
00077 typedef std::map<std::string, std_msgs::ColorRGBA> ObjectColorMap;
00078
00080 typedef std::map<std::string, object_recognition_msgs::ObjectType> ObjectTypeMap;
00081
00085 class PlanningScene : private boost::noncopyable,
00086 public boost::enable_shared_from_this<PlanningScene>
00087 {
00088 public:
00089
00091 PlanningScene(const robot_model::RobotModelPtr &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 COLLISION_MAP_NS;
00101 static const std::string OCTOMAP_NS;
00102 static const std::string DEFAULT_SCENE_NAME;
00103
00104 ~PlanningScene();
00105
00107 const std::string& getName() const
00108 {
00109 return name_;
00110 }
00111
00113 void setName(const std::string &name)
00114 {
00115 name_ = name;
00116 }
00117
00135 void addCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr& allocator);
00136
00146 void setActiveCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr& allocator,
00147 bool exclusive = false);
00148
00155 bool setActiveCollisionDetector(const std::string& collision_detector_name);
00156
00157 const std::string& getActiveCollisionDetectorName() const
00158 {
00159 return active_collision_->alloc_->getName();
00160 }
00161
00164 void getCollisionDetectorNames(std::vector<std::string>& names) const;
00165
00178 PlanningScenePtr diff() const;
00179
00182 PlanningScenePtr diff(const moveit_msgs::PlanningScene &msg) const;
00183
00185 const PlanningSceneConstPtr& getParent() const
00186 {
00187 return parent_;
00188 }
00189
00191 const std::string& getPlanningFrame() const
00192 {
00193
00194 return ftf_ ? ftf_->getTargetFrame() : parent_->getPlanningFrame();
00195 }
00196
00198 const robot_model::RobotModelConstPtr& getRobotModel() const
00199 {
00200
00201 return kmodel_;
00202 }
00203
00205 const robot_state::RobotState& getCurrentState() const
00206 {
00207
00208 return kstate_ ? *kstate_ : parent_->getCurrentState();
00209 }
00211 robot_state::RobotState& getCurrentStateNonConst();
00212
00214 robot_state::RobotStatePtr getCurrentStateUpdated(const moveit_msgs::RobotState &update) const;
00215
00217 const collision_detection::AllowedCollisionMatrix& getAllowedCollisionMatrix() const
00218 {
00219 return acm_ ? *acm_ : parent_->getAllowedCollisionMatrix();
00220 }
00222 collision_detection::AllowedCollisionMatrix& getAllowedCollisionMatrixNonConst();
00223
00225 const robot_state::Transforms& getTransforms() const
00226 {
00227
00228 return (ftf_ || !parent_) ? *ftf_ : parent_->getTransforms();
00229 }
00231 robot_state::Transforms& getTransformsNonConst();
00232
00235 const Eigen::Affine3d& getFrameTransform(const std::string &id) const;
00236
00239 const Eigen::Affine3d& getFrameTransform(const robot_state::RobotState &state, const std::string &id) const;
00240
00242 bool knowsFrameTransform(const std::string &id) const;
00243
00245 bool knowsFrameTransform(const robot_state::RobotState &state, const std::string &id) const;
00246
00248 const collision_detection::WorldConstPtr& getWorld() const
00249 {
00250
00251 return world_const_;
00252 }
00253
00254
00255 const collision_detection::WorldPtr& getWorldNonConst()
00256 {
00257
00258 return world_;
00259 }
00260
00262 const collision_detection::CollisionWorldConstPtr& getCollisionWorld() const
00263 {
00264
00265 return active_collision_->cworld_const_;
00266 }
00267
00269 const collision_detection::CollisionRobotConstPtr& getCollisionRobot() const
00270 {
00271 return active_collision_->getCollisionRobot();
00272 }
00273
00275 const collision_detection::CollisionRobotConstPtr& getCollisionRobotUnpadded() const
00276 {
00277 return active_collision_->getCollisionRobotUnpadded();
00278 }
00279
00281 const collision_detection::CollisionWorldConstPtr& getCollisionWorld(const std::string& collision_detector_name) const;
00282
00284 const collision_detection::CollisionRobotConstPtr& getCollisionRobot(const std::string& collision_detector_name) const;
00285
00287 const collision_detection::CollisionRobotConstPtr& getCollisionRobotUnpadded(const std::string& collision_detector_name) const;
00288
00293 const collision_detection::CollisionRobotPtr& getCollisionRobotNonConst();
00294
00298 void propogateRobotPadding();
00299
00301 void checkCollision(const collision_detection::CollisionRequest& req,
00302 collision_detection::CollisionResult &res) const;
00303
00305 void checkCollision(const collision_detection::CollisionRequest& req,
00306 collision_detection::CollisionResult &res,
00307 const robot_state::RobotState &kstate) const;
00308
00311 void checkCollision(const collision_detection::CollisionRequest& req,
00312 collision_detection::CollisionResult &res,
00313 const robot_state::RobotState &kstate,
00314 const collision_detection::AllowedCollisionMatrix& acm) const;
00315
00318 void checkCollisionUnpadded(const collision_detection::CollisionRequest& req,
00319 collision_detection::CollisionResult &res) const;
00320
00323 void checkCollisionUnpadded(const collision_detection::CollisionRequest& req,
00324 collision_detection::CollisionResult &res,
00325 const robot_state::RobotState &kstate) const;
00326
00329 void checkCollisionUnpadded(const collision_detection::CollisionRequest& req,
00330 collision_detection::CollisionResult &res,
00331 const robot_state::RobotState &kstate,
00332 const collision_detection::AllowedCollisionMatrix& acm) const;
00333
00335 void checkSelfCollision(const collision_detection::CollisionRequest& req,
00336 collision_detection::CollisionResult &res) const;
00337
00339 void checkSelfCollision(const collision_detection::CollisionRequest& req,
00340 collision_detection::CollisionResult &res,
00341 const robot_state::RobotState &kstate) const;
00342
00345 void checkSelfCollision(const collision_detection::CollisionRequest& req,
00346 collision_detection::CollisionResult &res,
00347 const robot_state::RobotState &kstate,
00348 const collision_detection::AllowedCollisionMatrix& acm) const;
00349
00351 void getCollidingLinks(std::vector<std::string> &links) const;
00352
00354 void getCollidingLinks(std::vector<std::string> &links,
00355 const robot_state::RobotState &kstate) const;
00356
00359 void getCollidingLinks(std::vector<std::string> &links,
00360 const robot_state::RobotState &kstate,
00361 const collision_detection::AllowedCollisionMatrix& acm) const;
00362
00364 double distanceToCollision(const robot_state::RobotState &kstate) const;
00365
00367 double distanceToCollisionUnpadded(const robot_state::RobotState &kstate) const;
00368
00370 double distanceToCollision(const robot_state::RobotState &kstate, const collision_detection::AllowedCollisionMatrix& acm) const;
00371
00373 double distanceToCollisionUnpadded(const robot_state::RobotState &kstate, const collision_detection::AllowedCollisionMatrix& acm) const;
00374
00376 void saveGeometryToStream(std::ostream &out) const;
00377
00379 void loadGeometryFromStream(std::istream &in);
00380
00383 void getPlanningSceneDiffMsg(moveit_msgs::PlanningScene &scene) const;
00384
00387 void getPlanningSceneMsg(moveit_msgs::PlanningScene &scene) const;
00388
00391 void getPlanningSceneMsg(moveit_msgs::PlanningScene &scene, const moveit_msgs::PlanningSceneComponents &comp) const;
00392
00396 void setPlanningSceneDiffMsg(const moveit_msgs::PlanningScene &scene);
00397
00399 void setPlanningSceneMsg(const moveit_msgs::PlanningScene &scene);
00400
00402 void usePlanningSceneMsg(const moveit_msgs::PlanningScene &scene);
00403
00404 bool processCollisionObjectMsg(const moveit_msgs::CollisionObject &object);
00405 bool processAttachedCollisionObjectMsg(const moveit_msgs::AttachedCollisionObject &object);
00406
00407 void processPlanningSceneWorldMsg(const moveit_msgs::PlanningSceneWorld &world);
00408
00409 void processCollisionMapMsg(const moveit_msgs::CollisionMap &map);
00410 void processOctomapMsg(const octomap_msgs::OctomapWithPose &map);
00411 void processOctomapMsg(const octomap_msgs::Octomap &map);
00412 void processOctomapPtr(const boost::shared_ptr<const octomap::OcTree> &octree, const Eigen::Affine3d &t);
00413
00417 void setCurrentState(const moveit_msgs::RobotState &state);
00418
00420 void setCurrentState(const robot_state::RobotState &state);
00421
00423 void setAttachedBodyUpdateCallback(const robot_state::AttachedBodyCallback &callback);
00424
00426 void setCollisionObjectUpdateCallback(const collision_detection::World::ObserverCallbackFn &callback);
00427
00428 bool hasObjectColor(const std::string &id) const;
00429
00430 const std_msgs::ColorRGBA& getObjectColor(const std::string &id) const;
00431 void setObjectColor(const std::string &id, const std_msgs::ColorRGBA &color);
00432 void removeObjectColor(const std::string &id);
00433 void getKnownObjectColors(ObjectColorMap &kc) const;
00434
00435 bool hasObjectType(const std::string &id) const;
00436
00437 const object_recognition_msgs::ObjectType& getObjectType(const std::string &id) const;
00438 void setObjectType(const std::string &id, const object_recognition_msgs::ObjectType &type);
00439 void removeObjectType(const std::string &id);
00440 void getKnownObjectTypes(ObjectTypeMap &kc) const;
00441
00443 void clearDiffs();
00444
00447 void pushDiffs(const PlanningScenePtr &scene);
00448
00452 void decoupleParent();
00453
00456 void setStateFeasibilityPredicate(const StateFeasibilityFn &fn)
00457 {
00458 state_feasibility_ = fn;
00459 }
00460
00462 const StateFeasibilityFn& getStateFeasibilityPredicate() const
00463 {
00464 return state_feasibility_;
00465 }
00466
00468 void setMotionFeasibilityPredicate(const MotionFeasibilityFn &fn)
00469 {
00470 motion_feasibility_ = fn;
00471 }
00472
00474 const MotionFeasibilityFn& getMotionFeasibilityPredicate() const
00475 {
00476 return motion_feasibility_;
00477 }
00478
00480 bool isStateColliding(const std::string &group = "", bool verbose = false) const;
00481
00483 bool isStateColliding(const moveit_msgs::RobotState &state, const std::string &group = "", bool verbose = false) const;
00484
00486 bool isStateColliding(const robot_state::RobotState &state, const std::string &group = "", bool verbose = false) const;
00487
00489 bool isStateFeasible(const moveit_msgs::RobotState &state, bool verbose = false) const;
00490
00492 bool isStateFeasible(const robot_state::RobotState &state, bool verbose = false) const;
00493
00495 bool isStateConstrained(const moveit_msgs::RobotState &state, const moveit_msgs::Constraints &constr, bool verbose = false) const;
00496
00498 bool isStateConstrained(const robot_state::RobotState &state, const moveit_msgs::Constraints &constr, bool verbose = false) const;
00499
00501 bool isStateConstrained(const moveit_msgs::RobotState &state, const kinematic_constraints::KinematicConstraintSet &constr, bool verbose = false) const;
00502
00504 bool isStateConstrained(const robot_state::RobotState &state, const kinematic_constraints::KinematicConstraintSet &constr, bool verbose = false) const;
00505
00507 bool isStateValid(const moveit_msgs::RobotState &state, const std::string &group = "", bool verbose = false) const;
00508
00510 bool isStateValid(const robot_state::RobotState &state, const std::string &group = "", bool verbose = false) const;
00511
00513 bool isStateValid(const moveit_msgs::RobotState &state, const moveit_msgs::Constraints &constr, const std::string &group = "", bool verbose = false) const;
00514
00516 bool isStateValid(const robot_state::RobotState &state, const moveit_msgs::Constraints &constr, const std::string &group = "", bool verbose = false) const;
00517
00519 bool isStateValid(const robot_state::RobotState &state, const kinematic_constraints::KinematicConstraintSet &constr, const std::string &group = "", bool verbose = false) const;
00520
00522 bool isPathValid(const moveit_msgs::RobotState &start_state,
00523 const moveit_msgs::RobotTrajectory &trajectory,
00524 const std::string &group = "", bool verbose = false, std::vector<std::size_t> *invalid_index = NULL) const;
00525
00527 bool isPathValid(const moveit_msgs::RobotState &start_state,
00528 const moveit_msgs::RobotTrajectory &trajectory,
00529 const moveit_msgs::Constraints& path_constraints,
00530 const std::string &group = "", bool verbose = false, std::vector<std::size_t> *invalid_index = NULL) const;
00531
00533 bool isPathValid(const moveit_msgs::RobotState &start_state,
00534 const moveit_msgs::RobotTrajectory &trajectory,
00535 const moveit_msgs::Constraints& path_constraints,
00536 const moveit_msgs::Constraints& goal_constraints,
00537 const std::string &group = "", bool verbose = false, std::vector<std::size_t> *invalid_index = NULL) const;
00538
00540 bool isPathValid(const moveit_msgs::RobotState &start_state,
00541 const moveit_msgs::RobotTrajectory &trajectory,
00542 const moveit_msgs::Constraints& path_constraints,
00543 const std::vector<moveit_msgs::Constraints>& goal_constraints,
00544 const std::string &group = "", bool verbose = false, std::vector<std::size_t> *invalid_index = NULL) const;
00545
00547 bool isPathValid(const robot_trajectory::RobotTrajectory &trajectory,
00548 const moveit_msgs::Constraints& path_constraints,
00549 const std::vector<moveit_msgs::Constraints>& goal_constraints,
00550 const std::string &group = "", bool verbose = false, std::vector<std::size_t> *invalid_index = NULL) const;
00551
00553 bool isPathValid(const robot_trajectory::RobotTrajectory &trajectory,
00554 const moveit_msgs::Constraints& path_constraints,
00555 const moveit_msgs::Constraints& goal_constraints,
00556 const std::string &group = "", bool verbose = false, std::vector<std::size_t> *invalid_index = NULL) const;
00557
00559 bool isPathValid(const robot_trajectory::RobotTrajectory &trajectory,
00560 const moveit_msgs::Constraints& path_constraints,
00561 const std::string &group = "", bool verbose = false, std::vector<std::size_t> *invalid_index = NULL) const;
00562
00564 bool isPathValid(const robot_trajectory::RobotTrajectory &trajectory,
00565 const std::string &group = "", bool verbose = false, std::vector<std::size_t> *invalid_index = NULL) const;
00566
00568 void getCostSources(const robot_trajectory::RobotTrajectory &trajectory, std::size_t max_costs,
00569 std::set<collision_detection::CostSource> &costs, double overlap_fraction = 0.9) const;
00570
00572 void getCostSources(const robot_trajectory::RobotTrajectory &trajectory, std::size_t max_costs,
00573 const std::string &group_name, std::set<collision_detection::CostSource> &costs, double overlap_fraction = 0.9) const;
00574
00576 void getCostSources(const robot_state::RobotState &state, std::size_t max_costs,
00577 std::set<collision_detection::CostSource> &costs) const;
00578
00580 void getCostSources(const robot_state::RobotState &state, std::size_t max_costs,
00581 const std::string &group_name, std::set<collision_detection::CostSource> &costs) const;
00582
00584 void printKnownObjects(std::ostream& out) const;
00585
00587 static bool isEmpty(const moveit_msgs::PlanningScene &msg);
00588
00590 static bool isEmpty(const moveit_msgs::PlanningSceneWorld &msg);
00591
00593 static bool isEmpty(const moveit_msgs::RobotState &msg);
00594
00596 static PlanningScenePtr clone(const PlanningSceneConstPtr &scene);
00597
00598 private:
00599
00600
00601 PlanningScene(const PlanningSceneConstPtr &parent);
00602
00603
00604
00605 void initialize();
00606
00607
00608 static robot_model::RobotModelPtr createRobotModel(const boost::shared_ptr<const urdf::ModelInterface> &urdf_model,
00609 const boost::shared_ptr<const srdf::Model> &srdf_model);
00610
00611 void getPlanningSceneMsgCollisionObject(moveit_msgs::PlanningScene &scene, const std::string &ns) const;
00612 void getPlanningSceneMsgCollisionObjects(moveit_msgs::PlanningScene &scene) const;
00613 void getPlanningSceneMsgCollisionMap(moveit_msgs::PlanningScene &scene) const;
00614 void getPlanningSceneMsgOctomap(moveit_msgs::PlanningScene &scene) const;
00615 void getPlanningSceneMsgObjectColors(moveit_msgs::PlanningScene &scene_msg) const;
00616
00617 struct CollisionDetector;
00618 typedef boost::shared_ptr<CollisionDetector> CollisionDetectorPtr;
00619 typedef boost::shared_ptr<const CollisionDetector> CollisionDetectorConstPtr;
00620
00621
00622 struct CollisionDetector
00623 {
00624 collision_detection::CollisionDetectorAllocatorPtr alloc_;
00625 collision_detection::CollisionRobotPtr crobot_unpadded_;
00626 collision_detection::CollisionRobotConstPtr crobot_unpadded_const_;
00627 collision_detection::CollisionRobotPtr crobot_;
00628 collision_detection::CollisionRobotConstPtr crobot_const_;
00629
00630 collision_detection::CollisionWorldPtr cworld_;
00631 collision_detection::CollisionWorldConstPtr cworld_const_;
00632
00633 CollisionDetectorConstPtr parent_;
00634
00635 const collision_detection::CollisionRobotConstPtr& getCollisionRobot() const
00636 {
00637 return crobot_const_ ? crobot_const_ : parent_->getCollisionRobot();
00638 }
00639 const collision_detection::CollisionRobotConstPtr& getCollisionRobotUnpadded() const
00640 {
00641 return crobot_unpadded_const_ ? crobot_unpadded_const_ : parent_->getCollisionRobotUnpadded();
00642 }
00643 void findParent(const PlanningScene& scene);
00644 void copyPadding(const CollisionDetector& src);
00645 };
00646 friend struct CollisionDetector;
00647
00648 typedef std::map<std::string, CollisionDetectorPtr>::iterator CollisionDetectorIterator;
00649 typedef std::map<std::string, CollisionDetectorPtr>::const_iterator CollisionDetectorConstIterator;
00650
00651 void allocateCollisionDetectors();
00652 void allocateCollisionDetectors(CollisionDetector& detector);
00653
00654
00655
00656 std::string name_;
00657
00658 PlanningSceneConstPtr parent_;
00659
00660 robot_model::RobotModelConstPtr kmodel_;
00661
00662 robot_state::RobotStatePtr kstate_;
00663 robot_state::AttachedBodyCallback current_state_attached_body_callback_;
00664
00665 robot_state::TransformsPtr ftf_;
00666
00667 collision_detection::WorldPtr world_;
00668 collision_detection::WorldConstPtr world_const_;
00669 collision_detection::WorldDiffPtr world_diff_;
00670 collision_detection::World::ObserverCallbackFn current_world_object_update_callback_;
00671 collision_detection::World::ObserverHandle current_world_object_update_observer_handle_;
00672
00673 std::map<std::string, CollisionDetectorPtr> collision_;
00674 CollisionDetectorPtr active_collision_;
00675
00676 collision_detection::AllowedCollisionMatrixPtr acm_;
00677
00678 StateFeasibilityFn state_feasibility_;
00679 MotionFeasibilityFn motion_feasibility_;
00680
00681 boost::scoped_ptr<ObjectColorMap> object_colors_;
00682
00683
00684 boost::scoped_ptr<ObjectTypeMap> object_types_;
00685
00686
00687 };
00688
00689 }
00690
00691
00692 #endif