example_raytrace_contours.py

from __future__ import print_function
import CMap2D
from CMap2D import flatten_contours, render_contours_in_lidar
import numpy as np
import matplotlib.pyplot as plt
from timeit import default_timer as timer

from map2d import gridshow

contours = [
    [(-3.5647773123575437, 5.835222687642457),
     (-5.035222687642456, 5.835222687642457),
     (-5.035222687642456, 4.364777312357544),
     (-3.5647773123575437, 4.364777312357544)],
    [(-3.5330464973133493, 5.866953502686651),
     (-5.266953502686651, 5.866953502686651),
     (-5.266953502686651, 4.133046497313349),
     (-3.5330464973133493, 4.133046497313349)],
    [(-3.649238872233948, 3.450761127766052),
     (-4.950761127766052, 3.450761127766052),
     (-4.950761127766052, 2.1492388722339486),
     (-3.649238872233948, 2.1492388722339486)],
    [(3.8844705995895206, -3.21552940041048),
     (2.91552940041048, -3.21552940041048),
     (2.91552940041048, -4.18447059958952),
     (3.8844705995895206, -4.18447059958952)],
    [(5.126810354965404, -5.773189645034597),
     (4.273189645034597, -5.773189645034597),
     (4.273189645034597, -6.626810354965404),
     (5.126810354965404, -6.626810354965404)],
    [(-1.0, 5.4), (-3.0, 5.4), (-3.0, 4.4), (-1.0, 4.4)],
    [(3.8000000000000003, 4.0),
     (1.8000000000000003, 4.0),
     (1.8000000000000003, 3.0),
     (3.8000000000000003, 3.0)],
    [(-4.6000000000000005, 7.1000000000000005),
     (-7.6000000000000005, 7.1000000000000005),
     (-7.6000000000000005, 6.1000000000000005),
     (-4.6000000000000005, 6.1000000000000005)],
    [(3.1, -1.9000000000000004),
     (2.1, -1.9000000000000004),
     (2.1, -4.9),
     (3.1, -4.9)]
]

flat_contours = flatten_contours(contours)
ranges = np.ones((1080,), dtype=np.float32) * np.inf
angles = np.linspace(0, 2*np.pi, 1080, dtype=np.float32)
lidar_xy = np.array([0., -4.], dtype=np.float32)
render_contours_in_lidar(ranges, angles, flat_contours, lidar_xy)

CMap2D.CMap2D().plot_contours(contours, 'k')
for i in range(len(ranges)):
    plt.plot([lidar_xy[0], lidar_xy[0]+ranges[i] * np.cos(angles[i])],
             [lidar_xy[1], lidar_xy[1]+ranges[i] * np.sin(angles[i])], 'r')
plt.show()