CrazySim: A Software-in-the-Loop Simulator Interface for Crazyflies

[llanesc]

Introduction

I had an internship over the summer that involved using Crazyflies. I had never used them before, but I had 5-6 years of experience with PX4 + ROS 2 on custom racing drones. One thing that immediately stood out was the lack of a simulator feature like the SITL feature in PX4. This requires running the Crazyflie firmware on a desktop machine which communicates with the simulator of choice with its sensor package. On PX4 they use MAVLINK along with a custom Gazebo plugin to pass messages between the firmware and simulator. I found this repository https://github.com/wuwushrek/sim_cf that is the only implementation currently of Crazyflie SITL, but the firmware used is very outdated (approximately 5 years ago) and it uses ROS 1 which is nearly at end of support. I followed the same structure as in this repository while making changes to the firmware. The plugin I made for Gazebo Garden is a totally different implementation. I basically wanted users to be able to design their scripts with CFLib as they would for hardware and test that it actually flies in a simulator by just changing the URI to a UDP prefix. Then, when they are satisfied they can change to radio and deploy their script on a real Crazyflie. I also wanted there to not be a direct dependency on ROS 2 because not everyone knows ROS. Those that do want ROS can use Crazyswarm2 with CFLib as the backend.

CrazySim

The main code can be found here https://github.com/gtfactslab/Llanes_ICRA2024. If you just want to use the simulator, then don't worry about the ros2_ws folder. The code is not polished and I need to update it with the current master branch updates, but I can clean up a bit if we want to push this to master. A major concern I had was the build system. I used the CMake file from sim_cf and modified it to support the latest firmware. I also added a feature to also build the Gazebo plugin project as well. I tried to add the SITL feature as a platform using the make file system, but since the target platform is a Linux machine it would require significant changes to the make file approach. I also had to modify the proportional gain for the z-axis because it was a bit sluggish in the simulator. I can return it to the master gains and try to figure out other solutions.

Architecture

The simulator used is Gazebo Garden. This has a rendering engine that enables the use of optical flow and laser distance sensor integration in the future. I made the Gazebo Garden system plugin so that it talks to the firmware first, then users can connect a custom CFLib code with a modified UdpDriver class. The communication protocol is CRTP, but this could probably be changed to support communication with CPX instead. I'll have to look into this so we can avoid changing the CFLib master. The port for the firmware instance is 19950+N where N is the Crazyflie index starting from zero. The port used on CFLib to communicate with the Crazyflie is 19850+N. I didn't see a way to have two CRTP links open at once, so I had to use the plugin as a relay between CFLib and the firmware. This also enables a simulated communication delay feature to test control scripts with simulated radio delay. The launch scripts below are derived from PX4 where we use Jinja2 as a template system to dynamically create N number of sdf files (one for each Crazyflie) on the Linux system tmp folder. Select from any of the three launch script options below.

Option 1: Spawning a single crazyflie model with initial position (x = 0, y = 0)

bash tools/simulators/gz/sitl_singleagent.sh -m crazyflie -x 0 -y 0

Option 2: Spawning 8 crazyflie models to form a perfect square

bash tools/simulators/gz/sitl_multiagent_square.sh -n 8 -m crazyflie

Option 3: Spawning multiple crazyflie models with positions defined in the agents.txt file. New vehicles are defined by adding a new line with comma deliminated initial position x,y.

bash tools/simulators/gz/sitl_multiagent_text.sh -m crazyflie

Below is an architecture diagram of the code.

Testing

I did some testing with model predictive control using Crazyswarm2 and my simulator. Below is a simulation of 16 Crazyflies flying a lemniscate trajectory. This simulator is part of a submission to ICRA 2024 with the MPC code used for test flights.

Integration

Let me know what you think and if you had any issues setting up the simulator. The documentation might be missing dependencies, so I'll try to work through the issues and fix them as they are discovered.

[knmcguire]

This is very cool! Awesome job and thanks for sharing! I need to find some time to try this one out but this seems very interesting.

Just tagging some people who will be interested in this work too: @whoenig and @Jaeyoung-Lim (he probably made the scripts for PX4). So that we can have a discussion and discuss the future of the integration.

