def detect_objects():
'''
TODO: Need to figure out the target frame in which the cluster point co-ordinates are returned.
'''
service_name = '/object_detection'
rospy.wait_for_service(service_name)
try:
table_object_detector = rospy.ServiceProxy(service_name,
TabletopDetection)
req = TabletopDetectionRequest()
req.return_clusters = False
req.return_models = True
req.num_models = 1
resp = TabletopDetectionResponse()
resp = table_object_detector(req)
if resp.detection.result == resp.detection.SUCCESS:
points = resp.detection.clusters[0].points
x_points = [point.x for point in points]
y_points = [point.y for point in points]
z_points = [point.z for point in points]
x = (min(x_points) + max(x_points)) / 2.0
y = (min(y_points) + max(y_points)) / 2.0
z = (min(z_points) + max(z_points)) / 2.0
return x, y, z
else:
print resp.detection.result
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def detect_objects(self):
rospy.loginfo("calling tabletop detection")
det_req = TabletopDetectionRequest()
det_req.return_clusters = 1
det_req.return_models = 1
#det_req.num_models = num_models
#call tabletop detection, get a detection result (attempt 3 times)
for try_num in range(3):
try:
det_res = self.grasper_detect_srv(det_req)
except rospy.ServiceException, e:
rospy.logerr("error when calling %s: %s" %
(self.grasper_detect_name, e))
self.throw_exception()
return ([], None)
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)
def __detect(self, detector):
req = TabletopDetectionRequest()
req.return_clusters = True
req.return_models = True
try:
reply = detector(req)
except rospy.ServiceException, e:
rospy.logerr("error when calling object_detection: %s" % e)
return None
def call_object_detector():
req = TabletopDetectionRequest()
req.return_clusters = 1
req.return_models = 0
service_name = "/object_detection"
rospy.loginfo("waiting for object_detection service")
rospy.wait_for_service(service_name)
serv = rospy.ServiceProxy(service_name, TabletopDetection)
try:
res = serv(req)
except rospy.ServiceException, e:
rospy.logerr("error when calling object_detection: %s" % e)
return 0
def call_tabletop_object_detection(self):
rospy.loginfo("object detection service call.")
det_req = TabletopDetectionRequest()
det_req.return_clusters = 1
det_req.return_models = 1
det_req.num_models = 0
#call tabletop detection, get a detection result
for try_num in range(3):
try:
det_res = self.detect_srv(det_req)
except rospy.ServiceException, e:
rospy.logerr("error when calling %s: %s" %
("/object_detection", e))
self.throw_exception()
return ([], None)
if det_res.detection.result == det_res.detection.SUCCESS:
rospy.loginfo("tabletop detection reports success")
break
else:
rospy.logerr(
"tabletop detection failed with error. Trying again")
rospy.loginfo("waiting for object_detection service")
rospy.wait_for_service(grasper_detect_name)
rospy.loginfo("waiting for collision_map_processing service")
rospy.wait_for_service(collision_map_processing_name)
# create service proxies
grasper_detect_srv = rospy.ServiceProxy(grasper_detect_name,
TabletopDetection)
collision_map_processing_srv = rospy.ServiceProxy(
collision_map_processing_name, TabletopCollisionMapProcessing)
'''
object detection
'''
rospy.loginfo("detecting objects...")
# set up detection request
det_req = TabletopDetectionRequest()
# we want recognized database objects returned, set this to 0 if using the pipeline without the database
det_req.return_models = 1
# we want the individual object point clouds returned as well
det_req.return_clusters = 1
# call tabletop detection, get a detection result
try:
det_res = grasper_detect_srv(det_req)
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" %
collision_processing_srv = rospy.ServiceProxy(
COLLISION_PROCESSING_SERVICE_NAME, TabletopCollisionMapProcessing)
#pickup client
pickup_client = actionlib.SimpleActionClient(PICKUP_ACTION_NAME,
PickupAction)
while not pickup_client.wait_for_server(rospy.Duration(2.0)):
rospy.loginfo("Witing for action client %s", PICKUP_ACTION_NAME)
#place client
place_client = actionlib.SimpleActionClient(PLACE_ACTION_NAME, PlaceAction)
while not place_client.wait_for_server(rospy.Duration(2.0)):
rospy.loginfo("Witing for action client %s", PICKUP_ACTION_NAME)
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()
