Category: Fun Friday

Hey, Victor here!

I’ve been flying FPV drones for some time and while I usually fly bigger drones (3-5 inch props) I have always wanted to put an analog camera on the Crazyflie to fly it in FPV. So, a few weeks ago I put together a simple FPV deck using off-the shelf components! The deck simply consists of a camera, VTX and a DC-DC converter, soldered onto a prototype deck.

The deck is very simple and consists of only four components and the price (as of writing) is approximately 50$ in total.

  1. Prototyping deck
  2. Camera: RunCam Atom 10x10mm 800TVL FPV Camera
  3. VTX: TBS Unify Pro Nano 5G8
  4. DC-DC converter: Voltage 5V boost converter (necessary since the camera and the VTX requries 5V.)

I did the wiring as follows:

I soldered the components onto the prototype deck and used some hot glue to attach the camera, as well as on and around the antenna to prevent it from breaking off when crashing. The deck weighs a total of 8.5 grams including connection pins.

I used the newly released upgrade kit on the Crazyflie which made it easier to fly since the motors and propellers makes the drone a lot faster and easier to control flying manually. The upgrade kit also increases the lift capacity of the drone, which is nice so that the extra weight of the camera deck doesn’t become a problem.

Radio Controller

When flying FPV race drones you typically want a nice radio controller and there are many options to choose from. I recently got myself a RadioMaster Zorro Radio Controller – 4-in-1 Multi-Protocol which supports a whole variety of different RC protocols, including the popular ones such as frsky, flysky and many more. You can run the popular OpenTX or EdgeTX firmware on it and the controller is equipped with multiple RF chips, whereas one of the chips is the nRF24L01. This means that we can control the Crazyflie with the controller! While I expected several hacks to make this work, thanks to the awesome Bitcraze community someone had already written support for the Crazyflie for the controller.

Below are the steps that I took to control the Crazyflie using a RadioMaster Zorro 4-in-1 controller. In short, we want two different firmwares: 1) Firmware for the remote controller (like the controller OS). 2) Firmware for the internal RF module. Please note that the details of the steps might change in the future, but hopefully it can still be helpful.

  1. Download the latest OpenTX or EdgeTX firmware.
  2. Clone the repository for the internal RF module: DIY Multiprotocol TX Module.
  3. Locate the file Multiprotocol/CFlie_nrf24l01.ino in the repository and set the address of the Crazyflie that you want to connect to in the method CFLIE_initialize_rx_tx_addr().
  4. Ensure that the #define CFLIE_NRF24L01_INO is uncommented in the file Multiprotocol/_Config.h
  5. Download Arduino IDE in order to build the code for the internal RF module.
  6. Open Arduino IDE from the Multiprotocol directory and build the code by Sketch -> Export Compiled Binary. This might take some time since the firmware is quite big. The binary can then be found in Multiprotocol/build/XXX.bin.
  7. Plug in the SD card of the remote controller or connect it to the computer using USB-C and start the controller as a storage device.
  8. Transfer the two firmware binaries to the firmware directory of the radio controller. Unplug the radio controller and install the EdgeTX/OpenTX binary as the radio firmware, and the Multiprotocol binary for the internal RF module.
  9. Create a new model and select the CFLIE protocol.

You should now be ready to fly! So turn on your Crazyflie and ensure that it’s on the address that you assigned in the CFLIE_initialize_rx_tx_addr() method in step 3. The radio should automatically find the correct channel so you shouldn’t have to worry about selecting the right channel.

Conclusions

I think the deck turned out really nice and it’s super cool to fly the Crazyflie in FPV! :) Some notes to consider:

  1. It’s possible to fly with the FPV deck with the normal motors and propellers of the Crazyflie but with the thrust upgrade kit the flying is easier and significantly more enjoyable since you can go a lot faster.
  2. Ensure that the battery is well and fully charged before flying.
  3. There’s no support for On-Screen Display (OSD) on this deck, but it would be a cool thing to test in the future. I believe that most flight controllers that supports onboard OSD has the MAX7456 or AT7456E chip, but there’s probably more ways to do it.
  4. The hot glue loosens up slightly from the heat dissipation of the VTX. I added some extra glue and it seems to hold quite well, even after multiple crashes.
  5. There are modules that contains the camera and the VTX in the same package, which might be a good/better option for the Crazyflie buying them separately and soldering them together.

Please let me know if you’ve found any mistakes in the text above or if you have any other cool ideas or hacks about FPV for the Crazyflie! :)

Cheers,
Victor

As you probably noticed already, this summer I experimented with ROS2 and connecting the Crazyflie with multi-ranger to several mapping and navigation nodes (see this and this blogpost). First I started with an experimental repo on my personal Github account called crazyflie_ros2_experimental, where I managed to do some mapping and navigation already. In August we started porting most of this functionality to the crazyswarm2 project, so that is what this blogpost is mostly about.

Crazyswarm goes ROS2

Most of you are already familiar with Crazyswarm for ROS1, which is a project that Wolfgang Hönig and James Preiss have maintained since its creation in 2017 at the University of Southern California. Since then, many have used and referred to this work, since the paper has been cited more than 260 times. From all the Crazyflie papers of the latest ICRA and IROS conferences, 50 % of the papers have used Crazyswarm as their communication middleware. If you haven’t heard about Crazyswarm yet, please check-out the nice BAMdays talk Wolfgang gave last year.

