Latency issues for offboard control using cf.setParam("motorPowerSet/m{i}", motor_power[i])

[smidmatej]

Hello,

I am attempting to use a MPC controller computed on my laptop running Ubuntu 20.04 to control a single CrazyFlie 2.0. My controller takes the state of the cf from CrazySwarm using a Vicon tracker. This state is then sent to my MPC controller running as a ROS node which computes the control action as the activation of individual motors on the quadcopter [u1, u2, u3, u4] (The motor configuration on the cf matches my model inside MPC). The computed control action is then relayed using cf.setParam("motorPowerSet/m{i}", u[i]) to the CrazySwarm server (hover_swarm.launch).

def motorPowerSet(cf, motor_power : list):
    cf.setParam("motorPowerSet/m1", int(motor_power[0]))
    cf.setParam("motorPowerSet/m2", int(motor_power[1]))
    cf.setParam("motorPowerSet/m3", int(motor_power[2]))
    cf.setParam("motorPowerSet/m4", int(motor_power[3]))

The problem I encountered is that the time it takes for the CrazySwarm server to update the parameters is longer than I expected. The execution of the motorPowerSet method takes approximately 60ms. On the CrazySwarm side, each parameter update takes ~15ms.
`

[ INFO] [1678358488.277826005]: [cf4] Update parameters
[ INFO] [1678358488.296228588]: [cf4] Update parameters
[ INFO] [1678358488.311242723]: [cf4] Update parameters
[ INFO] [1678358488.325518071]: [cf4] Update parameters
[ INFO] [1678358488.339628882]: [cf4] Update parameters
[ INFO] [1678358488.359660311]: [cf4] Update parameters
`

This control frequency is too low to control the crazyflie, except for exceptionally slow and careful liftoff (the cf still crashes quickly). My question is whether there is any plausible solution to this problem. Is it reasonable to try to connect more than 1 Crazyradio PA and have each one handle setParam for 1 motor each? Or could latency be improved by using CFLib directly, instead of using CrazySwarm as the API?

I estimate I would need at least 50Hz control frequency to achieve adequate performance. Right now, i am at 16Hz. Any help would be greatly appreciated!

[whoenig]

You can use allcfs.setParam to use broadcasts rather than unicast (unicast waits for receiving an ack). In Crazyswarm1, you should also disable logging if possible to reduce latency.
If you are just starting out, take a look at Crazyswarm2 for ROS2.

I doubt using cflib has a lower latency (and it doesn't support broadcasts).

Generally, what you want to achieve doesn't work well, due to the latency even of the state estimate. The on-board attitude controller runs at 250 Hz and any reduction causes visible vibrations.

[smidmatej]

I tried using allcfs.setParam instead, but it increased the latency

setParam script output:

Broadcast: 0.132713079 seconds  
Unicast:   0.052923441 seconds  
Broadcast: 0.148249864 seconds  
Unicast:   0.050703525 seconds  

Crazyswarm output:

[ INFO] [1678445493.458182833]: [cf4] Update parameters
[ INFO] [1678445493.472317097]: [cf4] Update parameters
[ INFO] [1678445493.485616187]: [cf4] Update parameters 
[ INFO] [1678445493.499644405]: [cf4] Update parameters 
[ INFO] [1678445493.509410340]: UpdateParams!   
[ INFO] [1678445493.544081605]: UpdateParams!  
[ INFO] [1678445493.577481121]: UpdateParams!  
[ INFO] [1678445493.611436546]: UpdateParams!  

Script calling setParam

while True:
        time_a = rospy.Time.now()
        motorPowerSet(cf, np.array([0, 0, 0, 0]))
        time_b = rospy.Time.now()
        motorPowerSet(allcfs, np.array([0, 0, 0, 0]))
        time_c = rospy.Time.now()

        print(f"Unicast:   {(time_b - time_a).to_sec()} seconds")
        print(f"Broadcast: {(time_c - time_b).to_sec()} seconds")

Script calling setParam

from pycrazyswarm import Crazyswarm

import rospy
import numpy as np

def main():

    swarm = Crazyswarm()
    cf = swarm.allcfs.crazyflies[0]
    allcfs = swarm.allcfs
    allcfs.setParam("motorPowerSet/enable", 1)

    while True:
        time_a = rospy.Time.now()
        motorPowerSet(cf, np.array([0, 0, 0, 0]))
        time_b = rospy.Time.now()
        motorPowerSet(allcfs, np.array([0, 0, 0, 0]))
        time_c = rospy.Time.now()

        print(f"Unicast:   {(time_b - time_a).to_sec()} seconds")
        print(f"Broadcast: {(time_c - time_b).to_sec()} seconds")

def motorPowerSet(cf, motor_power : np.ndarray):
    cf.setParam("motorPowerSet/m1", int(motor_power[0]))
    cf.setParam("motorPowerSet/m2", int(motor_power[1]))
    cf.setParam("motorPowerSet/m3", int(motor_power[2]))
    cf.setParam("motorPowerSet/m4", int(motor_power[3]))

if __name__ == "__main__":
    main()
    rospy.spin()

Logging

I disabled logging by setting

enable_logging: False
enable_logging_pose: False

in hover_swarm.launch but I don't notice any improvement (I already had enable_logging: False before).
I already have my project implemented in ROS, so switching to ROS2 is not an option for me right now.

Any idea why using broadcasting takes longer than unicasting?

[whoenig]

In hover_launch.py set broadcasting_num_repeats to 1. The default is 15, see https://github.com/USC-ACTLab/crazyswarm/blob/master/ros_ws/src/crazyswarm/src/crazyswarm_server.cpp#L1413-L1414, i.e., each broadcast is repeated 15 times (with a wait of default 1ms in between).

[smidmatej]

I added the parameter broadcasting_num_repeats to hower_swarm.launch.

      ...
      enable_parameters: True
      enable_logging: False
      enable_logging_pose: False
      broadcasting_num_repeats: 1
    </rosparam>

This lowered the broadcast response time, but only to ~50ms. There has to be more overhead. It doesn't make sense for the broadcast communication to take the same amount of time as unicast.

Unicast:   0.056652546 seconds
Broadcast: 0.052135229 seconds
Unicast:   0.052063941 seconds
Broadcast: 0.046496392 seconds
Unicast:   0.06109786 seconds
Broadcast: 0.047077418 seconds

[whoenig]

This overhead might come from a ROS service call. At least the broadcast now takes a bit less time than the unicast. For other commands, we use ROS topics which should have lower latency.

My recommendation would be to change the firmware to set the motor speed using one of the existing cmd topics.

[xkhainguyen]

@whoenig
Hi, this is an interesting topic which I would love to join. When you said

change the firmware to set the motor speed using one of the existing cmd topics.

How can it be faster? I thought they used the same communication method?
I am implementing a single full-state LQR controller running at 50-100Hz (onboard). Do you have any insights or comments about that? Is it even practical?