add # pragma: no cover

This commit is contained in:
Atsushi Sakai
2019-02-03 10:20:23 +09:00
parent f0b6b7a94d
commit af854d6d1a
25 changed files with 55 additions and 58 deletions

View File

@@ -656,7 +656,7 @@ def main():
print(f'Converged after {it + 1} iterations.') print(f'Converged after {it + 1} iterations.')
break break
if show_animation: if show_animation: # pragma: no cover
plot_animation(X, U) plot_animation(X, U)
print("done!!") print("done!!")

View File

@@ -122,7 +122,7 @@ def main():
ex, ey, er, error = circle_fitting(x, y) ex, ey, er, error = circle_fitting(x, y)
print("Error:", error) print("Error:", error)
if show_animation: if show_animation: # pragma: no cover
plt.cla() plt.cla()
plt.axis("equal") plt.axis("equal")
plt.plot(0.0, 0.0, "*r") plt.plot(0.0, 0.0, "*r")

View File

@@ -71,7 +71,7 @@ def main():
gmap, minx, maxx, miny, maxy = generate_gaussian_grid_map( gmap, minx, maxx, miny, maxy = generate_gaussian_grid_map(
ox, oy, xyreso, STD) ox, oy, xyreso, STD)
if show_animation: if show_animation: # pragma: no cover
plt.cla() plt.cla()
draw_heatmap(gmap, minx, maxx, miny, maxy, xyreso) draw_heatmap(gmap, minx, maxx, miny, maxy, xyreso)
plt.plot(ox, oy, "xr") plt.plot(ox, oy, "xr")

View File

@@ -161,7 +161,7 @@ def main():
clusters = kmeans_clustering(rx, ry, ncluster) clusters = kmeans_clustering(rx, ry, ncluster)
# for animation # for animation
if show_animation: if show_animation: # pragma: no cover
plt.cla() plt.cla()
inds = calc_association(cx, cy, clusters) inds = calc_association(cx, cy, clusters)
for ic in inds: for ic in inds:

View File

@@ -121,7 +121,8 @@ def main():
oy = (np.random.rand(4) - 0.5) * 10.0 oy = (np.random.rand(4) - 0.5) * 10.0
pmap, minx, maxx, miny, maxy, xyreso = generate_ray_casting_grid_map( pmap, minx, maxx, miny, maxy, xyreso = generate_ray_casting_grid_map(
ox, oy, xyreso, yawreso) ox, oy, xyreso, yawreso)
if show_animation:
if show_animation: # pragma: no cover
plt.cla() plt.cla()
draw_heatmap(pmap, minx, maxx, miny, maxy, xyreso) draw_heatmap(pmap, minx, maxx, miny, maxy, xyreso)
plt.plot(ox, oy, "xr") plt.plot(ox, oy, "xr")

View File

@@ -68,7 +68,7 @@ def a_star_planning(sx, sy, gx, gy, ox, oy, reso, rr):
current = openset[c_id] current = openset[c_id]
# show graph # show graph
if show_animation: if show_animation: # pragma: no cover
plt.plot(current.x * reso, current.y * reso, "xc") plt.plot(current.x * reso, current.y * reso, "xc")
if len(closedset.keys()) % 10 == 0: if len(closedset.keys()) % 10 == 0:
plt.pause(0.001) plt.pause(0.001)
@@ -214,7 +214,7 @@ def main():
ox.append(40.0) ox.append(40.0)
oy.append(60.0 - i) oy.append(60.0 - i)
if show_animation: if show_animation: # pragma: no cover
plt.plot(ox, oy, ".k") plt.plot(ox, oy, ".k")
plt.plot(sx, sy, "xr") plt.plot(sx, sy, "xr")
plt.plot(gx, gy, "xb") plt.plot(gx, gy, "xb")
@@ -223,7 +223,7 @@ def main():
rx, ry = a_star_planning(sx, sy, gx, gy, ox, oy, grid_size, robot_size) rx, ry = a_star_planning(sx, sy, gx, gy, ox, oy, grid_size, robot_size)
if show_animation: if show_animation: # pragma: no cover
plt.plot(rx, ry, "-r") plt.plot(rx, ry, "-r")
plt.show() plt.show()

View File

@@ -313,7 +313,7 @@ def test1(max_vel=0.5):
for j, t in enumerate(times): for j, t in enumerate(times):
state[:, j] = traj.calc_traj_point(t) state[:, j] = traj.calc_traj_point(t)
if show_animation: if show_animation: # pragma: no cover
# plot the path # plot the path
plt.plot(state[0, :], state[1, :]) plt.plot(state[0, :], state[1, :])
plt.pause(1.0) plt.pause(1.0)

View File

@@ -343,7 +343,7 @@ def main():
print("Goal") print("Goal")
break break
if show_animation: if show_animation: # pragma: no cover
plt.cla() plt.cla()
plt.plot(tx, ty) plt.plot(tx, ty)
plt.plot(ob[:, 0], ob[:, 1], "xk") plt.plot(ob[:, 0], ob[:, 1], "xk")
@@ -356,7 +356,7 @@ def main():
plt.pause(0.0001) plt.pause(0.0001)
print("Finish") print("Finish")
if show_animation: if show_animation: # pragma: no cover
plt.grid(True) plt.grid(True)
plt.pause(0.0001) plt.pause(0.0001)
plt.show() plt.show()

View File

@@ -304,7 +304,7 @@ def main():
path = rrt.planning(animation=show_animation) path = rrt.planning(animation=show_animation)
# Draw final path # Draw final path
if show_animation: if show_animation: # pragma: no cover
rrt.draw_graph() rrt.draw_graph()
plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r') plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
plt.grid(True) plt.grid(True)

View File

@@ -123,7 +123,7 @@ def optimize_trajectory(target, k0, p):
p += alpha * np.array(dp) p += alpha * np.array(dp)
# print(p.T) # print(p.T)
if show_animation: if show_animation: # pragma: no cover
show_trajectory(target, xc, yc) show_trajectory(target, xc, yc)
else: else:
xc, yc, yawc, p = None, None, None, None xc, yc, yawc, p = None, None, None, None

View File

@@ -393,7 +393,7 @@ def test():
px, py, pyaw, mode, clen = reeds_shepp_path_planning( px, py, pyaw, mode, clen = reeds_shepp_path_planning(
start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature, step_size) start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature, step_size)
if show_animation: if show_animation: # pragma: no cover
plt.cla() plt.cla()
plt.plot(px, py, label="final course " + str(mode)) plt.plot(px, py, label="final course " + str(mode))
@@ -430,7 +430,7 @@ def main():
px, py, pyaw, mode, clen = reeds_shepp_path_planning( px, py, pyaw, mode, clen = reeds_shepp_path_planning(
start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature, step_size) start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature, step_size)
if show_animation: if show_animation: # pragma: no cover
plt.cla() plt.cla()
plt.plot(px, py, label="final course " + str(mode)) plt.plot(px, py, label="final course " + str(mode))

View File

@@ -78,7 +78,7 @@ def VRM_planning(sx, sy, gx, gy, ox, oy, rr):
obkdtree = KDTree(np.vstack((ox, oy)).T) obkdtree = KDTree(np.vstack((ox, oy)).T)
sample_x, sample_y = sample_points(sx, sy, gx, gy, rr, ox, oy, obkdtree) sample_x, sample_y = sample_points(sx, sy, gx, gy, rr, ox, oy, obkdtree)
if show_animation: if show_animation: # pragma: no cover
plt.plot(sample_x, sample_y, ".b") plt.plot(sample_x, sample_y, ".b")
road_map = generate_roadmap(sample_x, sample_y, rr, obkdtree) road_map = generate_roadmap(sample_x, sample_y, rr, obkdtree)
@@ -183,7 +183,7 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y):
current = openset[c_id] current = openset[c_id]
# show graph # show graph
if show_animation and len(closedset.keys()) % 2 == 0: if show_animation and len(closedset.keys()) % 2 == 0: # pragma: no cover
plt.plot(current.x, current.y, "xg") plt.plot(current.x, current.y, "xg")
plt.pause(0.001) plt.pause(0.001)
@@ -287,7 +287,7 @@ def main():
ox.append(40.0) ox.append(40.0)
oy.append(60.0 - i) oy.append(60.0 - i)
if show_animation: if show_animation: # pragma: no cover
plt.plot(ox, oy, ".k") plt.plot(ox, oy, ".k")
plt.plot(sx, sy, "^r") plt.plot(sx, sy, "^r")
plt.plot(gx, gy, "^c") plt.plot(gx, gy, "^c")
@@ -298,7 +298,7 @@ def main():
assert rx, 'Cannot found path' assert rx, 'Cannot found path'
if show_animation: if show_animation: # pragma: no cover
plt.plot(rx, ry, "-r") plt.plot(rx, ry, "-r")
plt.show() plt.show()

View File

@@ -584,7 +584,7 @@ def main():
# update state # update state
plant_system.update_state(u_1s[0], u_2s[0]) plant_system.update_state(u_1s[0], u_2s[0])
if show_animation: if show_animation: # pragma: no cover
animation(plant_system, controller, dt) animation(plant_system, controller, dt)
plot_figures(plant_system, controller, iteration_num, dt) plot_figures(plant_system, controller, iteration_num, dt)

