Skip to content

Commit

Permalink
add # pragma: no cover
Browse files Browse the repository at this point in the history
  • Loading branch information
AtsushiSakai committed Feb 3, 2019
1 parent f0b6b7a commit af854d6
Show file tree
Hide file tree
Showing 25 changed files with 55 additions and 58 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -656,7 +656,7 @@ def main():
print(f'Converged after {it + 1} iterations.')
break

if show_animation:
if show_animation: # pragma: no cover
plot_animation(X, U)

print("done!!")
Expand Down
2 changes: 1 addition & 1 deletion Mapping/circle_fitting/circle_fitting.py
Original file line number Diff line number Diff line change
Expand Up @@ -122,7 +122,7 @@ def main():
ex, ey, er, error = circle_fitting(x, y)
print("Error:", error)

if show_animation:
if show_animation: # pragma: no cover
plt.cla()
plt.axis("equal")
plt.plot(0.0, 0.0, "*r")
Expand Down
2 changes: 1 addition & 1 deletion Mapping/gaussian_grid_map/gaussian_grid_map.py
Original file line number Diff line number Diff line change
Expand Up @@ -71,7 +71,7 @@ def main():
gmap, minx, maxx, miny, maxy = generate_gaussian_grid_map(
ox, oy, xyreso, STD)

if show_animation:
if show_animation: # pragma: no cover
plt.cla()
draw_heatmap(gmap, minx, maxx, miny, maxy, xyreso)
plt.plot(ox, oy, "xr")
Expand Down
2 changes: 1 addition & 1 deletion Mapping/kmeans_clustering/kmeans_clustering.py
Original file line number Diff line number Diff line change
Expand Up @@ -161,7 +161,7 @@ def main():
clusters = kmeans_clustering(rx, ry, ncluster)

# for animation
if show_animation:
if show_animation: # pragma: no cover
plt.cla()
inds = calc_association(cx, cy, clusters)
for ic in inds:
Expand Down
3 changes: 2 additions & 1 deletion Mapping/raycasting_grid_map/raycasting_grid_map.py
Original file line number Diff line number Diff line change
Expand Up @@ -121,7 +121,8 @@ def main():
oy = (np.random.rand(4) - 0.5) * 10.0
pmap, minx, maxx, miny, maxy, xyreso = generate_ray_casting_grid_map(
ox, oy, xyreso, yawreso)
if show_animation:

if show_animation: # pragma: no cover
plt.cla()
draw_heatmap(pmap, minx, maxx, miny, maxy, xyreso)
plt.plot(ox, oy, "xr")
Expand Down
6 changes: 3 additions & 3 deletions PathPlanning/AStar/a_star.py
Original file line number Diff line number Diff line change
Expand Up @@ -68,7 +68,7 @@ def a_star_planning(sx, sy, gx, gy, ox, oy, reso, rr):
current = openset[c_id]

# show graph
if show_animation:
if show_animation: # pragma: no cover
plt.plot(current.x * reso, current.y * reso, "xc")
if len(closedset.keys()) % 10 == 0:
plt.pause(0.001)
Expand Down Expand Up @@ -214,7 +214,7 @@ def main():
ox.append(40.0)
oy.append(60.0 - i)

if show_animation:
if show_animation: # pragma: no cover
plt.plot(ox, oy, ".k")
plt.plot(sx, sy, "xr")
plt.plot(gx, gy, "xb")
Expand All @@ -223,7 +223,7 @@ def main():

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.show()

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -313,7 +313,7 @@ def test1(max_vel=0.5):
for j, t in enumerate(times):
state[:, j] = traj.calc_traj_point(t)

if show_animation:
if show_animation: # pragma: no cover
# plot the path
plt.plot(state[0, :], state[1, :])
plt.pause(1.0)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -343,7 +343,7 @@ def main():
print("Goal")
break

if show_animation:
if show_animation: # pragma: no cover
plt.cla()
plt.plot(tx, ty)
plt.plot(ob[:, 0], ob[:, 1], "xk")
Expand All @@ -356,7 +356,7 @@ def main():
plt.pause(0.0001)

print("Finish")
if show_animation:
if show_animation: # pragma: no cover
plt.grid(True)
plt.pause(0.0001)
plt.show()
Expand Down
2 changes: 1 addition & 1 deletion PathPlanning/LQRRRTStar/lqr_rrt_star.py
Original file line number Diff line number Diff line change
Expand Up @@ -304,7 +304,7 @@ def main():
path = rrt.planning(animation=show_animation)