[llanesc]

Hi @knmcguire,

In the developer meeting today we talked about whether CrazySim works with the crazyflie-clients-python package. This was actually one of the first things I worked on to test the sim. I can publish the code for the modified client, although I had to comment out a bunch of tabs because they were causing issues for some reason. In summary, I added a SITL checkbox tool that allows the address box to enter IP-like addresses. The UdpDriver in CFLib also has a hard coded scan for port 19850.

5b7f988a-9fdb-4dfe-b5ab-807f6efb9983.mp4

[knmcguire]

ahhh oke! I tried it on the recent cfclient but it didn't work, but I was already aware that probably some changes are necessary. If you could indeed share that as well?

[llanesc]

Here is the link to the client: https://github.com/llanesc/crazyflie-clients-python/tree/sitl-release. You will also need to update the main Llanes_ICRA2024 repo because I hard coded the uri in the scan interface of UdpDriver and upgraded to the latest cflib (latest changes were originally from June 8). A better solution would be to check if the port is open and return the correct uri or empty otherwise. I made a bunch of "hacky" changes to the client to support SITL early on in this project because I was rushing to get it to work. There's a bunch of messages that are commented out like battery, link quality, and obviously there's no loco positioning deck to allow take off and land to be enabled in the client. I guess this just proves that it can be done correctly with a bit more effort from someone that knows the client more than me. I also made the change to the address because I thought that maybe users would want to change the address at some point. The port is also default 19850 because you can't run multiple drones from the client at once (that I know of).

[knmcguire]

Oh yeah no problem. I wasn't proposing of making a PR like this right out of the bat, and I completely understand the rush you were in. I used to be a PhD student too so I totally get the paper deadlines and hacking away just to get it to work 😄 It is just so that we can evaluate it and that we can make a plan to make it a bit more generic for the PR. Thanks for sharing!

[whoenig]

Very impressive work! Would be interesting to know how much effort maintaining such a code would be if added to the official firmware. The firmware repo itself looks very involved (with over 200 files being changed).

[llanesc]

The code is comprised of two major changes that would need to be maintained. First, a macro system to build the source without the microcontroller drivers/dependencies with some extra files to support socket links, simulated sensors, simulated power distribution, and some other features derived originally from sim_cf. Although sim_cf is very old and required fixes for almost all the files to support the changes to the latest firmware at the time which was in July I think. I'll need to update it to the latest firmware now. If I can somehow change the build system to the platform system, then it would be pretty autonomous to maintain. I tried this in June, but the make file was tailored towards building on microcontrollers. It was easier at the time to get it set up with a separate CMake file. The second major change is the Gazebo plugin. This is really only one source file, but it's the backbone of the simulator. There's a lot of potential upgrades that can be made to support things like simulated optical flow or laser distance sensors.

[knmcguire]

Yeah... it might be difficult to adjust kconfig to that... And we just recently had a change from regular make to kconfig so unfortunately I don't see realistically that we will change fully to cmake again. Perhaps a solution is to have the firmware as a submodule of a Crazyflie_sitl repo, and have the cmake in there?

Moreover, the amount of change in the code is also the reason why I wanted to talk strategy first here in discussions before going in head first and making PRs 😉. But let's all first try it out and see how it works on our machines (I will have time for it a bit more in November).

[llanesc]

This all sounds good to me. Let me know if you have any issues running it on your machines. Once you have the models spawned in Gazebo you can just change the URI in a CFlib script to udp://0.0.0.0:19850 with my modified crazyflie-lib-python package. The changes I made are to the UdpDriver class, but it may be possible to use the official version without my changes with another approach.

[Jaeyoung-Lim]

@llanesc Awesome work!

On PX4 they use MAVLINK along with a custom Gazebo plugin to pass messages between the firmware and simulator.

Just want to point out that PX4 has moved away from this architecture, due to the inflexibility of using a standard message set(MAVLINK_HIL*) for interacting with Gazebo garden.

