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))