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)