Skip to content

Instantly share code, notes, and snippets.

@jones2126

jones2126/cmd_vel_pub.py

Last active May 18, 2020
Embed
What would you like to do?
Python script to read text file with multiple linear.x and angular.z instructions and publish for ROS cmd_vel topic
#!/usr/bin/env python
"""
Produces cmd_vel statements based on input entered into a text file in the format:
linear.x, angular.z, duration (seconds)
0.5, 0.0, 10 <---- 0.5 m/s, go straight for 10 seconds
1.5, 1.0, 15 <---- 1.5 m/s, full left? for 15 seconds
An example command to access one element of the table that is loaded: print(mission_commands[2][1]) # row 2, element 1 - remember 0 reference
The .txt file needs at least two entries otherwise you will get the error: IndexError: invalid index to scalar variable.
sleep_time sets the rate at which twist messages will be published. At 0.1 the measure Hz using rostopic hz equals 9.9 (i.e. close to 10 HZ)
Ref: http://library.isr.ist.utl.pt/docs/roswiki/mini_max(2f)Tutorials(2f)Moving(20)the(20)Base.html
Ref: https://github.com/rje1974/Proyecto-Pochito/blob/master/software/base/tractor/catkin_ws/DerechaIquierdaDerechaIzquierda.py (Juan Eduardo Riva)
"""
import numpy as np
import rospy
from geometry_msgs.msg import Twist
mission_commands = []
def load_file():
global mission_commands
filename = 'cmd_vel_input.txt'
mission_commands = np.loadtxt(filename, delimiter=',')
def run_commands():
rospy.init_node('move') # first thing, init a node!
p = rospy.Publisher('cmd_vel', Twist, queue_size=2) # publish to cmd_vel
global mission_commands
speed_ref = 0
turn_ref = 1
time_ref = 2
sleep_time = 0.1
for row in mission_commands:
duration = int(row[time_ref])
iterations = int(duration / sleep_time)
twist = Twist() # create a twist message, and insert speed and angle info
twist.linear.x = row[speed_ref]
twist.linear.y = 0
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = row[turn_ref]
rospy.loginfo("Running command linear.x: %.2f angular.z: %.2f seconds: %d", row[speed_ref], row[turn_ref], int(row[time_ref]))
for i in range(iterations):
p.publish(twist)
rospy.sleep(sleep_time)
twist = Twist() # create a new message that will be blank
rospy.loginfo("Stopping!")
p.publish(twist)
if __name__ == '__main__':
load_file()
run_commands()
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment