Motors go full.py

From wikidb
Revision as of 13:16, 20 September 2017 by Edc (Talk | contribs)

(diff) ← Older revision | Latest revision (diff) | Newer revision → (diff)
Jump to: navigation, search
#!/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()