Produces cmd_vel statements based on input entered into a text file to move a defined set of meters ahead

meters_traveled_v1.py

#!/usr/bin/env python
"""
Produces cmd_vel statements based on input entered into a text file in the format:

linear.x, angular.z, distance (meters)
0.5, 0.0, 10  <---- 0.5 m/s, go straight for 10 meters
1.5, 1.0, 15  <---- 1.5 m/s, full left? for 15 meters

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) 

usage: rosrun beginner_tutorials meters_traveled_v1.py cmd_vel_input.txt

"""
import sys
import numpy as np
import rospy
from geometry_msgs.msg import Twist
from std_msgs.msg import Float32

mission_commands = []

def load_file(argv):
    global mission_commands
    inputfile = argv
    mission_commands = np.loadtxt(inputfile, delimiter=',')

def run_commands():
    global mission_commands
    rospy.init_node('move') # first thing, init a node!
    p = rospy.Publisher('cmd_vel', Twist, queue_size=2)  # publish to cmd_vel
    twist = Twist()  # create a twist message, and insert speed and angle info
    speed_ref = 0
    turn_ref = 1
    goal_ref = 2
    sleep_time = 0.1     
    for row in mission_commands:
        distance_goal = row[goal_ref]
        data = rospy.wait_for_message('/left_meters_travelled_msg', Float32)
        starting_point_mtrs_trvld=data.data
        #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 distance_goal: %.2f starting_point_mtrs_trvld: %.2f", 
        	row[speed_ref], row[turn_ref], row[goal_ref], starting_point_mtrs_trvld)   
        goal_met = 0
        while (goal_met != 1):
            p.publish(twist)
            rospy.sleep(sleep_time)
            data = rospy.wait_for_message('/left_meters_travelled_msg', Float32)
            curnt_pos_mtrs_trvld=data.data
            distance_travelled = abs(curnt_pos_mtrs_trvld - starting_point_mtrs_trvld)           
            if distance_travelled > distance_goal:
            	rospy.loginfo("Goal achieved - current position: %.2f distance_travelled: %.2f started at: %.2f distance_goal: %.2f", 
            		curnt_pos_mtrs_trvld, distance_travelled, starting_point_mtrs_trvld, distance_goal)              	
                goal_met = 1
    #twist = Twist() # create a new message that will be blank  # commenting out - attempting to smooth out going from one command to the next
    rospy.loginfo("Stopping!")
    #p.publish(twist)

if __name__ == '__main__':
    load_file(sys.argv[1])
    run_commands()