def main():
time_interval = 0.1 ###draw_mcl7###
world = World(60, time_interval, debug=False)
m = Map()
for ln in [(-4, 2), (2, -3), (3, 3)]:
m.append_landmark(Landmark(*ln))
world.append(m)
initial_pose = np.array([0, 0, 0]).T
# estimator = MCL(m, initial_pose, 100)
estimator = KalmanFilter(m, initial_pose)
circling = EstimationAgent(time_interval, 0.2, 10.0 / 180 * math.pi,
estimator)
r = Robot(initial_pose, sensor=Camera(m), agent=circling, color="red")
world.append(r)
world.draw()
def main():
world = World(30, 0.05)
m = Map()
m.append_landmark(Landmark(2, -2))
m.append_landmark(Landmark(-1, -3))
m.append_landmark(Landmark(3, 3))
world.append(m)
agent1 = Agent(1, 0.0)
agent2 = Agent(1, 30.0 / 180 * math.pi)
robot1 = Robot(np.array([2, 3, math.pi / 5 * 6]).T,
sensor=Camera(m),
agent=agent1)
robot2 = Robot(np.array([1, 0, math.pi / 5 * 6]).T,
agent=agent2,
sensor=Camera(m),
color='red')
world.append(robot1)
world.append(robot2)
world.draw()
