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