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. )

[r-bt]

Making some progress. The firmware was previously crashing but now I've fixed that. I can run sitl_singleagent.sh now, it launches gaezbo and the cf2 firmware, and in cfclient I can connect to udp://0.0.0.0:19850. However, when I click Take Off in cfclient the application hangs until I assume a timeout when it becomes responsive again. When I click left the drone doesn't move but it propellers do turn on.

I'm not seeing anything in the Crazyflie logs

SOCKET_LINK: Create socket succeed 
SOCKET_LINK: Binding succeed 
SOCKET_LINK: Waiting for connection with gazebo ... 
SOCKET_LINK: Connection established with gazebo 
SYS: ----------------------------
SYS: Software-in-the-Loop Simulator is up and running!
SYS: Build 3:6a4d2d006ba3 (v1.2-alpha +3) CLEAN
SENSORS: Forcing sensors to SensorImplementation_sitl
AK8963 I2C connection [OK].
LPS25H I2C connection [OK].
Entering sensorsTask() 
ESTIMATOR: Estimator type forced
ESTIMATOR: Using Kalman (2) estimator
CONTROLLER: Using PID (1) controller
SYS: About to run tests in system.c.
SENSORS TEST PASSED [OK].
SYS: Self test passed!
STAB: Wait for sensor calibration...
GYro Bias found 
STAB: Starting stabilizer loop
ESTKALMAN: State out of bounds, resetting
SUP: Can not fly
SUP: Ready to fly
STAB: WARNING: stabilizer loop rate is off (1049)
ESTKALMAN: WARNING: Kalman prediction rate off (98)
ESTKALMAN: WARNING: Kalman prediction rate off (98)
ESTKALMAN: WARNING: Kalman prediction rate off (97)

[r-bt]

Found at least one of the issues, I had to downgrade Gaezbo from Ionic to Harmonic. Once done, the crazyflie now moves when commanded although clicking Take off still causes the application to hang

[r-bt]

@llanesc Have you ever seen the drone become unstable during the motion_commander_demo flight. As the drones executes a turn it becomes unstable, flips over, and crashes to the ground?

Drone-Flipping-Motion-Commander-Demo.mov

[llanesc]

@r-bt Hey sorry I was out of town on vacation last week. I don't know if I've tested the motion_commander_demo flight before. The first part looks good though. Does this happen with the other demos?

[r-bt]

Yes, whenever the robot turns when it gets to around 100 - 140 degrees it eventually becomes unstable and then crashes

[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!