math.sin(lat) * math.cos(latend) * math.cos(lonend - lon)
return (math.degrees(math.atan2(y, x)) + 360) % 360
# a function, which returns a distance between two polar coordinates
def poldist(th1, r1, th2, r2):
d = math.sqrt(r1 * r1 + r2 * r2 - 2 * r1 * r2 *
math.cos(th2 / 180 * math.pi - th1 / 180 * math.pi))
return d
try:
print 'activity 0'
linespol = rplidar.getlines()
gpsh = gpsheading() # heading towards the destination point
cmpsh = cmps11.heading() # compass heading
print 'linespol: ', linespol
print 'gpsh: ', gpsh
print 'cmpsh: ', cmpsh
# if farer than 1.21m from the destination point
while (gpsdist() > 1.21):
# activity 0 analyzes rplidar data and plans the path
if (activity == 0):
# state 0 checks if there is an obsticle on the line to the
# destination point
if (state == 0):
print 'state 0'
print 'lncnt: ', lncnt
# if there are no obsticles in the 7m radius
if (len(linespol) == 0):
activity = 1
def run(self):
# gps thread to get latest frames of gps data
gpsp = gpsdthrd.gpspol()
gpsd = gpsdthrd.gpsd
gpsp.start()
gpscnt = 0
# while gpsd.fix.longitude==0:
# if (gpscnt==0):
# if (gpscnt==0): gpscnt+=1
# print 'waiting for gps'
print 'gps ready'
srffr = srf08.srf08(0x72)
srffc = srf08.srf08(0x71)
srffl = srf08.srf08(0x70)
srfrr = srf08.srf08(0x73)
srfrc = srf08.srf08(0x74)
srfrl = srf08.srf08(0x75)
lon = 0
lat = 0
cmpsh = 0
HOST = '192.168.2.100'
PORT = 60000
s = socket.socket()
s.connect((HOST, PORT))
while self.running:
# GPS data
# print 'time: ',time.time()
lon = gpsd.fix.longitude
lat = gpsd.fix.latitude
if (str(lon) == 'nan'):
lon = 0.0
lat = 0.0
# print 'lat: ',lat,' lon: ',lon
s.sendall("WRITE.GPS,LAT:" + str(lat) + ",LON:" + str(lon) + ".")
# CMPS11 data
cmpsh = cmps11.heading()
# print 'cmps: ',cmpsh
s.sendall("WRITE.COMPASS," + str(cmpsh) + ".")
# SRF08 data
srfrr.doranging()
srffrm = srffr.getranging()
srffcm = srffc.getranging()
srfflm = srffl.getranging()
srfrrm = srfrr.getranging()
srfrcm = srfrc.getranging()
srfrlm = srfrl.getranging()
# print
# "FL:",srfflm,"FC:",srffcm,"FR:",srffrm,"RL:",srfrlm,"RC:",srfrcm,"RR:",srfrrm
s.sendall("WRITE.RANGEFINDERS,FL:" + str(srfflm) + ",FC:" + str(srffcm) + ",FR:" + str(srffrm) +
",RL:" + str(srfrlm) + ",RC:" + str(srfrcm) + ",RR:" + str(srfrrm) + ".")
gpsp.running = False
gpsp.join()
print 'gpsp closed'
s.close()
def getlines():
dots = [] # used to store data from 'lidardata.csv' file
lines = [] # stores detected lines in cartesian coordinates
linespol = [] # stores detected lines in polar coordinates
dotsnumb = 0
lines.append([0, 0, 0, 0]) # creates an empty line
lncnt = 0
dtcnt = 0
# parameter(cm), identifies the max distance between two dots for them to
# be considered to be in the same line
gap = 20.0
#curvrear = 4.0
# parameter(degrees), identifies the angle deviation from the line to
# create an area, where if a dot is found
curvfront = 40.0
# in that area it is considered to be a part of the line
anglest = 0
anglend = 0
fx = 0
getdots()
cmpshd = cmps11.heading() # gets a current heading from compass
# converts lidar data from polar to rectangular coordinates and stores in
# 'dots' list
with open('/home/pi/Desktop/Autonomy/lidardata.csv', 'rb') as lidar:
readldr = csv.reader(lidar)
for row in readldr:
r = row[1]
ph = row[0]
dotsnumb = dotsnumb + 1
# note that length is converted from mm to cm
z = cmath.rect(float(r) / 10, float(ph) / 180 * cmath.pi)
dots.append([z.real, z.imag])
# loops through a list of dots and groups them into lines based on 'gap'
# and 'curvfront' parameters
for x in xrange(dotsnumb - 1):
# if a current row in 'lines' is empty
if (lines[lncnt][0] == 0):
if (dist(dots[dtcnt][0], dots[dtcnt][1], dots[dtcnt + 1][0], dots[dtcnt + 1][1]) < gap):
# starts a new line with a fixed start which is the current dot and end is the next dot,
# which may or may not be changed later based on the next dots
# (not)being in the same line
lines[lncnt][0] = dots[dtcnt][0]
lines[lncnt][1] = dots[dtcnt][1]
lines[lncnt][2] = dots[dtcnt + 1][0]
lines[lncnt][3] = dots[dtcnt + 1][1]
dtcnt = dtcnt + 1
else:
# creates a line, which consists of a single dot
# creates a new empty row in 'lines'
lines[lncnt][0] = dots[dtcnt][0]
lines[lncnt][1] = dots[dtcnt][1]
lines[lncnt][2] = dots[dtcnt][0]
lines[lncnt][3] = dots[dtcnt][1]
lines.append([0, 0, 0, 0])
lncnt = lncnt + 1
dtcnt = dtcnt + 1
else:
# creates a vector using the current dot, which is also a end of the current line, and previous dot, which is
# a part of the current line.
vect = ([dots[dtcnt][0] - dots[dtcnt - 1][0],
dots[dtcnt][1] - dots[dtcnt - 1][1]])
unitvct = ([vect[0] / dist(0, 0, vect[0], vect[1]),
vect[1] / dist(0, 0, vect[0], vect[1])])
# creates a vector with the length equal to 'gap' parameter
gapvct = ([unitvct[0] * gap, unitvct[1] * gap])
gapvctpol = cmath.polar(gapvct[0] + gapvct[1] * 1j)
# creates two vectors of the same length as gapvctpol, which are
# spread from it by +-'curvfront' degrees
zboundhigh = cmath.rect(
gapvctpol[0], (gapvctpol[1] + curvfront / 180 * cmath.pi))
zboundlow = cmath.rect(
gapvctpol[0], (gapvctpol[1] - curvfront / 180 * cmath.pi))
# move vectors to current line end coordinates, which can be viewed as two lines starting from the current line end
# and terminating at boundhigh and boundlow, creating an angle of
# (2*curvfront)
boundhigh = ([zboundhigh.real + dots[dtcnt][0],
zboundhigh.imag + dots[dtcnt][1]])
boundlow = ([zboundlow.real + dots[dtcnt][0],
zboundlow.imag + dots[dtcnt][1]])
# check if the next dot is in the area created by the above described lines using the formula:
# d = (x-x1)(y2-y1)-(y-y1)(x2-x1), where (x,y) is a dot and (x1,y1,x2,y2) is a line and d shows on which side
# of the line the dot is
linehigh = (dots[dtcnt + 1][0] - dots[dtcnt][0]) * (boundhigh[1] - dots[dtcnt][1]) -\
(dots[dtcnt + 1][1] - dots[dtcnt][1]) * \
(boundhigh[0] - dots[dtcnt][0])
linelow = (dots[dtcnt + 1][0] - dots[dtcnt][0]) * (boundlow[1] - dots[dtcnt][1]) -\
(dots[dtcnt + 1][1] - dots[dtcnt][1]) * \
(boundlow[0] - dots[dtcnt][0])
# here the code can be added which introduces the use of 'curvrear'
# parameter
# if the next dot is in the above described area then the current line end's coordinates are replaced by
# the next dote coordinates, so the next dot becomes the end of the
# current line
if ((((linehigh < 0 and linelow > 0) or (linehigh > 0 and linelow < 0)) and
dist(dots[dtcnt][0], dots[dtcnt][1], dots[dtcnt + 1][0], dots[dtcnt + 1][1]) < gap) or
dist(dots[dtcnt][0], dots[dtcnt][1], dots[dtcnt + 1][0], dots[dtcnt + 1][1]) < 2):
lines[lncnt][2] = dots[dtcnt + 1][0]
lines[lncnt][3] = dots[dtcnt + 1][1]
dtcnt = dtcnt + 1
# otherwise the current line stays as it is, and a new empty line
# is created
else:
lines.append([0, 0, 0, 0])
lncnt = lncnt + 1
dtcnt = dtcnt + 1
# checks if the first and the last lines are in the same line and links them if true, in which case the last line
# is discarded when lines are converted to linespol by using 'fx' parameter
# also checks if the last line is empty, ignors it if true and checks if the first line and
# the line before the empty line are in the same line, links them if true, in which case the last two lines are
# discarded, if false then only empty line is discarded
if (lines[len(lines) - 1][0] != 0):
# check if the last line is not a dot, since vector of a dot is 0, then
# apply the same process as above
if (lines[len(lines) - 1][2] != lines[len(lines) - 1][0] and lines[len(lines) - 1][3] != lines[len(lines) - 1][1]):
vect = ([lines[len(lines) - 1][2] - lines[len(lines) - 1][0],
lines[len(lines) - 1][3] - lines[len(lines) - 1][1]])
unitvct = ([vect[0] / dist(0, 0, vect[0], vect[1]),
vect[1] / dist(0, 0, vect[0], vect[1])])
gapvct = ([unitvct[0] * gap, unitvct[1] * gap])
gapvctpol = cmath.polar(gapvct[0] + gapvct[1] * 1j)
zboundhigh = cmath.rect(
gapvctpol[0], (gapvctpol[1] + curvfront / 180 * cmath.pi))
zboundlow = cmath.rect(
gapvctpol[0], (gapvctpol[1] - curvfront / 180 * cmath.pi))
boundhigh = ([zboundhigh.real + lines[len(lines) - 1]
[2], zboundhigh.imag + lines[len(lines) - 1][3]])
boundlow = ([zboundlow.real + lines[len(lines) - 1][2],
zboundlow.imag + lines[len(lines) - 1][3]])
linehigh = (lines[0][0] - lines[len(lines) - 1][2]) * (boundhigh[1] - lines[len(lines) - 1][3]) -\
(lines[0][1] - lines[len(lines) - 1][3]) * \
(boundhigh[0] - lines[len(lines) - 1][2])
linelow = (lines[0][0] - lines[len(lines) - 1][2]) * (boundlow[1] - lines[len(lines) - 1][3]) -\
(lines[0][1] - lines[len(lines) - 1][3]) * \
(boundlow[0] - lines[len(lines) - 1][2])
if ((((linehigh < 0 and linelow > 0) or (linehigh > 0 and linelow < 0)) and
dist(lines[len(lines) - 1][2], lines[len(lines) - 1][3], lines[0][0], lines[0][1]) < gap) or
dist(lines[len(lines) - 1][2], lines[len(lines) - 1][3], lines[0][0], lines[0][1]) < 2):
lines[0][0] = lines[len(lines) - 1][0]
lines[0][1] = lines[len(lines) - 1][1]
fx = 1
else:
fx = 0
cntt = 0
# the last line is empty
else:
if (lines[len(lines) - 2][2] != lines[len(lines) - 2][0] and lines[len(lines) - 2][3] != lines[len(lines) - 2][1]):
vect = ([lines[len(lines) - 2][2] - lines[len(lines) - 2][0],
lines[len(lines) - 2][3] - lines[len(lines) - 2][1]])
unitvct = ([vect[0] / dist(0, 0, vect[0], vect[1]),
vect[1] / dist(0, 0, vect[0], vect[1])])
gapvct = ([unitvct[0] * gap, unitvct[1] * gap])
gapvctpol = cmath.polar(gapvct[0] + gapvct[1] * 1j)
zboundhigh = cmath.rect(
gapvctpol[0], (gapvctpol[1] + curvfront / 180 * cmath.pi))
zboundlow = cmath.rect(
gapvctpol[0], (gapvctpol[1] - curvfront / 180 * cmath.pi))
boundhigh = ([zboundhigh.real + lines[len(lines) - 2]
[2], zboundhigh.imag + lines[len(lines) - 2][3]])
boundlow = ([zboundlow.real + lines[len(lines) - 2][2],
zboundlow.imag + lines[len(lines) - 2][3]])
linehigh = (lines[0][0] - lines[len(lines) - 2][2]) * (boundhigh[1] - lines[len(lines) - 2][3]) -\
(lines[0][1] - lines[len(lines) - 2][3]) * \
(boundhigh[0] - lines[len(lines) - 2][2])
linelow = (lines[0][0] - lines[len(lines) - 2][2]) * (boundlow[1] - lines[len(lines) - 2][3]) -\
(lines[0][1] - lines[len(lines) - 2][3]) * \
(boundlow[0] - lines[len(lines) - 2][2])
if ((((linehigh < 0 and linelow > 0) or (linehigh > 0 and linelow < 0)) and
dist(lines[len(lines) - 2][2], lines[len(lines) - 2][3], lines[0][0], lines[0][1]) < gap) or
dist(lines[len(lines) - 2][2], lines[len(lines) - 2][3], lines[0][0], lines[0][1]) < 2):
lines[0][0] = lines[len(lines) - 2][0]
lines[0][1] = lines[len(lines) - 2][1]
fx = 2
else:
fx = 1
else:
fx = 1
# converts from cartesian (lines) to polar (linespol) coordinates
for row in xrange(len(lines) - fx):
polstart = cmath.polar(lines[row][0] + lines[row][1] * 1j)
polend = cmath.polar(lines[row][2] + lines[row][3] * 1j)
startth = polstart[1]
endth = polend[1]
# check if line's start and end angles are not inside the 0-360 degrees range
# adjust it to that range and add compass heading to put the linespol
# in compass domain
if (startth / math.pi * 180 < 0):
startth = startth + math.pi * 2
if (startth / math.pi * 180 + cmpshd >= 360):
anglest = startth / math.pi * 180 + cmpshd - 360
else:
anglest = startth / math.pi * 180 + cmpshd
if (endth / math.pi * 180 < 0):
endth = polend[1] + math.pi * 2
if (endth / math.pi * 180 + cmpshd >= 360):
anglend = endth / math.pi * 180 + cmpshd - 360
else:
anglend = endth / math.pi * 180 + cmpshd
linespol.append([round(anglest, 4), round(polstart[0], 4),
round(anglend, 4), round(polend[0], 4)])
print 'len(lines): ', len(lines)
print 'len(linespol): ', len(linespol)
# drawing the lines obtained
#img = cv2.imread('lidarimg.jpg')
# for x1,y1,x2,y2 in lines:
# cv2.line(img,(int(x1)+700,int(y1)+700),(int(x2)+700,int(y2)+700),(0,0,255),1)
# cv2.imwrite('myfunctimg.jpg',img)
return linespol
def objdet():
srffr = srf08.srf08(0x72)
srffc = srf08.srf08(0x71)
srffl = srf08.srf08(0x70)
srfrr = srf08.srf08(0x73)
srfrc = srf08.srf08(0x74)
srfrl = srf08.srf08(0x75)
leftf = md03.md03(0x58)
leftr = md03.md03(0x5A)
rightf = md03.md03(0x5B)
rightr = md03.md03(0x59)
accel = 15
rotsp = 130
cmpsh = 0
linespol = []
firstcnt = 0
cnt = 0
objsize = 41 # defines the size(cm) of an object to look for
objtol = 3.7 # introduces tolerance parameter in cm
drivedist = 0
driveangle = 0
turnang = 0
turnangfx = 17 # defines an angle offset from the center of an object
stopdist = 37 # defines how far(cm) from an object the robot stops
try:
linespol = rplidar.getlines()
print linespol
if (len(linespol) != 0):
for lines in linespol:
# looks for objects
if (poldist(lines[0], lines[1], lines[2], lines[3]) >= objsize - objtol and
poldist(lines[0], lines[1], lines[2], lines[3]) <= objsize + objtol):
cnt += 1 # counts the number of objects
firstcnt += 1
# remembers the first object location
if (firstcnt == 1):
firstcnt += 1 # won't enter this if again
print 'turnang line: ', lines
# calculates an angle to face the center of an object
if (lines[2] - lines[0] < 0):
turnang = lines[2] - \
(lines[2] - lines[0] + 360) / 2
else:
turnang = lines[2] - (lines[2] - lines[0]) / 2
# creates an offset from the center of an object so that when the robot
# turns to that angle and gets rplidar data the first
# object will be this one
if (turnang - turnangfx < 0):
turnang = turnang - turnangfx + 360
else:
turnang = turnang - turnangfx
# check all the objects found
for x in xrange(0, cnt):
print 'turnang: ', turnang
cmpsh = cmps11.heading()
# the robot turns until the heading is turnang
if (turnang - cmpsh > 180):
cmpsh += 360
elif (turnang - cmpsh < -180):
cmpsh -= 360
while (turnang - cmpsh < -1.5 or turnang - cmpsh > 1.5):
cmpsh = cmps11.heading()
print 'cmpsh: ', cmpsh
if (turnang - cmpsh > 180):
cmpsh += 360
elif (turnang - cmpsh < -180):
cmpsh -= 360
# turn to the left
if (turnang - cmpsh < 0):
leftf.move(-rotsp, accel)
leftr.move(-rotsp, accel)
rightf.move(rotsp, accel)
rightr.move(rotsp, accel)
# turn to the right
elif (turnang - cmpsh > 0):
leftf.move(rotsp, accel)
leftr.move(rotsp, accel)
rightf.move(-rotsp, accel)
rightr.move(-rotsp, accel)
time.sleep(0.11)
leftf.move(0, accel)
leftr.move(0, accel)
rightf.move(0, accel)
rightr.move(0, accel)
time.sleep(1)
firstcnt = 0
scndcnt = 0
linespol = rplidar.getlines()
print linespol
for lines in linespol:
print 'firstcnt: ', firstcnt
print 'scndcnt: ', scndcnt
if (scndcnt == 2):
break
# looks for objects
if (poldist(lines[0], lines[1], lines[2], lines[3]) >= objsize - objtol and
poldist(lines[0], lines[1], lines[2], lines[3]) <= objsize + objtol):
firstcnt += 1
scndcnt += 1
if (firstcnt == 1):
print 'firstcnt line: ', lines
firstcnt += 1
# calculates the time to drive back after facing an object in order to
# get back to the starting position
drivedist = lines[1] - stopdist
# calculates an angle to face the center of an
# object
if (lines[2] - lines[0] < 0):
driveangle = lines[
2] - (lines[2] - lines[0] + 360) / 2
else:
driveangle = lines[2] - \
(lines[2] - lines[0]) / 2
if (scndcnt == 2):
print 'scndcnt line: ', lines
# calculates an angle to face the center of an
# object
if (lines[2] - lines[0] < 0):
turnang = lines[2] - \
(lines[2] - lines[0] + 360) / 2
else:
turnang = lines[2] - (lines[2] - lines[0]) / 2
# creates an offset from the center of an object so that when the robot
# turns to that angle and gets rplidar data the
# first object will be this one
if (turnang - turnangfx < 0):
turnang = turnang - turnangfx + 360
else:
turnang = turnang - turnangfx
print 'driveangle: ', driveangle
print 'drivedist: ', drivedist
cmpsh = cmps11.heading()
if (driveangle - cmpsh > 180):
cmpsh += 360
elif (driveangle - cmpsh < -180):
cmpsh -= 360
# turns to face the center of an object
while (driveangle - cmpsh < -1.5 or driveangle - cmpsh > 1.5):
cmpsh = cmps11.heading()
print 'cmpsh: ', cmpsh
if (driveangle - cmpsh > 180):
cmpsh += 360
elif (driveangle - cmpsh < -180):
cmpsh -= 360
# turn to the left
if (driveangle - cmpsh < 0):
leftf.move(-rotsp, accel)
leftr.move(-rotsp, accel)
rightf.move(rotsp, accel)
rightr.move(rotsp, accel)
# turn to the right
elif (driveangle - cmpsh > 0):
leftf.move(rotsp, accel)
leftr.move(rotsp, accel)
rightf.move(-rotsp, accel)
rightr.move(-rotsp, accel)
time.sleep(0.07)
leftf.move(0, accel)
leftr.move(0, accel)
rightf.move(0, accel)
rightr.move(0, accel)
time.sleep(0.5)
srfrr.doranging()
#srffrm = srffr.getranging()
srffcm = srffc.getranging()
# drives forward until the robot faces the object adjusting its
# path on the way
while(srffcm > stopdist):
cmpsh = cmps11.heading()
print 'cmpsh: ', cmpsh
if (driveangle - cmpsh > 180):
cmpsh += 360
elif (driveangle - cmpsh < -180):
cmpsh -= 360
if (driveangle - cmpsh > -1.5 and driveangle - cmpsh < 1.5):
leftf.move(250, accel)
leftr.move(250, accel)
rightf.move(250, accel)
rightr.move(250, accel)
elif (driveangle - cmpsh < 0):
leftf.move(170, accel)
leftr.move(170, accel)
rightf.move(250, accel)
rightr.move(250, accel)
elif (driveangle - cmpsh > 0):
leftf.move(250, accel)
leftr.move(250, accel)
rightf.move(170, accel)
rightr.move(170, accel)
srfrr.doranging()
#srffrm = srffr.getranging()
srffcm = srffc.getranging()
# print 'front: ',srffcm
#srfflm = srffl.getranging()
time.sleep(0.11)
leftf.move(0, accel)
leftr.move(0, accel)
rightf.move(0, accel)
rightr.move(0, accel)
time.sleep(2)
print 'image processing'
# add code for image processing
# drives back to the initial location
start = time.time()
while(time.time() - start < drivedist / 52):
leftf.move(-255, accel)
leftr.move(-255, accel)
rightf.move(-255, accel)
rightr.move(-255, accel)
time.sleep(0.1)
leftf.move(0, accel)
leftr.move(0, accel)
rightf.move(0, accel)
rightr.move(0, accel)
if (cnt == 0):
print 'No objects detected'
else:
print 'Done'
except KeyboardInterrupt:
leftf.move(0, accel)
leftr.move(0, accel)
rightf.move(0, accel)
rightr.move(0, accel)
print 'KeyboardInterrupt in object detection'
c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
return r * c
def gpsheading():
lon = math.radians(gpsd.fix.longitude)
lat = math.radians(gpsd.fix.latitude)
y = math.sin(lonend - lon) * math.cos(latend)
x = math.cos(lat) * math.sin(latend) - \
math.sin(lat) * math.cos(latend) * math.cos(lonend - lon)
return (math.degrees(math.atan2(y, x)) + 360) % 360
try:
if (gpsdist() > 1.1):
gpsh = gpsheading()
cmpsh = cmps11.heading()
if (gpsh - cmpsh > 180):
cmpsh += 360
elif (gpsh - cmpsh < -180):
cmpsh -= 360
while (gpsh - cmpsh < -1 or gpsh - cmpsh > 1):
cmpsh = cmps11.heading()
print 'cmpsh: ', cmpsh
print 'gpsh: ', gpsh
print 'lon: ', gpsd.fix.longitude
print 'lat: ', gpsd.fix.latitude
if (gpsh - cmpsh > 180):
cmpsh += 360
elif (gpsh - cmpsh < -180):
cmpsh -= 360
# turn to the left
