crazyswarm_server: add support for logging

[whoenig]

See also https://github.com/IMRCLab/crazyswarm2/discussions/55

[knmcguire]

Since this hasn't been implemented on the cpp side of things we should probably discuss on design as I want to implement this soon on the cflib server.

In crazyswarm1, the pose was set as a standard (enabled by enable_logging_pose) from stateEstimate.x, y, z and quat. And then people can add more logblock to this indicating which logs to send as topics.

Do we want to do something similar or were you thinking of another policy for this @whoenig ?

[whoenig]

I was thinking we could extend crazyflies.yaml similar to how we handle params. For example:

  firmware_logging:
    enabled: true

    default_topics:
     # remove to disable default topic
      pose:
        frequency: 10 # Hz
    custom_topics:
      topic_name1:
        frequency: 10 # Hz
        vars:
          stateEstimateZ: [x, y, z, quat]
          pm: [vbat]

This could be at any place, where we currently also allow firmware_params (i.e., per robot, per type, or all).

As before, we can have default topics, which are strongly typed. Unlike before, we can now also dynamically configure them (at least with the frequency, potentially with other options in the future). Custom topics can also be defined by defining a list of variables. An error is thrown if such a logging configuration doesn't fit in a single log block or the listed variables do not exist.

In this example, we would get topics "/pose" and "/topic_name1".

In Crazyswarm1, we also had the option to define the time base (ROS time or onboard timestamp). I don't think this makes much sense. If possible, we should always include both (i.e., header can have ROS time from when we received the data, but the data itself also always contains the onboard timestamp). In some case this might not be possible (e.g., for the /pose topic), in which case we can use the ROS time, only. If users need the onboard time for those topics, they can define their own custom topic instead.

[knmcguire]

Ah yes, that is a nice idea to have it fully be depended on the yaml file. And adding the frequency is a good idea too.

I've started yesterday afternoon a bit on the py node inferred from crazyswarm1 just to set things up. For now I have the pose still as standard on and custom log blocks like

 log_topics:
      log1: ["range.back", "range.front", "range.right", "range.left"]

and frequency is all 10 hz.

In that sense I like your yaml better, but we could also do the "vars" like is I did "log1". Then there is quite some repeatability, but in terms of logic in the code, the cflib needs the full log name anyway so than we can add logs in one forloop easily and make the code simpler.

What do you think?

[whoenig]

I think its fine to always have the full qualified name for the variables. Then we'd get:

  firmware_logging:
    enabled: true

    default_topics:
     # remove to disable default topic
      pose:
        frequency: 10 # Hz
    custom_topics:
      topic_name1:
        frequency: 10 # Hz
        vars: ["stateEstimateZ.x", "stateEstimateZ.y", "stateEstimateZ.z", "pm.vbat"]

It has the advantage that it is clearer what we are logging.

[knmcguire]

Ah yeah this looks really clear. I'll try to go for this format first and make a draft pr this or next week

[knmcguire]

So I have gotten pretty far with the logging framework of PR #59. It is not the prettiest of coding but functionality of setting the default logs and custom logblocks from crazyflies.yaml with priorities pretty much works so far.

Now I want to implement setting up dynamic log blocks and bit doubting the right way to do this. I though of two ways: 1- Setting up a parameter per logblock that is logging in /cf2/logs/topic_name1, and every new parameter with the logs prefix with a 'vars' with logging variables will be turned into a new log block. 2- Setting up a service that makes a new log block. Then we make a new type of service with a string array and an interger for frequency.

I prefer 2 more to be honest, as the topics are already a way to check if logs are on. Moreover we need to give the array of log names and frequency at the same time, which won't be possible with the parameter update framework.

Another thing we can consider is a combination for both. Initializing logblocks through a service, but using the parameter framework to change the individual parameters of the initialized logblock, like content of the array, frequency or to turn on/off that specific log block. Then we will have almost the same functionality as the logging configuration and logblock tab in the client.

[whoenig]

Great progress!

