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
00037 #ifndef OMPL_ROS_IK_GOAL_SAMPLEABLE_H_
00038 #define OMPL_ROS_IK_GOAL_SAMPLEABLE_H_
00039
00040
00041 #include <ompl/base/goals/GoalSampleableRegion.h>
00042 #include <ompl/base/ScopedState.h>
00043 #include <ompl/base/State.h>
00044
00045
00046 #include <planning_environment/models/collision_models_interface.h>
00047 #include <planning_models/kinematic_model.h>
00048 #include <planning_models/kinematic_state.h>
00049
00050
00051 #include <ompl_ros_interface/ompl_ros_state_validity_checker.h>
00052 #include <ompl_ros_interface/ompl_ros_projection_evaluator.h>
00053 #include <ompl_ros_interface/ompl_ros_planner_config.h>
00054
00055 #include <ompl_ros_interface/helpers/ompl_ros_conversions.h>
00056
00057
00058 #include <pluginlib/class_loader.h>
00059 #include <kinematics_base/kinematics_base.h>
00060
00061 namespace ompl_ros_interface
00062 {
00067 class OmplRosIKSampleableRegion : public ompl::base::GoalSampleableRegion
00068 {
00069 public:
00074 OmplRosIKSampleableRegion(const ompl::base::SpaceInformationPtr &space_information)
00075 : GoalSampleableRegion(space_information),
00076 kinematics_loader_("kinematics_base","kinematics::KinematicsBase"),
00077 scoped_state_(space_information)
00078 {
00079 }
00080
00089 bool initialize(const ompl::base::StateSpacePtr &state_space,
00090 const std::string &kinematics_solver_name,
00091 const std::string &group_name,
00092 const std::string &end_effector_name,
00093 const planning_environment::CollisionModelsInterface* cmi);
00094
00101 bool configureOnRequest(const arm_navigation_msgs::GetMotionPlan::Request &request,
00102 arm_navigation_msgs::GetMotionPlan::Response &response,
00103 const unsigned int &max_sample_count = 100);
00104
00109 virtual void sampleGoal(ompl::base::State *state) const;
00110
00115 virtual unsigned int maxSampleCount(void) const;
00116
00117 private:
00118 void sampleGoals(const unsigned int &number_goals,
00119 std::vector<arm_navigation_msgs::RobotState> &sampled_states_vector) const;
00120
00121 std::vector<geometry_msgs::PoseStamped> ik_poses_;
00122 unsigned int max_sample_count_;
00123 unsigned int ik_poses_counter_;
00124 ompl::base::StateSpacePtr state_space_;
00125 kinematics::KinematicsBase* kinematics_solver_;
00126
00127 std::string kinematics_solver_name_, group_name_, end_effector_name_;
00128 pluginlib::ClassLoader<kinematics::KinematicsBase> kinematics_loader_;
00129 ompl::base::ScopedState<ompl::base::CompoundStateSpace> scoped_state_;
00130 arm_navigation_msgs::RobotState seed_state_, solution_state_;
00131
00132 ompl_ros_interface::OmplStateToRobotStateMapping ompl_state_to_robot_state_mapping_;
00133 ompl_ros_interface::RobotStateToOmplStateMapping robot_state_to_ompl_state_mapping_;
00134 const planning_environment::CollisionModelsInterface* collision_models_interface_;
00135
00136 };
00137
00138 }
00139
00140 #endif //OMPL_ROS_IK_GOAL_SAMPLEABLE_H_