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_OMPL_INTERFACE_DETAIL_STATE_VALIDITY_CHECKER_
00038 #define MOVEIT_OMPL_INTERFACE_DETAIL_STATE_VALIDITY_CHECKER_
00039
00040 #include <moveit/ompl_interface/detail/threadsafe_state_storage.h>
00041 #include <moveit/collision_detection/collision_common.h>
00042 #include <ompl/base/StateValidityChecker.h>
00043
00044 namespace ompl_interface
00045 {
00046 class ModelBasedPlanningContext;
00047
00050 class StateValidityChecker : public ompl::base::StateValidityChecker
00051 {
00052 public:
00053 StateValidityChecker(const ModelBasedPlanningContext* planning_context);
00054
00055 virtual bool isValid(const ompl::base::State* state) const
00056 {
00057 return isValid(state, verbose_);
00058 }
00059
00060 virtual bool isValid(const ompl::base::State* state, double& dist) const
00061 {
00062 return isValid(state, dist, verbose_);
00063 }
00064
00065 bool isValid(const ompl::base::State* state, bool verbose) const;
00066 bool isValid(const ompl::base::State* state, double& dist, bool verbose) const;
00067
00068 virtual double cost(const ompl::base::State* state) const;
00069 virtual double clearance(const ompl::base::State* state) const;
00070
00071 void setVerbose(bool flag);
00072
00073 protected:
00074 bool isValidWithoutCache(const ompl::base::State* state, bool verbose) const;
00075 bool isValidWithoutCache(const ompl::base::State* state, double& dist, bool verbose) const;
00076
00077 bool isValidWithCache(const ompl::base::State* state, bool verbose) const;
00078 bool isValidWithCache(const ompl::base::State* state, double& dist, bool verbose) const;
00079
00080 const ModelBasedPlanningContext* planning_context_;
00081 std::string group_name_;
00082 TSStateStorage tss_;
00083 collision_detection::CollisionRequest collision_request_simple_;
00084 collision_detection::CollisionRequest collision_request_with_distance_;
00085 collision_detection::CollisionRequest collision_request_simple_verbose_;
00086 collision_detection::CollisionRequest collision_request_with_distance_verbose_;
00087
00088 collision_detection::CollisionRequest collision_request_with_cost_;
00089 bool verbose_;
00090 };
00091 }
00092
00093 #endif