| 87 | |
| 88 | |
| 89 | class LidarSimulator(): |
| 90 | |
| 91 | def __init__(self): |
| 92 | self.range_noise = 0.01 |
| 93 | |
| 94 | def get_observation_points(self, vlist, angle_reso): |
| 95 | x, y, angle, r = [], [], [], [] |
| 96 | |
| 97 | # store all points |
| 98 | for v in vlist: |
| 99 | |
| 100 | gx, gy = v.calc_global_contour() |
| 101 | |
| 102 | for vx, vy in zip(gx, gy): |
| 103 | vangle = math.atan2(vy, vx) |
| 104 | vr = np.hypot(vx, vy) * random.uniform(1.0 - self.range_noise, |
| 105 | 1.0 + self.range_noise) |
| 106 | |
| 107 | x.append(vx) |
| 108 | y.append(vy) |
| 109 | angle.append(vangle) |
| 110 | r.append(vr) |
| 111 | |
| 112 | # ray casting filter |
| 113 | rx, ry = self.ray_casting_filter(x, y, angle, r, angle_reso) |
| 114 | |
| 115 | return rx, ry |
| 116 | |
| 117 | def ray_casting_filter(self, xl, yl, thetal, rangel, angle_reso): |
| 118 | rx, ry = [], [] |
| 119 | rangedb = [float("inf") for _ in range( |
| 120 | int(np.floor((np.pi * 2.0) / angle_reso)) + 1)] |
| 121 | |
| 122 | for i in range(len(thetal)): |
| 123 | angleid = int(round(thetal[i] / angle_reso)) |
| 124 | |
| 125 | if rangedb[angleid] > rangel[i]: |
| 126 | rangedb[angleid] = rangel[i] |
| 127 | |
| 128 | for i in range(len(rangedb)): |
| 129 | t = i * angle_reso |
| 130 | if rangedb[i] != float("inf"): |
| 131 | rx.append(rangedb[i] * np.cos(t)) |
| 132 | ry.append(rangedb[i] * np.sin(t)) |
| 133 | |
| 134 | return rx, ry |
| 135 | |
| 136 | |
| 137 | def main(): |