# Draw final path
if show_animation:
if show_animation: # pragma: no cover
rrt.draw_graph()
plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
plt.grid(True)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -123,7 +123,7 @@ def optimize_trajectory(target, k0, p):
p += alpha * np.array(dp)
# print(p.T)

if show_animation:
if show_animation: # pragma: no cover
show_trajectory(target, xc, yc)
else:
xc, yc, yawc, p = None, None, None, None
Expand Down
4 changes: 2 additions & 2 deletions PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py
Original file line number Diff line number Diff line change
Expand Up @@ -393,7 +393,7 @@ def test():
px, py, pyaw, mode, clen = reeds_shepp_path_planning(
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.plot(px, py, label="final course " + str(mode))

Expand Down Expand Up @@ -430,7 +430,7 @@ def main():
px, py, pyaw, mode, clen = reeds_shepp_path_planning(
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.plot(px, py, label="final course " + str(mode))

Expand Down
8 changes: 4 additions & 4 deletions PathPlanning/VoronoiRoadMap/voronoi_road_map.py
Original file line number Diff line number Diff line change
Expand Up @@ -78,7 +78,7 @@ def VRM_planning(sx, sy, gx, gy, ox, oy, rr):
obkdtree = KDTree(np.vstack((ox, oy)).T)

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")

road_map = generate_roadmap(sample_x, sample_y, rr, obkdtree)
Expand Down Expand Up @@ -183,7 +183,7 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y):
current = openset[c_id]

# 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.pause(0.001)

Expand Down Expand Up @@ -287,7 +287,7 @@ def main():
ox.append(40.0)
oy.append(60.0 - i)

if show_animation:
if show_animation: # pragma: no cover
plt.plot(ox, oy, ".k")
plt.plot(sx, sy, "^r")
plt.plot(gx, gy, "^c")
Expand All @@ -298,7 +298,7 @@ def main():

assert rx, 'Cannot found path'

if show_animation:
if show_animation: # pragma: no cover
plt.plot(rx, ry, "-r")
plt.show()

Expand Down
2 changes: 1 addition & 1 deletion PathTracking/cgmres_nmpc/cgmres_nmpc.py
Original file line number Diff line number Diff line change
Expand Up @@ -584,7 +584,7 @@ def main():
# update state
plant_system.update_state(u_1s[0], u_2s[0])

if show_animation:
if show_animation: # pragma: no cover
animation(plant_system, controller, dt)
plot_figures(plant_system, controller, iteration_num, dt)

Expand Down
16 changes: 8 additions & 8 deletions PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py
Original file line number Diff line number Diff line change
@@ -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
Expand All @@ -8,11 +13,6 @@
import sys
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
Q = np.eye(5)
Expand Down Expand Up @@ -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.axis("equal")
plt.grid(True)
plt.title("speed[km/h]:" + str(round(state.v * 3.6, 2)) +
",target index:" + str(target_ind))
plt.title("speed[km/h]:" + str(round(state.v * 3.6, 2))
+ ",target index:" + str(target_ind))
plt.pause(0.0001)

return t, x, y, yaw, v
Expand Down Expand Up @@ -257,7 +257,7 @@ def main():

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.subplots(1)
plt.plot(ax, ay, "xb", label="waypoints")
Expand Down
2 changes: 1 addition & 1 deletion PathTracking/lqr_steer_control/lqr_steer_control.py
Original file line number Diff line number Diff line change
Expand Up @@ -255,7 +255,7 @@ def main():

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.subplots(1)
plt.plot(ax, ay, "xb", label="input")
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -427,7 +427,7 @@ def do_simulation(cx, cy, cyaw, ck, sp, dl, initial_state):
print("Goal")
break

if show_animation:
if show_animation: # pragma: no cover
plt.cla()
if ox is not None:
plt.plot(ox, oy, "xr", label="MPC")
Expand Down Expand Up @@ -563,7 +563,7 @@ def main():
t, x, y, yaw, v, d, a = do_simulation(
cx, cy, cyaw, ck, sp, dl, initial_state)

if show_animation:
if show_animation: # pragma: no cover
plt.close("all")
plt.subplots()
plt.plot(cx, cy, "-r", label="spline")
Expand Down Expand Up @@ -596,7 +596,7 @@ def main2():
t, x, y, yaw, v, d, a = do_simulation(
cx, cy, cyaw, ck, sp, dl, initial_state)

if show_animation:
if show_animation: # pragma: no cover
plt.close("all")
plt.subplots()
plt.plot(cx, cy, "-r", label="spline")
Expand Down
2 changes: 1 addition & 1 deletion PathTracking/move_to_pose/move_to_pose.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
y = y + v * np.sin(theta) * dt

if show_animation:
if show_animation: # pragma: no cover
plt.cla()
plt.arrow(x_start, y_start, np.cos(theta_start),
np.sin(theta_start), color='r', width=0.1)
Expand Down
4 changes: 2 additions & 2 deletions PathTracking/pure_pursuit/pure_pursuit.py
Original file line number Diff line number Diff line change
Expand Up @@ -126,7 +126,7 @@ def main():
v.append(state.v)
t.append(time)

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

if show_animation:
if show_animation: # pragma: no cover
plt.cla()
plt.plot(cx, cy, ".r", label="course")
plt.plot(x, y, "-b", label="trajectory")
Expand Down
9 changes: 4 additions & 5 deletions PathTracking/rear_wheel_feedback/rear_wheel_feedback.py
Original file line number Diff line number Diff line change
@@ -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.
Expand All @@ -8,10 +11,6 @@
import sys
sys.path.append("../../PathPlanning/CubicSpline/")

import math
import matplotlib.pyplot as plt
import cubic_spline_planner


Kp = 1.0 # speed propotional gain
# steering control parameter
Expand Down Expand Up @@ -202,7 +201,7 @@ def main():
# Test
assert goal_flag, "Cannot goal"

if show_animation:
if show_animation: # pragma: no cover
plt.close()
plt.subplots(1)
plt.plot(ax, ay, "xb", label="input")
Expand Down
4 changes: 2 additions & 2 deletions PathTracking/stanley_controller/stanley_controller.py
Original file line number Diff line number Diff line change
Expand Up @@ -184,7 +184,7 @@ def main():
v.append(state.v)
t.append(time)

if show_animation:
if show_animation: # pragma: no cover
plt.cla()
plt.plot(cx, cy, ".r", label="course")
plt.plot(x, y, "-b", label="trajectory")
Expand All @@ -197,7 +197,7 @@ def main():
# Test
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(x, y, "-b", label="trajectory")
plt.legend()
Expand Down
20 changes: 9 additions & 11 deletions SLAM/EKFSLAM/ekf_slam.py
Original file line number Diff line number Diff line change
Expand Up @@ -97,12 +97,12 @@ def observation(xTrue, xd, u, RFID):
def motion_model(x, u):

F = np.array([[1.0, 0, 0],
[0, 1.0, 0],
[0, 0, 1.0]])
[0, 1.0, 0],
[0, 0, 1.0]])

