ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
1 | initial version |
Below is a bit of python that will do what you want. I tried to make it a bit generic for you so you can extend it.
#!/usr/bin/python
import roslib; roslib.load_manifest('test')
import rospy
import sys
from geometry_msgs.msg import Twist
class Walk(object):
def __init__(self):
rospy.init_node('walk_reader_node')
self._init_params()
self._init_pubsub()
self.fh = open(self.file)
def _init_pubsub(self):
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
def _init_params(self):
self.file = rospy.get_param('~file', 'walk.dat')
def run(self):
r = rospy.Rate(10.0)
while not rospy.is_shutdown():
for line in self.fh:
l = line.split()
cmd = Twist()
cmd.linear.x = float(l[0])
cmd.angular.z = float(l[1])
self.cmd_vel_pub.publish(cmd)
r.sleep()
self.fh.close()
exit()
if __name__ == '__main__':
w = Walk()
w.run()