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