def test_plot_icp_cluster2cluster(laser_a, laser_b):
# source: a.
mst_a = EMST(laser_a)
clusters_a = Cluster(mst_a, k=1.0, min_size=10)
# destination: b.
mst_b = EMST(laser_b)
clusters_b = Cluster(mst_b, k=1.0, min_size=10)
# icp
dst_indices, T = icp(laser_a.cartesian,
laser_b.cartesian,
init_pose=None,
max_iterations=20,
tolerance=0.001)
matched_cluster_indices, _ = match_clusters(dst_indices, laser_a, laser_b,
clusters_a, clusters_b)
# plot
plt.figure()
# colors to iterate over
colors_a = ['r', 'y'] * 20
colors_b = ['b', 'g'] * 20
# plot source clusters and arrows to destination
count = 1
for cluster, color, cluster_b_index in zip(clusters_a.clusters, colors_a,
matched_cluster_indices):
plt.scatter(laser_a.cartesian[cluster, [0]],
laser_a.cartesian[cluster, [1]],
s=10.0,
c=color,
linewidths=0)
if cluster_b_index >= 0:
# plot arrow points to the corresponding cluster
x1 = np.mean(laser_a.cartesian[cluster, [0]])
y1 = np.mean(laser_a.cartesian[cluster, [1]])
cluster_b = clusters_b.clusters[cluster_b_index]
x2 = np.mean(laser_b.cartesian[cluster_b, [0]])
y2 = np.mean(laser_b.cartesian[cluster_b, [1]])
dx = x2 - x1
dy = y2 - y1
plt.arrow(x1, y1, dx, dy, width=0.001, head_width=0.05)
plt.text((x1 + x2) / 2.0, (y1 + y2) / 2.0, str(count), fontsize=18)
count += 1
# plot destination clusters
for cluster, color in zip(clusters_b.clusters, colors_b):
plt.scatter(laser_b.cartesian[cluster, [0]],
laser_b.cartesian[cluster, [1]],
s=10.0,
c=color,
linewidths=0)
plt.show()
def realtime_test():
''' Internal test.
'''
# laser sub
laser = LaserSub()
# init loop
r = rospy.Rate(1.0)
while not rospy.is_shutdown(
): # wait until first scan is received so that laser is initialized
if laser.first_recieved_done: # laser specs initialized by ROS sensor_msgs/LaserScan message
break
else:
rospy.loginfo(
'[coarse_level_association] Waiting for laser to init.')
r.sleep()
laser_prev = None
clusters_prev = None
clusters_id = None
# init plot
fig, ax = plt.subplots()
ax.set_aspect('equal', adjustable='box')
ax.set_title('Clusters Tracking', fontsize=18)
ax.axis([-5.0, 5.0, -5.0, 5.0])
ax.plot([0], [0], marker='>', markersize=20,
color="red") # plot the origin
# loop
r = rospy.Rate(30)
while not rospy.is_shutdown():
# --- 1. laser input (copy self.laser at this moment)
laser_now = copy.deepcopy(laser)
# --- 2. EMST (create input graph for segmentation)
mst = EMST(laser_now)
# --- 3. EGBIS (apply image segmentation technique to clustering)
clusters_now = Cluster(mst, k=1.0, min_size=10)
if clusters_id is None: # first set of clusters generated
clusters_id = [i for i in range(len(clusters_now.clusters))]
# --- 4. ICP
if laser_prev is not None:
dst_indices, T = icp(laser_now.cartesian,
laser_prev.cartesian,
init_pose=None,
max_iterations=20,
tolerance=0.001)
matched_cluster_indices, _ = match_clusters(
dst_indices, laser_now, laser_prev, clusters_now,
clusters_prev)
# --- 5. Visualize
laser_a = laser_now
laser_b = laser_prev
clusters_a = clusters_now
clusters_b = clusters_prev
clusters_id_new = []
# colors to iterate over
colors = ['b', 'g', 'r', 'c', 'm', 'y', 'k'] * 20
# plot source clusters with the same id as the matched destination clusters
ax_list = []
for cluster, color, cluster_b_index in zip(
clusters_a.clusters, colors, matched_cluster_indices):
a = ax.scatter(laser_a.cartesian[cluster, [0]],
laser_a.cartesian[cluster, [1]],
s=20.0,
c=color,
linewidths=0)
ax_list.append(a)
if cluster_b_index >= 0:
# show id
x1 = np.mean(laser_a.cartesian[cluster, [0]])
y1 = np.mean(laser_a.cartesian[cluster, [1]])
cluster_b = clusters_b.clusters[cluster_b_index]
x2 = np.mean(laser_b.cartesian[cluster_b, [0]])
y2 = np.mean(laser_b.cartesian[cluster_b, [1]])
_id = clusters_id[cluster_b_index]
clusters_id_new.append(_id)
a = ax.text((x1 + x2) / 2.0, (y1 + y2) / 2.0,
str(_id),
fontsize=22)
ax_list.append(a)
else: # no cluster_b match me.
_id = max(clusters_id) + 1
clusters_id_new.append(_id)
clusters_id = clusters_id_new
# plot
plt.pause(1e-12) # pause for real time display
for a in ax_list: # clear data on the plot
a.remove()
# end of loop
laser_prev = laser_now
clusters_prev = clusters_now
r.sleep()
# plot
plt.show()
def realtime_test():
# rate
rate = 3
# --- joint states
x = JointStates()
# laser sub
laser = LaserSub()
# init loop
r = rospy.Rate(1.0)
while not rospy.is_shutdown(
): # wait until first scan is received so that laser is initialized
if laser.first_recieved_done: # laser specs initialized by ROS sensor_msgs/LaserScan message
break
else:
rospy.loginfo('[test_coarse_level] Waiting for laser to init.')
r.sleep()
laser_prev = None
clusters_prev = None
# init plot
fig, ax = plt.subplots()
ax.set_aspect('equal', adjustable='box')
ax.set_title('Clusters Tracking', fontsize=22)
ax.axis([-5.0, 5.0, -5.0, 5.0])
ax.plot([0], [0], marker='>', markersize=20,
color="blue") # plot the origin
r = rospy.Rate(rate)
while not rospy.is_shutdown():
# ========================================= loop =========================================
_time = time.time()
# --- 1. laser input (copy self.laser at this moment)
laser_now = copy.deepcopy(laser)
#time_1 = time.time()
#duration_1 = time_1 - _time
# --- 2. EMST (create input graph for segmentation)
mst = EMST(laser_now, neighbor_size=10)
#time_2 = time.time()
#duration_2 = time_2 - time_1
# --- 3. EGBIS (apply image segmentation technique to clustering)
clusters_now = Cluster(mst, k=3.0, min_size=5)
#time_3 = time.time()
#duration_3 = time_3 - time_2
# --- 4. Coarse-level data association (ICP): update JointState.matched_track_indices
CoarseLevel(laser_now,
laser_prev,
clusters_now,
clusters_prev,
x,
tentative_threshold=10,
icp_max_dist=1.0)
#time_4 = time.time()
#duration_4 = time_4 - time_3
# --- 5. fake fine-level (no measurement matching, just replace with new ones)
assert len(x.xt) == len(x.xp)
assert len(x.matched_track_indices) == len(clusters_now.clusters)
for i in range(len(x.xt)):
xp_i = np.array([[], []])
# collect all cluster that matches this track
for cluster, track_i in zip(clusters_now.clusters,
x.matched_track_indices):
if track_i == i:
x_i = laser_now.cartesian[cluster, 0]
y_i = laser_now.cartesian[cluster, 1]
p_i = np.array([list(x_i), list(y_i)])
xp_i = np.concatenate((xp_i, p_i), axis=1)
# xp
x.xp[i] = xp_i
# xt
gamma_i = np.mean(x.xp[i][0])
delta_i = np.mean(x.xp[i][1])
dx = gamma_i - x.xt[i][0]
dy = delta_i - x.xt[i][1]
xt_i = np.array([
[gamma_i], # x
[delta_i], # y
[0], # theta
[dx], # vx
[dy], # vy
[0]
]) # vtheta
x.xt[i] = xt_i
#time_5 = time.time()
#duration_5 = time_5 - time_4
# --- 6. visualization
#colors = ['b', 'g', 'r', 'c', 'm', 'y', 'k']*20
ax_list = []
for xp_i, xt_i, xt_counter, xt_id in zip(x.xp, x.xt, x.xt_counter,
x.xt_id):
# xt - arrow
_x = xt_i[0][0]
_y = xt_i[1][0]
dx = (xt_i[3][0]) * 5 # enlarge the arrow length
dy = (xt_i[4][0]) * 5
a = ax.arrow(_x, _y, dx, dy, width=0.0001, head_width=0.05)
ax_list.append(a)
# xt - counter
if xt_counter < 0:
color = 'g' # mature
text = str(xt_id)
else:
color = 'r' # tentative
text = str(xt_id) + '(' + str(xt_counter) + ')'
# xp- box
left = min(xp_i[0])
bottom = min(xp_i[1])
width = abs(max(xp_i[0]) - left)
height = abs(max(xp_i[1]) - bottom)
p = plt.Rectangle((left, bottom),
width,
height,
fill=False,
edgecolor=color,
linewidth=2)
#p.set_transform(ax.transAxes)
#p.set_clip_on(False)
a = ax.add_patch(p)
ax_list.append(a)
# xt - text
top = bottom + height + 0.1
a = ax.text(left, top, text, color=color, fontsize=16)
ax_list.append(a)
# xp - measurements
a = ax.scatter(xp_i[0], xp_i[1], s=25.0, c='k', linewidths=0)
ax_list.append(a)
# plot
plt.pause(1e-12) # pause for real time display
for a in ax_list: # clear data on the plot
a.remove()
#time_6 = time.time()
#duration_6 = time_6 - time_5
# --- end of loop
laser_prev = laser_now
clusters_prev = clusters_now
duration = time.time() - _time
# rospy.loginfo(duration_1)
# rospy.loginfo(duration_2)
# rospy.loginfo(duration_3)
# rospy.loginfo(duration_4)
# rospy.loginfo(duration_5)
# rospy.loginfo(duration_6)
# rospy.loginfo(duration)
# rospy.loginfo('-------------')
if duration > 1.0 / rate:
rospy.loginfo(
'[track_2d_lidar] process_laser missed its control loop. It actually takes %f secs.'
% duration)
r.sleep()
# ========================================= loop =========================================
# plot
plt.show()
def realtime():
# params
k = rospy.get_param('tracking/EGBIS/k', 1.0) # node_name/a/a
min_size = rospy.get_param('tracking/EGBIS/min_size', 5)
icp_max_dist = rospy.get_param('tracking/ICP/icp_max_dist', 1.0)
inflation_size = rospy.get_param('tracking/CheckStatic/inflation_size', 8)
speed_threshold = rospy.get_param('tracking/CheckStatic/speed_threshold',
0.2)
static_threshold = rospy.get_param('tracking/CheckStatic/static_threshold',
0.7)
bagfile = rospy.get_param('tracking/bagfile', 'simple')
use_amcl = rospy.get_param('tracking/use_amcl', True)
# rate
rate = 5
# joint states
x = State(rate)
# laser_odom sub
laser_odom = LaserOdomSub(use_amcl)
# process kf
kf = ProcessKF(x,
inflate_size=inflation_size,
static_threshold=static_threshold,
speed_threshold=speed_threshold,
prediction_time=3.0)
# init loop
r = rospy.Rate(1.0)
while not rospy.is_shutdown(
): # wait until first scan is received so that laser_odom is initialized
if laser_odom.first_cartesian_done: # laser_odom specs initialized by ROS sensor_msgs/LaserScan message
break
else:
rospy.loginfo(
'[test_coarse_level] Waiting for laser_odom_sub to init.')
r.sleep()
laser_prev = None
clusters_prev = None
# init plot
vt_plt = VisualizeTracking(x, kf)
# vt_plt = VisualizeFromCar(laser_odom, x)
# bagfile
bag_duration = get_duration(
'/home/wuch/tracking_ws/src/Tracking/tracking_2d_lidar/bagfiles/' +
bagfile + '.bag')
start_time = time.time()
# write history
with open(
'/home/wuch/tracking_ws/src/Tracking/tracking_2d_lidar/outputs/history.csv',
'w') as csvfile:
writer = csv.writer(csvfile)
r = rospy.Rate(rate)
#while not rospy.is_shutdown():
while (time.time() - start_time < bag_duration):
# ========================================= loop =========================================
_time = time.time()
# --- 1. laser_odom input (copy self.laser_odom at this moment)
laser_odom.is_copying = True
while (laser_odom.updated == False):
pass
laser_odom_now = copy.deepcopy(laser_odom)
laser_odom.is_copying = False
assert len(laser_odom.cartesian) == len(laser_odom_now.cartesian)
# scatter_laser(laser_odom_now)
#time_1 = time.time()
#duration_1 = time_1 - _time
# --- 2. EMST (create input graph for segmentation)
mst = EMST(laser_odom_now, neighbor_size=10)
# plot_mst(mst, laser_odom_now)
#time_2 = time.time()
#duration_2 = time_2 - time_1
# --- 3. EGBIS (apply image segmentation technique to clustering)
clusters_now = Cluster(mst, k=k, min_size=min_size)
# plot_segmentation(clusters_now, laser_odom_now)
# time_3 = time.time()
# duration_3 = time_3 - time_2
# --- 4. Coarse-level data association (ICP): update JointState.track_indices
CoarseLevel(laser_odom_now,
laser_prev,
clusters_now,
clusters_prev,
x,
tentative_threshold=0,
icp_max_dist=icp_max_dist)
# time_4 = time.time()
# duration_4 = time_4 - time_3
# --- 5. KF
kf.predict_update(laser_odom_now, clusters_now, rate)
#kf.pub_estimated_vel()
kf.publish_to_costmap()
# kf.publish_kf_state(laser_odom_now)
kf.publish_static_scan()
#time_5 = time.time()
#duration_5 = time_5 - time_4
# --- 6. visualization
vt_plt.plot()
# vt_plt.plot(laser_odom_now)
#time_6 = time.time()
#duration_6 = time_6 - time_5
# --- end of loop
laser_prev = laser_odom_now
clusters_prev = clusters_now
# --- efficiency monitor
duration = time.time() - _time
# rospy.loginfo(duration_1)
# rospy.loginfo(duration_2)
# rospy.loginfo(duration_3)
# rospy.loginfo(duration_4)
# rospy.loginfo(duration_5)
# rospy.loginfo(duration_6)
# rospy.loginfo(duration)
# rospy.loginfo('-------------')
if duration > 1.0 / rate:
rospy.loginfo(
'[tracking] tracking missed its control loop. It actually takes %f secs.'
% duration)
r.sleep()
# ========================================= loop =========================================
for x_list, y_list in zip(kf.x_history, kf.y_history):
writer.writerow(list(x_list))
writer.writerow(list(y_list))
def run(self, process_odom_rate, process_laser_rate):
""" The working loop.
"""
########################
# prepare for the loop
########################
# scan visualize
#plt = PlotVisual(axis=[-5.0,5.0,-5.0,5.0])
# previous state
laser_prev = None
clusters_prev = None
########################
# beginning of the loop with controled frequency
########################
count = 0
multiple = int(process_odom_rate/process_laser_rate)
r = rospy.Rate(process_odom_rate)
while not rospy.is_shutdown():
count += 1
# process lidar
if count == multiple:
count = 0
_time = time.time()
# --- 1. laser input (copy self.laser at this moment)
laser_now = copy.deepcopy(self.laser)
# --- 2. EMST (create input graph for segmentation)
mst = EMST(laser_now, neighbor_size=10)
# --- 3. EGBIS (apply image segmentation technique to clustering)
clusters = Cluster(mst, k=1.0, min_size=10)
# --- 4. Coarse-level data association (ICP): update JointState.matched_track_indices
CoarseLevel(laser_now, laser_prev, clusters, clusters_prev, self.x, tentative_threshold = 5, icp_max_dist=0.5)
# --- 5. Fine-level data association
# --- visualization
_laser_plot = True
#plt.visualize(laser_now, [_laser_plot, mst, clusters])
#plt.visualize(laser_now, [_laser_plot, None, None])
#plt.visualize(laser_now, [None, mst, None])
#plt.visualize(laser_now, [None, None, clusters])
# --- end of the loop
laser_prev = laser_now
clusters_prev = clusters
# --- check loop rate
duration = time.time() - _time
rospy.loginfo(duration)
if duration > 1.0/process_laser_rate:
rospy.loginfo('[track_2d_lidar] Processing laser missed its control loop. It actually takes %f secs.' % duration)
# process odom
else:
_time = time.time()
# process odom
self.process_odom.run(odom=self.odom, x=self.x)
# check loop rate
duration = time.time() - _time
#rospy.loginfo(duration)
if duration > 1.0/process_odom_rate:
rospy.loginfo('[track_2d_lidar] Processing odom missed its control loop. It actually takes %f secs.' % duration)
r.sleep()
def realtime_test():
# rosparam
k = rospy.get_param('tracking/EGBIS/k', 1.0) # node_name/a/a
min_size = rospy.get_param('tracking/EGBIS/min_size', 5)
icp_max_dist = rospy.get_param('tracking/ICP/icp_max_dist', 1.0)
inflation_size = rospy.get_param('tracking/CheckStatic/inflation_size', 8)
speed_threshold = rospy.get_param('tracking/CheckStatic/speed_threshold',
0.2)
static_threshold = rospy.get_param('tracking/CheckStatic/static_threshold',
0.7)
# rate
rate = 5
# joint states
x = State(rate)
# laser_odom sub
laser_odom = LaserSub()
# process kf
kf = ProcessKF(x,
inflate_size=inflation_size,
static_threshold=static_threshold,
speed_threshold=speed_threshold,
prediction_time=3.0)
# init loop
r = rospy.Rate(1.0)
while not rospy.is_shutdown(
): # wait until first scan is received so that laser_odom is initialized
if laser_odom.first_recieved_done: # laser_odom specs initialized by ROS sensor_msgs/LaserScan message
break
else:
rospy.loginfo('[test_coarse_level] Waiting for laser_sub to init.')
r.sleep()
laser_prev = None
clusters_prev = None
# init plot
vt_plt = VisualizeTracking(x, kf)
r = rospy.Rate(rate)
while not rospy.is_shutdown():
# ========================================= loop =========================================
_time = time.time()
# --- 1. laser_odom input (copy self.laser_odom at this moment)
laser_odom_now = copy.deepcopy(laser_odom)
#time_1 = time.time()
#duration_1 = time_1 - _time
# --- 2. EMST (create input graph for segmentation)
mst = EMST(laser_odom_now, neighbor_size=10)
#time_2 = time.time()
#duration_2 = time_2 - time_1
# --- 3. EGBIS (apply image segmentation technique to clustering)
clusters_now = Cluster(mst, k=k, min_size=min_size)
# time_3 = time.time()
# duration_3 = time_3 - time_2
# --- 4. Coarse-level data association (ICP): update JointState.track_indices
CoarseLevel(laser_odom_now,
laser_prev,
clusters_now,
clusters_prev,
x,
tentative_threshold=0,
icp_max_dist=icp_max_dist)
# time_4 = time.time()
# duration_4 = time_4 - time_3
# --- 5. KF
kf.predict_update(laser_odom_now, clusters_now, rate)
kf.publish_kf_state()
#time_5 = time.time()
#duration_5 = time_5 - time_4
# --- 6. visualization
vt_plt.plot()
#time_6 = time.time()
#duration_6 = time_6 - time_5
# --- end of loop
laser_prev = laser_odom_now
clusters_prev = clusters_now
# --- efficiency monitor
duration = time.time() - _time
# rospy.loginfo(duration_1)
# rospy.loginfo(duration_2)
# rospy.loginfo(duration_3)
# rospy.loginfo(duration_4)
# rospy.loginfo(duration_5)
# rospy.loginfo(duration_6)
# rospy.loginfo(duration)
# rospy.loginfo('-------------')
if duration > 1.0 / rate:
rospy.loginfo(
'[tracking] tracking missed its control loop. It actually takes %f secs.'
% duration)
r.sleep()
# ========================================= loop =========================================
vt_plt.show()