Unfortunately, ROS1 will not be there forever and will be phased out anno 2025 and will not be supported for Ubuntu 22.04 and up. Therefore, Wolfgang, now at the Intelligent Multi-robot Coordination Lab at TU Berlin, has already started with the ROS2 port of Crazyswarm, namely Crazyswarm2. Here the same principle of the C++ based Crazyflie server and the python wrapper were been implemented, along with the simple position based simulation and Teleop nodes. Mind that the name Crazyswarm2 is just the project name out of historic reasons, but the package itself can also be used for individual Crazyflies as well. That is why the package names will be called crazyflie_*

Porting the Summer Hack project to Crazyswarm2

The crazyflie_ros2_experimental was fun to hack around, as it was (as the name suggests) experimental and I didn’t need to worry about releases, bugfixes etc. However, the problem of developing only here, is that the further you go the more work it becomes to make it more official. That is when Wolfgang and I sat down and started talking about porting what I’ve done in the summer into Crazyswarm2. This is also a good opportunity to get more involved with the project, especially with so many Crazyfliers using the ROS as well.

The first step was to write a second crazyflie_server node that relied on the python CFlib. This means that many of the variables I used to hardcode in the experimental node, needed to be defined within the parameter structure of ROS2. The crazyflies.yaml is where anything relevant for the server (like the URIs and parameters) needs to be defined. Both the C++ backend server and the CFlib backend server are using the same parameters. Also the functionality of the both servers are pretty similar, except for that logging is only possible on the CFlib version and uploading/follow trajectories is only possible on the C++ version. An overview will be provided soon on the Crazyswarm2 documentation website.

The second step was to make the crazyflie_server (cflib) node suitable to be connected to external packages that I’ve worked with during the hack project. Therefore, there are some special logging modes, that enables the server to not only output topics based on logging, but Pose/Odometry/LaserScan messages along with Transforms. This allowed the SLAM_toolbox to use the data from the Crazyflie itself to create a map, which you can see an example of in this tutorial.

Moreover, for the navigation it was important that incoming Twist messages either from keyboard or from a navigation toolkit were handled properly. Most of these packages assume a 2D non-holonomic robot, but a quadcopter like the Crazyflie needs to first take off, stay in the air and land. Therefore in the examples, a separate node (vel_mux.py) was written to receive incoming Twist messages, first have the Crazyflie take off in high level commander, and keep sending hover commands to keep it in the air until a land service is called.

What’s next?

As you probably noticed, the project is still under development, but at least it is now at a good state that we feel comfortable to presented at the upcoming ROScon :) We also want to include an more official simulation package, especially now that the Crazyflie has recently became part of the official release of Webots 2022b, but we are currently waiting on the webots_ros2 to be released in the ubuntu packages. Moreover, the idea is to provide multiple simulation backends that based on the requirement of the topic (swarms, vision-based etc), the user can select the simulation most useful for their situation. Also, we would like to even out the missing items (trajectory handling, logging) in both the cflib and cpp backend of the crazyflie_server so that they can be used interchangeably. Also, I saw that the experimental simple mapper node has been featured on social media, so perhaps we should be converting that to Crazyswarm2 as well :)

So once we got the most of the above mentioned issues out the way, that will be the time that we can start discussing the official release of a ROS2 Crazyflie package with its source code residing in the Crazyswarm2 repository. In the meantime, it would be awesome that anybody that is interested in ROS2, or want to soon upgrade their Crazyswarm(1) packages to ROS2 to give the package a whirl. The more people that are trying it out and report bugs/proposing fixes, the more stable it becomes and closer it will come to an official release! Please join us and start any discussions on the Crazyswarm2 project github repository.

Before the summer vacations, I had the opportunity to spend some time working on AI deck improvements (blog post). One of the goals I set was to get CRTP over WiFi working, and try to fix issues along the way. The idea was to put together a small example where you could fly the Crazyflie using the keyboard and see the streamed image along the way. This would require both CRTP to the Crazyflie (logging and commands) as well as CPX to the GAP8 for the images. Just before heading off to vacation I managed to get the demo working, this post is about the results and som of the things that changed.

Link drivers

When using the Crazyflie Python library you connect to a Crazyflie using a URI. The first part of the URI (i.e radio or usb) selects what link driver to use for the connection. For example radio://0/80/2M/E7E7E7E7E7 selects the radio link driver, USB dongle 0 and communication at 2Mbit on channel E7E7E7E7E7.

While working on this demo there were two major things changed in the link drivers. The first one was the implementation of the serial link (serial://) which is now using CPX for CRTP to the Crazyflie. The usecase for this link driver is to connect a Raspberry Pi via a serial port to the Crazyflie on a larger platform.

The second change was to add a new link driver for connecting to the Crazyflie via TCP. Using this link driver it’s possible to connect to the Crazyflie via the network. It’s also possible to get the underlying protocol, the CPX object, for using CPX directly. This is used for communicating with for example the GAP8 to get images.

In the new TCP link driver the URI starts with tcp:// and has either an IP or a host name, followed by the port. Here’s two examples:

  • tcp://aideck-AABBCCDD.local:5000
  • tcp://196.168.0.100:5000

Comparison with the Crazyradio PA

So can WiFi be used now instead of the Crazyradio PA? Well, it depends. Using WiFi will give you larger throughput but you will trade this for latency. In our tests the latency is both larger and very random. In the demo I fly with the Flow V2 deck, which means latency isn’t that much of an issue. But if you were to fly without positioning and just use a joystick, this would not work out.

The Demo!

Below is a video of some flying at our office, to try it out yourself have a look at the example code here. Although the demo was mostly intended for improving CPX, we’ve made use of it at the office to collect training data for the AI deck.

The Crazyflie with AIdeck during over WiFI controlled flight.

Improvements

Unfortunately I was a bit short on time and the changes for mDNS discovery never made it it. Because of this there’s no way to “scan” or discover AI decks, so to connect you will need to know the IP or the host name. For now you can retrieve that by connecting to your AI-deck equipped Crazyflie with the CFclient and look at the console tab.

A part from that there’s more improvements to be made, with a better structure for using CPX (more like the CRTP stack with functions) in the library and more examples. There’s also still a few bugs to iron out, for example there’s still the improved FPS and WiFi throughput issues.

IMAV 2022

Next week from 13th to 16th of September Barbara, Kristoffer and Kimberly will be present at the international Micro Aerial Vehicle Conference and Competition (IMAV) hosted by the MAVlab of the TU Delft in the Netherlands. One of the competitions is called the nano quadcopter challenge, where teams will program a Crazyflie + AI deck combo to navigate through an obstacle field, so we are excited to see what solutions will come out of that. If any of you happens to be at the conference/competition, drop by our table to say hello!

Now it is time to give a little update about the ongoing ROS2 related projects. About a month ago we gave you an heads-up about the Summer ROS2 project I was working on, and even though the end goal hasn’t been reached yet, enough has happened in the mean time to write a blogpost about it!

Crazyflie Navigation

Last time showed mostly mapping of a single room, so currently I’m trying to map a bigger portion of the office. This was initially more difficult then initially anticipated, since it worked quite well in simulation, but in real life the multi-ranger deck saw obstacles that weren’t there. Later we found out that was due to this year old issue of the multi-ranger’s driver incapability to handle out-of-range measurements properly (see this ongoing PR). With that, larger scale mapping starts to become possible, which you can see here with the simple mapper node:

If you look at the video until the end, you can notice that the map starts to diverge a bit since the position + orientation is solely based on the flow deck and gyroscopes , which is a big reason to get the SLAM toolbox to work with the multi-ranger. However, it is difficult to combine it with such a sparse ‘Lidar’ , so while that still requires some tuning, I’ve taken this opportunity to see how far I get with the non-slam mapping and the NAV2 package!

As you see from the video, the Crazyflie until the second hallway. Afterwards it was commanded to fly back based on a NAV2 waypoint in RViz2. In the beginning it seemed to do quite well, but around the door of the last room, the Crazyflie got into a bit of trouble. The doorway entrance is already as small as it is, and around that moment is also when the mapping started to diverge, the new map covered the old map, blocking the original pathway back into the room. But still, it came pretty close!

The diverging of the map is currently the blocker for larger office navigation, so it would be nice to get some better localization to work so that the map is not constantly changed due to the divergence of position estimates, but I’m pretty hopeful I’ll be able to figure that out in the next few weeks.

Crazyflie ROS2 node with CrazySwarm2

Based on the poll we set out in the last blogpost, it seemed that many of you were mostly positive for work towards a ROS2 node for the Crazyflie! As some of you know, the Crazyswarm project, that many of you already use for your research, is currently being ported to ROS2 with efforts of Wolfgang Hönig’s IMRCLab with the Crazyswarm2 project. Instead of in parallel creating separate ROS2 nodes and just to add to the confusion for the community, we have decided with Wolfgang to place all of the ROS2 related development into Crazyswarm2. The name of the project will be the same out of historical reasons, but since this is meant to be the standard Crazyflie ROS2 package, the names of each nodes will be more generic upon official release in the future.

To this end, we’ve pushed a cflib python version of the crazyflie ros2 node called crazyflie_server_py, a bit based on my hackish efforts of the crazyflie_ros2_experimental version, such that the users will have a choice of which communication backend to use for the Crazyflie. For now the node simply creates services for each individual Crazyflie and the entire swarm for take_off, land and go_to commands. Next up are logging and parameter handling, positioning support and broadcasting implementation for the CFlib, so please keep an eye on this ticket to see the process.

So hopefully, once the summer project has been completed, I can start porting the navigation capabilities into the the Crazyswarm2 repository with a nice tutorial :)

ROScon talk

As mentioned in a previous blogpost, we’ll actually be talking about the Crazyflie ROS2 efforts at ROScon 2022 in Kyoto in collaboration with Wolfgang. You can find the talk here in the ROScon program, so hopefully I’ll see you at the talk or the week after at IROS!

In the first years that I started at Bitcraze I’ve been focused mostly on embedded development and algorithmic design like the app layer, controllers and estimators and such, however recently I started to be quite interested in the robotic integration between the Crazyflie and other (open-source) projects and users. This means that I’ll be dwelling more often in the space between Bitcraze and the community, which is something that I do really enjoy I noticed during the Grand Tour. It also initiated my work with simulators which I think would be very useful for the community too. The summer fun project that I’ve been now working on is to integrate the Crazyflie with ROS2 to integrate standard navigational packages, which will be the topic of this blogpost!

ROS2 Crazyflie Node

So first I worked on the ROS2 node that actually communicates with the Crazyflie directly. I think many of you are familiar with the USC’s CrazySwarm project, of which the ROS2 variant, CrazySwarm2, is already available for most functionalities. Even though the name says CrazySwarm, this can be very easily used for only one Crazyflie too. The CrazySwam2 is currently under more development by the IMRClab of TU Berlin, but please take a look if you want to give it a go!

For now while Crazyswarm2 is still under development, I used the Bitcraze Crazyflie python library to make a more hackish node that just publishes exactly the information I want. I am focusing on the scenario with the STEM ranging bundle, aka the Crazyflie + Flowdeck (optical flow + distance sensor) + Multi-ranger (5 x distance sensors) combo, where the node logs the multi-ranger data and the odometry from the Flowdeck with the Crazyradio and outputs that into necessary /scan and /odom topics. Moreover, it also outputs several tf2 transforms that makes it possible to either visualize it in RVIZ and/or connect it to any other packages and it should react to incoming twist messages as well.

Development with a Simulator

And of course… I went in head first and connected it directly with the SLAM toolbox. I have worked with ROS1 in the past, but I had my first experience working with that package in the course: Build Mobile Robots with ROS2 (by Weekly Robotic Newsletter’s Mat Sadowski), so I couldn’t wait to try it on a real platform like the Crazyflie. However, tuning this was of course more work than I thought, as the map that I got out of it first was mostly a sparse collection of dots. Of course the SLAM toolbox is meant for lidars and not something that provided sparse range distances like the Multiranger. Then I decided to take one or two steps back, and first connect a simulator to make tuning a bit easier.

Luckily, I’ve already started to look at simulators, and was quite far in the Webots integration of the Crazyflie. Actually… Webots’ next release (2022b) will contain a Crazyflie as standard! Once it is out, I’ll write a blogpost about that separately :). As luck has it, Webots also has good ROS2 integration as well, and even won the ‘Best ROS Software’ award by The Construct’s ROS awards! Another reason is that I wanted to try out a different simulator for ROS2 this time to complement what I’ve learned in the ROS2 course I mentioned earlier.

So I used the webots driver node to write a simulated Crazyflie that should output the same information as the real Crazyflie node, so that I can easily hack around and try out different things without constantly disturbing my cats from their slumber :). Anyway, I won’t go into to the simulator too much and save that for another blogpost!

Simple Mapping

I decided to also take another additional step before going full SLAM, which is to make a simple mapper node first! This takes the estimated state estimate of the Crazyflie and the Multiranger’s range values and it creates an occupancy grid type map of it. I do have to give kudos to the Marcus’ cflib Pointcloud script and Webot’s simple mapper example, as I did look at them for some reference. But still with the examples, integration and connecting the dots together is quite some work. Luckily I had the simulator to try things out with!

So first I put the Crazyflie in an apartment simulator, flew around and see if any decent maps comes out of it and it seemed it did! Of course, the simulated Crazyflie’s ‘odometry’ comes from near perfect position estimate, so I didn’t expect any problems there (and in such a situation you would actually not really need the localization part of SLAM). This still needs some improvements to be done, like now range measurements that don’t see anything are excluded from drawing, but still it was pretty cool to map the virtual environment.

So it was off to try it out on a real crazyflie. In one of our meeting rooms, I had one Crazyflie take off, let it turn around with a twist message in a /cmd_vel topic and made a map of the room I was currently in. The effect of the 4 range sensors rotating around and creating a map in one go, makes me think of these retro video transitions. And the odometry drift does not seem as bad for it to be possible, but I haven’t mapped our entire office yet so that might be different!

What’s next?

So I’m not stopping here for sure, I want to extend this functionality further and for sure get it to work with the SLAM_toolbox properly! But if the simple mapper already can produce such quality, I’m pretty sure that this can be done in one way or the other. What I could also do, is first generate a simple map and already have a go at the NAV2 package with that one… there are many roads to Rome here!

Currently I’m doing my work on my personal Github account in the crazyflie_ros2_experimental repository. Everything is still very much in development, hackish and quite specific for one use case but that is expected to change once things are working better, so please check the planning in the project’s readme. In the mean time, you can indicate to us in this vote if this is an interesting direction for us to go towards. Not that it will stop me from continuing this project since it is too much fun, but it is always good to know if certain efforts are appreciated!

In December we had a blogpost where we gave an overview of existing simulation models that were out there. In the mean time, I have done some work during my Fun Fridays to get this to work even further. Currently I moved the efforts from my personal Github repo to the Bitcraze organization github called crazyflie-simulation. It is all still very much work in progress but in this blogpost I will explain the content of the repository and what these elements can already do.

Low Poly CAD model

The first thing that you will need to have for any simulation, is a 3D model of the Crazyflie. There is of course already great models available from the CrazyS project, the sim_cf project and the multi_uav_simulator, which are completely fine to use as well. But since we have direct access to the exact geometries of the real crazyflie itself, I wanted to see if I could abstract the shapes myself. And also I would like to improve my Blender skills, so this seemed to be a nice project to work with! Moreover, it might be handy to have a central place if anybody is looking for a 3D simulation model of the Crazyflie.

For simulations with only one or a few Crazyflie, the higher resolution models from the other repository are absolutely sufficient, especially if you are not using a very complicated physics geometry model (because that is where most of the computation is). But if you would like to simulate very big swarms, then the polygon count will have more influences on the speed of the simulation. So I managed to make it to 1970 vertices with the below Crazyflie model, which is not too bad! I am sure that we can make it even with lesser polygons but this is perhaps a good place to start out with for now.

In the crazyflie-simulation, you can find the Blender, stl files and collada files under the folder ‘meshes’.

Webots model

We implemented the above model in a Webots simulator, which was much easier to implement than I thought! The tutorials they provide are great so I was able to get the model flying within a day or two. By combining the propeller node and rotational motor, and adjusting the thrust and drag coefficient to be a bit more ‘Crazyflie like’, it was able to take off. It would be nice to perhaps base these coefficients on the system identification of the Crazyflie, like what was done for this bachelor thesis, but for now our goal is just to make it fly!

The webots model can found in the same simulation repository under /webots/. You can try out the model by

webots webots/world/crazyfly_world.wbt

It would then be possible to control the pitch and roll with the arrow keys of your keyboard while it is maintaining a current height of 1 meter. This is current state of the code as of commit 79640a.

Ignition Gazebo model

Ignition will be the replacement for Gazebo Classic, which is already a well known simulator for many of you. Writing controllers and plugins is slightly more challenging as it is only in C++ but it is such a landmark in the world of simulation, it only makes sense that we will try to make a Gazebo model of the Crazyflie as well! In the previous blogpost I mentioned that I already experimented a bit with Ignition Gazebo, as it has the nice multicopter motor model plugin standard within the framework now. Then I tried to make it controllable with the intergrated multicopter velocity control plugin but I wasn’t super successful, probably because I didn’t have the right coefficients and gains! I will rekindle these efforts another time, but if anybody would like to try that out, please do so!

First I made my own controller plugin for the gazebo model, which can be found in the repository in a different branch under /gazebo-ignition/. This controller plugin needs to be built first and it’s bin file added to the path IGN_GAZEBO_SYSTEM_PLUGIN_PATH, and the Crazyflie model in IGN_GAZEBO_RESOURCE_PATH , but then if you try to fly the model with the following:

 ign gazebo crazyflie_world.sdf

It will take off and hover nicely. Unfortunately, if you try out the key publisher widget with the arrow keys, you see that the Crazyflie immediately crashes. So there is still something fishy there! Please check out the issue list of the repo to check the state on that.

Controllers

So the reason why I made my own controller plugins for the above mentioned simulation models, is that I want to experiment with a way that we can separate the crazyflie firmware controllers, make a code wrapper for them, and use those controllers directly in the simulator. So this way it will become a hybrid software in the loop without having to compile the entire firmware that contains all kinds of extra things that the simulation probably does not need. We can’t do this hybrid SITL yet, but at least it would be nice to have the elements in place to make it possible.

Currently I’m only experimenting with a simple fixed height and attitude PID controller written in C, and some extra files to make it possible to make a python wrapper for those. The C-controller itself you can try out in Webots as of this commit 79640a, but hopefully we will have the python version of it working too.

What is next?

As you probably noticed, the simulation work are still very much work in process and there is still a lot enhancements to add or fix. Currently this is only done on available Fridays so the progress is not super fast unfortunately, but at least there is one model flying.

Some other elements that we would like to work on:

  • Velocity controller, so that the models are able to react on twist messages.
  • Crazyflie firmware bindings of controllers
  • Better system variables (at least so that the ign gazebo model and the webots model are more similar)
  • CFlib integration
  • Add a multiranger and/or camera.
  • and more!

I might turn a couple of these into topics that would be good for contribution, so that any community members can help out with. Please keep an eye on the issue list, and we are communicating on the Crazyswarm2 Discussion page about simulations if you want to share your thoughts on this as well.

Base station geometry estimation is a function in the python client (in the lighthouse tab), where the system estimates the position and orientation of the base stations. The user places the Crazyflie on the floor (in the desired origin) and clicks a button to measure the angles to the base stations, which are used to estimate the geometry. The current implementation is fairly basic and has some issues associated with it:

  1. All base stations must be received from the point where the Crazyflie is located
  2. Only 2 base stations are supported
  3. The coordinate system is not properly aligned with the room
  4. The generated geometry is not as good as it could be, that is the position/orientation is sub-optimal
  5. The code has a dependency to OpenCV which causes problems for ROS users

I have been working on a solution for these problems as my fun Friday project and in this blog post I will tell you a bit more about the problems and a possible solution.

Screenshot from the client of a geometry with 4 base stations.

What are the problems to be solved?

In the current implementation, the user places the Crazyflie in the origin, with the front of the Crazyflie pointing in the direction of the positive X-axis. When the user hits the “Estimate Geometry” button, the angles to the visible base stations are recorded and the solvePnP() function in OpenCV is used to estimate their poses (position and orientation). This is all fine but it also has its limitations and in the following section we will outline what the limitations are and how to solve them.

All base stations must be received in the origin and only 2 base stations are supported

Currently the Crazyflie ecosystem supports up to 2 base stations and this works fine for a flight space of around 4×4 meters. With more base stations it would be possible to cover larger areas or multiple rooms, which is a feature that many users have been asking for. In these scenarios it will not be possible to receive all base stations from one position any more though, and it will require a new method for geometry estimation using multiple measurements. Suppose base station 1 and 2 are received in one position and 2 and 3 in another, then we can map the measurements together since we know base station 2 must have the same pose in both measurements. This way it is possible to relate all base station poses to each other, provided there are measurements that link them together.

The coordinate system is not properly aligned with the room

When generating the geometry in the current implementation, the orientation of the Lighthouse deck is used to define the coordinate system: forward of the deck defines the X-axis, left defines the Y-axis and up the Z-axis. The problem is that the deck might not be perfectly aligned with the Crazyflie, the floor might not be completely flat or the Crazyflie might not point exactly in the desired direction. A pretty small misalignment will result in fairly large errors a couple of meters away, resulting in unexpected behavior, for instance not flying at constant height. Expanding to more base stations and larger systems, the problem will become even bigger and a better solution is clearly needed.

If we used the position of the Crazyflie when placed at multiple positions, we could use this information to rotate the coordinate system to be better aligned. For instance, suppose we measured some points on the floor of the flight space, then we could make sure the XY-plane of the coordinate system goes through those points, or at least as close as possible. Similarly one or more measurements along the X-axis would help to define the rotation around the Z-axis.

The generated geometry is not as good as it could be

The lighthouse positioning system is based on measuring the angles between the sensors on the Lighthouse deck and the base stations. One can think of it as four beams or rays, going from each base station to the sensors on the deck, for which we measure the direction very precisely seen from the base station’s point of view. The purpose of the geometry estimation is to figure out the position and orientation of the base stations so that we can calculate how the beams are oriented in the flight space instead. By looking at where the beams from two base stations intersect we know where the sensors are located and can calculate the position of the Crazyflie. This is a somewhat simplified picture of how it works but it is sufficient for the following discussion.

So what happens if the geometry is not completely correct? If the estimated positions or orientations of the base stations are slightly off, the beams will not intersect and we have to use some method to find the point closest to the two beams instead to use as the sensor position. In a real world system there will always be errors and the implementation must be able to handle them, but we want to keep them as small as possible. Further more we want to make sure the errors are uniformly distributed in the flight space so that we get equally good results everywhere.

In the current estimation process, where we take a measurement in one position, we are able to generate a geometry that is good at that point, but due to noise in the measurements and other subtleties the error at the edges of the flight space might be several centimeters.

The solution to this problem is to measure the angles in multiple positions and try to find a geometry where the error is equally small for all of them. It does not guarantee that the error will be equal everywhere, but if we make measurements in the volume we plan to fly in we know it will be OK where we need it to be. It should also be a much better geometry, for the full covered volume, than what we can be achieved by measuring in one point only.

One bonus problem that hopefully will be solved by this approach is the moving back and forth that sometimes can be seen in a Lighthouse 2 system. What happens is that the base stations interfere with each other from time to time (by design) and most of the time the Crazyflie gets positioning information from both base stations, but every couple of seconds only from one of them. When both are available the “average” position is used, but when only one is received, the Crazyflie will “jump” to the position indicated by that base station (the simplified model from above with crossing beams does not hold in this case, sorry!). If the difference between the suggested positions of the two base stations (the error in the geometry) is large there will be a noticeable motion in the Crazyflie.

The code has a dependency to Open CV

In the current solution we use the solvePnP() function in Open CV to estimate the geometry. Open CV is an awesome library but unfortunately it has turned out that this dependency interferes with ROS, and since a fair amount of our users also use ROS, we would like to get rid of it if possible.

Luckily I found an open source implementation of IPPE, an algorithm that finds the pose of an object based on points seen by a camera, that we can use instead. There is actually an option to use Ippe in OpenCV’s solvePnP(), but we used another flavor.

The solution

The core idea is to first collect measurements of beams in many positions in the flight space by moving a Crazyflie around and record the lighthouse angles. Secondly an equation system is created that takes the poses of the base stations and all the recorded Crazyflie poses as input and as output calculates the lighthouse angles those poses would correspond to for all the sensors. Finally the output is compared to the recorded values and poses are adjusted using the least-square solver in scipy to find the poses that minimizes the difference between the measurements and the output from the equation system.

Before we can solve the equation system we have to record the angles from the base stations. There is a handy function in the Crazyflie that pushes measured lighthouse angles to the PC via the radio, and by letting the user move the Crazyflie around in space we get the angles along that path. What we are looking for though are angles collected in discrete positions and as an approximation I group measurements together based on time. The assumption is that if two angle measurements are closer than 10 ms in time, the Crazyflie did not move very far and they can be considered to be taken in the same position. The output of this process is a list of samples where each sample contains the measured lighthouse angles of one or more base stations for one specific Crazyflie pose. After this has been done, the list is filtered to only contain samples with two or more base stations.

We also need an initial guess of the base station and Crazyflie poses for the least-square solver to make the solution converge. I use IPPE for this and use the first sample as the reference to define a temporary global reference frame. Suppose the first sample contains angles for base stations 2 and 3, we can then use IPPE to calculate an estimate of the pose of the two base stations, in the Crazyflie reference frame of this sample. Since we use the first sample as the reference for the global reference frame (that is the pose of the Crazyflie in this sample is the origin by definition), those poses are also equal to the base station poses in the global reference frame.

Suppose the next sample contains lighthouse angles for base stations 1 and 2, using IPPE we can estimate the base station poses for base stations 1 and 2 in the reference frame of the Crazyflie in this sample. Since the relative positions of the base stations is the same, regardless of reference frame, we can rotate/translate the poses of the base stations so that base station 2 pose matches the pose of base stations 2 in the first sample. We now have an estimate of the poses of base stations 1, 2 and 3, further more the transformation used represents the pose of the Crazyflie in sample 2. Repeating the process for all samples gives us a pretty good idea of where all the base stations are located as well as the pose of the Crazyflie in all the samples.

We can now feed the initial guess and the equation system into scipy and hopefully get a refined solution back. From the estimated poses of the base stations and Crazyflie samples it is possible to calculate the distance between sensors and beams which gives us an approximation of how good the solution is.

The final step is to align the coordinate system with the room, as mentioned earlier the solution we have this far is based on the pose of the Crazyflie in the first sample. The way it is done in the suggested implementation is to ask the user to place the Crazyflie at some points in the desired origin, on the positive X-axis and in the XY-plane and measure the angles in these positions. The measurements are included as samples in the above process which means we will get the estimated positions as a part of the over all solution, in the temporary global reference frame. The task at hand is then to find the rotation/translation from the temporary global reference frame to the one indicated by the positions sampled by the user. Again we do a least-square optimization to find the transformation that minimizes the error in the sampled points. We can now calculate the final solution by applying the transformation to the base station poses we got earlier.

Does it work?

Yes, it seems to work pretty well, I have not had the time to do extensive testing yet but the results looks promising. In our flight arena with 4 base stations, the solution seems to generally be acceptable. We don’t know the exact poses of our base stations since it is very hard to measure, but they are mounted in the same truss and should be at similar heights.

Base stations at:
1: (-3.7104629351065146, -0.27330674065567867, 2.960720481536423)
2: (-0.9233909349006646, -2.9651389799486356, 2.9781503155699176)
3: (-0.12450705551081238, 3.430497907723026, 3.011201684709142)
4: (2.74012584124908, 0.5856524795079388, 3.023133069381165)
Solution match per base station:
1: {'mean_error': 0.0026020322270174697, 'max_error': 0.013310934923630531, 'std_error': 0.0028768923969783836}
2: {'mean_error': 0.0015240237742724164, 'max_error': 0.005526851773945277, 'std_error': 0.0011560341160273498}
3: {'mean_error': 0.002193101969828834, 'max_error': 0.006778096051979129, 'std_error': 0.0015768914826109067}
4: {'mean_error': 0.0033752667182490796, 'max_error': 0.014997173956894249, 'std_error': 0.00354931189334688}

The above snippet is part of the output from one run and as can be seen the estimated height is between 2.96 and 3.02 m. You can also see that the estimated average error for sensor positions is in the order of 2-3 mm while the maximum error is 1.5 cm.

Below is graph of the recorded Crazyflie positions in the final solutions. Note the three single points at the bottom that are from the origin, the X-axis and XY-plane.

Estimated positions of the Crazyflie

I did some testing on larger systems with 6-8 base stations this Friday and it seemed to be harder to get a solution that converges which indicates that there might be something to look into here.

Try it out

This is still work in progress, but if you want to try it out, you can find the code in this pull request. Run the examples/lighthouse/bs_geometry_estimation.py script, you will get instructions on the screen as you go.

Officially the firmware supports 2 base stations , but most of the code is designed to handle up to 16 and if you want to test the functionality with more than two base stations you have to update PULSE_PROCESSOR_N_BASE_STATIONS and re-flash your Crazyflie.

Any feedback is welcome, please use the pull request.

I have returned from my family visit in California, who I’ve haven’t seen them in 3 years due to Covid. To spend the most possible time with them, the plan was that I would still work full time for Bitcraze from my father’s home. The problem became however, that it wouldn’t fit so well in our current way of work as I would miss all the morning stand up meetings due to the large time difference between Sweden and California (-9 hours). That is why we settled that I would work on separate projects/investigations during my time away. So I thought it would be a great opportunity to dig into ROS and 3D simulations again and see what the latest state of that is! So about the simulations is what I’ll be mostly talking about right in this blog post, in terms of what simulators are out there and what simulation development is currently ongoing.

Need for simulation?

Why would it be actually be necessary to have a simulation in our current frame work? Just to give an example, my new colleague Jonas recently tried out his hand on the CFlib swarm class for the first time for the BAMdays tutorials, and simulator would have been great during that initial porcess. Namely, most of the crashes were not necessary due to low batteries or bad communication, but mostly due to the fact that he was not able to double check his script beforehand. If one is able to check if all the programmed positions of the Crazyflies are implemented as they should before an actual flight, this would prevented a lot of broken propellers!

Just to note here that there are a lot of types of simulations that you can think of. Earlier this year had our ex-interns Max and Josephine finish an Renode simulation of the Crazyflie’s microcontrollers. We’ve also seen the word Simulink pop-up multiple times on the forum which indicates that quite some control classes are investigating the dynamic model of the Crazyflie. However, the type of simulation that I’m currently referring to are the 3D simulators in which a robot or quadcopter can move and interact with a virtual environment, with usually an physics engine in effect.

Crazyflie in Gazebo (+ROS)

During some initial investigation there were already some simulations that pop out. First of all I went and looked into what is available for Gazebo at the moment, which is:

CrazyS is based on the RotorS simulation with some additional off-board crazyflie controllers for position control. I wasn’t able to build it for my Ubuntu 20.04 just yet myself, but that there is ongoing work to port CrazyS to ROS Noetic. For now on a virtual machine with ROS melodic it build just fine! Note my laptop did had to work quite hard when I wanted to simulate more than 1 Crazyflie, but the physics and plugins that were made for Gazebo is enabling many to do a lot for their research. Please check out the core papers about CrazyS!

Sim_cf is perhaps a little lesser known, but the project does stand out as it has some interesting features to it. It is for instance, possible to use the actual c-based firmware in software-in-the-loop (SITL) mode, which controls the simulated Crazyflie. It is even possible to use an actual crazyflie with an hardware-in-the-loop (HITL) simulation. Eventhough the project is not actively maintained anymore, I did manage to build it from source for ROS Noetic and Gazebo 11, although I was not able to fly more than 4 do to errors.

Other Simulators

Ofcourse Gazebo is not the only possibility out there. I also had a quick go at another simulator called Webots, which is quite an interesting option indeed as well. Currently there is only one quadcopter model available, so it only makes sense for it to also contain an Crazyflie! They do use their own robotic format, so probably the easiest process would be, is to convert an existing model for Gazebo/ROS into an format that Webots can understand.

Also, quite recently, a trending tweet has brought us to the attention of a Rviz based Crazyflie simulation! This looks quite promising as well, so I will try this out quite soon too.

Screenshot from 2021-11-15 11-56-48
Crazyflie in Ignition Gazebo

Ongoing work in Ignition Gazebo

So in the future, the current Gazebo in its form will disappear and will be only be part of Ignition. So that is why it made sense for me to start playing with an separate Crazyflie model and plugins for the Ignition frame work instead. Moreover, it seems that quite some elements and plugins based on the RotorS simulation for the original gazebo, are now fully integrated within the Ignition gazebo framework, which should make it more easier to make quadcopter models fly. Currently it’s still work in progress, so right now is only to be found on my personal github repository, but as soon as it becomes more fleshed out and stable, this will probably transferred to Bitcraze’s github repos and we will write a more elaborate blogpost about it. For now, I’ll try to work on it further as my Fun Friday project!

In the mean time, we have started a simulation discussion thread in the Crazyswarm2 repository, which is an ongoing port of Crazyswarm to ROS2. It would be the ideal situation if we would be able to use this simulator for both Crazyswarm and our native CFlib! But I’ve mostly have used Gazebo in the past, so if there are any other simulators that we should try out too, please join the discussion and let us know!

During the preparations for BAM, for example the awesome demo that Kristoffer did (link to demo), we had a lot of discussions about landing on charging pads. The Qi deck is a good solution, but the design is a bit complex and makes it hard to have other electronics pointing dowards (although we’ve seen some solutions to this). So after spending a few days thinking about this, I set out to spend a few fun Fridays looking at solutions for this. The results (so far) are detailed in this post.

Contact charger and decks

The idea I decided to try out was something we’ve been discussion on-and-off for a few years, using contact charging instead of Qi. So I revived a few of our old ideas and starting looking for parts. This is always a fun phase of an idea, the sky is the limit! I’m kind of used to electronic parts, but I find looking for mechanical parts is a lot harder. I don’t really have the vocabulary for this and all the parts look the same size in the product photos…

Finally the solution I wanted to test was using small pogo-pins. The idea is that the pogo-pins could be included into any design of new boards and easily enable charging. With the parts found and the ideas fresh again, I jumped into KiCad (5.99) and started the design. Two different designs were made, pictured below.

4 segments (2xGND and 2x5V)
Two circles, one GND and 5V

Why two solutions? I wasn’t sure which one would work well. I suspected the 4 segments would be better, but I was hoping for the two circles, since you would not need additional electronics to handle 5V/GND switched depending on how you landed.

Since there was two solutions for the pad, there’s also two solutions for the deck. I wasn’t sure about what pogo pins to use, so there’s two different sets of pins (which makes it a bit confusing).

Deck for segmented charger (with diode bridge on back)
Deck for circular charger (without diode bridge)

With the components and PCBs in hand I started testing, and…the results were not promising. The design was made to work with our current Qi charging pad (link). Although the alignment for Qi is important, the tolerance was not as tight as what I needed. Back to the drawing board, this time the mechanical one. I used FreeCAD to create a new version where the Crazyflie would align better. After a few iterations with the 3D printer I ended up with this:

Updated charge pad design
Crazyflie on 3D printed charging pad

Putting it all together, the results were promising. With the new mechanical design the area the Crazyflie can land in is larger and the alignment much better. I think it’s good enough to show that the concept can work. The next step is to look a bit more at the pins, they might be a bit too easy to damage.

Crazyflie on circular charge pad
Charge pads and decks

The Glow deck

I mentioned above that the idea was to be able to put charging on decks with other designs on them. To test out this concept I revived another old idea, the Glow deck! The Glow deck is a design where the LEDs are visible from the side and also a bit stronger. To achieve this a diffuse lens would be mounted on the PCB.

The first version of the Glow I made was years ago. It contained a high-power LED, which was really bright…and really warm. We stopped measuring the PCB temperature at 140 C, so that wasn’t something that was going to work out. It also only had one color and I wanted more.

This time around I decided to try two different versions. The reason for the two versions was that I wanted to see if more power actually added something (except complexity and price) when it came to visual effects while flying. One version uses 4xWS2812B LEDs (the same as on the LED-deck) while the other one uses a RGBW high-power LED. I quickly ran into the same problem as with the charging deck, mechanics. I wasn’t able to find a good size of a diffuse lens. Not wanting to be blocked by this I decided on designing one myself and printing it. Not sure how the size would affect the effect I designed two versions.

2 versions of diffuse lens (3D printed)
Left: WS2812B version, right: high-power RGBW version

I haven’t had the time to try out the high-power LED version yet, but the results for the WS2812B version with the diffuse lens looks promising. It’s hard to see the effect in the images, but at least it shows that it’s possible to see the LED from the sides.

Conclusions

I think both the contact charging and the Glow looks promising and I’m happy with the results so far. But making one prototype is easy, making something that can be manufactured and properly sourced is different. If any of these ever turn into a product, the challenge will be finding the right pins, lens, charge base etc.

For the next step, I’m excited to see the high-power version of the Glow in action. This time around I’ve added proper dimming and temperature measurement :-) If you have any ideas, comments or questions drop a line below!