View File

@@ -1,3 +1,8 @@
import cubic_spline_planner
import scipy.linalg as la
import matplotlib.pyplot as plt
import math
import numpy as np
""" """
Path tracking simulation with LQR speed and steering control Path tracking simulation with LQR speed and steering control
@@ -8,11 +13,6 @@ author Atsushi Sakai (@Atsushi_twi)
import sys import sys
sys.path.append("../../PathPlanning/CubicSpline/") sys.path.append("../../PathPlanning/CubicSpline/")
import numpy as np
import math
import matplotlib.pyplot as plt
import scipy.linalg as la
import cubic_spline_planner
# LQR parameter # LQR parameter
Q = np.eye(5) Q = np.eye(5)
@@ -206,8 +206,8 @@ def closed_loop_prediction(cx, cy, cyaw, ck, speed_profile, goal):
plt.plot(cx[target_ind], cy[target_ind], "xg", label="target") plt.plot(cx[target_ind], cy[target_ind], "xg", label="target")
plt.axis("equal") plt.axis("equal")
plt.grid(True) plt.grid(True)
plt.title("speed[km/h]:" + str(round(state.v * 3.6, 2)) + plt.title("speed[km/h]:" + str(round(state.v * 3.6, 2))
",target index:" + str(target_ind)) + ",target index:" + str(target_ind))
plt.pause(0.0001) plt.pause(0.0001)
return t, x, y, yaw, v return t, x, y, yaw, v
@@ -257,7 +257,7 @@ def main():
t, x, y, yaw, v = closed_loop_prediction(cx, cy, cyaw, ck, sp, goal) t, x, y, yaw, v = closed_loop_prediction(cx, cy, cyaw, ck, sp, goal)
if show_animation: if show_animation: # pragma: no cover
plt.close() plt.close()
plt.subplots(1) plt.subplots(1)
plt.plot(ax, ay, "xb", label="waypoints") plt.plot(ax, ay, "xb", label="waypoints")

View File

@@ -255,7 +255,7 @@ def main():
t, x, y, yaw, v = closed_loop_prediction(cx, cy, cyaw, ck, sp, goal) t, x, y, yaw, v = closed_loop_prediction(cx, cy, cyaw, ck, sp, goal)
if show_animation: if show_animation: # pragma: no cover
plt.close() plt.close()
plt.subplots(1) plt.subplots(1)
plt.plot(ax, ay, "xb", label="input") plt.plot(ax, ay, "xb", label="input")

View File

@@ -427,7 +427,7 @@ def do_simulation(cx, cy, cyaw, ck, sp, dl, initial_state):
print("Goal") print("Goal")
break break
if show_animation: if show_animation: # pragma: no cover
plt.cla() plt.cla()
if ox is not None: if ox is not None:
plt.plot(ox, oy, "xr", label="MPC") plt.plot(ox, oy, "xr", label="MPC")
@@ -563,7 +563,7 @@ def main():
t, x, y, yaw, v, d, a = do_simulation( t, x, y, yaw, v, d, a = do_simulation(
cx, cy, cyaw, ck, sp, dl, initial_state) cx, cy, cyaw, ck, sp, dl, initial_state)
if show_animation: if show_animation: # pragma: no cover
plt.close("all") plt.close("all")
plt.subplots() plt.subplots()
plt.plot(cx, cy, "-r", label="spline") plt.plot(cx, cy, "-r", label="spline")
@@ -596,7 +596,7 @@ def main2():
t, x, y, yaw, v, d, a = do_simulation( t, x, y, yaw, v, d, a = do_simulation(
cx, cy, cyaw, ck, sp, dl, initial_state) cx, cy, cyaw, ck, sp, dl, initial_state)
if show_animation: if show_animation: # pragma: no cover
plt.close("all") plt.close("all")
plt.subplots() plt.subplots()
plt.plot(cx, cy, "-r", label="spline") plt.plot(cx, cy, "-r", label="spline")

View File

@@ -67,7 +67,7 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal):
x = x + v * np.cos(theta) * dt x = x + v * np.cos(theta) * dt
y = y + v * np.sin(theta) * dt y = y + v * np.sin(theta) * dt
if show_animation: if show_animation: # pragma: no cover
plt.cla() plt.cla()
plt.arrow(x_start, y_start, np.cos(theta_start), plt.arrow(x_start, y_start, np.cos(theta_start),
np.sin(theta_start), color='r', width=0.1) np.sin(theta_start), color='r', width=0.1)

View File

@@ -126,7 +126,7 @@ def main():
v.append(state.v) v.append(state.v)
t.append(time) t.append(time)
if show_animation: if show_animation: # pragma: no cover
plt.cla() plt.cla()
plt.plot(cx, cy, ".r", label="course") plt.plot(cx, cy, ".r", label="course")
plt.plot(x, y, "-b", label="trajectory") plt.plot(x, y, "-b", label="trajectory")
@@ -139,7 +139,7 @@ def main():
# Test # Test
assert lastIndex >= target_ind, "Cannot goal" assert lastIndex >= target_ind, "Cannot goal"
if show_animation: if show_animation: # pragma: no cover
plt.cla() plt.cla()
plt.plot(cx, cy, ".r", label="course") plt.plot(cx, cy, ".r", label="course")
plt.plot(x, y, "-b", label="trajectory") plt.plot(x, y, "-b", label="trajectory")

View File

@@ -1,3 +1,6 @@
import cubic_spline_planner
import matplotlib.pyplot as plt
import math
""" """
Path tracking simulation with rear wheel feedback steering control and PID speed control. Path tracking simulation with rear wheel feedback steering control and PID speed control.
@@ -8,10 +11,6 @@ author: Atsushi Sakai(@Atsushi_twi)
import sys import sys
sys.path.append("../../PathPlanning/CubicSpline/") sys.path.append("../../PathPlanning/CubicSpline/")
import math
import matplotlib.pyplot as plt
import cubic_spline_planner
Kp = 1.0 # speed propotional gain Kp = 1.0 # speed propotional gain
# steering control parameter # steering control parameter
@@ -202,7 +201,7 @@ def main():
# Test # Test
assert goal_flag, "Cannot goal" assert goal_flag, "Cannot goal"
if show_animation: if show_animation: # pragma: no cover
plt.close() plt.close()
plt.subplots(1) plt.subplots(1)
plt.plot(ax, ay, "xb", label="input") plt.plot(ax, ay, "xb", label="input")

View File

@@ -184,7 +184,7 @@ def main():
v.append(state.v) v.append(state.v)
t.append(time) t.append(time)
if show_animation: if show_animation: # pragma: no cover
plt.cla() plt.cla()
plt.plot(cx, cy, ".r", label="course") plt.plot(cx, cy, ".r", label="course")
plt.plot(x, y, "-b", label="trajectory") plt.plot(x, y, "-b", label="trajectory")
@@ -197,7 +197,7 @@ def main():
# Test # Test
assert last_idx >= target_idx, "Cannot reach goal" assert last_idx >= target_idx, "Cannot reach goal"
if show_animation: if show_animation: # pragma: no cover
plt.plot(cx, cy, ".r", label="course") plt.plot(cx, cy, ".r", label="course")
plt.plot(x, y, "-b", label="trajectory") plt.plot(x, y, "-b", label="trajectory")
plt.legend() plt.legend()

View File

