Connection failure for 3 or more crazyflies

kevinaubertlomellini commented 9 months ago

I install the simulator and is working very well with 1 or 2 crazyflies. However, when I try to use 3 or more crazyflies on Gazebo. The program is not able to connect all the crazyflies, just making a connection for 1 or 2 at much. For example for 3 crazyflies on the Gazebo simulator I obtain the next when I launch the command ros2 launch crazyflie launch.py backend:=cflib: [INFO] [launch]: All log files can be found below /home/kevin/.ros/log/2024-02-07-12-56-40-422276-UBUNTU-ROG-112994 [INFO] [launch]: Default logging verbosity is set to INFO [INFO] [motion_capture_tracking_node-1]: process started with pid [112995] [INFO] [teleop-2]: process started with pid [112997] [INFO] [joy_node-3]: process started with pid [112999]marhes [INFO] [crazyflie_server.py-4]: process started with pid [113001] [INFO] [rviz2-5]: process started with pid [113021] [motion_capture_tracking_node-1] logClouds=0 [teleop-2] [INFO] [1707335800.789703037] [teleop]: Mode changed to cmd_vel_world [rviz2-5] [INFO] [1707335801.642947720] [rviz2]: Stereo is NOT SUPPORTED [rviz2-5] [INFO] [1707335801.643116350] [rviz2]: OpenGl version: 4.6 (GLSL 4.6) [rviz2-5] [INFO] [1707335801.703454938] [rviz2]: Stereo is NOT SUPPORTED [crazyflie_server.py-4] [INFO] [1707335803.003786589] [crazyflie_server]: udp://0.0.0.0:19851 is fully connected! As you can see, the program can only connect to 1 crazy fly and then it freezes. On the other hand on the gazebo command I obtain the next one: Starting gazebo gui [Msg] OdometryPublisher publishing odometry on [/cf_2/odom] [Msg] OdometryPublisher publishing odometry with covariance on [/model/crazyflie_2/odometry_with_covariance] [Msg] OdometryPublisher publishing Pose_V (TF) on [/model/crazyflie_2/pose] [Msg] Init subs and Pubs done : [Msg] Received firmware handshake message... [Wrn] [Model.hh:69] Skipping serialization / deserialization for models with //pose/@relative_to attribute. [Wrn] [Component.hh:144] Trying to serialize component with data type [N3sdf3v135WorldE], which doesn't have operator<<. Component will not be serialized. libEGL warning: egl: failed to create dri2 screen libEGL warning: egl: failed to create dri2 screen [Msg] Received CfLib handshake message... [Msg] Received CfLib handshake message... [Msg] Received CfLib handshake message...

I try installing the program in different computers but the result is the same. Do you know how to solve this problem? Thank you in advance

llanesc commented 9 months ago

Do you have cf_1, cf_2, and cf_3 enabled set to true in crazyswarm2/crazyflie/config/crazyflies.yaml? I tried spawning 3 and connecting them and here's what I got.

Screenshot from 2024-02-07 15-28-49

kevinaubertlomellini commented 9 months ago

Yes, the 3 crazyflies are enabled set to true in crazyswarm2/crazyflie/config/crazyflies.yaml. This is what I have: Screenshot from 2024-02-07 13-46-23 It get connected with 1 or 2 crazyflies and then it freezes there.

llanesc commented 9 months ago

Could you test if the CFLib swarm features are working? You can do this by running the crazyflie-lib-python/examples/swarm/swarmSequenceCircle.py example with the following modifications:

URI0 = 'udp://0.0.0.0:19850'
URI1 = 'udp://0.0.0.0:19851'
URI2 = 'udp://0.0.0.0:19852'
URI3 = 'udp://0.0.0.0:19853'
URI4 = 'udp://0.0.0.0:19854'

You will need to spawn 5 agents so use this command in the firmware:

bash tools/crazyflie-simulation/simulator_files/gazebo/launch/sitl_multiagent_square.sh -n 5 -m crazyflie

kevinaubertlomellini commented 9 months ago

I run the script but nothing happen: Screenshot from 2024-02-07 14-19-44

llanesc commented 9 months ago

I seem to get similar behavior when I do 16 Crazyflies. There could be some dropouts or timeouts not being met because the simulator plugin slows down. What are your CPU specs? I could easily do up to 12-15 drones with a 16 core CPU. I'll investigate this a bit further and see if I can make the communication more robust.

kevinaubertlomellini commented 9 months ago

My CPU has 6 core. I am going to try it in a computer with higher capacity. Thanks

llanesc commented 9 months ago

I removed the cflib comm delay feature and added a wait_dequeue_bulk instead of try_dequeue in sendCfLibThread(). I can now repeatedly run 16 agents. Could you pull the latest main, rebuild, and try running 3 again?

kevinaubertlomellini commented 9 months ago

With this change, the simulator is letting me have 3 crazyflies and sometimes 4. However, if I open another program like the screen recording one, the communication to the crazyflies starts to fail and they crash even when there is only one crazyflie on the Gazebo. It would be very helpful if the communication could be improved to need fewer resources. Thank you

llanesc commented 9 months ago

The issue may be related to timeouts on receiving packets on the CFLib side. Screen recording software can momentarily increase resource consumption and cause a lag on the Gazebo end. You can confirm this if you see a drop in the percentage on the bottom right in Gazebo. I've never experienced this on my machine, so I wasn't aware it was occurring. It's good that you have brought this issue to my attention. I've recently been working on removing the plugin entirely and replacing it with a module in the firmware directly that bridges the gap with Gazebo. I'll keep this issue open while I work on improving communications.

However, there is a bottleneck in that the firmware runs best when the Gazebo real time factor is close to 100%. This is a much larger fix that is related to the firmware requiring lock stepping so that the firmware runs synchronized with the sim time. I'm bringing this issue up with Bitcraze to see if we can come up with a solution. In the mean time you can increase the max step size in crazyflie-firmware/tools/crazyflie-simulation/simulator_files/gazebo/worlds/crazysim_default.sdf to something like 0.002 and change the IMU rate in the models/crazyflie/crazyflie.sdf.jinja to 500Hz. That may improve your performance while you use recording software.