Crazyswarm(1) never supported dynamic log configuration changes. It would be indeed nice to have this for Crazyswarm2, but I'd say this doesn't have a super high priority, as the use-cases are limited.

I think it should pretty much be option 2, with two services: firmware_logging_remove(string: topic_name), and firmware_logging_add(string topic_name, int freq, string[] vars). This still doesn't cover everything cflib can do, but I think it is sufficient for most users.

[knmcguire]

Ah yes removing should be possible too. Great! I know what to do then so I'll make two services.

Yeah we can always see what we can add more based on feedback of others so let's see.

[knmcguire]

So, before getting into getting logging for the cpp, perhaps we should discuss the handling of logging to topics?

What I've done before now, is that for each logging variable I make a topic in the same approximate topic type: https://github.com/IMRCLab/crazyswarm2/blob/c66df6278eb984163fde058419e596e2741f4c97/crazyflie/scripts/crazyflie_server.py#L534

This is different than it was done in Crazyswarm2 as that was immediately put in an array, but this allows the user to use RQT to plot the values more easily, and the types should be more aligned, and I feel is perhaps a cleaner approach than putting it all in one array with it all being floats. But that was at least my opinion

What do you think @whoenig ? You had some thoughts about this change right?

[whoenig]

I implemented the basic /pose topic (see draft PR).

There are two issues I see with your proposed change for "dynamic" topics:

1. We essentially lose the time synchronization (packets within one log block are from the same timestamp). Of course, we could add the on-board timestamp in each published message, but then piecing information together on the subscriber side can be still challenging.
2. The general overhead of having so many messages being send around.

The advantages are (as you stated): nicer to use for the user (plotting a bit easier and log blocks are somehow an "implementation detail")

Ideally, one would be able to construct message types dynamically. However, as far as I know this is not possible in ROS2 (and was also not possible in ROS1).

One option to address type safety is to use arrays of all different types, although then it might be confusing how the "mapping" works, i.e., which variables refers to what.

Overall I still prefer the old way, as it feels a bit more "powerful". I would avoid making it a user-option, just because of the added complexity. However, I am also open for some third option.

[knmcguire]

Hi! Sorry for the delay!

At least how I envisioned this to be, was the same functionality as the client but then for a swarm, and then you also get the type with it. Unfortunately it is not possible to dynamically make a new msg with the content of the log-block and the right types.. but perhaps that is just something we need to live with the limitations of ROS at this point, also since the types aren't completely one to one anyway as you can see here.

But would you still like to use for that array our own message or ROS2' Multiarray. I guess the problem with that that it does not have a timestamp... so then we need to make a GenericLogData equivalent.

However, I do think in time people would maybe want more control over it in which standard message type the message will be in, but maybe that is something that the default messages can solve for now.

So then the list can become: 1- Generic logblocks : userdefined logblocks that outputs just a 1d array with all types as doubles/floats and timestamp 2- Default logblocks: predefined logblocks and ROS2 standard types for the outgoing topics which is defined within the server code And maybe a third for in the future 3- Userdefined logging to topics: logblocks and ROS2 standard types for the outgoing topics that users can define without hardcoding it in the server.

What do you think?

[whoenig]

Sounds great - thanks! I'll update the current cpp backend draft accordingly. One thing I don't like about Multiarray (and similar concepts) is that they send the field name in every single message. For a high-resolution image this might be negligible, but for our small logging messages it seems a lot of unnecessary overhead.

[knmcguire]

what do you mean with field? The layout you mean? It is indeed overkill, but my biggest blocker is the lack of timestamps, so no go anyway :)

[knmcguire]

If I'm correct, the Cpp backend does not yet support dynamic log enabling right @whoenig ? Just so that we know what the status is of this ticket.

[whoenig]

Yes, that's correct. Perhaps we should include that in this big comparison table in the docs?

[knmcguire]

ah yes probably we should, I'll add it to the PR once I get around testing it

[whoenig]

This works now in both cflib and cpp backends. The missing dynamic adding/removing of log topics has been documented.