-
Notifications
You must be signed in to change notification settings - Fork 1
/
teleop_twist_keyboard control
147 lines (99 loc) · 2.8 KB
/
teleop_twist_keyboard control
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
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
#!/usr/bin/env python
# -*- coding: utf-8 -*
import RPi.GPIO as GPIO
import time
import numpy as np
import matplotlib.image
import matplotlib.pyplot
import math
import os
i=0;
import rospy
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
from std_msgs.msg import Float32, Float64, String
global distance_forward, distance_left, distance_right
global Kinect_ranges
global PWMOutput
GPIO.setmode(GPIO.BOARD)
Motor_rightA = 11
Motor_rightB = 13
Motor_rightE = 33
Motor_leftA = 16
Motor_leftB = 18
Motor_leftE = 32
GPIO.setup(Motor_rightA,GPIO.OUT)
GPIO.setup(Motor_rightB,GPIO.OUT)
GPIO.setup(Motor_rightE,GPIO.OUT)
GPIO.setup(Motor_leftA,GPIO.OUT)
GPIO.setup(Motor_leftB,GPIO.OUT)
GPIO.setup(Motor_leftE,GPIO.OUT)
pwm1=GPIO.PWM(33, 100)
pwm2=GPIO.PWM(32,100)
pwm1.start(0)
pwm2.start(0)
def MoveForward():
PWMOutput= 50
pwm1.ChangeDutyCycle(PWMOutput)
pwm2.ChangeDutyCycle(PWMOutput)
GPIO.output(Motor_rightA,GPIO.HIGH)
GPIO.output(Motor_rightB,GPIO.LOW)
GPIO.output(Motor_leftA,GPIO.HIGH)
GPIO.output(Motor_leftB,GPIO.LOW)
time.sleep(0.1)
def MoveBack():
PWMOutput= 50
pwm1.ChangeDutyCycle(PWMOutput)
pwm2.ChangeDutyCycle(PWMOutput)
GPIO.output(Motor_rightA,GPIO.LOW)
GPIO.output(Motor_rightB,GPIO.HIGH)
GPIO.output(Motor_leftA,GPIO.LOW)
GPIO.output(Motor_leftB,GPIO.HIGH)
time.sleep(0.1)
def MoveLeft():
GPIO.output(Motor_rightA,GPIO.HIGH)
GPIO.output(Motor_rightB,GPIO.LOW)
GPIO.output(Motor_leftA,GPIO.LOW)
GPIO.output(Motor_leftB,GPIO.HIGH)
pwm1.ChangeDutyCycle(100)
pwm2.ChangeDutyCycle(100)
time.sleep(0.1)
MoveForward()
def MoveRight():
GPIO.output(Motor_rightA,GPIO.LOW)
GPIO.output(Motor_rightB,GPIO.HIGH)
GPIO.output(Motor_leftA,GPIO.HIGH)
GPIO.output(Motor_leftB,GPIO.LOW)
pwm1.ChangeDutyCycle(100)
pwm2.ChangeDutyCycle(100)
time.sleep(0.1)
MoveForward()
def callback(msg):
lin_X= msg.linear.x
lin_Y= msg.linear.y
lin_Z= msg.linear.z
ang_X =msg.angular.x
ang_Y =msg.angular.y
ang_Z =msg.angular.z
if lin_X==0 and lin_Y==0 and lin_Z==0 and ang_X==0 and ang_Y==0 and ang_Z==$
PWMOutput = 0
pwm1.ChangeDutyCycle(PWMOutput)
pwm2.ChangeDutyCycle(PWMOutput)
elif lin_X>0 :
MoveForward()
elif ang_Z>0 and lin_X==0 :
MoveLeft()
elif ang_Z<0 and lin_X==0 :
MoveRight()
elif lin_X<0 :
MoveBack()
rate.sleep()
def listener():
rospy.init_node('teleop_control', anonymous=True)
rate=rospy.Rate(10)
rospy.Subscriber("/cmd_vel", Twist, callback)
rospy.spin()
if __name__ == '__main__':
rospy.init_node('teleop_control', anonymous=True)
rate=rospy.Rate(10)
listener()