@@ -97,12 +97,12 @@ def observation(xTrue, xd, u, RFID):
def motion_model(x, u): def motion_model(x, u):
F = np.array([[1.0, 0, 0], F = np.array([[1.0, 0, 0],
[0, 1.0, 0], [0, 1.0, 0],
[0, 0, 1.0]]) [0, 0, 1.0]])
B = np.array([[DT * math.cos(x[2, 0]), 0], B = np.array([[DT * math.cos(x[2, 0]), 0],
[DT * math.sin(x[2, 0]), 0], [DT * math.sin(x[2, 0]), 0],
[0.0, DT]]) [0.0, DT]])
x = (F @ x) + (B @ u) x = (F @ x) + (B @ u)
return x return x
@@ -119,8 +119,8 @@ def jacob_motion(x, u):
(STATE_SIZE, LM_SIZE * calc_n_LM(x))))) (STATE_SIZE, LM_SIZE * calc_n_LM(x)))))
jF = np.array([[0.0, 0.0, -DT * u[0] * math.sin(x[2, 0])], jF = np.array([[0.0, 0.0, -DT * u[0] * math.sin(x[2, 0])],
[0.0, 0.0, DT * u[0] * math.cos(x[2, 0])], [0.0, 0.0, DT * u[0] * math.cos(x[2, 0])],
[0.0, 0.0, 0.0]]) [0.0, 0.0, 0.0]])
G = np.eye(STATE_SIZE) + Fx.T * jF * Fx G = np.eye(STATE_SIZE) + Fx.T * jF * Fx
@@ -170,7 +170,7 @@ def calc_innovation(lm, xEst, PEst, z, LMid):
delta = lm - xEst[0:2] delta = lm - xEst[0:2]
q = (delta.T @ delta)[0, 0] q = (delta.T @ delta)[0, 0]
#zangle = math.atan2(delta[1], delta[0]) - xEst[2] #zangle = math.atan2(delta[1], delta[0]) - xEst[2]
zangle = math.atan2(delta[1,0], delta[0,0]) - xEst[2] zangle = math.atan2(delta[1, 0], delta[0, 0]) - xEst[2]
zp = np.array([[math.sqrt(q), pi_2_pi(zangle)]]) zp = np.array([[math.sqrt(q), pi_2_pi(zangle)]])
y = (z - zp).T y = (z - zp).T
y[1] = pi_2_pi(y[1]) y[1] = pi_2_pi(y[1])
@@ -183,7 +183,7 @@ def calc_innovation(lm, xEst, PEst, z, LMid):
def jacobH(q, delta, x, i): def jacobH(q, delta, x, i):
sq = math.sqrt(q) sq = math.sqrt(q)
G = np.array([[-sq * delta[0, 0], - sq * delta[1, 0], 0, sq * delta[0, 0], sq * delta[1, 0]], G = np.array([[-sq * delta[0, 0], - sq * delta[1, 0], 0, sq * delta[0, 0], sq * delta[1, 0]],
[delta[1, 0], - delta[0, 0], - 1.0, - delta[1, 0], delta[0, 0]]]) [delta[1, 0], - delta[0, 0], - 1.0, - delta[1, 0], delta[0, 0]]])
G = G / q G = G / q
nLM = calc_n_LM(x) nLM = calc_n_LM(x)
@@ -235,13 +235,12 @@ def main():
x_state = xEst[0:STATE_SIZE] x_state = xEst[0:STATE_SIZE]
# store data history # store data history
hxEst = np.hstack((hxEst, x_state)) hxEst = np.hstack((hxEst, x_state))
hxDR = np.hstack((hxDR, xDR)) hxDR = np.hstack((hxDR, xDR))
hxTrue = np.hstack((hxTrue, xTrue)) hxTrue = np.hstack((hxTrue, xTrue))
if show_animation: if show_animation: # pragma: no cover
plt.cla() plt.cla()
plt.plot(RFID[:, 0], RFID[:, 1], "*k") plt.plot(RFID[:, 0], RFID[:, 1], "*k")
@@ -263,6 +262,5 @@ def main():
plt.pause(0.001) plt.pause(0.001)
if __name__ == '__main__': if __name__ == '__main__':
main() main()

View File

@@ -373,7 +373,7 @@ def main():
hxDR = np.hstack((hxDR, xDR)) hxDR = np.hstack((hxDR, xDR))
hxTrue = np.hstack((hxTrue, xTrue)) hxTrue = np.hstack((hxTrue, xTrue))
if show_animation: if show_animation: # pragma: no cover
plt.cla() plt.cla()
plt.plot(RFID[:, 0], RFID[:, 1], "*k") plt.plot(RFID[:, 0], RFID[:, 1], "*k")

View File

@@ -402,7 +402,7 @@ def main():
hxDR = np.hstack((hxDR, xDR)) hxDR = np.hstack((hxDR, xDR))
hxTrue = np.hstack((hxTrue, xTrue)) hxTrue = np.hstack((hxTrue, xTrue))
if show_animation: if show_animation: # pragma: no cover
plt.cla() plt.cla()
plt.plot(RFID[:, 0], RFID[:, 1], "*k") plt.plot(RFID[:, 0], RFID[:, 1], "*k")

View File

@@ -295,14 +295,13 @@ def main():
xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID) xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)
hz.append(z) hz.append(z)
if dtime >= show_graph_dtime: if dtime >= show_graph_dtime:
x_opt = graph_based_slam(hxDR, hz) x_opt = graph_based_slam(hxDR, hz)
dtime = 0.0 dtime = 0.0
if show_animation: if show_animation: # pragma: no cover
plt.cla() plt.cla()
plt.plot(RFID[:, 0], RFID[:, 1], "*k") plt.plot(RFID[:, 0], RFID[:, 1], "*k")

View File

@@ -33,7 +33,7 @@ def ICP_matching(ppoints, cpoints):
while dError >= EPS: while dError >= EPS:
count += 1 count += 1
if show_animation: if show_animation: # pragma: no cover
plt.cla() plt.cla()
plt.plot(ppoints[0, :], ppoints[1, :], ".r") plt.plot(ppoints[0, :], ppoints[1, :], ".r")
plt.plot(cpoints[0, :], cpoints[1, :], ".b") plt.plot(cpoints[0, :], cpoints[1, :], ".b")