Rospy node with 2 suscribers skips one of their callbacks

edit

Hello everyone,

I'm trying to make a node that suscribes from two topics (an odometry one and a lidar one), create a map of the environment and make the robot goes to a desired goal. I therefore have two callbacks functions, one linked to the odometry which fill the map with the position of the robot and publishes in /cmd_vel to move it and one linked to the lidar which fill the map with the position of the obstacles. My problem is, when I launch this node, it seems to skip randomly one of this callback (currently the odometry one, but it has not always been the case).

My main is the following :

def main():
rospy.init_node('mpoa', anonymous=True)

global cmd

# Creating the publisher
cmd = Twist()
# Creating the suscribers
rospy.Subscriber("/odom", Odometry, odom_callback, queue_size=1)
rospy.Subscriber("/scan", LaserScan, scan_callback, queue_size=1)
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
rate = rospy.Rate(1)


while not rospy.is_shutdown():
    # We don't need these commands
    cmd.linear.z = 0
    cmd.angular.y = 0
    cmd.angular.x = 0
    cmd.linear.y = 0
    pub.publish(cmd)
    #print("publishing command")
    rate.sleep()

rospy.spin()

And my two callbacks, being at the beginning of the code :

def odom_callback(odom):
global initgrid
global fieldgrid
global yaw
global cellX
global cellY
global cmd

xpos = odom.pose.pose.position.x  # get the X measurement in m of the robot pos
ypos = odom.pose.pose.position.y  # get the Y measurement in m of the robot pos
yaw = quater_to_euler(odom.pose.pose.orientation)[2]
# get the X coordinate of the cell where the robot is
cellX = int((resolution-1)/2) + int(xpos/size)
# get the Y coordinate of the cell where the robot is
cellY = int((resolution-1)/2) + int(ypos/size)
reset_robot_pos(initgrid)
initgrid[cellX, cellY] = -1


obj_angle_wf = math.atan2(fieldgrid[cellX,cellY,1], fieldgrid[cellX,cellY,0])
print("objective angle : " + str(obj_angle_wf)) 
print("yaw : " + str(yaw))
if yaw < obj_angle_wf - 0.2 or yaw > obj_angle_wf + 0.2:
    cmd.linear.x = 0
    cmd.angular.z = (obj_angle_wf-yaw)/2
    print("rotating")
    #print("angular speed applied : " + str(cmd.angular.z))
else:
    cmd.angular.z = 0
    normF = math.sqrt(fieldgrid[cellX,cellY,1]**2+fieldgrid[cellX,cellY,0]**2)
    cmd.linear.x = 0.2+normF/(3*resolution)
    print("moving forward")

def scan_callback(scan):
global ranges
global initgrid
global fieldgrid

print("entering scan callback")
ranges = scan.ranges
for i in range(len(ranges)):
    if ranges[i] < scan.range_max:
        angle_wf = yaw + i*scan.angle_increment
        while(angle_wf > math.pi*2):
            angle_wf -= math.pi*2
        dx_bot_to_range = ranges[i]*math.cos(angle_wf)
        dy_bot_to_range = ranges[i]*math.sin(angle_wf)

        cell_X_obstacle = cellX+int(dx_bot_to_range/size)
        cell_Y_obstacle = cellY+int(dy_bot_to_range/size)
        initgrid[cell_X_obstacle, cell_Y_obstacle] = 1

fieldgrid = create_att_field(initgrid) + create_rep_field(initgrid)
#np.set_printoptions(threshold=np.inf, suppress=True)
#print(fieldgrid)
print("leaving scan callback")

So currently, with the following code, the robot would not move (since the variable that is published in cmd_vel is set in the odom_callback, which is skipped), and the only thing printed in the ... (more)