Go to the documentation of this file.00001 #include "rail_pick_and_place_wrapper/pick_and_place.h"
00002
00003 using namespace std;
00004
00005 pickAndPlace::pickAndPlace() : acHome("jaco_arm/home_arm", true)
00006 {
00007 objectSubscriber = n.subscribe("rail_segmentation/segmented_objects", 1, &pickAndPlace::objectCallback, this);
00008
00009 pickupServer = n.advertiseService("rail_pick_and_place/pickup_object", &pickAndPlace::pickup, this);
00010 pickupSegmentedServer = n.advertiseService("rail_pick_and_place/pickup_segmented_object", &pickAndPlace::pickupSegmented, this);
00011
00012 recognizeAndGraspClient = n.serviceClient<rail_pick_and_place_msgs::RecognizeAndGrasp>("rail_recognition/recognize_and_pickup");
00013 graspRecognizedClient = n.serviceClient<rail_pick_and_place_msgs::GraspRecognized>("rail_recognition/grasp_recognized");
00014 segmentClient = n.serviceClient<rail_segmentation::Segment>("rail_segmentation/segment");
00015
00016 ROS_INFO("Waiting for arm action server...");
00017 acHome.waitForServer();
00018 ROS_INFO("Finished waiting for arm action server.");
00019 }
00020
00021 bool pickAndPlace::pickup(rail_pick_and_place_msgs::PickupObject::Request &req, rail_pick_and_place_msgs::PickupObject::Response &res)
00022 {
00023 rail_pick_and_place_msgs::RecognizeAndGrasp::Request pickupReq;
00024 rail_pick_and_place_msgs::RecognizeAndGrasp::Response pickupRes;
00025
00026
00027 pickupReq.objectIndices.clear();
00028 if (req.objectName.compare("bowl") == 0)
00029 {
00030 pickupReq.objectIndices.push_back(0);
00031 }
00032 else if (req.objectName.compare("cup") == 0)
00033 {
00034 pickupReq.objectIndices.push_back(1);
00035 }
00036 else if (req.objectName.compare("fork") == 0 || req.objectName.compare("spoon") == 0)
00037 {
00038 pickupReq.objectIndices.push_back(2);
00039 }
00040
00041 int attempts = 0;
00042
00043 while (attempts < 1)
00044 {
00045
00046 ROS_INFO("Retracting arm...");
00047 wpi_jaco_msgs::HomeArmGoal retractGoal;
00048 retractGoal.retract = true;
00049 retractGoal.retractPosition.position = true;
00050 retractGoal.retractPosition.armCommand = true;
00051 retractGoal.retractPosition.fingerCommand = false;
00052 retractGoal.retractPosition.repeat = false;
00053 retractGoal.retractPosition.joints.resize(6);
00054 retractGoal.retractPosition.joints[0] = -2.57;
00055 retractGoal.retractPosition.joints[1] = 1.39;
00056 retractGoal.retractPosition.joints[2] = .527;
00057 retractGoal.retractPosition.joints[3] = -.084;
00058 retractGoal.retractPosition.joints[4] = .515;
00059 retractGoal.retractPosition.joints[5] = -1.745;
00060 acHome.sendGoal(retractGoal);
00061 acHome.waitForResult(ros::Duration(15.0));
00062 ros::Duration(9.0).sleep();
00063
00064
00065 ROS_INFO("Segmenting...");
00066 rail_segmentation::Segment::Request segmentReq;
00067 rail_segmentation::Segment::Response segmentRes;
00068 segmentReq.useMapFrame = false;
00069 segmentReq.clear = true;
00070 segmentReq.segmentOnRobot = false;
00071 if (!segmentClient.call(segmentReq, segmentRes))
00072 {
00073 ROS_INFO("Error calling segmentation client.");
00074 res.success = false;
00075 return false;
00076 }
00077
00078
00079 ROS_INFO("Readying arm...");
00080 wpi_jaco_msgs::HomeArmGoal homeGoal;
00081 homeGoal.retract = false;
00082 acHome.sendGoal(homeGoal);
00083 acHome.waitForResult(ros::Duration(15.0));
00084
00085
00086 for (unsigned int i = 0; i < segmentRes.objects.size(); i ++)
00087 {
00088 ROS_INFO("Starting pickup...");
00089 sensor_msgs::convertPointCloud2ToPointCloud(segmentRes.objects[i], pickupReq.cloud);
00090 pickupReq.numAttempts = 6;
00091 recognizeAndGraspClient.call(pickupReq, pickupRes);
00092 if (pickupRes.success)
00093 {
00094 ROS_INFO("Object pickup complete.");
00095 res.success = true;
00096 return true;
00097 }
00098 }
00099
00100 attempts ++;
00101 }
00102
00103 ROS_INFO("Object pickup failed.");
00104 res.success = false;
00105 return true;
00106 }
00107
00108 void pickAndPlace::objectCallback(const rail_segmentation::SegmentedObjectList& list)
00109 {
00110 ROS_INFO("Received new objects");
00111 objectList = list;
00112 }
00113
00114 bool pickAndPlace::pickupSegmented(rail_pick_and_place_msgs::PickupSegmentedObject::Request &req,
00115 rail_pick_and_place_msgs::PickupSegmentedObject::Response &res)
00116 {
00117 ROS_INFO("Starting pickup...");
00118
00119 if (req.objectIndex >= objectList.objects.size())
00120 {
00121 ROS_INFO("Pickup index out of bounds of segmented object list, pickup failed.");
00122 res.success = false;
00123 return true;
00124 }
00125
00126 if (objectList.objects[req.objectIndex].recognized)
00127 {
00128
00129 rail_pick_and_place_msgs::GraspRecognized::Request graspReq;
00130 rail_pick_and_place_msgs::GraspRecognized::Response graspRes;
00131 graspReq.numAttempts = 6;
00132 graspReq.grasps = objectList.objects[req.objectIndex].graspPoses;
00133 graspReq.objectIndex = objectList.objects[req.objectIndex].model;
00134
00135 if (!graspRecognizedClient.call(graspReq, graspRes))
00136 {
00137 ROS_INFO("Grasping service client call failed.");
00138 res.success = false;
00139 return false;
00140 }
00141
00142 if (graspRes.success)
00143 {
00144 ROS_INFO("Object pickup complete.");
00145 res.success = true;
00146 return true;
00147 }
00148 else
00149 {
00150 ROS_INFO("Object pickup failed.");
00151 res.success = false;
00152 return true;
00153 }
00154
00155 }
00156 else
00157 {
00158
00159 rail_pick_and_place_msgs::RecognizeAndGrasp::Request pickupReq;
00160 rail_pick_and_place_msgs::RecognizeAndGrasp::Response pickupRes;
00161
00162 pickupReq.objectIndices.clear();
00163 sensor_msgs::convertPointCloud2ToPointCloud(objectList.objects[req.objectIndex].objectCloud, pickupReq.cloud);
00164 pickupReq.numAttempts = 6;
00165
00166 recognizeAndGraspClient.call(pickupReq, pickupRes);
00167 if (pickupRes.success)
00168 {
00169 ROS_INFO("Object pickup complete.");
00170 res.success = true;
00171 }
00172 else
00173 {
00174 ROS_INFO("Pickup failed.");
00175 res.success = false;
00176 }
00177 }
00178 return true;
00179 }
00180
00181 int main(int argc, char **argv)
00182 {
00183 ros::init(argc, argv, "rail_pick_and_place");
00184
00185 pickAndPlace p;
00186
00187 ros::spin();
00188
00189 return 0;
00190 }