00001 00013 #ifndef RAIL_PICK_AND_PLACE_MODEL_GENERATOR_H_ 00014 #define RAIL_PICK_AND_PLACE_MODEL_GENERATOR_H_ 00015 00016 // RAIL Recognition 00017 #include "PCLGraspModel.h" 00018 00019 // ROS 00020 #include <actionlib/server/simple_action_server.h> 00021 #include <graspdb/graspdb.h> 00022 #include <rail_pick_and_place_msgs/GenerateModelsAction.h> 00023 #include <ros/ros.h> 00024 00025 // PCL 00026 #include <pcl/point_cloud.h> 00027 #include <pcl/point_types.h> 00028 00029 namespace rail 00030 { 00031 namespace pick_and_place 00032 { 00033 00041 class ModelGenerator 00042 { 00043 public: 00045 static const bool DEFAULT_DEBUG = false; 00046 00052 ModelGenerator(); 00053 00059 virtual ~ModelGenerator(); 00060 00068 bool okay() const; 00069 00070 private: 00078 void generateModelsCallback(const rail_pick_and_place_msgs::GenerateModelsGoalConstPtr &goal); 00079 00090 void generateAndStoreModels(std::vector<PCLGraspModel> &grasp_models, const int max_model_size, 00091 std::vector<uint32_t> &new_model_ids); 00092 00104 bool registrationCheck(const PCLGraspModel &base, const PCLGraspModel &target, PCLGraspModel &result) const; 00105 00107 bool debug_, okay_; 00109 graspdb::Client *graspdb_; 00110 00112 ros::NodeHandle node_, private_node_; 00114 actionlib::SimpleActionServer<rail_pick_and_place_msgs::GenerateModelsAction> as_; 00116 ros::Publisher debug_pc_pub_, debug_poses_pub_; 00117 }; 00118 00119 } 00120 } 00121 00122 #endif