cluster_bounding_box_finder_server.py
Go to the documentation of this file.
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                    FindClusterBoundingBox2, FindClusterBoundingBox2Response
00046 import object_manipulator.cluster_bounding_box_finder as cluster_bounding_box_finder
00047 import object_manipulator.cluster_bounding_box_finder_3d as cluster_bounding_box_finder_3d
00048 import scipy
00049 from object_manipulator.convert_functions import *
00050 import tf
00051 
00052 ##class for the find cluster bounding box service
00053 class ClusterBoundingBoxFinderServer:
00054 
00055     def __init__(self, tf_listener = None):
00056         if tf_listener == None:
00057             self.tf_listener = tf.TransformListener()
00058         else:
00059             self.tf_listener = tf_listener
00060         self.cbbf = cluster_bounding_box_finder.ClusterBoundingBoxFinder(self.tf_listener)
00061         self.cbbf3d = cluster_bounding_box_finder_3d.ClusterBoundingBoxFinder3D(self.tf_listener)
00062 
00063         #services for PointCloud
00064         rospy.Service('find_cluster_bounding_box', FindClusterBoundingBox, self.find_cluster_bounding_box_callback)
00065         rospy.Service('find_cluster_bounding_box_3d', FindClusterBoundingBox, self.find_cluster_bounding_box_3d_callback)
00066 
00067         #services for PointCloud2
00068         rospy.Service('find_cluster_bounding_box2', FindClusterBoundingBox2, self.find_cluster_bounding_box2_callback)
00069         rospy.Service('find_cluster_bounding_box2_3d', FindClusterBoundingBox2, self.find_cluster_bounding_box2_3d_callback)
00070 
00071         
00072     ##service callback for the find_cluster_bounding_box service
00073     def find_cluster_bounding_box_callback(self, req):
00074         rospy.loginfo("finding the bounding box for a point cluster")
00075         (box_pose, box_dims, error) = self.find_cluster_bounding_box(req)
00076         return FindClusterBoundingBoxResponse(box_pose, box_dims, error)
00077 
00078 
00079     ##service callback for the find_cluster_bounding_box2 service
00080     def find_cluster_bounding_box2_callback(self, req):
00081         rospy.loginfo("finding the bounding box for a PointCloud2 point cluster")
00082         (box_pose, box_dims, error) = self.find_cluster_bounding_box(req)
00083         return FindClusterBoundingBox2Response(box_pose, box_dims, error)
00084 
00085 
00086     ##service callback for the find_cluster_bounding_box service
00087     def find_cluster_bounding_box_3d_callback(self, req):
00088         rospy.loginfo("finding the bounding box for a point cluster (3D)")
00089         (box_pose, box_dims, error) = self.find_cluster_bounding_box_3d(req)
00090         return FindClusterBoundingBoxResponse(box_pose, box_dims, error)
00091 
00092 
00093     ##service callback for the find_cluster_bounding_box2 service
00094     def find_cluster_bounding_box2_3d_callback(self, req):
00095         rospy.loginfo("finding the bounding box for a PointCloud2 point cluster (3D)")
00096         (box_pose, box_dims, error) = self.find_cluster_bounding_box_3d(req)
00097         return FindClusterBoundingBox2Response(box_pose, box_dims, error)
00098 
00099 
00100     ##find the cluster bounding box with the z-axis in the z-up-frame being fixed
00101     def find_cluster_bounding_box(self, req):
00102 
00103         #use PCA to find the object frame and bounding box dims
00104         (object_points, object_bounding_box_dims, object_bounding_box, object_to_base_frame, object_to_cluster_frame) = \
00105                            self.cbbf.find_object_frame_and_bounding_box(req.cluster)
00106 
00107         #problem with finding bounding box
00108         if object_points == None:
00109             return (PoseStamped(), Vector3(0,0,0), 1)
00110 
00111         #find the frame at the center of the box, not at the bottom
00112         transform_mat = scipy.matrix([[1,0,0,0],
00113                                       [0,1,0,0],
00114                                       [0,0,1,object_bounding_box_dims[2]/2.],
00115                                       [0,0,0,1]])
00116         center_mat = object_to_base_frame * transform_mat
00117 
00118         #convert frame to PoseStamped
00119         pose = mat_to_pose(center_mat)
00120         pose_stamped = stamp_pose(pose, self.cbbf.base_frame)
00121 
00122         #transform pose to cluster's frame_id
00123         transformed_pose_stamped = change_pose_stamped_frame(self.tf_listener, pose_stamped, req.cluster.header.frame_id)
00124 
00125         #rospy.loginfo("returning cluster bounding box response:"+str(pose_stamped)+str(object_bounding_box_dims))
00126         return (transformed_pose_stamped, Vector3(*object_bounding_box_dims), 0)
00127 
00128 
00129     ##find the cluster bounding box without any special fixed axes
00130     def find_cluster_bounding_box_3d(self, req):
00131 
00132         #use PCA to find the object frame and bounding box dims
00133         (object_points, object_bounding_box_dims, object_bounding_box, object_to_base_frame, object_to_cluster_frame) = \
00134                            self.cbbf3d.find_object_frame_and_bounding_box(req.cluster)
00135 
00136         #problem with finding bounding box
00137         if object_points == None:
00138             return (PoseStamped(), Vector3(0,0,0), 1)
00139 
00140         center_mat = object_to_cluster_frame 
00141 
00142         #convert frame to PoseStamped
00143         pose = mat_to_pose(center_mat)
00144         pose_stamped = stamp_pose(pose, req.cluster.header.frame_id)
00145 
00146         #rospy.loginfo("returning cluster bounding box response:"+str(pose_stamped)+"\n"+str(object_bounding_box_dims))
00147         return (pose_stamped, Vector3(*object_bounding_box_dims), 0)
00148 
00149 
00150 if __name__ == '__main__':
00151     rospy.init_node('cluster_bounding_box_finder', anonymous=True)
00152 
00153     cbbfs = ClusterBoundingBoxFinderServer()
00154     
00155     rospy.loginfo("cluster bounding box finder is ready for queries")
00156     rospy.spin()


object_manipulator
Author(s): Matei Ciocarlie and Kaijen Hsiao
autogenerated on Thu Jan 2 2014 11:39:04