SLAM simulation converts polar coordinates to map frame, not cartesian

[tomaz-suller]

During execution of main, the result from lidar is converted to the coordinate system of the map:

hector_slam_fpga/software_simulation/slam.py

Lines 271 to 272 in 3223e58

	S = lidar(xi, env, 720, use_inf=False)
	S_map = [np.array([world2map(si[0]), si[1]]) for si in S]

However, the output of lidar is in polar, rather than in cartesian, coordinates:

hector_slam_fpga/software_simulation/slam.py

Lines 17 to 34 in 3223e58

	def lidar(xi, env, n_measures=360, use_inf=False):
	sensor_px, sensor_py, psi = xi
	size_x, size_y = env.T.shape
	size_max = max(size_x, size_y)
	acc = []
	for theta in np.linspace(0, 2*pi, n_measures):
	theta += psi
	measure = np.array([np.Inf if use_inf else size_max, theta])
	for rho in range(1, size_max+1):
	check_px = math.ceil(sensor_px + rho*cos(theta))
	check_py = math.ceil(sensor_py + rho*sin(theta))
	if (0 < check_px <= size_x and
	0 < check_py <= size_y and
	not env[check_py, check_px]):
	measure = np.array([rho-1, theta])
	break
	acc.append(measure)
	return acc

Therefore, this operation is bound to produce invalid results when executed.