print_bb.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 ###############################################################################
00003 # \file
00004 #
00005 # $Id:$
00006 #
00007 # Copyright (C) Brno University of Technology
00008 #
00009 # This file is part of software developed by dcgm-robotics@FIT group.
00010 # 
00011 # Author: Zdenek Materna (imaterna@fit.vutbr.cz)
00012 # Supervised by: Michal Spanel (spanel@fit.vutbr.cz)
00013 # Date: dd/mm/2012
00014 #
00015 # This file is free software: you can redistribute it and/or modify
00016 # it under the terms of the GNU Lesser General Public License as published by
00017 # the Free Software Foundation, either version 3 of the License, or
00018 # (at your option) any later version.
00019 # 
00020 # This file is distributed in the hope that it will be useful,
00021 # but WITHOUT ANY WARRANTY; without even the implied warranty of
00022 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00023 # GNU Lesser General Public License for more details.
00024 # 
00025 # You should have received a copy of the GNU Lesser General Public License
00026 # along with this file.  If not, see <http://www.gnu.org/licenses/>.
00027 #
00028 import roslib; roslib.load_manifest('srs_user_tests')
00029 import rospy
00030 from srs_interaction_primitives.srv import GetUnknownObject, AddUnknownObject, RemovePrimitive
00031 from geometry_msgs.msg import Pose, Vector3
00032 from srs_interaction_primitives.msg import PoseType
00033 
00034 def main():
00035     
00036     rospy.init_node('print_bb_node')
00037     
00038     s_get = rospy.ServiceProxy("/interaction_primitives/get_unknown_object", GetUnknownObject)
00039     s_add = rospy.ServiceProxy("/interaction_primitives/add_unknown_object", AddUnknownObject)
00040     s_remove = rospy.ServiceProxy("/interaction_primitives/remove_primitive", RemovePrimitive)
00041     
00042     rospy.wait_for_service("/interaction_primitives/get_unknown_object")
00043     rospy.wait_for_service("/interaction_primitives/add_unknown_object")
00044     rospy.wait_for_service("/interaction_primitives/remove_primitive")
00045     
00046     bb_pose = Pose()
00047     
00048     bb_pose.position.x = rospy.get_param("~bb/position/x")
00049     bb_pose.position.y = rospy.get_param("~bb/position/y")
00050     bb_pose.position.z = rospy.get_param("~bb/position/z")
00051     
00052     bb_pose.orientation.x = rospy.get_param("~bb/orientation/x")
00053     bb_pose.orientation.y = rospy.get_param("~bb/orientation/y")
00054     bb_pose.orientation.z = rospy.get_param("~bb/orientation/z")
00055     bb_pose.orientation.w = rospy.get_param("~bb/orientation/w")
00056     
00057     lwh = Vector3()
00058     
00059     lwh.x = 2*rospy.get_param("~bb/lwh/x")
00060     lwh.y = 2*rospy.get_param("~bb/lwh/y")
00061     lwh.z = rospy.get_param("~bb/lwh/z")
00062     
00063     try:
00064             
00065       ret = s_add(frame_id= "/map",
00066                   name="unknown_object",
00067                   description="",
00068                   pose_type = PoseType.POSE_BASE,
00069                   pose = bb_pose,
00070                   scale = lwh,
00071                   disable_material=True)
00072             
00073     except Exception, e:
00074         
00075       rospy.logerr('Error on calling service: %s',str(e))
00076       
00077       
00078     raw_input("Please press enter when finished!")
00079     
00080     ret = None
00081     
00082     try:
00083             
00084       ret = s_get(name= "unknown_object")
00085             
00086     except Exception, e:
00087         
00088       rospy.logerr('Error on calling service: %s',str(e))
00089       
00090     print "bb:"
00091     print " position:"
00092     print "  x: " + str(ret.pose.position.x)
00093     print "  y: " + str(ret.pose.position.y)
00094     print "  z: " + str(ret.pose.position.z - (ret.scale.z/2.0))
00095     #print "  z: " + str(ret.pose.position.z)
00096     
00097     print " orientation:"
00098     print "  x: " + str(ret.pose.orientation.x)
00099     print "  y: " + str(ret.pose.orientation.y)
00100     print "  z: " + str(ret.pose.orientation.z)
00101     print "  w: " + str(ret.pose.orientation.w)
00102     
00103     print " lwh:"
00104     print "  x: " + str(ret.scale.x/2.0)
00105     print "  y: " + str(ret.scale.y/2.0)
00106     print "  z: " + str(ret.scale.z)
00107     
00108     
00109     try:
00110             
00111       ret = s_remove(name = "unknown_object")
00112             
00113     except Exception, e:
00114         
00115       rospy.logerr('Error on calling service: %s',str(e)) 
00116     
00117 if __name__ == '__main__':
00118   try:
00119     main()
00120   except rospy.ROSInterruptException: pass


srs_user_tests
Author(s): SRS team
autogenerated on Sun Jan 5 2014 12:14:05