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 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] 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 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))] 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 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 Dubins.path_length(config1, config2, 1.0 / model.TURNING_RADIUS)