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