pr2_pickup_object_demo: plot_link

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # set pr2 to a pose for grasping
00004 
00005 PKG = 'pr2_pickup_object_demo'
00006 
00007 import roslib; roslib.load_manifest(PKG)
00008 import sys
00009 import rospy
00010 import os
00011 import numpy
00012 import time
00013 import rxtools.rxplot_main
00014 from sensor_msgs.msg import LaserScan, Image, PointCloud
00015 from gazebo.msg import *
00016 from gazebo.srv import *
00017 from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Wrench, Vector3
00018 
00019 def getPlotNames(link_states):
00020   global topic_list, legends, got_topic, print_list
00021   if not got_topic:
00022     got_topic = True
00023     legends = ''
00024 
00025     for j in print_list:
00026       name = link_states.name[int(j)]
00027       sub_topics = '\"/gazebo/link_states/pose['+j+']/position/x\" \"/gazebo/link_states/pose['+j+']/position/y\" \"/gazebo/link_states/pose['+j+']/position/z\"'
00028       if legends == '':
00029         legends = name + "_x, " + name + "_y, " + name + "_z" 
00030         topic_list = sub_topics
00031       else:
00032         legends = legends + "," + name + "_x, " + name + "_y, " + name + "_z" 
00033         topic_list = topic_list + " " + sub_topics
00034       rospy.loginfo("name[%d]: %s"%(int(j), name))
00035       rospy.loginfo("topic[%d]: %s"%(int(j), sub_topics))
00036       #rospy.loginfo("names: %s"%legends)
00037 
00038 
00039 if __name__ == '__main__':
00040   global topic_list, legends, got_topic, print_list
00041 
00042   print_list = sys.argv[1:]
00043   print 'argv: ',print_list
00044 
00045   topic_list = ''
00046   legends = ''
00047   got_topic = False
00048 
00049   rospy.init_node('rxplot',anonymous=True)
00050   rospy.Subscriber("/gazebo/link_states",LinkStates, getPlotNames)
00051 
00052   while (not got_topic):
00053     time.sleep(1)
00054 
00055   #print "topic_list: ",topic_list
00056   #print "legends:    ",legends
00057 
00058   cmd = 'rxplot '+topic_list+' -l \"'+legends+'\" '
00059   print "\ncmd: ",cmd,'\n'
00060   os.system(cmd)
00061