$search
00001 #!/usr/bin/python 00002 # Software License Agreement (BSD License) 00003 # 00004 # Copyright (c) 2009, Willow Garage, Inc. 00005 # All rights reserved. 00006 # 00007 # Redistribution and use in source and binary forms, with or without 00008 # modification, are permitted provided that the following conditions 00009 # are met: 00010 # 00011 # * Redistributions of source code must retain the above copyright 00012 # notice, this list of conditions and the following disclaimer. 00013 # * Redistributions in binary form must reproduce the above 00014 # copyright notice, this list of conditions and the following 00015 # disclaimer in the documentation and/or other materials provided 00016 # with the distribution. 00017 # * Neither the name of the Willow Garage nor the names of its 00018 # contributors may be used to endorse or promote products derived 00019 # from this software without specific prior written permission. 00020 # 00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 # POSSIBILITY OF SUCH DAMAGE. 00033 # 00034 # author: Kaijen Hsiao 00035 00036 ## @package pyexample 00037 #Offers a service to find the principal directions and bounding box of a 00038 #point cluster 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 import object_manipulator.cluster_bounding_box_finder as cluster_bounding_box_finder 00046 import scipy 00047 from object_manipulator.convert_functions import * 00048 00049 ##class for the find cluster bounding box service 00050 class ClusterBoundingBoxFinderServer: 00051 00052 def __init__(self): 00053 self.cbbf = cluster_bounding_box_finder.ClusterBoundingBoxFinder() 00054 00055 s = rospy.Service('find_cluster_bounding_box', FindClusterBoundingBox, self.find_cluster_bounding_box_callback) 00056 00057 00058 ##service callback for the find_cluster_bounding_box service 00059 def find_cluster_bounding_box_callback(self, req): 00060 rospy.loginfo("finding the bounding box for a point cluster") 00061 00062 #use PCA to find the object frame and bounding box dims 00063 (object_points, object_bounding_box_dims, object_bounding_box, object_to_base_frame, object_to_cluster_frame) = \ 00064 self.cbbf.find_object_frame_and_bounding_box(req.cluster) 00065 00066 #problem with finding bounding box 00067 if object_points == None: 00068 return FindClusterBoundingBoxResponse(PoseStamped(), Vector3(0,0,0), 1) 00069 00070 #find the frame at the center of the box, not at the bottom 00071 transform_mat = scipy.matrix([[1,0,0,0], 00072 [0,1,0,0], 00073 [0,0,1,object_bounding_box_dims[2]/2.], 00074 [0,0,0,1]]) 00075 center_mat = object_to_base_frame * transform_mat 00076 00077 #convert frame to PoseStamped 00078 pose = mat_to_pose(center_mat) 00079 pose_stamped = stamp_pose(pose, self.cbbf.base_frame) 00080 00081 #transform pose to cluster's frame_id 00082 transformed_pose_stamped = change_pose_stamped_frame(self.cbbf.tf_listener, pose_stamped, req.cluster.header.frame_id) 00083 00084 #rospy.loginfo("returning cluster bounding box response:"+str(pose_stamped)+str(object_bounding_box_dims)) 00085 return FindClusterBoundingBoxResponse(transformed_pose_stamped, Vector3(*object_bounding_box_dims), 0) 00086 00087 00088 if __name__ == '__main__': 00089 rospy.init_node('cluster_bounding_box_finder', anonymous=True) 00090 00091 cbbfs = ClusterBoundingBoxFinderServer() 00092 00093 rospy.loginfo("cluster bounding box finder is ready for queries") 00094 rospy.spin()