$search
00001 #!/usr/bin/python 00002 # Software License Agreement (BSD License) 00003 # 00004 # Copyright (c) 2011, Willow Garage, Inc. 00005 # All rights reserved. 00006 # 00007 # Redistribution and use in source and binary forms, with or without 00008 # modification, are permitted provided that the following conditions 00009 # are met: 00010 # 00011 # * Redistributions of source code must retain the above copyright 00012 # notice, this list of conditions and the following disclaimer. 00013 # * Redistributions in binary form must reproduce the above 00014 # copyright notice, this list of conditions and the following 00015 # disclaimer in the documentation and/or other materials provided 00016 # with the distribution. 00017 # * Neither the name of the Willow Garage nor the names of its 00018 # contributors may be used to endorse or promote products derived 00019 # from this software without specific prior written permission. 00020 # 00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 # POSSIBILITY OF SUCH DAMAGE. 00033 # 00034 # author: Kaijen Hsiao 00035 00036 ## @package segmented_clutter_grasp_planner 00037 # Test script for the segmented_clutter_grasp_planner_server 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 #call plan_point_cluster_grasp to get candidate grasps for a cluster 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 #initialize the node, tf listener and broadcaster, and rviz drawing helper class 00077 rospy.init_node('test_point_cluster_grasp_planner', anonymous=True) 00078 draw_functions = draw_functions.DrawFunctions('grasp_markers') 00079 00080 #set the region to be grocery bag, shelf, or table 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 #test asking the pregrasp to be just above the object's bounding box 00088 rospy.set_param("segmented_clutter_grasp_planner_server/pregrasp_just_outside_box", True) 00089 00090 #using the right arm 00091 arm_name = "right_arm" 00092 00093 #keep going until the user says to quit (or until Ctrl-C is pressed) 00094 while(not rospy.is_shutdown()): 00095 00096 cloud_topic = "/camera/rgb/points" 00097 #cloud_topic = "/narrow_stereo_textured/points2" 00098 00099 #grab a point cloud from cloud_topic 00100 print "waiting for point cloud" 00101 point_cloud = rospy.wait_for_message(cloud_topic, PointCloud2) 00102 print "point cloud received" 00103 00104 #set the bounding region 00105 box_pose = PoseStamped() 00106 box_dims = Vector3() 00107 box_pose.header.frame_id = "/base_link"; 00108 00109 # (grocery bag to the front right side of robot) x: 0 to 0.5 y: -0.3 to -0.7 z: 0.22 to 1 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 # (right-arm reachable table in front of robot) x: .41 to .84 y: -0.38 to +0.18 z: 0.68 to 1.0 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 # (right-arm reachable shelf in front of robot) x: .42 to .84 y: -0.17 to +0.07 z: 0.70 to 1.247 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 #plan grasps on the point cloud in that area 00137 grasps = call_plan_segmented_clutter_grasps(point_cloud, box_pose, box_dims, arm_name) 00138 00139 #draw the resulting grasps (all at once, or one at a time) 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 #clear out the grasps drawn 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