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
00039 from __future__ import division
00040 import roslib
00041 roslib.load_manifest('segmented_clutter_grasp_planner')
00042 import rospy
00043 from object_manipulation_msgs.srv import GraspPlanning, GraspPlanningRequest
00044 import object_manipulator.draw_functions as draw_functions
00045 from sensor_msgs.msg import PointCloud2
00046 from geometry_msgs.msg import PoseStamped, Vector3
00047
00048
00049
00050 def call_plan_segmented_clutter_grasps(point_cloud, box_pose, box_dims, arm_name):
00051
00052 req = GraspPlanningRequest()
00053 req.arm_name = arm_name
00054 req.target.reference_frame_id = box_pose.header.frame_id
00055 req.target.region.cloud = point_cloud
00056 req.target.region.roi_box_pose = box_pose
00057 req.target.region.roi_box_dims = box_dims
00058
00059 service_name = "plan_segmented_clutter_grasps"
00060 rospy.loginfo("waiting for %s"%service_name)
00061 rospy.wait_for_service(service_name)
00062 rospy.loginfo("service found")
00063 serv = rospy.ServiceProxy(service_name, GraspPlanning)
00064 try:
00065 res = serv(req)
00066 except rospy.ServiceException, e:
00067 rospy.logerr("error when calling plan_segmented_clutter_grasps: %s"%e)
00068 return 0
00069 if res.error_code.value != 0:
00070 return []
00071
00072 return res.grasps
00073
00074
00075 if __name__ == "__main__":
00076
00077 rospy.init_node('test_point_cluster_grasp_planner', anonymous=True)
00078 draw_functions = draw_functions.DrawFunctions('grasp_markers')
00079
00080
00081 mode = 'table'
00082 if mode == 'shelf':
00083 rospy.set_param("segmented_clutter_grasp_planner_server/side_opening", True)
00084 else:
00085 rospy.set_param("segmented_clutter_grasp_planner_server/side_opening", False)
00086
00087
00088 rospy.set_param("segmented_clutter_grasp_planner_server/pregrasp_just_outside_box", True)
00089
00090
00091 arm_name = "right_arm"
00092
00093
00094 while(not rospy.is_shutdown()):
00095
00096 cloud_topic = "/camera/rgb/points"
00097
00098
00099
00100 print "waiting for point cloud"
00101 point_cloud = rospy.wait_for_message(cloud_topic, PointCloud2)
00102 print "point cloud received"
00103
00104
00105 box_pose = PoseStamped()
00106 box_dims = Vector3()
00107 box_pose.header.frame_id = "/base_link";
00108
00109
00110 if mode == 'grocery bag':
00111 box_pose.pose.position.x = 0.25
00112 box_pose.pose.position.y = -0.5
00113 box_pose.pose.position.z = 0.53
00114 box_dims.x = 0.5
00115 box_dims.y = 0.4
00116 box_dims.z = 0.6
00117
00118
00119 elif mode == 'table':
00120 box_pose.pose.position.x = (.41+.84)/2.
00121 box_pose.pose.position.y = (-.38+.18)/2.
00122 box_pose.pose.position.z = (.68+1.0)/2.
00123 box_dims.x = .84-.41
00124 box_dims.y = .18+.38;
00125 box_dims.z = 1.0-.68;
00126
00127
00128 else:
00129 box_pose.pose.position.x = (.42+.84)/2.
00130 box_pose.pose.position.y = (-.17+.07)/2.
00131 box_pose.pose.position.z = (.70+1.247)/2.
00132 box_dims.x = .84-.42
00133 box_dims.y = .17+.07
00134 box_dims.z = 1.247-.70
00135
00136
00137 grasps = call_plan_segmented_clutter_grasps(point_cloud, box_pose, box_dims, arm_name)
00138
00139
00140 print "number of grasps returned:", len(grasps)
00141 grasp_poses = [grasp.grasp_pose for grasp in grasps]
00142 draw_functions.draw_grasps(grasp_poses, box_pose.header.frame_id, pause = 0)
00143 print "done drawing grasps, press enter to continue"
00144 raw_input()
00145
00146
00147 draw_functions.clear_grasps(num = 1000)
00148
00149 print "Run again? Enter to run, q to exit"
00150 c = raw_input()
00151 if c == 'q':
00152 break
00153
00154
00155