-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathSensor Fusion Obstacle Avoidance
167 lines (113 loc) · 3.38 KB
/
Sensor Fusion Obstacle Avoidance
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
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
#!/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;
GPIO.cleanup()
global distance_forward, distance_left, distance_right
distance_left=100
distance_forward=100
distance_right=100
import rospy
from sensor_msgs.msg import LaserScan
from std_msgs.msg import Float32, Float64, String
global Kinect_ranges
Kinect_ranges=np.zeros(640)
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 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(70)
pwm2.ChangeDutyCycle(70)
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(70)
pwm2.ChangeDutyCycle(70)
time.sleep(0.1)
MoveForward()
def callback1(msg):
global Kinect_ranges
Kinect_ranges = msg.ranges
def callback2(data):
global distance_forward
distance_forward=data.data
if distance_left < 20.0:
PWMOutput = 0
pwm1.ChangeDutyCycle(PWMOutput)
pwm2.ChangeDutyCycle(PWMOutput)
def callback3(data):
global distance_left
distance_left=data.data
if distance_left < 13.0:
MoveRight()
def callback4(data):
global distance_right
distance_right=data.data
if distance_right < 13.0:
MoveLeft()
def listener():
rospy.init_node('Obstacle_Avoidance', anonymous=True)
rate=rospy.Rate(10)
rospy.Subscriber("/depth_scan", LaserScan, callback1)
rospy.Subscriber("/distance_forward", Float32 , callback2)
rospy.Subscriber("/distance_left", Float32, callback3)
rospy.Subscriber("/distance_right", Float32, callback4)
if __name__ == '__main__':
rospy.init_node('Obstacle_Avoidance', anonymous=True)
rate=rospy.Rate(10)
#GPIO.cleanup()
hello_str = "Obstacle Avoidance Node Started %s" % rospy.get_time()
rospy.loginfo(hello_str)
MoveForward()
#global Kinect_ranges
while not rospy.is_shutdown():
listener()
for i in range(210,420):
if (Kinect_ranges[i]<1) and (Kinect_ranges[i]>0):
hello_str = "Kinect_stop"
rospy.loginfo(hello_str)
PWMOutput = 0
pwm1.ChangeDutyCycle(PWMOutput)
pwm2.ChangeDutyCycle(PWMOutput)
break
else:
MoveForward()
rate.sleep()