def runRRT(dynamics, robotSize, data, start=[0, 50], end=[1950, 1650]):
data = data[::-1, :]
plt.imshow(data.T,
interpolation='nearest') # show 2D representation of map
img = obstaclefinder.imgToObs() # create img from imported picture
if dynamics == 'HOUND':
scaleFactor = 3
elif dynamics == 'HIPPO':
scaleFactor = 6
obs = img.obsSpaceGen(robotSize, data, scaleFactor, debug=False)
plt.imshow(data, interpolation='nearest') #show 2D representation of map
#initialize RRT
#print(obs[0].shape)
r = RRT.rrt(N=7000,
obstacles=obs.T,
obstacletype='array',
maxcoords=obs[0].shape,
origin=start + [0, '', 0],
goal=end,
live=False,
divis=10)
#Perform RRT
trajectory = r.rrt(dynamics, plotting=True)
return trajectory
#(verbose = True, plotting=True, dynamics)
def runRRT(dynamics, robotSize, data, start=[0, 50], end=[1950, 1650]):
data = data[::-1, :]
plt.imshow(data.T,
interpolation='nearest') # show 2D representation of map
img = obstaclefinder.imgToObs() # create img from imported picture
if dynamics == 'HOUND':
scaleFactor = 3
elif dynamics == 'HIPPO':
scaleFactor = 6
obs = img.obsSpaceGen(robotSize, data, scaleFactor, debug=False)
plt.imshow(data, interpolation='nearest') #show 2D representation of map
#############################################################################################
"""
#SET THE CAR THAT IS BEING TESTED
Contestant = "Hound" or "Hippo" whoever is running the maze
"""
Contestant = "Hound"
#############################################################################################
if dynamics == 'HOUND' and Contestant == "Hippo":
return [[0, 0, 0, [[0, 0, 100]]]]
if dynamics == 'HIPPO' and Contestant == "Hound":
return [[0, 0, 0, [[0, 0, 0, 0, 100]]]]
#initialize RRT
#print(obs[0].shape)
r = RRT.rrt(N=5000,
obstacles=obs.T,
obstacletype='array',
maxcoords=obs[0].shape,
origin=start + [0, '', 0],
goal=end,
live=False,
divis=10)
#Perform RRT
trajectory = r.rrt(dynamics, plotting=True)
return trajectory
#(verbose = True, plotting=True, dynamics)
import sys
import matplotlib.pyplot as plt
import RRT
import obstaclefinder
import math
import numpy as np
robotradius = 85
scale = 5 #set scale-up to increase obstacle accuracy
img = obstaclefinder.imgToObs(imagepath = "/home/sokchetraeung/EE183DB/ECE183DA/lab3/maze.bmp") #create img from imported picture
[image,obsimg] = img.obsfind(scale,robotradius/scale) #takes scale up factor and obstacle expansion factor and produces display array and obstacle array
plt.imshow(image.T,interpolation='nearest') #show 2D representation of map
#initialize RRT
r = RRT.rrt(N = 3000,obstacles = obsimg, obstacletype = 'array', maxcoords = image.shape,
origin = [35*scale,215*scale,0,0,'',0],goal = [125*scale,30*scale,math.pi/2],live = True, divis = 5,scale = scale,arb = False)
#Perform RRT
trajectory = r.rrt("hippo" ,verbose = True,plotting=True)
#print trajectory
print(trajectory)
#print input list
for x in trajectory:
print(x[3])
#print(r.outputList)
def runRRT(dynamics, robotsize, data ,start = [0,50],end = [1950,1650]):
data = data[::-1, :]
plt.imshow(data.T, interpolation='nearest') # show 2D representation of map
img = obstaclefinder.imgToObs() # create img from imported picture
##########################################################################################
"""
THIS SECTION DETERMINES PARAMETER OF CONFIG SPACE:
img.obsSpaceGen(robotSize, data, scaleFactor, debug=False)
robotSize = [length in pixels, width in pixels]
For scale, the area covered by the box is roughly 1200 by 1200 pixels
As this increases so does compuation time
data = camera view
(DO NOT ALTER)
scaleFactor = Downsizing of resolution i.e 4 means 16 pixels become 1 pixel
As scale factor goes up, computation speed increases but accuracy decreases
debug = True spits out config slices, False supresses slices
Will show 0,30,45,60,90,120,135,150,180 degree slices
"""
robotSize = [100,200]
scaleFactor = 6
debug_val = True
"""
THIS SECTION DETERMINES PARAMETER OF PLANNER
r = RRT.rrt(N = 10000,obstacles = obs.T, obstacletype = 'array', maxcoords = obs[0].shape,
origin = start+[0,'',0],goal = end, live = False, divis = 10)
N = Upper limit on nodes generated in RRT
As N goes up, higher chance of success but longer run time
obstacles = obstacles as determined by config
(DO NOT ALTER)
obstacletype = format of obstacle data
(DO NOT ALTER)
maxcoords = size of space
(DO NOT ALTER)
origin = Start position of car
(DO NOT ALTER)
goal = Goal, may alter first three values for x,y,theta (DO NOT ALTER REMAINING VALUES)
This is set by green cone
live = Live Plotting (Turned Off, takes too long)
(DO NOT ALTER)
divis = Time division of live plotter
(DO NOT ALTER)
"""
N = 10000
##########################################################################################
#initialize RRT
if dynamics == "HIPPO":
debug_val = False
obs = img.obsSpaceGen(robotSize, data, scaleFactor, debug=debug_val)
plt.imshow(data,interpolation='nearest') #show 2D representation of map
r = RRT.rrt(N = N,obstacles = obs.T, obstacletype = 'array', maxcoords = obs[0].shape,
origin = start+[0,'',0],goal = end, live = False, divis = 10)
#Perform RRT
trajectory = r.rrt(dynamics,plotting=True)
return trajectory
#(verbose = True, plotting=True, dynamics)