def find_cluster_bounding_box_center(self, cluster):
fcbb = FindClusterBoundingBoxRequest()
fcbb.cluster = cluster
res = self.find_cluster_bbox(fcbb)
if res.error_code == FindClusterBoundingBoxResponse.TF_ERROR:
rospy.logerr('Unable to find bounding box for requested point cluster')
return None
return res.pose.pose.position
def find_cluster_bounding_box_center(self, cluster):
fcbb = FindClusterBoundingBoxRequest()
fcbb.cluster = cluster
res = self.find_cluster_bbox(fcbb)
if res.error_code == FindClusterBoundingBoxResponse.TF_ERROR:
rospy.logerr(
'Unable to find bounding box for requested point cluster')
return None
return res.pose.pose.position
def call_find_cluster_bounding_box(cluster):
req = FindClusterBoundingBoxRequest()
req.cluster = cluster
service_name = "find_cluster_bounding_box"
rospy.loginfo("waiting for find_cluster_bounding_box service")
rospy.wait_for_service(service_name)
rospy.loginfo("service found")
serv = rospy.ServiceProxy(service_name, FindClusterBoundingBox)
try:
res = serv(req)
except rospy.ServiceException, e:
rospy.logerr("error when calling find_cluster_bounding_box: %s"%e)
return 0
def detect_bounding_box(self,
cluster=None,
cluster_choser="find_random_cluster"):
"""Finds the bounding box of a PointCloud. If no PointCloud is
passed then it will use self.try_to_detect() to find an object.
The resulting FindClusterBoundingBoxResponse msg is stored in
self.last_box_msg.
Example usage:
>> res = detector.detect_narrow()
#check res is not None
>> cluster = detector.find_biggest_cluster(res.detection.clusters)
#any other cluster selector is ok
>> box_msg = detector.detect_bounding_box(cluster)
Parameters:
cluster: a PointCloud msg. It can be returned by one of the detect_*
methods. If none an object will be searched for.
cluster_choser: if cluster is None, use this choser to select when
detecting. Default to find_random_cluster.
Return:
a FindClusterBoundingBoxResponse msg.
"""
try:
finder = self.__getattribute__(cluster_choser)
except AttributeError:
rospy.logwarn(
"Cluster choser %s does not exist, using the random one" %
cluster_choser)
finder = self.find_random_cluster
if cluster is None:
detection_result = self.try_to_detect()
if detection_result is None:
rospy.logerr("No way there is an object in front of me!")
self.last_box_msg = None
return None
cluster = finder(detection_result.detection.clusters)
req = FindClusterBoundingBoxRequest()
req.cluster = cluster
try:
self.last_box_msg = self.box_detector(req)
except rospy.ServiceException, e:
rospy.logerr("error when calling find_cluster_bounding_box: %s" %
e)
return None
def detect_bounding_box(self, cluster = None,
cluster_choser = "find_random_cluster"):
"""Finds the bounding box of a PointCloud. If no PointCloud is
passed then it will use self.try_to_detect() to find an object.
The resulting FindClusterBoundingBoxResponse msg is stored in
self.last_box_msg.
Example usage:
>> res = detector.detect_narrow()
#check res is not None
>> cluster = detector.find_biggest_cluster(res.detection.clusters)
#any other cluster selector is ok
>> box_msg = detector.detect_bounding_box(cluster)
Parameters:
cluster: a PointCloud or PointCloud2 msg. It can be returned by one of the detect_*
methods. If none an object will be searched for.
cluster_choser: if cluster is None, use this choser to select when
detecting. Default to find_random_cluster.
Return:
a FindClusterBoundingBoxResponse msg.
"""
try:
finder = self.__getattribute__(cluster_choser)
except AttributeError:
rospy.logwarn("Cluster choser %s does not exist, using the random one" %
cluster_choser)
finder = self.find_random_cluster
if cluster is None:
detection_result = self.try_to_detect()
if detection_result is None:
rospy.logerr("No way there is an object in front of me!")
self.last_box_msg = None
return None
cluster = finder(detection_result.detection.clusters)
req = FindClusterBoundingBoxRequest()
#using only PointCloud2 now
if type(cluster) is PointCloud:
cluster = PointCloud_to_PointCloud2(cluster)
req.cluster = cluster
try:
self.last_box_msg = self.box_detector(req)
except rospy.ServiceException, e:
rospy.logerr("error when calling find_cluster_bounding_box: %s"%e)
return None
def call_find_cluster_bounding_box(self, cluster):
req = FindClusterBoundingBoxRequest()
req.cluster = cluster
service_name = "find_cluster_bounding_box"
rospy.loginfo("waiting for find_cluster_bounding_box service")
rospy.wait_for_service(service_name)
rospy.loginfo("service found")
serv = rospy.ServiceProxy(service_name, FindClusterBoundingBox)
try:
res = serv(req)
except rospy.ServiceException, e:
rospy.logerr("error when calling find_cluster_bounding_box: %s" %
e)
return 0
def pickBook(book):
#global pullDone
#if pullDone == False:
# return
global bounding_box_srv, find_bounding_box_name
bb_req = FindClusterBoundingBoxRequest()
bb_req.cluster = book.cluster
#print 'displaying target book cloud'
#draw_pts(df,book.cluster.points)
try:
bb_res = bounding_box_srv(bb_req)
except rospy.ServiceException, e:
rospy.logerr("error when calling %s: %s"%(find_bounding_box_name, e))
sys.exit()
def pullBook(book):
if len(book.cluster.points) == 0:
rospy.loginfo('bad pullbook request, too few points')
return
rospy.loginfo('pulling book')
global pullDone
if pullDone == True:
return
global bounding_box_srv, find_bounding_box_name
bb_req = FindClusterBoundingBoxRequest()
bb_req.cluster = book.cluster
try:
bb_res = bounding_box_srv(bb_req)
except rospy.ServiceException, e:
rospy.logerr("error when calling %s: %s"%(find_bounding_box_name, e))
sys.exit()
