-
Notifications
You must be signed in to change notification settings - Fork 0
/
control.py
57 lines (39 loc) · 1.2 KB
/
control.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
#!/usr/bin/env python3.6
import pygame
import rospy
from std_msgs.msg import Int16MultiArray, Float64MultiArray, String, Int32, Int32MultiArray, MultiArrayLayout, MultiArrayDimension
import os
import numpy as np
def publishData():
pub_arr_key = Int32MultiArray()
pub = rospy.Publisher('driver', Int32MultiArray, queue_size=10)
rospy.init_node('driver', anonymous=True)
rate = rospy.Rate(10)
#motor speed values
key_arr = [0,0]
while not rospy.is_shutdown():
x = input()
if x=='w':
key_arr[0] = 1
key_arr[1] = 1
elif x=='s':
key_arr[0] = -1
key_arr[1] = -1
elif x=='a':
key_arr[0] = 1
key_arr[1] = -1
elif x=='d':
key_arr[0] = -1
key_arr[1] = 1
else:
key_arr[0] = 0
key_arr[1] = 0
array_for_publishing = Int32MultiArray(data=key_arr)
rospy.loginfo(array_for_publishing)
pub.publish(array_for_publishing)
rate.sleep()
if __name__ == '__main__':
try:
publishData()
except rospy.ROSInterruptException:
pass