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] # 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, 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
class HaltonEnvironment(object):
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)
# Prepares the graph for planning from source to target
# source: the starting position of the car (in meters and radians)
# target: the target position of the car (in meters and radians)
def set_source_and_target(self, source, target):
self.source = source
self.target = target
GraphGenerator.insert_vertices(self.graph, [source, target], self.radius)
# Get the configuration associated with a node id
def get_config(self, vid):
return [float(a) for a in self.graph.node[str(vid)]["state"].split()]
# Get a list of the configurations that are reachable from the configuration corresponding to vid
def get_successors(self, vid):
successors = [int(i) for i in self.graph.neighbors(str(vid))]
freeSuccessors = []
for i in successors:
config1 = self.get_config(vid)
config2 = self.get_config(i)
if self.manager.get_edge_validity(config1, config2):
freeSuccessors.append(i)
return freeSuccessors
# Check if a config is valid
# config2D: The configuration to check (in meters and radians)
def get_state_validity(self, config2D):
return self.manager.get_state_validity(config2D)
# Get the distance between two nodes
def get_distance(self, vid1, vid2):
G = self.graph
return float(G[str(vid1)][str(vid2)]['length'])
# Get the heuristic value
def get_heuristic(self, vid, tid):
config1 = self.get_config(vid)
config2 = self.get_config(tid)
return Dubins.path_length(config1, config2, 1.0/model.TURNING_RADIUS)
class ObstaclesTable(wx.grid.PyGridTableBase):
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 object(self):
return self.obsName
def GetNumberRows(self):
return self.om.size()
def GetNumberCols(self):
return len(self.colLabels)
def GetColLabelValue(self,col):
return self.colLabels[col]
def GetRowLabelValue(self,row):
return self.om.obstacles[row].label(self.envName)
def IsEmptyCell(self,row,col):
return False
def GetValue(self,row,col):
if (col == NAME_POS):
return self.om.obstacles[row].name()
elif (col == DEFINITION_POS):
return self.om.obstacles[row].definition(self.envName)
elif (col == CATEGORY_POS):
return self.om.obstacles[row].category(self.envName)
elif (col == ORIGINATOR_POS):
return self.om.obstacles[row].originator()
def SetValue(self,row,col,value):
if (col == NAME_POS):
self.om.obstacles[row].setName(value)
elif (col == DEFINITION_POS):
self.om.obstacles[row].setDefinition(self.envName,value)
elif (col == CATEGORY_POS):
self.om.obstacles[row].setCategory(self.envName,value)
elif (col == ORIGINATOR_POS):
self.om.obstacles[row].setOriginator(value)
def AppendRows(self,numRows=1):
pos = self.om.size() - 1
self.InsertRows(pos,numRows)
def InsertRows(self,pos,numRows=1):
onDlg = wx.TextEntryDialog(None,'Enter obstacle name','New Obstacle','',style=wx.OK|wx.CANCEL)
if (onDlg.ShowModal() == wx.ID_OK):
obsName = onDlg.GetValue()
if (obsName != ''):
if (pos == -1):
pos = 0
newPos = pos + 1
try:
self.om.add(newPos,obsName)
except ARM.ARMException,errorText:
dlg = wx.MessageDialog(self.GetView(),str(errorText),'Add Obstacle',wx.OK | wx.ICON_ERROR)
dlg.ShowModal()
dlg.Destroy()
return
self.addToView(newPos)
grid = self.GetView()
grid.SetCellEditor(newPos,NAME_POS,wx.grid.GridCellAutoWrapStringEditor())
grid.SetCellRenderer(newPos,NAME_POS,wx.grid.GridCellAutoWrapStringRenderer())
grid.SetCellEditor(newPos,DEFINITION_POS,wx.grid.GridCellAutoWrapStringEditor())
grid.SetCellRenderer(newPos,DEFINITION_POS,wx.grid.GridCellAutoWrapStringRenderer())
grid.SetCellEditor(newPos,CATEGORY_POS,wx.grid.GridCellChoiceEditor(catChoices))
grid.SetCellEditor(newPos,ORIGINATOR_POS,wx.grid.GridCellAutoWrapStringEditor())
grid.SetCellRenderer(newPos,ORIGINATOR_POS,wx.grid.GridCellAutoWrapStringRenderer())
onDlg.Destroy()
return True
class HaltonEnvironment(object):
def __init__(self, mapMsg, graphFile, source, target, car_width,
car_length, neighbor_radius, collision_delta):
# Setup member variables
self.source = source
self.target = target
self.manager = ObstacleManager(mapMsg, car_width, car_length,
collision_delta)
# Generate the Graph on the fly if required
self.radius = neighbor_radius
if graphFile is None:
n = 500
bases = [2, 3]
lower = [0, 0]
upper = [64, 75]
G = GraphGenerator.euclidean_halton_graph(n, self.radius, bases,
lower, upper, source,
target, mapMsg,
car_width, car_length,
collision_delta)
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 set_source_and_target(self, source, target):
self.source = source
self.target = target
GraphGenerator.insert_vertices(self.graph, [source, target],
self.radius)
def get_config(self, vid):
return [float(a) for a in self.graph.node[str(vid)]["state"].split()]
def get_successors(self, vid):
successors = [int(i) for i in self.graph.neighbors(str(vid))]
return successors
def get_state_validity(self, config2D):
return self.manager.get_state_validity(config2D)
def get_distance(self, vid1, vid2):
G = self.graph
return float(G[str(vid1)][str(vid2)]['length'])
def get_heuristic(self, vid, tid):
config1 = self.get_config(vid)
config2 = self.get_config(tid)
return numpy.linalg.norm(numpy.array(config1) - numpy.array(config2))
