ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Subscribers not making callbacks

asked 2017-08-02 10:24:28 -0500

j3ven7 gravatar image

updated 2017-08-02 11:17:20 -0500

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()
edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted
3

answered 2017-08-02 11:27:03 -0500

Almost certainly the issue has to do with scoping in Python. In your callbacks you likely have some line looking approximately like gps_xy = msg.data.something. This is creating a new variable local to the callback that goes out of scope as soon as the callback is done running. Here are several ways you could fix this:

  • Force the variables that you want updated to be global variables with the global keyword in your callbacks.
  • Make the variables, callbacks, and pubs/subs part of a class. Then your callback will receive a copy of self as its first argument, and you'll be able to access self.gps_xy
  • Use the callback_args argument in the Subscriber instantiation method to pass additional arguments to your callback. In this case, it would be the variables that you would want updated within the callbacks.
edit flag offensive delete link more

Question Tools

2 followers

Stats

Asked: 2017-08-02 10:24:28 -0500

Seen: 2,242 times

Last updated: Aug 02 '17