Motors go full.py

Usage

Take the place of the navigation stack by publishing cmd_vel messages of type Twist. Execution is of the form:

 motors_go_full.py <linear.x> <angular.z>

It sets the linear.x value to 0.20 m/sec and the angular.z value to 0.1 radians/sec.

 roscd floor_hugger/test/
 python motors_go_full.py 0.20 0.1

Code

#!/usr/bin/env python

import sys
import rospy
from geometry_msgs.msg import Twist

def motors_go(linear_speed, angular_speed):
    print (linear_speed, angular_speed)
    vel_pub = rospy.Publisher ('cmd_vel', Twist, queue_size=10)
    rate = rospy.Rate(10)
    twist = Twist()
    twist.linear.x  = 0.0
    twist.linear.y  = 0.0
    twist.linear.z  = 0.0
    twist.angular.x = 0.0
    twist.angular.y = 0.0
    twist.angular.z = 0.0
    while not rospy.is_shutdown():
        # print (twist.linear.x)
        twist.linear.x = float(linear_speed)
        twist.angular.z = float(angular_speed)
        vel_pub.publish(twist)
        rate.sleep()
    twist.linear.x = 0.0
    vel_pub.publish(twist)
    

if __name__ == '__main__':
    print ('Motors Go')
    rospy.init_node('motor_go', anonymous=True)
    motors_go(sys.argv[1], sys.argv[2])
    rospy.spin()