def __init__(self, mapMsg, graphFile, source, target):
# Setup member variables
self.source = source
self.target = target
self.manager = ObstacleManager(mapMsg)
# Generate the Graph on the fly if required
self.radius = 100
if graphFile is None:
n = 500
bases = [2,3,5] # Should be prime numbers
lower = [0,0,0] # The lowest possible values for the config
upper = [64,75,2*numpy.pi] # The highest possible values for the config
G = GraphGenerator.euclidean_halton_graph(n, self.radius, bases, lower, upper, source, target, mapMsg) # Create the graph
nx.write_graphml(G, "haltonGraph.graphml") # Save the graph
self.graph = nx.read_graphml("haltonGraph.graphml") # Read the graph (that we just saved, probably not necessary)
else:
# Check if graph file exists
print(os.path.isfile(graphFile))
if not os.path.isfile(graphFile):
print "ERROR: graph file not found!"
quit()
self.graph = nx.read_graphml(graphFile) # Load the graph
print "graph loaded"
# Insert source and target if provided
if source is not None:
GraphGenerator.insert_vertices(self.graph, [source], self.radius)
if target is not None:
GraphGenerator.insert_vertices(self.graph, [target], self.radius)
def __init__(self,obsCombo,envCombo): wx.grid.PyGridTableBase.__init__(self) self.dimension = 'obstacle' self.colLabels = ['Name','Definition','Category','Originator'] self.om = ObstacleManager(obsCombo,envCombo) self.obsName = obsCombo.GetValue() self.envName = envCombo.GetValue()
def __init__(self, mapMsg, graphFile, source, target):
# Setup member variables
self.source = source
self.target = target
self.manager = ObstacleManager(mapMsg)
# Generate the Graph on the fly if required
self.radius = 100
if graphFile is None:
n = 500
bases = [2, 3, 5]
lower = [0, 0, 0]
upper = [64, 75, 2 * numpy.pi]
G = GraphGenerator.euclidean_halton_graph(n, self.radius, bases,
lower, upper, source,
target, mapMsg)
nx.write_graphml(G, "haltonGraph.graphml")
self.graph = nx.read_graphml("haltonGraph.graphml")
else:
# Check if graph file exists
if not os.path.isfile(graphFile):
print "ERROR: map file not found!"
quit()
self.graph = nx.read_graphml(graphFile)
if source is not None:
GraphGenerator.insert_vertices(self.graph, [source],
self.radius)
if target is not None:
GraphGenerator.insert_vertices(self.graph, [target],
self.radius)
def euclidean_halton_graph(n, radius, bases, lower, upper, source, target,
mapFile):
manager = ObstacleManager(mapFile)
G = nx.DiGraph()
upper = numpy.array(upper)
lower = numpy.array(lower)
scale = upper - lower
offset = lower
position = []
numVertices = 0
haltonIndex = 1
if source is not None:
position.append(source)
num_vertices += 1
if target is not None:
position.append(target)
num_vertices += 1
while numVertices < n:
p = wrap_around(
numpy.array(
[halton_sequence_value(haltonIndex, base) for base in bases]))
p = p * scale + offset
if manager.get_state_validity(p):
position.append(p)
numVertices += 1
haltonIndex += 1
state = [" ".join(str(x) for x in p) for p in position]
for i in range(n):
node_id = i
G.add_node(str(node_id), state=state[i])
for i in range(n - 1):
print i
for j in range(i + 1, n):
edgeLength = Dubins.path_length(position[i], position[j],
1.0 / model.TURNING_RADIUS)
euclideanLength = numpy.linalg.norm(position[i][0:2] -
position[j][0:2])
if edgeLength < radius:
G.add_edge(str(i), str(j), length=str(edgeLength))
edgeLength = Dubins.path_length(position[j], position[i],
1.0 / model.TURNING_RADIUS)
if edgeLength < radius:
G.add_edge(str(j), str(i), length=str(edgeLength))
return G
def euclidean_halton_graph(n, radius, bases, lower, upper, source, target,
mapFile, car_width, car_length, collision_delta):
manager = ObstacleManager(mapFile, car_width, car_length, collision_delta)
G = nx.DiGraph()
upper = numpy.array(upper)
lower = numpy.array(lower)
scale = upper - lower
offset = lower
position = []
numVertices = 0
haltonIndex = 1
if source is not None:
position.append(source)
num_vertices += 1
if target is not None:
position.append(target)
num_vertices += 1
print '[GraphGenerator] Populating node...'
while numVertices < n:
p = wrap_around(
numpy.array(
[halton_sequence_value(haltonIndex, base) for base in bases]))
p = p * scale + offset
if manager.get_state_validity(p):
position.append(p)
numVertices += 1
haltonIndex += 1
state = [" ".join(str(x) for x in p) for p in position]
for i in range(n):
node_id = i
G.add_node(str(node_id), state=state[i])
print '[Graph Generator] Generating KD Tree...'
position = numpy.array(position)
tree = spatial.KDTree(position)
print '[GraphGenerator] Populating edges...'
for i in xrange(n):
distances, indices = tree.query(position[numpy.newaxis, i],
k=100,
distance_upper_bound=radius)
distances = distances.squeeze()
indices = indices.squeeze()
edgeCount = 0
for j in xrange(indices.shape[0]):
if indices[j] >= len(position):
break
if distances[j] > numpy.finfo(float).eps:
edgeLength = numpy.linalg.norm(position[i] -
position[indices[j]])
G.add_edge(str(i), str(indices[j]), length=str(edgeLength))
edgeCount += 1
print '[GraphGenerator] %d of %d nodes complete, edges: %d' % (
i, n, edgeCount)
print '[GraphGenerator] Graph generation complete'
return G
def euclidean_halton_graph(n, radius, bases, lower, upper, source, target, mapFile):
manager = ObstacleManager(mapFile)
G = nx.DiGraph()
upper = numpy.array(upper)
lower = numpy.array(lower)
scale = upper-lower
offset = lower
print("scale", scale)
position = []
numVertices = 0
haltonIndex = 1
if source is not None:
print("source", source)
position.append(source)
numVertices += 1
if target is not None:
print("target", target)
position.append(target)
numVertices += 1
print('num vertices', numVertices)
print('n', n)
while numVertices < n:
p = wrap_around(numpy.array([halton_sequence_value(haltonIndex,base) for base in bases]))
p = p * scale + offset
if manager.get_state_validity(p):
position.append(p)
numVertices += 1
haltonIndex += 1
print("position", position)
print("position", [type(p) for p in position])
state = [" ".join(str(x) for x in p) for p in position]
for i in range(n):
node_id = i
G.add_node(str(node_id), state = state[i])
for i in range(n-1):
print i
for j in range(i+1,n):
edgeLength = Dubins.path_length(position[i], position[j], 1.0/model.TURNING_RADIUS)
euclideanLength = numpy.linalg.norm(position[i][0:2] - position[j][0:2])
if edgeLength < radius:
G.add_edge(str(i), str(j), length = str(edgeLength))
edgeLength = Dubins.path_length(position[j], position[i], 1.0/model.TURNING_RADIUS)
if edgeLength < radius:
G.add_edge(str(j), str(i), length = str(edgeLength))
pos = {}
for n in list(G.nodes(data = True)):
pose = numpy.array(map(float, n[1]['state'].split(' ')))
pxl_pose = Utils.our_world_to_map(pose, mapFile.info)
pos[n[0]] = (pxl_pose[0], pxl_pose[1])
return G, pos
def euclidean_uniform_graph(n, radius, bases, lower, upper, source, target, mapFile):
manager = ObstacleManager(mapFile)
G = nx.DiGraph()
upper = numpy.array(upper)
lower = numpy.array(lower)
scale = upper-lower
offset = lower
print("scale", scale)
position = []
numVertices = 0
if source is not None:
print("source", source)
position.append(source)
numVertices += 1
if target is not None:
print("target", target)
position.append(target)
numVertices += 1
print("SHAPE",manager.mapImageBW.shape)
valid_xs, valid_ys, _ = np.where(manager.mapImageBW == 0)
random_indices = np.arange(valid_xs.shape[0])
numpy.random.shuffle(random_indices)
selected_xs, selected_ys = valid_xs[random_indices], valid_ys[random_indices]
selected_thetas = np.random.uniform(0, 2*np.pi, valid_xs.shape[0])
print('num vertices', numVertices)
print('n', n)
selected_configs = np.column_stack((selected_xs, selected_ys, selected_thetas))
Utils.map_to_world(selected_configs, mapFile.info)
for p in selected_configs:
if manager.get_state_validity(p):
position.append(p)
numVertices += 1
if numVertices >= n:
break
print("position", position)
print("position", [type(p) for p in position])
state = [" ".join(str(x) for x in p) for p in position]
for i in range(n):
node_id = i
G.add_node(str(node_id), state = state[i])
for i in range(n-1):
print i
for j in range(i+1,n):
edgeLength = Dubins.path_length(position[i], position[j], 1.0/model.TURNING_RADIUS)
euclideanLength = numpy.linalg.norm(position[i][0:2] - position[j][0:2])
if edgeLength < radius:
G.add_edge(str(i), str(j), length = str(edgeLength))
edgeLength = Dubins.path_length(position[j], position[i], 1.0/model.TURNING_RADIUS)
if edgeLength < radius:
G.add_edge(str(j), str(i), length = str(edgeLength))
pos = {}
for n in list(G.nodes(data = True)):
pose = numpy.array(map(float, n[1]['state'].split(' ')))
pxl_pose = Utils.our_world_to_map(pose, mapFile.info)
pos[n[0]] = (pxl_pose[0], pxl_pose[1])
return G, pos