B = np.array([[DT * math.cos(x[2, 0]), 0],
[DT * math.sin(x[2, 0]), 0],
[0.0, DT]])
[DT * math.sin(x[2, 0]), 0],
[0.0, DT]])

x = (F @ x) + (B @ u)
return x
Expand All @@ -119,8 +119,8 @@ def jacob_motion(x, u):
(STATE_SIZE, LM_SIZE * calc_n_LM(x)))))

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, 0.0]])
[0.0, 0.0, DT * u[0] * math.cos(x[2, 0])],
[0.0, 0.0, 0.0]])

G = np.eye(STATE_SIZE) + Fx.T * jF * Fx

Expand Down Expand Up @@ -170,7 +170,7 @@ def calc_innovation(lm, xEst, PEst, z, LMid):
delta = lm - xEst[0:2]
q = (delta.T @ delta)[0, 0]
#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)]])
y = (z - zp).T
y[1] = pi_2_pi(y[1])
Expand All @@ -183,7 +183,7 @@ def calc_innovation(lm, xEst, PEst, z, LMid):
def jacobH(q, delta, x, i):
sq = math.sqrt(q)
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
nLM = calc_n_LM(x)
Expand Down Expand Up @@ -235,13 +235,12 @@ def main():

x_state = xEst[0:STATE_SIZE]


# store data history
hxEst = np.hstack((hxEst, x_state))
hxDR = np.hstack((hxDR, xDR))
hxTrue = np.hstack((hxTrue, xTrue))

if show_animation:
if show_animation: # pragma: no cover
plt.cla()

plt.plot(RFID[:, 0], RFID[:, 1], "*k")
Expand All @@ -263,6 +262,5 @@ def main():
plt.pause(0.001)



if __name__ == '__main__':
main()
Loading

0 comments on commit af854d6

Please sign in to comment.