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_CONSTRAINT_SAMPLERS_CONSTRAINT_SAMPLER_
00038 #define MOVEIT_CONSTRAINT_SAMPLERS_CONSTRAINT_SAMPLER_
00039
00040 #include <moveit/planning_scene/planning_scene.h>
00041 #include <moveit/kinematic_constraints/kinematic_constraint.h>
00042 #include <boost/shared_ptr.hpp>
00043 #include <vector>
00044
00053 namespace constraint_samplers
00054 {
00059 class ConstraintSampler
00060 {
00061 public:
00062
00065 static const unsigned int DEFAULT_MAX_SAMPLING_ATTEMPTS = 2;
00066
00076 ConstraintSampler(const planning_scene::PlanningSceneConstPtr &scene, const std::string &group_name);
00077
00078 virtual ~ConstraintSampler()
00079 {
00080 }
00081
00090 virtual bool configure(const moveit_msgs::Constraints &constr) = 0;
00091
00097 const std::string& getGroupName() const
00098 {
00099 return getJointModelGroup()->getName();
00100 }
00101
00108 const robot_model::JointModelGroup* getJointModelGroup() const
00109 {
00110 return jmg_;
00111 }
00112
00119 const planning_scene::PlanningSceneConstPtr& getPlanningScene() const
00120 {
00121 return scene_;
00122 }
00123
00135 const std::vector<std::string>& getFrameDependency() const
00136 {
00137 return frame_depends_;
00138 }
00139
00144 const robot_state::GroupStateValidityCallbackFn& getGroupStateValidityCallback() const
00145 {
00146 return group_state_validity_callback_;
00147 }
00148
00155 void setGroupStateValidityCallback(const robot_state::GroupStateValidityCallbackFn &callback)
00156 {
00157 group_state_validity_callback_ = callback;
00158 }
00159
00160
00171 bool sample(robot_state::RobotState &state)
00172 {
00173 return sample(state, state, DEFAULT_MAX_SAMPLING_ATTEMPTS);
00174 }
00175
00176
00188 bool sample(robot_state::RobotState &state, unsigned int max_attempts)
00189 {
00190 return sample(state, state, max_attempts);
00191 }
00192
00203 bool sample(robot_state::RobotState &state, const robot_state::RobotState &reference_state)
00204 {
00205 return sample(state, reference_state, DEFAULT_MAX_SAMPLING_ATTEMPTS);
00206 }
00207
00218 bool project(robot_state::RobotState &state)
00219 {
00220 return project(state, DEFAULT_MAX_SAMPLING_ATTEMPTS);
00221 }
00222
00234 virtual bool sample(robot_state::RobotState &state,
00235 const robot_state::RobotState &reference_state,
00236 unsigned int max_attempts) = 0;
00237
00248 virtual bool project(robot_state::RobotState &state,
00249 unsigned int max_attempts) = 0;
00250
00258 bool isValid() const
00259 {
00260 return is_valid_;
00261 }
00262
00264 bool getVerbose() const
00265 {
00266 return verbose_;
00267 }
00268
00270 void setVerbose(bool flag)
00271 {
00272 verbose_ = flag;
00273 }
00274
00280 virtual const std::string& getName() const = 0;
00281
00282 protected:
00283
00288 virtual void clear();
00289
00290 bool is_valid_;
00292 planning_scene::PlanningSceneConstPtr scene_;
00293 const robot_model::JointModelGroup *jmg_;
00295 std::vector<std::string> frame_depends_;
00296 robot_state::GroupStateValidityCallbackFn group_state_validity_callback_;
00297 bool verbose_;
00298 };
00299
00300 MOVEIT_CLASS_FORWARD(ConstraintSampler);
00301
00302 }
00303
00304
00305 #endif