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 cmdvel is set in the odomcallback, which is skipped), and the only thing printed in the terminal is :

entering scan callback / goal cell : [50,18] / leaving scan callback

In a loop.

Thank you in advancd for your help.