00001 00013 #ifndef RAIL_PICK_AND_PLACE_GRASP_COLLECTOR_H_ 00014 #define RAIL_PICK_AND_PLACE_GRASP_COLLECTOR_H_ 00015 00016 // ROS 00017 #include <actionlib/client/simple_action_client.h> 00018 #include <actionlib/server/simple_action_server.h> 00019 #include <geometry_msgs/Point32.h> 00020 #include <graspdb/graspdb.h> 00021 #include <rail_manipulation_msgs/GripperAction.h> 00022 #include <rail_manipulation_msgs/LiftAction.h> 00023 #include <rail_manipulation_msgs/SegmentedObjectList.h> 00024 #include <rail_manipulation_msgs/VerifyGraspAction.h> 00025 #include <rail_pick_and_place_msgs/GraspAndStoreAction.h> 00026 #include <ros/ros.h> 00027 #include <sensor_msgs/point_cloud_conversion.h> 00028 #include <tf2_ros/transform_listener.h> 00029 00030 // Boost 00031 #include <boost/thread/mutex.hpp> 00032 00033 namespace rail 00034 { 00035 namespace pick_and_place 00036 { 00037 00045 class GraspCollector 00046 { 00047 public: 00049 static const bool DEFAULT_DEBUG = false; 00051 static const int AC_WAIT_TIME = 10; 00052 00059 GraspCollector(); 00060 00066 virtual ~GraspCollector(); 00067 00075 bool okay() const; 00076 00077 private: 00086 void graspAndStore(const rail_pick_and_place_msgs::GraspAndStoreGoalConstPtr &goal); 00087 00095 void segmentedObjectsCallback(const rail_manipulation_msgs::SegmentedObjectList::Ptr &object_list); 00096 00098 boost::mutex mutex_; 00099 00101 bool debug_, okay_; 00103 std::string robot_fixed_frame_id_, eef_frame_id_; 00105 graspdb::Client *graspdb_; 00106 00108 ros::NodeHandle node_, private_node_; 00110 ros::Publisher debug_pub_; 00112 ros::Subscriber segmented_objects_sub_; 00114 rail_manipulation_msgs::SegmentedObjectList::Ptr object_list_; 00116 actionlib::SimpleActionServer<rail_pick_and_place_msgs::GraspAndStoreAction> as_; 00118 actionlib::SimpleActionClient<rail_manipulation_msgs::GripperAction> *gripper_ac_; 00120 actionlib::SimpleActionClient<rail_manipulation_msgs::LiftAction> *lift_ac_; 00122 actionlib::SimpleActionClient<rail_manipulation_msgs::VerifyGraspAction> *verify_grasp_ac_; 00123 00125 tf2_ros::Buffer tf_buffer_; 00127 tf2_ros::TransformListener tf_listener_; 00129 ros::Duration ac_wait_time_; 00130 }; 00131 00132 } 00133 } 00134 00135 #endif