def og_cb(og_msg, param_list): global occupancy_difference_threshold, connected_comonents_size_threshold rospy.loginfo('og_cb called') diff_og = param_list[0] curr_og = rog.og_msg_to_og3d(og_msg, to_binary=False) if diff_og == None: param_list[0] = curr_og return pog.subtract(diff_og, curr_og) param_list[0] = curr_og diff_og.to_binary(occupancy_difference_threshold) # filter the noise connect_structure = np.zeros((3, 3, 3), dtype=int) connect_structure[1, 1, :] = 1 # connect_structure[1,1,0] = 0 diff_og.grid = ni.binary_opening(diff_og.grid, connect_structure, iterations=1) # diff_og.grid, n_labels = diff_og.connected_comonents(connected_comonents_size_threshold) print 'np.all(diff_og == 0)', np.all(diff_og.grid == 0) diff_og_msg = rog.og3d_to_og_msg(diff_og) diff_og_msg.header.frame_id = og_msg.header.frame_id diff_og_msg.header.stamp = og_msg.header.stamp param_list[1].publish(diff_og_msg)
def og_cb(og_msg, param_list): global occupancy_difference_threshold, connected_comonents_size_threshold rospy.loginfo('og_cb called') diff_og = param_list[0] curr_og = rog.og_msg_to_og3d(og_msg, to_binary = False) if diff_og == None: param_list[0] = curr_og return pog.subtract(diff_og, curr_og) param_list[0] = curr_og diff_og.to_binary(occupancy_difference_threshold) # filter the noise connect_structure = np.zeros((3,3,3), dtype=int) connect_structure[1,1,:] = 1 # connect_structure[1,1,0] = 0 diff_og.grid = ni.binary_opening(diff_og.grid, connect_structure, iterations = 1) # diff_og.grid, n_labels = diff_og.connected_comonents(connected_comonents_size_threshold) print 'np.all(diff_og == 0)', np.all(diff_og.grid == 0) diff_og_msg = rog.og3d_to_og_msg(diff_og) diff_og_msg.header.frame_id = og_msg.header.frame_id diff_og_msg.header.stamp = og_msg.header.stamp param_list[1].publish(diff_og_msg)
def vis_occupancy_cb(og, param_list): og3d = rog.og_msg_to_og3d(og, to_binary=False) param_list[0] = og3d param_list[1] = True
def relay_cb(og, og_pub): rospy.logout('relay_cb called') og3d = rog.og_msg_to_og3d(og) og_new = rog.og3d_to_og_msg(og3d) og_pub.publish(og_new)
def mayavi_cb(og, param_list): og3d = rog.og_msg_to_og3d(og) param_list[0] = og3d param_list[1] = True
def vis_occupancy_cb(og, param_list): og3d = rog.og_msg_to_og3d(og, to_binary = False) param_list[0] = og3d param_list[1] = True