Subscribers not making callbacks
I have a listener function that subscribes to two topics as shown below, but the callbacks (which I confirmed are updating) don't seem to update some variables in the listener. I confirm that the variable (in this case gps_xy) is updated in the gps callback, but for some reason, the value is never modified in listener(). I thought since python is multi-threaded that spinning should not be necessary but I could be wrong. Any ideas? Also this code is executable, I call the listener in a standard if __name__ == '__main__':
def listener():
pub = rospy.Publisher('kalman_pub', Float32MultiArray, queue_size=10)
rospy.init_node('kalman', anonymous=True)
rospy.Subscriber("bike_state", Float32MultiArray, bike_state)
rospy.Subscriber("gps", Float32MultiArray, gps)
#rospy.spin()
rate = rospy.Rate(5)
#Run until the nodes are shutdown (end.sh run OR start.sh was killed)
while not rospy.is_shutdown():
dim = [MultiArrayDimension('data', 1, 4)]
layout = MultiArrayLayout(dim, 0)
# The Kalman filter wants the GPS data in matrix form
#Build matrix from gps x,y coordinates and bike velocity and yaw
gps_matrix = np.matrix(gps_xy + bike_yv + time_step)
#rospy.loginfo(gps_matrix)
#save gps state values for later plotting
gps_data.append(gps_matrix)
C = np.matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
#If we have
if len(gps_data) == 1:
P_initial = np.matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
x_pos = gps_matrix[:,0]
y_pos = gps_matrix[:,1]
yaw = gps_matrix[:,2]
v = gps_matrix[:,3]
v_0 = v.item(0)
yaw_0 = yaw.item(0)
x_dot_0 = v_0 * np.cos(yaw_0)
y_dot_0 = v_0 * np.sin(yaw_0)
s_initial = np.matrix([[x_pos.item(0)], [y_pos.item(0)], [x_dot_0], [y_dot_0]])
#output matrix - returns a tuple - first entry - kalman state (x,y,x',y')
# - second entry - prediction error (p)
output_matrix = kalman_real_time.kalman_no_loop(gps_matrix, C, s_initial, P_initial)
else:
output_matrix = kalman_real_time.kalman_no_loop(gps_matrix, C,
kalman_state_matrix, p_state_matrix)
#rospy.loginfo(output_matrix.shape)
kalman_state_matrix = output_matrix[0]
p_state_matrix = output_matrix[1]
#Change output_matrix to a standard array for publishing
kalman_state = output_matrix[0].flatten().tolist()[0]
#rospy.loginfo(kalman_state)
p_state = output_matrix[1].flatten()
#save predicted state values for later plotting
#kalman_data.append(kalman_state_matrix)
#pub.publish(layout, kalman_state)
pub.publish(layout, kalman_state)
rate.sleep()