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