Rospy: topic delays and asynchronous behavior

edit

Hi there! :)

My main goal/idea: I'm trying to implement some basic formation flight for quadrotors with ROS in the V-REP simulator.

Problem: Before I even started with the formation flight dynamics or the formation itself I wanted to test if I can control two (or more) multicopters 'simultaneously' (sequentially is acceptable if its nearly indistinguishable behavior for me). This is when I noticed strange delays which seemed to increase over time (see https://youtu.be/iKRSo5yZ3rg). Long bug hunting sessions and research brought me to this question (& answer) on here: answers.ros.org/[...]/unexpected-delay-in-rospy-subscriber/ After adding the mentioned buff_size to my subscribers I now have nearly eliminated the delay but face another problem (maybe it's still the same but not yet resolved) in which my quadrotors now fly more asynchronous and not at the same time or at least successively (1-2-1-2 etc.). Possibly this behavior existed before but was not noticeable with the delays.

Examples: https://youtu.be/0CCGFzQ9Jbg and https://youtu.be/aQHChi48OUY (higher rospy-rate)

The setup:

Version: ROS Melodic

Code: Rospy-scripts and own 'msg' & launch files

My platform: Kubuntu 18.04 under Windows 10 host (VirtualBox)

Node overview: (https://imgur.com/a/hexN3b2)

Msg:

"Drone.msg"

int8 number
float32 x
float32 y
float32 z

Code (superfluous 'global' declarations etc. omitted):

"simulation_connector"

[...]
queue_size_var = 1
buff_size_var = 2**24
tcp_nodelay_var = False

def main():

    [...]
    sim_call_publisher = rospy.Publisher('calls_from_sim', Drone, queue_size=2)

    rospy.init_node('Simulation Connector', anonymous=True)

    get_get_names = rospy.Subscriber('topic_names_getTargetPos', String, callback_to_get_drone_target_position_topic_names)
    get_set_names = rospy.Subscriber('topic_names_setTargetPos', String, callback_to_set_drone_target_position_topic_names)
    rospy.Subscriber('commands_to_connector', Drone, callback_to_set_new_position, queue_size=queue_size_var, buff_size=buff_size_var, tcp_nodelay=tcp_nodelay_var)

    targeted_run_rate = rospy.Rate(rate_frequency_in_hz)
    targeted_run_rate.sleep()

    while number_of_recognized_topics < number_of_drones*2:
        targeted_run_rate.sleep()

    [...]

    publisher_init()
    subscriber_init()

    while not rospy.is_shutdown():
        targeted_run_rate.sleep()

def callback_to_get_drone_target_position_topic_names(data):
    [...]

    if data.data not in get_drone_target_position_topic_names:
        rospy.loginfo(rospy.get_caller_id() + ' New topic name for getTargetPos: %s', data.data)
        get_drone_target_position_topic_names.append(data.data)    
        number_of_recognized_topics += 1
    else:
        rospy.loginfo(rospy.get_caller_id() + ' Already have topic name for getTargetPos: %s', data.data)
    targeted_run_rate.sleep()


def callback_to_set_drone_target_position_topic_names(data):
    [...]

    if data.data not in set_drone_target_position_topic_names:
        rospy.loginfo(rospy.get_caller_id() + ' New topic name for setTargetPos: %s', data.data)
        set_drone_target_position_topic_names.append(data.data)    
        number_of_recognized_topics += 1
    else:
        rospy.loginfo(rospy.get_caller_id() + ' Already have topic name for setTargetPos: %s', data.data)
    targeted_run_rate.sleep()


def callback_to_set_new_position(data):
    [...]
    new_pos = Point(float(data.x), float(data.y), float(data.z))

    rospy.loginfo(rospy.get_caller_id() + ' New position for %d --> x: %f y: %f z: %f', data.number, new_pos.x, new_pos.y, new_pos.z)

    set_drone_target_position_publishers[data.number].publish(new_pos)


def publisher_init():
    [...]
    for i in range(number_of_drones):
        set_drone_target_position_publishers.append(rospy.Publisher(set_drone_target_position_topic_names[i], Point, queue_size=2))


def subscriber_init():
    [...]
    for i in range(number_of_drones):
        get_drone_target_position_subscribers.append(rospy.Subscriber(get_drone_target_position_topic_names[i], Point, forward_target_positions, i, queue_size=queue_size_var, buff_size=buff_size_var, tcp_nodelay=tcp_nodelay_var))


def forward_target_positions(data, args):
    rospy.loginfo(rospy.get_caller_id() + ' I got the position from %d --> x: %f y: %f z: %f', args, data.x, data.y, data.z)
    global sim_call_publisher
    sim_call_publisher.publish(Drone(args, data.x, data.y, data.z))


if __name__ == '__main__':
    try:
        main()
    except rospy.ROSInterruptException:
        pass

"connection_observation_module"

[...]
position_offset = 0.1
queue_size_var = 1 ...