mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-01-13 21:48:06 -05:00
145 lines
3.7 KiB
Python
145 lines
3.7 KiB
Python
"""
|
|
|
|
Simulator
|
|
|
|
author: Atsushi Sakai
|
|
|
|
"""
|
|
|
|
import numpy as np
|
|
import matplotlib.pyplot as plt
|
|
import math
|
|
import random
|
|
|
|
|
|
class VehicleSimulator():
|
|
|
|
def __init__(self, ix, iy, iyaw, iv, max_v, w, L):
|
|
self.x = ix
|
|
self.y = iy
|
|
self.yaw = iyaw
|
|
self.v = iv
|
|
self.max_v = max_v
|
|
self.W = w
|
|
self.L = L
|
|
self._calc_vehicle_contour()
|
|
|
|
def update(self, dt, a, omega):
|
|
self.x += self.v * np.cos(self.yaw) * dt
|
|
self.y += self.v * np.sin(self.yaw) * dt
|
|
self.yaw += omega * dt
|
|
self.v += a * dt
|
|
if self.v >= self.max_v:
|
|
self.v = self.max_v
|
|
|
|
def plot(self):
|
|
plt.plot(self.x, self.y, ".b")
|
|
|
|
# convert global coordinate
|
|
gx, gy = self.calc_global_contour()
|
|
plt.plot(gx, gy, "--b")
|
|
|
|
def calc_global_contour(self):
|
|
gx = [(ix * np.cos(self.yaw) + iy * np.sin(self.yaw)) +
|
|
self.x for (ix, iy) in zip(self.vc_x, self.vc_y)]
|
|
gy = [(ix * np.sin(self.yaw) - iy * np.cos(self.yaw)) +
|
|
self.y for (ix, iy) in zip(self.vc_x, self.vc_y)]
|
|
|
|
return gx, gy
|
|
|
|
def _calc_vehicle_contour(self):
|
|
|
|
self.vc_x = []
|
|
self.vc_y = []
|
|
|
|
self.vc_x.append(self.L / 2.0)
|
|
self.vc_y.append(self.W / 2.0)
|
|
|
|
self.vc_x.append(self.L / 2.0)
|
|
self.vc_y.append(-self.W / 2.0)
|
|
|
|
self.vc_x.append(-self.L / 2.0)
|
|
self.vc_y.append(-self.W / 2.0)
|
|
|
|
self.vc_x.append(-self.L / 2.0)
|
|
self.vc_y.append(self.W / 2.0)
|
|
|
|
self.vc_x.append(self.L / 2.0)
|
|
self.vc_y.append(self.W / 2.0)
|
|
|
|
self.vc_x, self.vc_y = self._interporate(self.vc_x, self.vc_y)
|
|
|
|
def _interporate(self, x, y):
|
|
rx, ry = [], []
|
|
dtheta = 0.05
|
|
for i in range(len(x) - 1):
|
|
rx.extend([(1.0 - θ) * x[i] + θ * x[i + 1]
|
|
for θ in np.arange(0.0, 1.0, dtheta)])
|
|
ry.extend([(1.0 - θ) * y[i] + θ * y[i + 1]
|
|
for θ in np.arange(0.0, 1.0, dtheta)])
|
|
|
|
rx.extend([(1.0 - θ) * x[len(x) - 1] + θ * x[1]
|
|
for θ in np.arange(0.0, 1.0, dtheta)])
|
|
ry.extend([(1.0 - θ) * y[len(y) - 1] + θ * y[1]
|
|
for θ in np.arange(0.0, 1.0, dtheta)])
|
|
|
|
return rx, ry
|
|
|
|
|
|
class LidarSimulator():
|
|
|
|
def __init__(self):
|
|
self.range_noise = 0.01
|
|
|
|
def get_observation_points(self, vlist, angle_reso):
|
|
x, y, angle, r = [], [], [], []
|
|
|
|
# store all points
|
|
for v in vlist:
|
|
|
|
gx, gy = v.calc_global_contour()
|
|
|
|
for vx, vy in zip(gx, gy):
|
|
vangle = math.atan2(vy, vx)
|
|
vr = np.hypot(vx, vy) * random.uniform(1.0 - self.range_noise,
|
|
1.0 + self.range_noise)
|
|
|
|
x.append(vx)
|
|
y.append(vy)
|
|
angle.append(vangle)
|
|
r.append(vr)
|
|
|
|
# ray casting filter
|
|
rx, ry = self.ray_casting_filter(x, y, angle, r, angle_reso)
|
|
|
|
return rx, ry
|
|
|
|
def ray_casting_filter(self, xl, yl, thetal, rangel, angle_reso):
|
|
rx, ry = [], []
|
|
rangedb = [float("inf") for _ in range(
|
|
int(np.floor((np.pi * 2.0) / angle_reso)) + 1)]
|
|
|
|
for i in range(len(thetal)):
|
|
angleid = int(round(thetal[i] / angle_reso))
|
|
|
|
if rangedb[angleid] > rangel[i]:
|
|
rangedb[angleid] = rangel[i]
|
|
|
|
for i in range(len(rangedb)):
|
|
t = i * angle_reso
|
|
if rangedb[i] != float("inf"):
|
|
rx.append(rangedb[i] * np.cos(t))
|
|
ry.append(rangedb[i] * np.sin(t))
|
|
|
|
return rx, ry
|
|
|
|
|
|
def main():
|
|
print("start!!")
|
|
|
|
print("done!!")
|
|
|
|
|
|
if __name__ == '__main__':
|
|
main()
|