00001
00002
00003 """
00004 usage: %(progname)s [--incremental] [--loop] <track_file1.txt> ...
00005 """
00006
00007 import roslib; roslib.load_manifest('articulation_models')
00008 import rospy
00009 import sys
00010 from articulation_models.msg import TrackMsg
00011 from geometry_msgs.msg import Pose, Point, Quaternion
00012 from sensor_msgs.msg import ChannelFloat32
00013 import logging
00014 import fileinput
00015 import getopt
00016
00017 def usage(progname):
00018 print __doc__ % vars()
00019
00020
00021 def talker(files,flag_incremental,flag_loop):
00022 pub = rospy.Publisher('track', TrackMsg)
00023
00024 if flag_incremental:
00025 rospy.loginfo("Publishing track incrementally, (will sleep 1s after each new pose)")
00026 else:
00027 rospy.loginfo("Publishing a single track per file, immediately")
00028 rospy.sleep(0.4)
00029
00030 iter=0
00031 while iter==0 or flag_loop:
00032
00033 for filename in files:
00034 rospy.loginfo("Opening logfile '%s'",filename)
00035
00036 msg = TrackMsg()
00037 channel_w = None
00038 channel_h = None
00039 for line in fileinput.input(filename):
00040 line = line.strip()
00041 rospy.loginfo("Reading line: '%s'",line)
00042 data = line.strip().split()
00043 data_float = [float(column) for column in data]
00044 if len(data) < 3:
00045 continue
00046
00047 msg.header.stamp = rospy.get_rostime()
00048 msg.header.frame_id = "/"
00049
00050 msg.id = 0
00051 pose = Pose( Point(0,0,0), Quaternion(0,0,0,1) )
00052
00053 if len(data) >= 3:
00054
00055 pose.position.x = data_float[0]
00056 pose.position.y = data_float[1]
00057 pose.position.z = data_float[2]
00058
00059 if len(data) >= 7:
00060
00061 pose.orientation.x = data_float[3]
00062 pose.orientation.y = data_float[4]
00063 pose.orientation.z = data_float[5]
00064 pose.orientation.w = data_float[6]
00065
00066 if len(data) >= 9:
00067 if channel_w is None or channel_h is None:
00068 channel_w = ChannelFloat32('width', [])
00069 channel_h = ChannelFloat32('height', [])
00070 msg.channels.append(channel_w)
00071 msg.channels.append(channel_h)
00072 channel_w.values.append(data_float[7])
00073 channel_h.values.append(data_float[8])
00074
00075 msg.pose.append(pose)
00076 if flag_incremental:
00077 pub.publish(msg)
00078 rospy.sleep(1.0)
00079 if rospy.is_shutdown():
00080 sys.exit()
00081 if not flag_incremental:
00082 pub.publish(msg)
00083 iter += 1
00084 if flag_loop:
00085 rospy.loginfo("Infinitely looping over file list, iteration %d",iter)
00086 rospy.sleep(1.0)
00087
00088 def main():
00089 files = []
00090
00091 optlist, files = getopt.getopt(sys.argv[1:], "", ["help", "incremental", "loop"])
00092
00093 flag_incremental = False
00094 flag_loop = False
00095 for (field, val) in optlist:
00096 if field == "--help":
00097 usage(sys.argv[0])
00098 return
00099 elif field == "--incremental":
00100 flag_incremental = True
00101 elif field == "--loop":
00102 flag_loop = True
00103
00104 if len(files)==0:
00105 usage(sys.argv[0])
00106 return
00107
00108 try:
00109 rospy.init_node('file_publisher')
00110 talker(files,flag_incremental,flag_loop)
00111 except rospy.ROSInterruptException: pass
00112
00113 if __name__ == '__main__':
00114 main()
00115
