def main_control(self): self.slam = SLAM(GridMap(), LSLAMGUI()) rospy.Subscriber('scan_odom', scan_odom, self.Call, queue_size=1, buff_size=52428800) rospy.spin()
if __name__ == '__main__': # Initialize nodes and walls, scan #nodes = from_files() #walls = [Wall.Wall(Point(0,0), Point(0,0))] # #Drawer.draw(nodes, walls) # ##for n in nodes: ## n.pred_x = 0 ## n.pred_y = 0 ## n.pred_angle = 0 ## n.real_x = 0 ## n.real_y = 0 ## n.real_angle = 0 #Drawer.draw(nodes, []) #s = SLAM.SLAM(nodes) #print(s.solve_multi()) ##s.gravitate() #Drawer.draw(nodes, walls) #Drawer.draw(nodes, []) for i in range(3): i = 'samapt/data%d.txt' % i for j in range(3): j = 'samapt/data%d.txt' % (j + 3) for k in range(3): k = 'samapt/data%d.txt' % (k + 6) nodes = from_files([i, j, k]) s = SLAM.SLAM(nodes) print(s.solve_multi()) Drawer.draw(nodes, [], quickclose=True)
def getFrame(): for i in FRAME_GENERATOR: return i return -1, -1 ##################### # READ FRAMES END ##################### # GLOBAL VARIABLES # poseFig, poseAxis = plt.subplots() slamAlgorithm = SLAM() frameA, depthA = getFrame() frameB, depthB = getFrame() images = [frameA, frameB] depths = [depthA, depthB] i = 2 while True: # SLAMMING slamAlgorithm.process(images, depths, i) i += 1 # plt.cla() # trajectory = slamAlgorithm.get_trajectory() # visualize_trajectory(trajectory)