00001 //ROS 00002 #include <ros/ros.h> 00003 #include <actionlib/client/simple_action_client.h> 00004 #include <rail_pick_and_place_msgs/GraspRecognized.h> 00005 #include <rail_pick_and_place_msgs/PickupObject.h> 00006 #include <rail_pick_and_place_msgs/PickupSegmentedObject.h> 00007 #include <rail_pick_and_place_msgs/RecognizeAndGrasp.h> 00008 #include <rail_segmentation/Segment.h> 00009 #include <rail_segmentation/SegmentedObjectList.h> 00010 #include <sensor_msgs/point_cloud_conversion.h> 00011 #include <wpi_jaco_msgs/HomeArmAction.h> 00012 00013 class pickAndPlace 00014 { 00015 public: 00016 //ROS publishers, subscribers, and action servers 00017 ros::NodeHandle n; 00018 00019 ros::Subscriber objectSubscriber; 00020 00021 ros::ServiceServer pickupServer; 00022 ros::ServiceServer pickupSegmentedServer; 00023 00024 ros::ServiceClient recognizeAndGraspClient; 00025 ros::ServiceClient segmentClient; 00026 ros::ServiceClient graspRecognizedClient; 00027 00028 actionlib::SimpleActionClient<wpi_jaco_msgs::HomeArmAction> acHome; 00029 00030 rail_segmentation::SegmentedObjectList objectList; 00031 00035 pickAndPlace(); 00036 00043 bool pickup(rail_pick_and_place_msgs::PickupObject::Request &req, 00044 rail_pick_and_place_msgs::PickupObject::Response &res); 00045 00052 bool pickupSegmented(rail_pick_and_place_msgs::PickupSegmentedObject::Request &req, 00053 rail_pick_and_place_msgs::PickupSegmentedObject::Response &res); 00054 00059 void objectCallback(const rail_segmentation::SegmentedObjectList &list); 00060 };