00001 00032 #ifndef JOINT_INTERPOLATION_PLANNER_H 00033 #define JOINT_INTERPOLATION_PLANNER_H 00034 00035 #include <moveit/planning_interface/planning_interface.h> 00036 #include <moveit/planning_scene/planning_scene.h> 00037 #include <constrained_ik/moveit_interface/constrained_ik_planning_context.h> 00038 #include <boost/atomic.hpp> 00039 00040 namespace constrained_ik 00041 { 00051 class JointInterpolationPlanner : public constrained_ik::CLIKPlanningContext 00052 { 00053 public: 00054 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00055 00061 JointInterpolationPlanner(const std::string &name, const std::string &group) : constrained_ik::CLIKPlanningContext(name, group), terminate_(false) {} 00062 00067 JointInterpolationPlanner(const JointInterpolationPlanner &other) : constrained_ik::CLIKPlanningContext(other), terminate_(false) {} 00068 00070 void clear() override { terminate_ = false; } 00071 00076 bool terminate() override 00077 { 00078 terminate_ = true; 00079 return true; 00080 } 00081 00087 bool solve(planning_interface::MotionPlanResponse &res) override; 00088 00094 bool solve(planning_interface::MotionPlanDetailedResponse &res) override; 00095 00096 private: 00097 boost::atomic<bool> terminate_; 00098 }; 00099 } //namespace constrained_ik 00100 00101 #endif // JOINT_INTERPOLATION_PLANNER_H