Go to the documentation of this file.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_COLLISION_DETECTION_FCL_COLLISION_WORLD_FCL_
00038 #define MOVEIT_COLLISION_DETECTION_FCL_COLLISION_WORLD_FCL_
00039
00040 #include <moveit/collision_detection_fcl/collision_robot_fcl.h>
00041 #include <fcl/broadphase/broadphase.h>
00042
00043 namespace collision_detection
00044 {
00045
00046 class CollisionWorldFCL : public CollisionWorld
00047 {
00048 public:
00049
00050 CollisionWorldFCL();
00051 explicit CollisionWorldFCL(const WorldPtr& world);
00052 CollisionWorldFCL(const CollisionWorldFCL &other, const WorldPtr& world);
00053 virtual ~CollisionWorldFCL();
00054
00055
00056 virtual void checkRobotCollision(const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::RobotState &state) const;
00057 virtual void checkRobotCollision(const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::RobotState &state, const AllowedCollisionMatrix &acm) const;
00058 virtual void checkRobotCollision(const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::RobotState &state1, const robot_state::RobotState &state2) const;
00059 virtual void checkRobotCollision(const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::RobotState &state1, const robot_state::RobotState &state2, const AllowedCollisionMatrix &acm) const;
00060 virtual void checkWorldCollision(const CollisionRequest &req, CollisionResult &res, const CollisionWorld &other_world) const;
00061 virtual void checkWorldCollision(const CollisionRequest &req, CollisionResult &res, const CollisionWorld &other_world, const AllowedCollisionMatrix &acm) const;
00062
00063 virtual double distanceRobot(const CollisionRobot &robot, const robot_state::RobotState &state) const;
00064 virtual double distanceRobot(const CollisionRobot &robot, const robot_state::RobotState &state, const AllowedCollisionMatrix &acm) const;
00065 virtual double distanceWorld(const CollisionWorld &world) const;
00066 virtual double distanceWorld(const CollisionWorld &world, const AllowedCollisionMatrix &acm) const;
00067
00068 virtual void setWorld(const WorldPtr& world);
00069
00070 protected:
00071
00072 void checkWorldCollisionHelper(const CollisionRequest &req, CollisionResult &res, const CollisionWorld &other_world, const AllowedCollisionMatrix *acm) const;
00073 void checkRobotCollisionHelper(const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::RobotState &state, const AllowedCollisionMatrix *acm) const;
00074 double distanceRobotHelper(const CollisionRobot &robot, const robot_state::RobotState &state, const AllowedCollisionMatrix *acm) const;
00075 double distanceWorldHelper(const CollisionWorld &world, const AllowedCollisionMatrix *acm) const;
00076
00077 void constructFCLObject(const World::Object *obj, FCLObject &fcl_obj) const;
00078 void updateFCLObject(const std::string &id);
00079
00080
00081 boost::scoped_ptr<fcl::BroadPhaseCollisionManager> manager_;
00082 std::map<std::string, FCLObject > fcl_objs_;
00083
00084 private:
00085 void initialize();
00086 void notifyObjectChange(const ObjectConstPtr& obj, World::Action action);
00087 World::ObserverHandle observer_handle_;
00088 };
00089
00090 }
00091
00092 #endif