()
| 214 | |
| 215 | |
| 216 | def main(): |
| 217 | |
| 218 | # simulation parameters |
| 219 | simtime = 30.0 # simulation time |
| 220 | dt = 0.2 # time tick |
| 221 | |
| 222 | angle_reso = np.deg2rad(3.0) # sensor angle resolution |
| 223 | |
| 224 | v1 = VehicleSimulator(-10.0, 0.0, np.deg2rad(90.0), |
| 225 | 0.0, 50.0 / 3.6, 3.0, 5.0) |
| 226 | v2 = VehicleSimulator(20.0, 10.0, np.deg2rad(180.0), |
| 227 | 0.0, 50.0 / 3.6, 4.0, 10.0) |
| 228 | |
| 229 | lshapefitting = LShapeFitting() |
| 230 | lidar_sim = LidarSimulator() |
| 231 | |
| 232 | time = 0.0 |
| 233 | while time <= simtime: |
| 234 | time += dt |
| 235 | |
| 236 | v1.update(dt, 0.1, 0.0) |
| 237 | v2.update(dt, 0.1, -0.05) |
| 238 | |
| 239 | ox, oy = lidar_sim.get_observation_points([v1, v2], angle_reso) |
| 240 | |
| 241 | rects, idsets = lshapefitting.fitting(ox, oy) |
| 242 | |
| 243 | if show_animation: # pragma: no cover |
| 244 | plt.cla() |
| 245 | plt.axis("equal") |
| 246 | plt.plot(0.0, 0.0, "*r") |
| 247 | v1.plot() |
| 248 | v2.plot() |
| 249 | |
| 250 | # Plot range observation |
| 251 | for ids in idsets: |
| 252 | x = [ox[i] for i in range(len(ox)) if i in ids] |
| 253 | y = [oy[i] for i in range(len(ox)) if i in ids] |
| 254 | |
| 255 | for (ix, iy) in zip(x, y): |
| 256 | plt.plot([0.0, ix], [0.0, iy], "-og") |
| 257 | |
| 258 | plt.plot([ox[i] for i in range(len(ox)) if i in ids], |
| 259 | [oy[i] for i in range(len(ox)) if i in ids], |
| 260 | "o") |
| 261 | for rect in rects: |
| 262 | rect.plot() |
| 263 | |
| 264 | plt.pause(0.1) |
| 265 | |
| 266 | print("Done") |
| 267 | |
| 268 | |
| 269 | if __name__ == '__main__': |
no test coverage detected