mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-04-22 03:00:22 -04:00
add # pragma: no cover
This commit is contained in:
@@ -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!!")
|
||||||
|
|||||||
@@ -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")
|
||||||
|
|||||||
@@ -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")
|
||||||
|
|||||||
@@ -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:
|
||||||
|
|||||||
@@ -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")
|
||||||
|
|||||||
@@ -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()
|
||||||
|
|
||||||
|
|||||||
@@ -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)
|
||||||
|
|||||||
@@ -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()
|
||||||
|
|||||||
@@ -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)
|
||||||
|
|||||||
@@ -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
|
||||||
|
|||||||
@@ -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))
|
||||||
|
|
||||||
|
|||||||
@@ -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()
|
||||||
|
|
||||||
|
|||||||
@@ -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)
|
||||||
|
|
||||||
|
|||||||
@@ -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")
|
||||||
|
|||||||
@@ -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")
|
||||||
|
|||||||
@@ -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")
|
||||||
|
|||||||
@@ -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)
|
||||||
|
|||||||
@@ -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")
|
||||||
|
|||||||
@@ -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")
|
||||||
|
|||||||
@@ -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()
|
||||||
|
|||||||
@@ -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()
|
||||||
@@ -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")
|
||||||
|
|
||||||
|
|||||||
@@ -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")
|
||||||
|
|
||||||
|
|||||||
@@ -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")
|
||||||
|
|||||||
@@ -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")
|
||||||
|
|||||||
Reference in New Issue
Block a user