class Simulator:
# This class simulates a robot moving in a world. It updates the robot's
# state and measurements every tick.
def __init__(self, map_filename, robot_filename):
self.robot = Robot()
self.world = FeatureBasedWorld()
self.tick = 0.0
self.pose_history = [] # ground truth trajectory
self.robot.LoadFromFile(robot_filename)
self.world.LoadFromFile(map_filename)
self.pose_history.append(self.robot.GetPose()) # initial pose
def Update(self, t, action):
'''
Update the simulator.
===INPUT===
t: the time of update.
action: the active action from last Update() to t.
===OUTPUT===
A list of Measurement, which the robot observes at tick t.
'''
dt = t - self.tick
self.tick = t
rp = self.robot.ApplyAction(action, dt)
self.pose_history.append(rp)
return self.robot.MeasureWorld(self.world)
def GetTrajectory(self):
'''
Get the ground truth trajectory.
'''
return self.pose_history
def GetMap(self):
'''
Get the ground truth map.
'''
return self.world
print('\nAction apply to pose:')
print(str(ra.ApplyToPose(rp, 1)))
print('\nPose apply action:')
print(str(rp.ApplyAction(ra, 1)))
############
print('')
ra = RobotAction(1, 1)
print(str(ra))
print('')
rp = RobotPose(0, 0, 0)
print(str(rp))
print('\nAction apply to pose:')
print(str(ra.ApplyToPose(rp, 1)))
print('\nPose apply action:')
print(str(rp.ApplyAction(ra, 1)))
###########
print('\nRobot apply action:')
print(str(r.ApplyAction(ra, 1)))
###########
print('\nRobot load from file:')
r.LoadFromFile('../testcase/robot1.txt')
print(str(r))
