print_gripper_pose.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 geometry_msgs.msg import Pose, PoseStamped, Vector3
00031 from visualization_msgs.msg import InteractiveMarkerUpdate
00032 from threading import Lock
00033 from tf import TransformListener
00034 
00035 tf = None
00036 gr_lock = Lock()
00037 gr_pose = None
00038 planning_started = False
00039 
00040 def update_cb(msg):
00041     
00042     global planning_started
00043     global tf
00044     global gr_pose
00045     global gr_lock
00046     
00047     if len(msg.poses) != 1:
00048         
00049          return
00050     
00051     if msg.poses[0].name  != "MPR 0_end_control":
00052         
00053         return
00054     
00055     tmp = PoseStamped()
00056     tmp.header.stamp = rospy.Time(0)
00057     tmp.header.frame_id = msg.poses[0].header.frame_id
00058     tmp.pose = msg.poses[0].pose
00059     
00060     tf.waitForTransform('/map',msg.poses[0].header.frame_id,rospy.Time(0), rospy.Duration(1.0))
00061     tmp = tf.transformPose('/map',tmp)
00062     
00063     gr_lock.acquire()
00064     gr_pose = tmp.pose
00065     gr_lock.release()
00066     
00067     if (planning_started == False):
00068         
00069         rospy.loginfo('Planning has been started!')
00070         planning_started = True
00071 
00072 def main():
00073     
00074     global planning_started
00075     global tf
00076     global gr_pose
00077     global gr_lock
00078     
00079     rospy.init_node('print_gripper_pose_node')
00080     
00081     tf = TransformListener()
00082     
00083     bb_pose = Pose()
00084     lwh = Vector3()
00085     
00086     bb_pose.position.x = rospy.get_param("~bb/position/x")
00087     bb_pose.position.y = rospy.get_param("~bb/position/y")
00088     bb_pose.position.z = rospy.get_param("~bb/position/z")
00089     
00090     lwh.x = rospy.get_param("~bb/lwh/x")
00091     lwh.y = rospy.get_param("~bb/lwh/y")
00092     lwh.z = rospy.get_param("~bb/lwh/z")
00093     
00094     rospy.loginfo('Please start arm planning')
00095     
00096     subs = rospy.Subscriber('/planning_scene_warehouse_viewer_controls/update',InteractiveMarkerUpdate,update_cb)
00097     
00098     r = rospy.Rate(0.5)
00099     
00100     while planning_started == False:
00101         
00102         r.sleep()      
00103     
00104     raw_input("Please press enter when finished!")
00105     
00106     subs.unregister()
00107     
00108     gr_lock.acquire()
00109     
00110     gr_pose.position.x -= bb_pose.position.x
00111     gr_pose.position.y -= bb_pose.position.y
00112     gr_pose.position.z -= bb_pose.position.z
00113     
00114     gr_lock.release()
00115     
00116     print  "gripper:"
00117     print " position:"
00118     print "  x: " + str(gr_pose.position.x)
00119     print "  y: " + str(gr_pose.position.y)
00120     print "  z: " + str(gr_pose.position.z)
00121     
00122     
00123 if __name__ == '__main__':
00124   try:
00125     main()
00126   except rospy.ROSInterruptException: pass


srs_user_tests
Author(s): SRS team
autogenerated on Mon Oct 6 2014 08:53:11