00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040 from __future__ import division
00041 import roslib
00042 roslib.load_manifest('object_manipulator')
00043 import rospy
00044 from object_manipulation_msgs.srv import FindClusterBoundingBox, FindClusterBoundingBoxResponse, \
00045 FindClusterBoundingBox2, FindClusterBoundingBox2Response
00046 import object_manipulator.cluster_bounding_box_finder as cluster_bounding_box_finder
00047 import scipy
00048 from object_manipulator.convert_functions import *
00049
00050
00051 class ClusterBoundingBoxFinderServer:
00052
00053 def __init__(self):
00054 self.cbbf = cluster_bounding_box_finder.ClusterBoundingBoxFinder()
00055
00056
00057 s = rospy.Service('find_cluster_bounding_box', FindClusterBoundingBox, self.find_cluster_bounding_box_callback)
00058
00059
00060 s = rospy.Service('find_cluster_bounding_box2', FindClusterBoundingBox2, self.find_cluster_bounding_box2_callback)
00061
00062
00063
00064 def find_cluster_bounding_box_callback(self, req):
00065 rospy.loginfo("finding the bounding box for a point cluster")
00066 (box_pose, box_dims, error) = self.find_cluster_bounding_box(req)
00067 return FindClusterBoundingBoxResponse(box_pose, box_dims, error)
00068
00069
00070
00071 def find_cluster_bounding_box2_callback(self, req):
00072 rospy.loginfo("finding the bounding box for a point cluster")
00073 (box_pose, box_dims, error) = self.find_cluster_bounding_box(req)
00074 return FindClusterBoundingBox2Response(box_pose, box_dims, error)
00075
00076
00077
00078 def find_cluster_bounding_box(self, req):
00079
00080
00081 (object_points, object_bounding_box_dims, object_bounding_box, object_to_base_frame, object_to_cluster_frame) = \
00082 self.cbbf.find_object_frame_and_bounding_box(req.cluster)
00083
00084
00085 if object_points == None:
00086 return (PoseStamped(), Vector3(0,0,0), 1)
00087
00088
00089 transform_mat = scipy.matrix([[1,0,0,0],
00090 [0,1,0,0],
00091 [0,0,1,object_bounding_box_dims[2]/2.],
00092 [0,0,0,1]])
00093 center_mat = object_to_base_frame * transform_mat
00094
00095
00096 pose = mat_to_pose(center_mat)
00097 pose_stamped = stamp_pose(pose, self.cbbf.base_frame)
00098
00099
00100 transformed_pose_stamped = change_pose_stamped_frame(self.cbbf.tf_listener, pose_stamped, req.cluster.header.frame_id)
00101
00102
00103 return (transformed_pose_stamped, Vector3(*object_bounding_box_dims), 0)
00104
00105
00106 if __name__ == '__main__':
00107 rospy.init_node('cluster_bounding_box_finder', anonymous=True)
00108
00109 cbbfs = ClusterBoundingBoxFinderServer()
00110
00111 rospy.loginfo("cluster bounding box finder is ready for queries")
00112 rospy.spin()