def get_and_save_data():
for scene, angle in zip(["0", "l90", "r90"], [(90, 180), (0, 90),
(180, 270)]):
environment = VrepEnvironment(settings.SCENES + '/room_calib_' +
scene + '.ttt')
# environment.start_vrep()
environment.connect()
environment.load_scene(settings.SCENES + '/room_calib_' + scene +
'.ttt')
environment.start_simulation()
agent = Pioneer(environment)
time.sleep(1)
sample = agent.read_lidars()
ranges = np.array(sample)[angle[0]:angle[1]]
for _ in range(0, 29):
sample = agent.read_lidars()
ranges += np.array(sample)[angle[0]:angle[1]]
ranges = ranges / 10
np.savetxt("ranges_" + scene + ".txt", ranges)
environment.stop_simulation()
environment.disconnect()
del environment
def perform(self, agent: Pioneer):
if self == self.Forward:
agent.change_velocity([0.15, 0.15])
elif self == self.Left:
agent.change_velocity([-0.05, 0.05])
elif self == self.Right:
agent.change_velocity([0.05, -0.05])
elif self == self.Stay:
return
elif self == self.Save:
return
else:
logger.error(f"unknown action {self}")
def loop(agent: Pioneer, state: State):
ranges = agent.read_lidars()
ranges = np.array(ranges)
data = convert(ranges)
# action = state.control.apply(agent)
# if action == Action.Save:
# np.savetxt("data.txt", data)
# np.savetxt("ranges.txt", ranges)
# logger.info("stored current ranges and data")
action = brain(state, data, ranges)
print(action)
doors = find_doors(data, ranges)
state.plot.update(doors, *data, action)
report_status(doors)
action.perform(agent)
#do a random move to not get stuck around an "island"
j=j+1
if j=10000:
random()
j=0
##########
##########
if __name__ == "__main__":
plt.ion()
# Initialize and start the environment
environment = VrepEnvironment(settings.SCENES + '/room_static.ttt') # Open the file containing our scene (robot and its environment)
environment.connect() # Connect python to the simulator's remote API
agent = Pioneer(environment)
display = Display(agent, False)
print('\nDemonstration of Simultaneous Localization and Mapping using CoppeliaSim robot simulation software. \nPress "CTRL+C" to exit.\n')
start = time.time()
step = 0
done = False
environment.start_simulation()
time.sleep(1)
try:
while step < settings.simulation_steps and not done:
display.update() # Update the SLAM display
loop(agent) # Control loop
step += 1
except KeyboardInterrupt: