CrazySim: A Software-in-the-Loop Simulator for the Crazyflie

2024-04-15 | Christian Llanes | Leave a comment

This week we have a guest blogpost by Christian Llanes, a Robotics PhD of from Formal Methods & Autonomous Control of Transportation Systems Lab of the Georgia Institute of Technology. Enjoy!

Why do we need simulators?

Simulators are one of the most important tools used in robotics research. They usually are designed for different purposes with different levels of complexity. For example, simulators with low computational overhead that are parallelizable are mainly used for either training reinforcement learning algorithms or Monte Carlo sampling for verification of task completion in a nondeterministic environment. Some simulators also use rendering engines for the graphical display of models and the environment or when cameras are intended to be used in the robotics platform. Simulation is also useful for the development and deployment of new robotics firmware features where the firmware is compiled on a test machine and run in the loop with a simulated sensor suite. This simulator configuration is known as software-in-the-loop (SITL) because the vehicle firmware is intended to be run in the loop with the simulated vehicle physics and/or rendering engine. This feature is supported by autopilot suites such as PX4, ArduPilot, CogniPilot, and BetaFlight. This feature is not officially supported yet for Crazyflies because it requires a large overhaul of the firmware to be able to compile on a desktop machine and interact with different simulators such as Gazebo, Webots, PyBullet, CoppeliaSim, Isaac Sim, or Unreal Engine.

CrazySim

Last summer I began working with Crazyflies and noticed this Crazyflie simulator gap. I stumbled on a community-developed project for Crazyflie SITL called sim_cf. This project is exactly what I was looking for. However, the firmware used by the project is from July 2019 and the official firmware has had over 2000 commits made since then. The project also uses ROS 1, Gazebo Classic, and doesn’t support the Crazyflie Python library (CFLib). Using this project as a starting point I set out to develop CrazySim–a Crazyflie SITL project that doesn’t require ROS, uses Gazebo Sim, and supports connectivity through CFLib. Using CFLib we can connect the simulator to external software such as Crazyswarm2 or the Crazyflie ground station client. Users test their control algorithms in the external software using the simulator interface before deploying to real flight hardware.

Using the Crazyflie Client for PID Tuning

We have also provided a modified Crazyflie client for CrazySim support. The Crazyflie client is a cool tool for testing a single drone in hardware. We can perform command based flight control, look at real time plots, save log data, and tune PID values in real time. The PID values are typically tuned for an out of the box Crazyflie. However, when we modify the Crazyflie and add extra weight through batteries, decks, and upgraded thrust motors then the behavior of the Crazyflie will change. If a user wants to tune a custom Crazyflie setup, then they can add additional models in this folder with their own motor and mass properties. Then they just need to add it to the list of supported models in either of the launch scripts. There is already an example model for the thrust upgrade bundle. Documentation for installing the custom client can be found here.

PID tuning a simulated Crazyflie using CrazySim on the Crazyflie PC client.

Crazyswarm2

We can now connect to the simulated Crazyflie firmware using CFLib. Therefore, we can set up a ROS 2 interface through Crazyswarm2 for swarm command and control through ROS 2 topics and services. To do this we first startup the drones using any of the launch scripts.

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

Then, we bring up Crazyswarm2 after setting up the configuration file for the number of drones chosen.

ros2 launch crazyflie launch.py backend:=cflib

We demonstrate an example of how we can control a swarm of drones using Crazyswarm2 GoTo service commands.

Crazyswarm2 GoTo service commands using CrazySim.

ICRA 2024

CrazySim is also being presented as a paper at the 2024 IEEE International Conference on Robotics and Automation in Yokohama, Japan. If you are attending this conference and are interested in this work, then I invite you to my presentation and let me know that you are coming from this blog post after. For the paper, I created a multi agent decentralized model predictive controller (MPC) case study on ROS 2 to demonstrate the CrazySim simulation to hardware deployment workflow. Simulating larger swarms with MPC may require a high performance computer. The simulations in this work were performed on an AMD Ryzen 9 5950X desktop processor.

Links

Other Crazyflie SITL projects:

Python bindings for the Crazyflie Firmware

2024-04-01 | Kimberly McGuire | Leave a comment

Today, we’d like to take the opportunity to spotlight a feature that’s been in our code base for some time, yet hasn’t been the subject of a blog post: the Python bindings for our Crazyflie firmware. You may have noticed it mentioned in previous blog posts, and now we’ll delve into more detail about what it is, how we and others are utilizing it, and what its future holds.

Schematized visualization of code within the Crazyflie

What are the Python bindings?

Language bindings, in essence, are libraries that encapsulate chunks of code, enabling one programming language to interface with another. For instance, consider the project Zenoh. Its core library is crafted in Rust, but it offers bindings/wrappings for numerous other languages like Python, C/C++, and so on. This allows Zenoh’s API to be utilized in scripts or executables written in those languages. This approach significantly broadens the functionality without necessitating the rewriting of code across multiple programs. A case in point from the realm of robotics is ROS(1), which initially created all of their APIs for different languages from scratch—a maintenance nightmare. To address this, for ROS 2, they developed the primary functionality entirely in C and provided wrappers for all other programming languages. This strategy eliminates the need to ‘reinvent the wheel’ with each iteration.

Rather than redeveloping the firmware in Python, our esteemed collaborators Wolfgang Hönig and James Preiss took a pragmatic approach. They selected parts of the Crazyflie firmware and wrapped them for Python use. You can see the process in this ticket. This was a crucial step for the simulation of the original Crazyswarm (ROS1) project and was continued for its use in the Crazyswarm2 project, which is based on ROS 2. They opted for SWIG, a tool specifically designed to wrap C or C++ programs for use with higher-level target languages. This includes not only Python, but also C#, GO, Javascript, and more, making it the clear choice for implementing those bindings at the time. We also strongly recommend checking out a previous blogpost by Simon D. Levy, who used Haskell to wrap the C-based Crazyflie Firmware for C++.

Where are the Python bindings being used?

As previously mentioned, the Crazyswarm1 & 2 projects heavily utilize Python bindings for testing key components of the firmware (such as the high-level commander, planner, and controller) and for a (hybrid) software-in-the-loop simulation. During the project’s installation, these Python bindings must be compiled so they can be used during simulation. This approach allows users to first test their trajectories in a simulated environment before deploying them on actual Crazyflies. The advantage is that minimal or no modifications are required to achieve the same results. While simulations do not perfectly mirror real-world conditions, they are beneficial because they operate with the same controller as the one used on the Crazyflie itself. In our own Crazyflie simulation in Webots, it’s also possible to use these same bindings in the simulator by following these instructions.

Three controllers (PID, Mellinger, and Brescianini), intra-drone collision avoidance, and the high-level commander planner have all been converted into Python bindings. Recently, we’ve added a new component: the Extended Kalman Filter (EKF). This addition is ideal as it allows us to test the filter with recorded data from a real Crazyflie and experiment with different measurement models. As we discussed in a previous blogpost, estimators are complex due to their dependence on chance and environmental factors. It’s beneficial for developers to have more control over the inputs and expected outputs. However, the EKF is deeply integrated into the interconnected processes within the Crazyflie Firmware. After a significant refactoring effort, these were added to the bindings by creating an EKF emulator (see this PR). This enabled Kristoffer to further enhance the TDOA outlier filter for the Crazyflie by emulating the full process of the EKF, including IMU data.

In addition to SITL simulation and EKF development, Python bindings are also invaluable for continuous integration. They enable comprehensive testing that encompasses not just isolated code snippets, but entire processes. For instance, if there’s a recording of a Crazyflie flight complete with sensor data (such as flow, height, and IMU data), and it’s supplemented with a recorded ground truth (from lighthouse/mocap), this sensor data can be fed into the EKF Python binding. We can then compare the outputted pose with the ground truth to verify accuracy. The same principle applies to the controllers. Consequently, if any changes are made to the firmware that affect these crucial aspects of Crazyflie flight, these tests can readily detect them.

If you like to try the python bindings tests for yourself, clone the Crazyflie-firmware repo and build/install the python bindings via these instructions. Make sure you are in the root of the repository and run: python3 -m pytest test_python/. Mind that you might need to put the bindings in the same path with export PYTHONPATH=<PATH_TO_>/crazyflie-firmware/build:$PYTHONPATH (please see this open ticket)

The next steps of the python bindings

We’ve seen how Python bindings have proven to be extremely useful, and we’re keen to further expand their application. At present, only the Loco positioning system has been incorporated into the EKF part of the Python bindings. Work is now underway to enable this for the Lighthouse system (see this draft PR). Incorporating the Lighthouse system will be somewhat more complex, but fortunately, much of the groundwork has already been laid, so we hope it won’t be too challenging. However, we have encountered issues when using the controller bindings with simulation (see this open ticket). It appears that some hardware-specific timing has been hardcoded throughout the PID controller in particular. Therefore, work needs to be done to separate the hardware abstraction from the code, necessitating additional refactoring work for the controller.

Recent projects like Sim_CF2 (see this blogpost) and Crazysim (see this discussion thread) have successfully compiled the Crazyflie firmware to run as a standalone node on a computer. This allows users to connect it to the Crazyflie Python library as if it were an actual Crazyflie. This full Software-In-The-Loop (SITL) functionality, already possible with autopilot suites like PX4 and Ardupilot, is something we at Bitcraze are eager to implement as well. However, considering the extensive work required by the aforementioned SITL projects to truly separate the hardware abstraction layer from the codebase, we anticipate that refactoring the entire firmware will be a substantial task. We’re excited to see what we can achieve in this area.

Indeed, even with a more comprehensive Software-In-The-Loop (SITL) solution, there’s no reason to completely abandon Python bindings. For developments requiring more input/output control—such as the creation of a new controller or an addition to the Extended Kalman Filter (EKF)—it’s beneficial to start with just that portion of the firmware code. Python bindings and a SITL build can coexist, each offering its own advantages and disadvantages for different stages of the development process. By leveraging the tools at our disposal, we can minimize the risk of damaging Crazyflies during development. Let’s continue to make the most of these valuable resources!

Simulation sim_cf2 for Multi-Agent Algorithm Development

2024-01-22 | Thomas Izycki | Leave a comment

Today we have a guest blogpost by Thomas Izycki (Technische Hochschule Augsburg) and Klaus Kefferpütz (Ingolstadt Technical University of Applied Sciences, former Augsburg Technical University of Applied Sciences) talking about implementing Software-in -the-Loop for swarm simulation of the Crazyflie.

When our cooperative control lab at the Technical University of Applied Sciences Augsburg was founded a few years ago, our goal was to develop distributed algorithms for teams of UAVs. We quickly decided to use Crazyflies with the algorithms directly implemented in the firmware, thus having a platform for a truly decentralized system. Our ongoing projects focus on cooperative path planning, navigation, and communication.

Since working with several drones at once, which have to communicate and coordinate with each other, can quickly become confusing and very time-consuming, a simulation was needed. It should preferably offer the possibility to integrate the firmware directly in the simulation environment and ideally also offer an interface to the crazyflie python API. With the relocation of our laboratory from Augsburg to the Technical University of Applied Sciences Ingolstadt, which however does not yet have a permanently established flying space, the need for a simulation environment was further increased in order to be less dependent on hardware accessibility. A look at the community and the available simulations quickly led us to the sim_cf flight simulator for the Crazyflie. A fantastic project supporting the use of the actual Crazyflie firmware in software-in-the-loop (SITL) mode and even in hardware-in-the-loop (HITL) mode on a real Crazyflie using the FreeRTOS Linux Port together with ROS and Gazebo. Unfortunately, the project has not been maintained for several years and also had no integration with the Crazyflie Python API. After a short chat with Franck Djeumou and his agreement to use the code of the original sim_cf simulation, the project was ready for an upgrade.

sim_cf2 Flight Simulator

With support for ROS coming to an end we decided to migrate the project to ROS2 and additionally support the current version of the Crazyflie firmware, which at this time is release version 2023.11. With the addition of a driver to connect the cflib to the SITL process, the same python cflib-based scripts can now be used with real Crazyflies and for use in the simulation environment. Only the corresponding driver needs to be loaded during initialization. Overall, the focus of the sim_cf2 simulation is now on using the Crazyflie python API instead of commanding the Crazyflies via ROS.

As for the Crazyflie firmware the whole build process has been fully integrated into the firmware’s KBuild build system. This also allows the use of the same code base for simulation and execution on the real Crazyflie. Depending on the configuration, the firmware is compiled for the STM32F on the Crazyflie or the host system running the SITL.

Components of the sim_cf2 Flight Simulator

To run the simulation, the three modules must therefore be started separately. Gazebo is started using the main launch file. The number and initial pose of the simulated Crazyflies is also defined in this file. Once the firmware for the host system has been built, the desired number of SITL instances can be started using the attached script. Finally, a cflib-based script, the Crazyflie client or the multi-agent client presented below is started. With no radio dongles attached to the computer, the simulation driver is initialized automatically and a connection to the simulated Crazyflie can be established.

There are still a few open issues, including the absence of implemented decks for positioning, such as LPS and Lighthouse. Currently, the absolute position is sent from Gazebo to the SITL instance and fused in the estimator. Moreover, Gazebo requires quite a lot of computing power. We were able to run a maximum of four Crazyflies simultaneously on a relatively old laptop. However, a modern desktop CPU with multiple cores allows for simulating a significantly larger number of Crazyflies.

Multi-Agent Client

Independent of the simulation we designed a GUI for controlling and monitoring our multi-agent teams. It currently supports up to eight Crazyflies but could be upgraded for bigger teams in the future. So far it has been enough for our requirements.

A central feature is the interactive map, which makes up about half of the gui. This is a 2D representation of the flight area with a coordinate system drawn in. Connected Crazyflies are displayed as small circles on the map and a new target position can be assigned by clicking on the map after they have been selected by their corresponding button. If required, obstacles or paths to be flown can also be drawn into the map.

An important aspect of a truly decentralized system is peer to peer communication. It allows information to be exchanged directly between agents and ideally takes place without a central entity. Currently peer to peer communication is not available in the Crazyflie ecosystem, but is in development.

For this reason, we have implemented a workaround in our client, enabling a pseudo-decentralized communication system. This involves adding an additional layer to the Crazy RealTime Protocol (CRTP), which we named the Multi-Agent Communication Protocol (MACP). It consists of an additional packet header made of the destination ID, source ID and a port as endpoint identifier. Every ID is unique and directly derived from the Crazyflie’s address.

These MACP packets are sent via the unused CRTP port 0x9. The packet routing mechanism implemented in the client forwards the packets to their destination. It is also possible to send packets as a broadcast or to address the client directly.

On the firmware side, we have added a corresponding interface to simplify the sending and receiving of macp packets. It is analogous to the CRTP implementation and allows the registration of callback functions or queues for incoming packets on corresponding ports. It can be activated via KBuild.

At least for use in the laboratory and the development of distributed algorithms, this method has proven its worth for us.

Project

We hope that the sim_cf2 simulation can also be useful to others. The complete source code is available on GitHub. Further information concerning installation and configuration can be found in the readme files in the respective repositories.

We would also like to point out that other simulations have been created that are based on sim_cf and therefore offer similar functionality. One of these projects is CrazySim, also available on GitHub. Moreover, there are ongoing efforts to officially integrate such a software-in-the-loop simulation into the Crazyflie firmware and ecosystem.

sim_cf2:

https://github.com/CrazyflieTHI/sim_cf2

Multi-Agent Client:

https://github.com/CrazyflieTHI/crazyflie-client-multi-agent-thi

Show-and-tell post of CrazySim:

https://github.com/orgs/bitcraze/discussions/995

For the original sim_cf Flight Simulator for the Crazyflie visit:

https://github.com/wuwushrek/sim_cf

Development plans for Crazyflie Simulation

2023-10-23 | Kimberly McGuire | Leave a comment

It seems that many of you are very interested in simulation. We might have gotten the hint when we noticed that our July’s development meeting had our best attendance so far! Therefore, we will be planning a new developer meeting to discuss the upcoming plans for supporting simulation for the Crazyflie.

Getting Started with Simulation tutorial

Perhaps you are not aware, but there is actually a Getting Started tutorial for simulation that has been available for a little over 2 months now. Unfortunately, circumstances prevented us from writing a blog post about it, but we’ve noticed that not all of you are aware of it yet!

The getting-started tutorial demonstrates how to set up the Webots simulator, which already includes Crazyflie models and some cool examples:

An example that you can control the Crazyflie with the keyboard
An example that the Crazyflie does wall following autonomously

The latter is based on the example app layer for wall-following in the crazyflie-firmware repository. Starting this year, there’s also a Python library equivalent available.

The tutorial concludes with instructions on how to edit these controllers. Alternatively, you can choose to run the files directly from the crazyflie-simulation repository. After completing the tutorial, you can explore the simulation repository documentation for more information and to access additional examples.

Upcoming plans

With so many plans and so little time! This is a common phrase at Bitcraze, and it’s a symptom of being an overly ambitious, but too small, team. By the way, we are still looking for more people :). Nonetheless, we have big plans to take our Crazyflie simulation to the next level:

ROS 2 Crazyflie model for Webots: The Crazyflie has been a part of the Webots standard robots for 2 years now, but we still need to implement the Crazyflie into the Webots ROS 2 repository.
Better (new) Gazebo support: Currently, we only have a very simple example for Gazebo, which is limited to motors with no control input. Working with the C++ API can be a bit challenging, so it might be worth considering the use of ROS 2 in the loop here. Let’s see what comes out of it.
Integration into Crazyswarm2: Once the Webots ROS2 node has been released, integrating the Crazyflie simulation into Crazyswarm2 will become more straightforward.
Improvement to the Python bindings: We’ve had Python bindings for controllers and the high-level commander for a while. Recently, we also added Python bindings for the estimator (currently for loco positioning only). However, there are still some issues to address with the Python bindings for the controllers due to timing issues with the simulators.
Linking with our CFLIB: Currently, both Webots and the Crazyflie Python library use entirely different APIs. This means that these scripts are not compatible and you’ll need to be creative not to reuse new code. However, wouldn’t it be nice to use a python example from the python library with a --sim and that it would actually control the Crazyflie in the simulator instead?

Of course, there are probably more improvements that we haven’t thought of yet, but that’s why we have developer meetings!

Come and join us at the Developer meeting.

We will be hosting another developer meeting on November 1st at 15:00 Central European Time (accounting for the time-shift from summer to autumn). You can find details on how to join in the discussion thread here.

Just for your information, I (Kimberly) am the main driving force behind our simulation efforts. However, I’m currently on partial sick leave and will soon be on full leave for a while. I kindly ask for your patience with the pace of ongoing developments. Remember, it’s an open-source project, so if you’d like to contribute and help out, we would greatly appreciate it :)