def reset(self):
self.img_map = cv2.imread(self.map_path, cv2.IMREAD_GRAYSCALE)
self.env = gslam.SingleBotLaser2DGrid((0, 0, 0), self.img_map,
self.bot_param)
temp = self.SearchPos(self.goal_dist, self.img_map)
self.env.setPose(temp)
self.traj = [temp]
self.path = [self.env.pose]
self.nav_pos = self.SearchPos(self.goal_dist * 2, self.img_map)
self.map = gslam.GridMap(self.map_param, gsize=self.grid_size)
self.sensor_data = self.env.scan()
info_gain = SensorMapping(self.map, self.env.pose, self.bot_param,
self.sensor_data['data'], self.grid_size)
fsize = self.obs_size
self.obs = self.map.getObserv(self.env.pose[0:2], self.env.pose[2],
int(fsize / 2), int(fsize / 2)).reshape(
[fsize, fsize, 1])
self.dist = np.abs(self.env.pose[0] -
self.nav_pos[0]) + np.abs(self.env.pose[1] -
self.nav_pos[1])
sdata = np.array(self.sensor_data['data'],
dtype=np.float32) / self.bot_param['max_dist']
return {
"map": self.obs,
"sensor": sdata,
"goal": self.getRelPos(),
"info_gain": info_gain
}
}
pyparam = [
params['sensor_size'], params['start_angle'], params['end_angle'],
params['max_dist']
]
im = cv2.imread('map_01.png', cv2.IMREAD_GRAYSCALE)
print(im)
bot = gslam.SingleBotLaser2DGrid((120.0, 80.0, 180.0), im, params)
#bot.pose = (140.0, 80.0)
pose = bot.pose
print(pose)
scan = bot.scan()
img = Viewer.DrawEnv(im, 1.0, pose, scan['data'], pyparam)
cv2.imshow('env', img)
grid = gslam.GridMap([0.4, -0.4, 5.0, -5.0], 2)
pf_size = 5
pf = gslam.ParticleFilter(pose, params, grid, pf_size)
traj = pf.getParticle(0).getTraj().copy()
print(traj)
traj_truth = [(120.0, 80.0, 180.0)]
timestemp = 0
pf.markParticles(0)
print("Rec: ")
for i in range(pf_size):
print(pf.getParticle(i).getIdRecord())
# Main Loop
while (1):
# Input Control
action = -1
action0 = -1