def reset3_request_cb(userdata, request):
print "Creating resetting request for tabletop_collision_map_processing"
deleting_request = TabletopCollisionMapProcessingRequest()
deleting_request.reset_collision_models = True
deleting_request.reset_attached_models = True
return deleting_request
def call_collision_map_processing(
self, detection_result, reset_collision_models=True, reset_attached_models=True, desired_frame="/base_link"
):
"""Calls the collision map processing service. Given the result of
detect, adds the objects to the collision map and returns them as
instances of GraspableObjects with correct collision names.
Parameters:
detection_result: a tabletop_object_detector/TabletopDetectionResponse msg,
usually returned by detect().
reset_collision_models: whether the current collision models should be reset before adding new models
reset_attached_models: whether the current list of models attached to the robot should be reset
desired_frame: what tf frame the results should be returned in
"""
if detection_result is None:
rospy.logerr("Error: using a None detection_result")
return None
rospy.loginfo("Calling collision map processing")
processing_call = TabletopCollisionMapProcessingRequest()
processing_call.detection_result = detection_result.detection
processing_call.reset_attached_models = reset_attached_models
processing_call.reset_collision_models = reset_collision_models
processing_call.desired_frame = desired_frame
try:
self.last_collision_processing_msg = self.collision_processing.call(processing_call)
except ServiceException, e:
rospy.logerr("Error calling collision map: %s" % str(e))
self.last_collision_processing_msg = None
def reset_collision_map(self):
req = TabletopCollisionMapProcessingRequest()
req.detection_result = TabletopDetectionResult()
req.reset_collision_models = True
req.reset_attached_models = True
self.collision_map_processing_srv(req)
rospy.loginfo('collision map reset')
def reset_collision_map(self):
req = TabletopCollisionMapProcessingRequest()
req.detection_result = TabletopDetectionResult()
req.reset_collision_models = True
req.reset_attached_models = True
self.collision_map_processing_srv(req)
rospy.loginfo('collision map reset')
def update_collision_map(self, tabletop_detection_result):
req = TabletopCollisionMapProcessingRequest()
req.detection_result = tabletop_detection_result
req.reset_collision_models = True
req.reset_attached_models = True
req.desired_frame = 'base_link'
res = self.collision_map_processing_srv(req)
rospy.loginfo('found %d clusters: %s, surface name: %s' %
(len(res.graspable_objects), res.collision_object_names, res.collision_support_surface_name))
return res
def update_collision_map(self, tabletop_detection_result):
rospy.loginfo('collision_map update in progress')
req = TabletopCollisionMapProcessingRequest()
req.detection_result = tabletop_detection_result
req.reset_collision_models = True
req.reset_attached_models = True
req.desired_frame = 'base_link'
res = self.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(res.graspable_objects), res.collision_object_names, res.collision_support_surface_name))
return res
def call_collision_map_processing(self, detection_result):
if detection_result is None:
rospy.logerr("Error: using a None detection_result")
return None
rospy.loginfo("Calling collision map processing")
processing_call = TabletopCollisionMapProcessingRequest()
processing_call.detection_result = detection_result.detection
# ask for the exising map and collision models to be reset
processing_call.reset_attached_models = True
processing_call.reset_collision_models = True
processing_call.desired_frame = "base_link"
try:
self.last_collision_processing_msg = self.collision_processing.call(processing_call)
except ServiceException, e:
rospy.logerr("Error calling collision map: %s" % str(e))
self.last_collision_processing_msg = None
def update_collision_map(self, tabletop_detection_result):
req = TabletopCollisionMapProcessingRequest()
req.detection_result = tabletop_detection_result
req.reset_collision_models = True
req.reset_attached_models = True
req.desired_frame = 'base_link'
res = self.collision_map_processing_srv(req)
rospy.loginfo('found %d clusters: %s, surface name: %s' %
(len(res.graspable_objects), res.collision_object_names, res.collision_support_surface_name))
return res
def update_collision_map(self, tabletop_detection_result):
rospy.loginfo('collision_map update in progress')
req = TabletopCollisionMapProcessingRequest()
req.detection_result = tabletop_detection_result
req.reset_collision_models = True
req.reset_attached_models = True
req.desired_frame = 'base_link'
res = self.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(res.graspable_objects), res.collision_object_names,
res.collision_support_surface_name))
return res
def call_collision_map_processing(self, detection_result):
if detection_result is None:
rospy.logerr("Error: using a None detection_result")
return None
rospy.loginfo("Calling collision map processing")
processing_call = TabletopCollisionMapProcessingRequest()
processing_call.detection_result = detection_result.detection
# ask for the exising map and collision models to be reset
processing_call.reset_attached_models = True
processing_call.reset_collision_models = True
processing_call.desired_frame = "base_link"
try:
self.last_collision_processing_msg = self.collision_processing.call(
processing_call)
except ServiceException, e:
rospy.logerr("Error calling collision map: %s" % str(e))
self.last_collision_processing_msg = None
TabletopSegmentation)
res = get_obj()
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'
def pick_up(self, whicharm="l", NUM_TRIES=5, near_pose=None):
self.pickup_client.cancel_goal()
for i in range(NUM_TRIES):
rospy.loginfo("Attempt %d to grasp " % i )
# call the tabletop detection
rospy.loginfo("Calling tabletop detector")
detection_call = TabletopDetectionRequest()
# we want recognized database objects returned
# set this to false if you are using the pipeline without the database
detection_call.return_clusters = True
#we want the individual object point clouds returned as well
detection_call.return_models = True
detection_call.num_models = 1
detection_call_response = self.object_detection_srv(detection_call).detection
if not detection_call_response:
rospy.loginfo("Tabletop detection service failed")
success=False; break
if detection_call_response.result != detection_call_response.SUCCESS:
rospy.loginfo("Tabletop detection returned error code %d",
detection_call_response.result)
success=False; break
if len(detection_call_response.clusters)==0\
and len(detection_call_response.models)==0 :
rospy.loginfo("The tabletop detector detected the table, "
"but found no objects")
success=False; break
# call collision map processing
rospy.loginfo("Calling collision map processing")
processing_call = TabletopCollisionMapProcessingRequest()
# pass the result of the tabletop detection
processing_call.detection_result = detection_call_response
print detection_call_response.models
import pdb;pdb.set_trace()
# ask for the existing map and collision models to be reset
processing_call.reset_collision_models = True
processing_call.reset_attached_models = True
#ask for the results to be returned in base link frame
processing_call.desired_frame = "base_link"
processing_call_response = self.collision_processing_srv(processing_call)
if not processing_call_response:
rospy.loginfo("Collision map processing service failed")
success=False; break
#the collision map processor returns instances of graspable objects
if len(processing_call_response.graspable_objects) == 0:
rospy.loginfo("Collision map processing returned no graspable objects")
success=False; break
graspable_objects = processing_call_response.graspable_objects
# sort graspable objects
import pdb;
# sort graspable objects
objects_with_offsets = [None]*len(graspable_objects)
#near_point = np.array(list(near_pose[:3]) + [1.0])
for i, obj in enumerate(graspable_objects):
pdb.set_trace()
points = point_cloud_to_mat(obj.cluster)
#centroid =np.reshape(np.mean(points, axis=1), 4) - near_point
#offset = np.linalg.norm(centroid - near_point)
#objects_with_offsets[i] = (offset,obj)
#ordering = sorted(objects_with_offsets, key=lambda item:float((item[0])))
#graspable_objects = [items[1] for items in ordering]
#print [items[0] for items in ordering]
for i, grasp_object in enumerate(graspable_objects):
# call object pickup
rospy.loginfo("Calling the pickup action");
pickup_goal = PickupGoal()
# pass one of the graspable objects returned
# by the collision map processor
#pickup_goal.target = processing_call_response.graspable_objects[0]
pickup_goal.target = grasp_object
# pass the name that the object has in the collision environment
# this name was also returned by the collision map processor
pickup_goal.collision_object_name = \
processing_call_response.collision_object_names[i]
# pass the collision name of the table, also returned by the collision
# map processor
pickup_goal.collision_support_surface_name = \
processing_call_response.collision_support_surface_name
# pick up the object with the left arm
if whicharm=="l":
pickup_goal.arm_name = "left_arm"
else:
pickup_goal.arm_name = "right_arm"
# we will be lifting the object along the "vertical" direction
# which is along the z axis in the base_link frame
direction = Vector3Stamped()
direction.header.stamp = rospy.Time.now()
direction.header.frame_id = "base_link"
direction.vector.x = 0
direction.vector.y = 0
direction.vector.z = 1
pickup_goal.lift.direction = direction
#request a vertical lift of 10cm after grasping the object
pickup_goal.lift.desired_distance = 0.1;
pickup_goal.lift.min_distance = 0.05;
#do not use tactile-based grasping or tactile-based lift
pickup_goal.use_reactive_lift = False;
pickup_goal.use_reactive_execution = False;
#pickup_goal.allow_gripper_support_collision = True
#pickup_goal.ignore_collisions = True
#send the goal
rospy.loginfo("Waiting for the pickup action...")
self.pickup_client.send_goal_and_wait(pickup_goal,rospy.Duration(15))
pickup_result =self.pickup_client.get_result()
assert isinstance(pickup_result, PickupResult)
success = pickup_result.manipulation_result.value ==\
pickup_result.manipulation_result.SUCCESS
if success:
rospy.loginfo("Success! Grasped Object.")
return success
if not success:
rospy.loginfo("failed to grasp object")
return success
detection_call = TabletopDetectionRequest()
detection_call.return_clusters = True
detection_call.return_models = True
detection_reply = object_detection_srv.call(detection_call)
if len(detection_reply.detection.clusters) == 0:
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()
except rospy.ServiceException, e:
rospy.logerr("error when calling %s: %s" % (grasper_detect_name, e))
sys.exit()
if det_res.detection.result == det_res.detection.SUCCESS:
rospy.loginfo("tabletop detection reports success")
else:
rospy.logerr("tabletop detection failed with error code %d" %
det_res.detection.result)
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:
if det_res.detection.result == det_res.detection.SUCCESS:
rospy.loginfo("tabletop detection reports success")
break
else:
rospy.logerr("tabletop detection failed with error %s, trying again"%det_res.detection.result)
else:
rospy.logerr("tabletop detection failed too many times. Returning.")
return ([], None)
self.detected_clusters = det_res.detection.clusters
rospy.loginfo("Detected " + str(len(det_res.detection.clusters)) + " objects")
#rospy.loginfo("Detected " + str(len(col_res.graspable_objects)) + " objects")
if(len(det_res.detection.clusters) > 0):
col_req = TabletopCollisionMapProcessingRequest()
col_req.reset_collision_models = 1
col_req.reset_attached_models = 1
col_req.detection_result = det_res.detection
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 = self.collision_map_processing_srv(col_req)
except rospy.ServiceException, e:
rospy.logerr("error when calling %s: %s"%(self.collision_map_processing_name, e))
self.throw_exception()
return (None)
min_dist = float("inf")
rospy.loginfo('connected to GraspPlanning service')
segmentation_result = tabletop_segmentation_srv()
if segmentation_result.result != TabletopSegmentationResponse.SUCCESS or len(segmentation_result.clusters) == 0:
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'
print 'got object_manipulator/object_manipulator_pickup'
get_obj = rospy.ServiceProxy('/tabletop_segmentation', TabletopSegmentation)
res = get_obj()
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]
rospy.loginfo("Calling tabletop detector")
detection_call = TabletopDetectionRequest()
detection_call.return_clusters = True
detection_call.return_models = True
detection_reply = object_detection_srv.call(detection_call)
if len(detection_reply.detection.clusters) == 0:
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()
