Python Graph.addNode方法代码示例

# 需要导入模块: import Graph [as 别名]
# 或者: from Graph import addNode [as 别名]
def initGraph(data):
	g = Graph()
	for i in range(len(data)/2):
		g.addNode(i)
		for j in range(i):
			g.addEdge(i, j, distance(float(data[i * 2]) * scale, float(data[i * 2 + 1]) * scale, float(data[j * 2]) * scale, float(data[j * 2 + 1]) * scale))
	return g

# 需要导入模块: import Graph [as 别名]
# 或者: from Graph import addNode [as 别名]
class DotParser:
   def __init__(self):
      self.Graph = Graph()
      self.FileName = None
      
   def readFile(self, filename):
      self.FileName = filename
      FILE = open(filename,'r')
      FILE = FILE.read()

      # find outer { }
      outer = 0
      for x in FILE:
         outer = outer + 1
         if x == "{":
            break
      if outer == 0:
         print "ERROR"
         exit()
      outerClose = outer
      braceStack = 1
      for x in range(outer,len(FILE)):
         if FILE[x] == "{":
            braceStack = braceStack + 1
         if FILE[x] == "}":
            braceStack = braceStack - 1
         
         if braceStack == 0:
            break
         outerClose = outerClose + 1

      
      header = FILE[:outer-1]
      self.readNameAndType(header)
            

      # From within the outer { } braces, we parse the graph
      graphText = FILE[outer:outerClose].strip().split("\n")
      commentBlock = False
      
      for line in graphText:                  
         # Strip away surrounding whitespace
         line = line.strip()

         #Handling comments
         if commentBlock and ("*/" not in line):
            continue
         elif "*/" in line:
            commentBlock = False
            line = line[line.index("*/")+2:]
         if "/*" in line:
            commentBlock = True
            line = line[:line.index("/*")]
         if "//" in line:
            line = line[:line.index("//")]                  
         if len(line) == 0:
            continue

         # Gets rid of the semicolon at the end.
         if line[-1] == ";":
            line = line[:-1]

         # Directed Edges
         if "->" in line:
            #Strip away the edge attributes
            (line,attributes) = stripAttributes(line)

            edgeLine = line.strip().split("->")
            n1 = Node(edgeLine[0].strip())
            n2 = Node(edgeLine[1].strip())
            e = Edge(n1,n2,"->")
            self.Graph.addEdge(e)
         # Undirected Edges
         elif "--" in line:
            #Strip away the edge attributes
            (line,attributes) = stripAttributes(line)

            edgeLine = line.strip().split("--")
            n1 = Node(edgeLine[0].strip())
            n2 = Node(edgeLine[1].strip())
            e1 = Edge(n1,n2,"--")
            e2 = Edge(n2,n1,"--")
            
            e1.Attributes = attributes
            e2.Attributes = attributes            

            self.Graph.addEdge(e1)
            self.Graph.addEdge(e2)
         elif "[" in line and "]" in line:
            #Information about the node
            (line,attributes) = stripAttributes(line)            
            name = line.strip()
            n = Node(name)
            n.Attributes = attributes

            self.Graph.addNode(n)

      for n in self.Graph.AdjacencyList:
         self.Graph.addNode(n)
      return self.Graph
#.........这里部分代码省略.........

# 需要导入模块: import Graph [as 别名]
# 或者: from Graph import addNode [as 别名]
class Robot:

    def __init__(self, sensors, motion, initialPosition, name):
        self.sensors = sensors
        self.motion = motion
        # initialize the graph
        self.graph = Graph()
        self.name = name

        # create a first node for the initial position
        node = Node(initialPosition, "position", 0)
        node.robot = self.name
        self.graph.addNode(node)

        # add a prior
        edge = PriorEdge(initialPosition, node, np.diag([1e-20,1e-20,1e-20]))
        self.graph.addEdge(edge)

        # keep track of the most recent position for dead reakoning
        self.pos = initialPosition
        self.posNode = node # node of the current position
        

    def move(self, cmd):
        """create a new node and edge with dead reakoned initial value"""
        nextPos = self.motion.move(self.pos, cmd)
        # create a new node for the next position
        # the descriptor is the index in time
        nextPosNode = Node(nextPos, "position", self.posNode.descriptor + 1)
        nextPosNode.robot = self.name
        self.graph.addNode(nextPosNode)
        # create a new edge between them
        edge = MotionEdge(self.motion, cmd, self.posNode, nextPosNode, "motion")
        self.graph.addEdge(edge)
        # update internal state
        self.pos = nextPos
        self.posNode = nextPosNode


    def simSense(self, simMap, simPos):
        """
        Simulate sensing landmarks on a map. The simulator gives the 
        robots actual position. 
        Create a new node for the landmark if necessay with a dead
        reakoned initial value with respect to the position node.
        """

        for sensor in self.sensors:
            sensorObsverations = sensor.simSense(simPos, simMap)

            for lmObs in sensorObsverations:
                # check if this landmark has been observed before
                lmNode = self.graph.getNodeOfTypeAndDescriptor(lmObs['sensorType'], lmObs['descriptor'])
                if lmNode == None: 
                    # if this landmark hasn't been observed before
                    # create a new node
                    lmPos = sensor.deadReckon(self.pos,lmObs['obs'])
                    # print self.pos
                    # print lmPos
                    # print lmObs['obs']
                    # wait()
                    lmNode = Node(lmPos, lmObs['sensorType'], lmObs['descriptor'])
                    self.graph.addNode(lmNode)
                else:
                    # update landmark guess as the average
                    lmPos = sensor.deadReckon(self.pos,lmObs['obs'])
                    lmNode.value = (lmNode.value + lmPos)/2.
                # create an edge between the nodes
                edge = ObservationEdge(sensor, lmObs['obs'], self.posNode, lmNode, "observation")
                self.graph.addEdge(edge)


    def reset(self):
        # initial position
        node = self.graph.nodes[0]
        # prior
        edge = self.graph.edges[0]

        self.posNode = node
        self.pos = node.value
        self.graph = Graph()
        self.graph.addNode(node)
        self.graph.addEdge(edge)


    def trajectory(self):
        positions = self.graph.getNodesOfType("position")
        sortedPos = sorted(positions, key=lambda x: x.descriptor)
        traj = map(lambda x: x.value, sortedPos)
        return array(traj)

    def trajectoryXY(self):
        positions = filter(lambda x: x.nodeType == "position" and x.robot == self.name, self.graph.nodes)
        sortedPos = sorted(positions, key=lambda x: x.descriptor)
        traj = map(lambda x: x.value, sortedPos)
        traj = array(traj)
        return traj[:, 0:2]  # hack off the angle

    def position(self):
        traj = self.trajectory()
#.........这里部分代码省略.........