PX4 now uses a bridge that directly communicates internal uORB messages to gz topics: https://github.com/PX4/PX4-Autopilot/tree/main/src/modules/simulation/gz_bridge
This allows more flexible/tighter integration to the simulation. It allows the models to not be autopilot specific since the sdf models no longer contain autopilot configurations in the model.

[llanesc]

Hi @Jaeyoung-Lim, thanks for the reply. I haven't been following the major updates these past few months, but that's awesome that the PX4 team was able to develop that bridge. I know the ROS 2 bridge has been very useful for my research, so I expect this approach to be more flexible. It may be useful to adopt the sitl_multiagent_text.sh script I used for multiagent spawning in PX4. You just have to define new start x,y coordinates in a text file. The script takes each line as a new drone, so you don't have to define the number of drones and it's easy to modify the start coordinates.

[Jaeyoung-Lim]

@llanesc Feel free to contribute(PR)!

[knmcguire]

Hi! Not a question yet but just an update. I managed to run a single agent in gazebo and run a cflib script! I couldn't get the cfclient to run though as it couldn't find the udp address but I only had a half an hour to try this out.

I love it! I hope to look closer into the changes soon and start thinking strategy to intergrate this to our repos

[knmcguire]

My very humble attempt, but excited about this!

https://youtu.be/wXSiGaoWcko

[llanesc]

I made some changes today to the main repo. This includes a new simulated hardware communication latency feature within the Gazebo Sim plugin. To change the latency users can modify the "cflib_latency_ms" variable in the crazyflies.sdf.jinja file. I also updated the Gazebo Sim IMU and physics rates to 1000Hz. This has a bit better drone performance with the cost of multiagent performance. I opened another branch called efficient-multiagent that doesn't have these features.

[knmcguire]

Some initial thoughts when going through the code, to kind of start to think about strategy. I must say that I have a lot of respect for how you were able to go through all of these changes. I would have not gotten this to work in 2 weeks so I would assume that you have a very good ability to concentrate ('goede zitvlees' is what we would call it in Dutch 😄)

Anyhow. I made the following observations:

• There where a lot of extra defines. CONFIG_PLATFORM_SITL is the major contributor to many changes of these files, especially the file 'autoconf.h' is a tricky one. Wonder if we could have kbuild just generate an 'empty autoconf.h' or something... Also CONFIG_SENSORS_SITL is new and CONFIG_POWER_DISTRIBUTION_SITL seems perhaps something that can also be linked with the first. Then the question is, why the seperation?
• I've seen ENABLE_VERIF in a lot of places. I couldn't see the intention of that define. Is this something that is necessary for SITL or is this something specific for the paper results?
• I noticed that lot of changes were needed to the parameter structure, which is mostly due to storage usages of the eeprom with persistent parameters. Wonder if we could still have the same functionality but written as some kind of text file somewhere. I believe I saw an similar addition to the code (but ofcourse I lost that trace).
• There was a substitution necessary for asserts and math functions as many of those are STM or arm specific. Not sure if we can find a solution for that. Same thing with the usec_sitl, there is no going around giving a substitute for that I believe. In general I saw that there were more microprocessor specific functions disabled like ISR interrupts
• Some things needed to be disabled to handle the deck system, like reading the OW memory and such. Does that mean that all deck drivers are always 'on'?
• There were some files that were created that were copies of the original files, like main_sitl.c, system_sitl.c. Some make more sense like sensors or usec but I guess it would be better to try to have not having to make main_sitl and system_sitl because of the duplicate of code, but there are a lot of hardware specific stuff.
• All the simulation related things or the model predictive libraries should ofcourse not be included in a future PR to the firmware. The model of the crazyflie should be a standard in gazebo (which is in my immediate pipline). The gazeboplugin should probably be included in the crazyflie_simulation repo with crazyflie_cpp as submodule.

Don't see this as critique but rather as a thought-tsunami and remarks of myself going through the process of trying to figure out a strategy to integrate this in the Bitcraze ecosystem. The general thought that I had is that our firmware is very much written with hardware only in mind, and a lot of hardware-specific libraries and logic are heavily intertwined. It might just require a complete firmware overhaul to make it all maintainable (which is very important here if we want to have something that will last in the future). This is a large project but will benefit quite a lot of users, but it also means that during our quartely meeting we need to be able to allocate time for this. But it is good to start planning and laying the groundwork for it 😄

