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
00038 #ifndef MOVEIT_SIMPLE_GRASPS__GRASP_FILTER_
00039 #define MOVEIT_SIMPLE_GRASPS__GRASP_FILTER_
00040
00041
00042 #include <ros/ros.h>
00043 #include <tf_conversions/tf_eigen.h>
00044 #include <geometry_msgs/PoseArray.h>
00045 #include <geometry_msgs/PoseStamped.h>
00046 #include <moveit_msgs/Grasp.h>
00047
00048
00049 #include <moveit_simple_grasps/simple_grasps.h>
00050
00051
00052 #include <moveit_visual_tools/visual_tools.h>
00053
00054
00055 #include <moveit/robot_state/robot_state.h>
00056 #include <moveit/kinematics_plugin_loader/kinematics_plugin_loader.h>
00057
00058
00059 #include <boost/thread.hpp>
00060 #include <math.h>
00061 #define _USE_MATH_DEFINES
00062
00063 namespace moveit_simple_grasps
00064 {
00065
00066
00067 struct IkThreadStruct
00068 {
00069 IkThreadStruct(
00070 std::vector<moveit_msgs::Grasp> &possible_grasps,
00071 std::vector<moveit_msgs::Grasp> &filtered_grasps,
00072 std::vector<trajectory_msgs::JointTrajectoryPoint> &ik_solutions,
00073 Eigen::Affine3d &link_transform,
00074 int grasps_id_start,
00075 int grasps_id_end,
00076 kinematics::KinematicsBaseConstPtr kin_solver,
00077 bool filter_pregrasp,
00078 std::string ee_parent_link,
00079 double timeout,
00080 boost::mutex *lock,
00081 int thread_id)
00082 : possible_grasps_(possible_grasps),
00083 filtered_grasps_(filtered_grasps),
00084 ik_solutions_(ik_solutions),
00085 link_transform_(link_transform),
00086 grasps_id_start_(grasps_id_start),
00087 grasps_id_end_(grasps_id_end),
00088 kin_solver_(kin_solver),
00089 filter_pregrasp_(filter_pregrasp),
00090 ee_parent_link_(ee_parent_link),
00091 timeout_(timeout),
00092 lock_(lock),
00093 thread_id_(thread_id)
00094 {
00095 }
00096 std::vector<moveit_msgs::Grasp> &possible_grasps_;
00097 std::vector<moveit_msgs::Grasp> &filtered_grasps_;
00098 std::vector<trajectory_msgs::JointTrajectoryPoint> &ik_solutions_;
00099 Eigen::Affine3d link_transform_;
00100 int grasps_id_start_;
00101 int grasps_id_end_;
00102 kinematics::KinematicsBaseConstPtr kin_solver_;
00103 bool filter_pregrasp_;
00104 std::string ee_parent_link_;
00105 double timeout_;
00106 boost::mutex *lock_;
00107 int thread_id_;
00108 };
00109
00110
00111
00112 class GraspFilter
00113 {
00114 private:
00115
00116 robot_state::RobotState robot_state_;
00117
00118
00119 std::map<std::string, std::vector<kinematics::KinematicsBaseConstPtr> > kin_solvers_;
00120
00121
00122 moveit_visual_tools::VisualToolsPtr visual_tools_;
00123
00124 bool verbose_;
00125
00126 public:
00127
00128
00129 GraspFilter( robot_state::RobotState robot_state,
00130 moveit_visual_tools::VisualToolsPtr& visual_tools );
00131
00132
00133 ~GraspFilter();
00134
00135
00136 bool chooseBestGrasp( const std::vector<moveit_msgs::Grasp>& possible_grasps,
00137 moveit_msgs::Grasp& chosen );
00138
00139
00140 bool filterNthGrasp(std::vector<moveit_msgs::Grasp>& possible_grasps, int n);
00141
00149 bool filterGrasps(std::vector<moveit_msgs::Grasp>& possible_grasps,
00150 std::vector<trajectory_msgs::JointTrajectoryPoint>& ik_solutions,
00151 bool filter_pregrasp, const std::string &ee_parent_link,
00152 const std::string& planning_group);
00153
00154 private:
00155
00160 void filterGraspThread(IkThreadStruct ik_thread_struct);
00161
00162
00163 };
00164
00165 typedef boost::shared_ptr<GraspFilter> GraspFilterPtr;
00166 typedef boost::shared_ptr<const GraspFilter> GraspFilterConstPtr;
00167
00168 }
00169
00170 #endif