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