(PS, just an important comment: I saw that crazyflie_cpp was copied and used, which is fo the gazebo plugin I assume. Don't forget to give credit to wolfgang Hönig for that code 😉. Same goes for the model and mesh files that I created. These are on the MIT license originally so you are free to use it, given that you give credit when distributing the code. )

[knmcguire]

Ah oke! if crazyflie_cpp is not used it is probably best to remove it. Then you just give add the credit for the other things that have been copied, although the proper way is to use submodules for any external dependencies (like crazyflie_simulation for the mesh files and perhaps the libraries used for the model predictive control)

It's up to you if you'd like to add those changes, but I would say that would be more to clean it up for the readers of the paper. The SITL contribution to the core firmware will require a lot more separate PRs and a splitting up of functionalities. The comments I gave before was just to start priming our brains for the big PR strategy and to have a feel of how long it will take (which it would be). There are some things that we will need to fix from Bitcraze side of things and things that you can contribute in particular. But of course be my guest to start improving things to your own code base, but I don't think you need to make a separate repository now (since integrating this into our firm and software will be more complicated and will take more time).

As I told you before, because of my health this period is a bit tough for me to allocate time to this project, but once things turn out for the better (hopefully end of January or February) we can reconnect and perhaps set up an online meeting to get this whole thing started?

[llanesc]

This all sounds good to me. Should I integrate the Gazebo plugin system within the crazyflie-simulation repo? I can fork, add a gz folder, and put the gazebo stuff in there. That way the fork will replace the simulators folder. I'll work on the changes I mentioned in my previous post over the next few weeks. Also, January or February works for me. Hope you feel better!

[knmcguire]

yeah sure why not :) there is already a gazebo folder so you can maybe add a plugin folder there for the PR https://github.com/bitcraze/crazyflie-simulation/tree/main/simulator_files/gazebo. The simulation repo is a little less 'strict' as it is a bit more experimental than the firmware repo so I think it is fine to add that.

You can also add documentation in the doc folder to explain about the plugin too.

[llanesc]

Hi Kimberly, I hope you had a wonderful holidays. I finally found some time to migrate to the crazyflie-simulation repo (see https://github.com/llanesc/crazyflie-simulation/tree/sitl-release). It should be updated in the master repo Llanes_ICRA2024.

There are a few more changes I need to add for credits, but before that I wanted to get some input from you on a upgrade to the simulator that would remove a bunch of files. I took some time to understand the PX4 Gazebo bridge and it inspired some ideas to migrate to a similar architecture for this repo. This architecture would be quite different from what we currently have as outlined below, but it would significantly simplify the Gazebo integration side.

• The Gazebo plugin would be replaced by an additional module in the firmware that is only compiled for SITL. The init() function would add the specified model to the world. I think there would be at least two models for the crazyflie with original motors and upgraded thrust motors. The module would have subscriber callbacks for the IMU and pass those onto the EKF module.
• The connection with CFLib and the firmware would be done directly since the plugin is not needed anymore as a middleman. This would remove the comms delay feature, but we could think about how to add it back if it's really needed. It also removes the need for the cmake file for compiling the plugin.
• The launch scripts only need to startup and pass model arguments and instance number to the cf2 executable. This means we don't need template sdf's and jinja2.
• The UDP connection would only be between the firmware and CFlib.

[knmcguire]

Hi! Thanks! Hope you had some nice holidays yourself!

We are currently in our quarterly meeting week so I'll have more time next week to take a look at it. I'll give some comments then!

[kevinaubertlomellini]

@llanesc Nice work!
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]

Hi @kevinaubertlomellini, thanks for the response. Could you copy this issue to https://github.com/gtfactslab/Llanes_ICRA2024/issues to track issues on that repo? Thanks!

[knmcguire]

btw, your work has been featured in the ROS news item last week! https://discourse.ros.org/t/ros-news-for-the-week-of-february-5th-2024/36011

[llanesc]

That's awesome! I also noticed it was featured for the week of January 29th as well. Very cool!