Provide velocity estimates from ROS and/or pycrazyswarm

[MauroPfister]

Hi

For our control algorithms we need the velocity of the crazyflies in world coordinates. At the moment we calculate and publish these with a separate python node but it would be nice if this was directly part of crazyswarm. I see two options how to implement this:

1. Add a method velocity() to the Crazyflie class which returns the velocity of the crazyflie instance. The tf package seems to have a function lookupTwist() which in my understanding calculates the velocity using simple finite difference. This approach would be easy, but I am not sure if it is efficient and fast enough.
2. Implement this velocity functionality on the cpp side. It probably does not make sense to continuously publish the velocity of all crazyflies if it is not needed. As far as I understand, a ROS service would make more sense here?

I am more than happy to implement the above but I probably need some guidance from @whoenig or @jpreiss. Please let me know if this is something you consider useful.

Thanks!

[whoenig]

I think this feature depends a bit on the use-case. We already have something like that in some firmware variants, where the velocity of neighbors is numerically estimated. If it is needed on the host side, I prefer option 1 - lookupTwist should be very fast. The downside is that there is no ROS2 equivalent for it and that it will be naturally very noisy (i.e., an additional filter might be needed). If you fly only a few CFs per radio, you could also log stateEstimate.vx etc, which will be much more accurate (estimated by the on-board EKF of each CF). On the client side, you could simply subscribe to the logging topic(s) accordingly. In all cases, I wouldn't recommend using a service call here, because they add an unknown runtime - publishing at a constant rate is a better option.

[MauroPfister]

Thanks for the reply. In our application we need the velocity on the host side because we run a high level trajectory planner. Logging the state estimates is not an option because we fly too many CFs per radio. Currently the trajectorythis planner is written in Matlab which is why I wrote a crazyswarm Matlab API similar to pycrazyswarm. Unfortunately, the Matlab ROS API currently has some performance issues which makes retrieving positions from a tf tree quite slow. As a workaround I run a Python node that publishes positions and velocities on a separate topic for each crazyflie and read these from Matlab.

I thought that publishing positions and velocities directly from the crazyswarm_server (without having to use pycrazyswarm) might be useful to others too. However, if you do not like this I am also fine with submitting a PR that adds this functionality to pycrazyswarm.

[swarm5]

I also encountered this problem: I need to get the flight vel of cf at a frequency of 20HZ, and each of my radios is connected to 7 cfs.

1. Can I use the getParam (stateEstimate.vx) instruction to obtain cf speed vector information? Under this configuration, will there be a big delay?
2. I hope to obtain the velocity of neighbors. @whoenig As you said, you have implemented this function in some firmware variants. Can you give me a link for reference?

[whoenig]

(it might be better to have this in a separate thread)

Do you need the data on-board the Crazyflie, or on the host computer? In either case, I'd rely on the position data and then numerically compute & filter the velocity data.

1. No, you would need to use logging, not getParam. My guess is that it would be too slow for 7 CFs/radio to do it that way.
2. There is some support to get the position of neighbors on-board using the peer-localization module (https://github.com/bitcraze/crazyflie-firmware/blob/master/src/modules/src/peer_localization.c).

[swarm5]

(it might be better to have this in a separate thread)

Do you need the data on-board the Crazyflie, or on the host computer? In either case, I'd rely on the position data and then numerically compute & filter the velocity data.

1. No, you would need to use logging, not getParam. My guess is that it would be too slow for 7 CFs/radio to do it that way.
2. There is some support to get the position of neighbors on-board using the peer-localization module (https://github.com/bitcraze/crazyflie-firmware/blob/master/src/modules/src/peer_localization.c).

@whoenig I have read the content of peer_localization.c, but it only obtains the neighbor's location information. If I want to obtain the neighbor's speed information, what should I do?

[whoenig]

The best would be numerical approximation, if that is good enough for your use case. See statement from earlier:

In either case, I'd rely on the position data and then numerically compute & filter the velocity data.