exit(1)
tdr = TabletopDetectionResult()
tdr.table = res.table
tdr.clusters = res.clusters
tdr.result = res.result
update_collision_map = rospy.ServiceProxy(
'/tabletop_collision_map_processing/tabletop_collision_map_processing',
TabletopCollisionMapProcessing)
req = TabletopCollisionMapProcessingRequest()
req.detection_result = tdr
req.reset_collision_models = True
req.reset_attached_models = True
req.reset_static_map = True
req.take_static_collision_map = True
req.desired_frame = 'base_link'
coll_map_res = update_collision_map(req)
rospy.loginfo('found %d clusters, grabbing first' %
len(coll_map_res.graspable_objects))
#obj_pcluster = res.clusters[0]
req = PickupGoal()
req.arm_name = 'left_arm'
req.target = coll_map_res.graspable_objects[0]
#req.target.type = GraspableObject.POINT_CLUSTER
#req.target.cluster = obj_pcluster
req.desired_approach_distance = 0.05
req.min_approach_distance = 0.03
if res.result != 4 or len(res.clusters) == 0:
rospy.logerr('TabletopSegmentation did not find any clusters')
exit(1)
tdr = TabletopDetectionResult()
tdr.table = res.table
tdr.clusters = res.clusters
tdr.result = res.result
update_collision_map = rospy.ServiceProxy('/tabletop_collision_map_processing/tabletop_collision_map_processing', TabletopCollisionMapProcessing)
req = TabletopCollisionMapProcessingRequest()
req.detection_result = tdr
req.reset_collision_models = True
req.reset_attached_models = True
req.reset_static_map = True
req.take_static_collision_map = True
req.desired_frame = 'base_link'
coll_map_res = update_collision_map(req)
rospy.loginfo('found %d clusters, grabbing first' % len(coll_map_res.graspable_objects))
#obj_pcluster = res.clusters[0]
req = PickupGoal()
req.arm_name = 'left_arm'
req. target = coll_map_res.graspable_objects[0]
#req.target.type = GraspableObject.POINT_CLUSTER
#req.target.cluster = obj_pcluster
req.desired_approach_distance = 0.05
req.min_approach_distance = 0.03
req.lift.direction.header.frame_id = 'base_link'
sys.exit()
'''
collision map processing
'''
rospy.loginfo("processing collision map...")
# set up collision map request
col_req = TabletopCollisionMapProcessingRequest()
# pass the result of the tabletop detection
col_req.detection_result = det_res.detection
# ask for the existing map and collision models to be reset
col_req.reset_static_map = 1
col_req.reset_collision_models = 1
col_req.reset_attached_models = 1
# ask for a new static collision map to be taken with the laser after the new models are added to the environment
col_req.take_static_collision_map = 1
# ask for the results to be returned in base_link frame
col_req.desired_frame = '/base_link'
# call collision map processing to add the detected objects to the collision map and get back a list of GraspableObjects
try:
col_res = collision_map_processing_srv(col_req)
except rospy.ServiceException, e:
rospy.logerr("error when calling %s: %s" %
(collision_map_processing_name, e))
sys.exit()
# print out detected objects
for (ind, object) in enumerate(col_res.graspable_objects):
if object.type == GraspableObject.DATABASE_MODEL:
rospy.loginfo("object %d: recognized object with id %d" %
rospy.logerr('TabletopSegmentation did not find any clusters')
exit(1)
rospy.loginfo('TabletopSegmentation found %d clusters, will update collision map now' % len(segmentation_result.clusters))
tdr = TabletopDetectionResult()
tdr.table = segmentation_result.table
tdr.clusters = segmentation_result.clusters
tdr.result = segmentation_result.result
req = TabletopCollisionMapProcessingRequest()
req.detection_result = tdr
req.reset_collision_models = True
req.reset_attached_models = True
req.reset_static_map = False
req.take_static_collision_map = False
req.desired_frame = 'base_link'
coll_map_res = collision_map_processing_srv(req)
rospy.loginfo('collision_map update is done')
rospy.loginfo('there are %d graspable objects %s, collision support surface name is "%s"' %
(len(coll_map_res.graspable_objects), coll_map_res.collision_object_names, coll_map_res.collision_support_surface_name))
req = GraspPlanningRequest()
req.arm_name = 'left_arm'
req.target = coll_map_res.graspable_objects[0]
req.collision_object_name = coll_map_res.collision_object_names[0]
req.collision_support_surface_name = coll_map_res.collision_support_surface_name
rospy.loginfo('trying to find a good grasp for graspable object %s' % coll_map_res.collision_object_names[0])
rospy.signal_shutdown("")
sys.exit()
rospy.loginfo("%d objects detected" %
len(detection_reply.detection.clusters))
# detection_reply.detection.cluster_model_indices = tuple(xrange(len(detection_reply.detection.clusters)))
rospy.loginfo("Calling collision map processing")
processing_call = TabletopCollisionMapProcessingRequest()
processing_call.detection_result = detection_reply.detection
# ask for the exising map and collision models to be reset
processing_call.reset_attached_models = False
processing_call.reset_collision_models = False
processing_call.reset_static_map = False
# after the new models are added to the environment
processing_call.take_static_collision_map = False
processing_call.desired_frame = "base_link"
processing_reply = collision_processing_srv.call(processing_call)
if len(processing_reply.graspable_objects) == 0:
rospy.logerr("Collision map processing returned no graspable objects")
rospy.signal_shutdown("")
sys.exit()
sys.exit()
rospy.loginfo("Calling Pickup")
max_len = 0
index = 0
for i, c in enumerate(detection_reply.detection.clusters):
rospy.logerr("No objects found!")
rospy.signal_shutdown("")
sys.exit()
rospy.loginfo("%d objects detected" % len(detection_reply.detection.clusters))
# detection_reply.detection.cluster_model_indices = tuple(xrange(len(detection_reply.detection.clusters)))
rospy.loginfo("Calling collision map processing")
processing_call = TabletopCollisionMapProcessingRequest()
processing_call.detection_result = detection_reply.detection
# ask for the exising map and collision models to be reset
processing_call.reset_attached_models = False
processing_call.reset_collision_models = False
processing_call.reset_static_map = False
# after the new models are added to the environment
processing_call.take_static_collision_map = False
processing_call.desired_frame = "base_link"
processing_reply = collision_processing_srv.call(processing_call)
if len(processing_reply.graspable_objects) == 0:
rospy.logerr("Collision map processing returned no graspable objects")
rospy.signal_shutdown("")
sys.exit()
sys.exit()
rospy.loginfo("Calling Pickup")
max_len = 0
index = 0
for i,c in enumerate(detection_reply.detection.clusters):
