Using Crazyswarm with Qualisys

[SkogsTomte]

Hello!

I'm currently trying to use crazyswarm2 with the Qualisys positioning system, as I previously have done with crazyswarm1. I am, however, running in to problems when the drone takes off. When running the hello world script, the crazyflie almost immediately flips over. I can see in rviz that the drone has correct orientation and position, so I have a hard time figuring out where the problem comes from. I can fly with the drone using the cfclient together with qualisys without problem. I am using the cpp backend.

I suspect I must have done something wrong in the config files, but I just can't figure out what. I'm very thankful for any input I can get on this.
This is what the configs look like:

crazyflies.yaml

# named list of all robots
robots:
  cf1:
    enabled: false
    uri: radio://0/100/2M/E7E7E7E701
    initial_position: [0, 0, 0]
    type: cf_type_1  # see robot_types
    # firmware_params:
    #   kalman:
    #     pNAcc_xy: 1.0 # default 0.5
    # firmware_logging:
    #   enabled: true
    #   custom_topics:
    #     topic_name3: 
    #       frequency: 1
    #       vars: ["acc.x", "acc.y"]
  cf2:
    enabled: true
    uri: radio://0/100/2M/E7E7E7E702
    initial_position: [0, -0.5, 0]
    type: cf_type_2  # see robot_types
    # firmware_params:
    #   kalman:
    #     pNAcc_xy: 1.0 # default 0.5
    #firmware_logging:
    #  custom_topics:
    #    topic_name3: 
    #      frequency: 1
    #      vars: ["acc.x", "acc.y", "acc.z"]
# Definition of the various robot types
robot_types:
  cf_type_1:
    motion_capture:
      enabled: true
      # only if enabled; see motion_capture.yaml
      marker: custom_frame_1
      dynamics: default
    big_quad: false
    battery:
      voltage_warning: 3.8  # V
      voltage_critical: 3.7 # V
    # firmware_params:
    #   kalman:
    #     pNAcc_xy: 1.0 # default 0.5
    #firmware_logging:
    #   enabled: true
    #   default_topics:
    #   pose:
    #     frequency: 1 # Hz
    #   custom_topics:
    #    topic_name3: 
    #      frequency: 1
    #      vars: ["acc.x", "acc.y", "acc.z"]

  cf_type_2:
    motion_capture:
      enabled: true
      # only if enabled; see motion_capture.yaml
      marker: custom_frame_2
      dynamics: default
    big_quad: false
    battery:
      voltage_warning: 3.8  # V
      voltage_critical: 3.7 # V

    # firmware_params:
    #   kalman:
    #     pNAcc_xy: 1.0 # default 0.5

# global settings for all robots
all:
  # firmware logging for all drones (use robot_types/type_name to set per type, or
  # robots/drone_name to set per drone)
  firmware_logging:
    enabled: false
    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"]
    #  topic_name2:
    #    frequency: 1 # Hz
    #    vars: ["stabilizer.roll", "stabilizer.pitch", "stabilizer.yaw"]
  # firmware parameters for all drones (use robot_types/type_name to set per type, or
  # robots/drone_name to set per drone)
  firmware_params:
    commander:
      enHighLevel: 1
    stabilizer:
      estimator: 2 # 1: complementary, 2: kalman
      controller: 2 # 1: PID, 2: mellinger
    # ring:
    #   effect: 16 # 6: double spinner, 7: solid color, 16: packetRate
    #   solidBlue: 255 # if set to solid color
    #   solidGreen: 0 # if set to solid color
    #   solidRed: 0 # if set to solid color
    #   headlightEnable: 0
    locSrv:
      extPosStdDev: 1e-3
      extQuatStdDev: 0.5e-1
    # kalman:
    #   resetEstimation: 1
  broadcasts:
    num_repeats: 15 # number of times broadcast commands are repeated
    delay_between_repeats_ms: 1 # delay in milliseconds between individual repeats

motion_capture.yaml

/motion_capture_tracking:
  ros__parameters:
    type: "qualisys"
    hostname: "192.168.0.50"

    mode: "motionCapture" #"libobjecttracker" # one of motionCapture,libRigidBodyTracker

    marker_configurations:
      default: # for standard Crazyflie
        offset: [0.0, 0.0, 0.0]
        points:
          p0: [   0.0,    0.0, 0.022]  # top
          p1: [-0.042,  0.042, 0.0  ]  # back left   (M3)
          p2: [-0.042, -0.042, 0.0  ]  # back right  (M2)
          p3: [ 0.042, -0.042, 0.0  ]  # front right (M1)
      default_single_marker:
        offset: [0.0, -0.01, -0.04]
        points:
          p0: [0.0177184,0.0139654,0.0557585]
      mocap_deck:
        offset: [0.0, 0.0, -0.01]
        points:
          p0: [0.03, 0.0, 0.0] # front
          p1: [0.00, -0.03, 0.0] # right
          p2: [-0.015, 0.0, 0.0] # back
          p3: [0.00, 0.03, 0.0] # left
      medium_frame:
        offset: [0.0, 0.0, -0.03]
        points:
          p0: [-0.00896228,-0.000716753,0.0716129]
          p1: [-0.0156318,0.0997402,0.0508162]
          p2: [0.0461693,-0.0881012,0.0380672]
          p3: [-0.0789959,-0.0269793,0.0461144]
      big_frame:
        offset: [0.0, 0.0, -0.06]
        points:
          p0: [0.0558163,-0.00196302,0.0945539]
          p1: [-0.0113941,0.00945842,0.0984811]
          p2: [-0.0306277,0.0514879,0.0520456]
          p3: [0.0535816,-0.0400775,0.0432799]
      custom_frame_1:
        offset: [0.0, 0.0, -0.032]
        points:
          #p0: [0.027805, -0.0023317, 0.0]
          #p1: [0.027942, 0.0023083, 0.0]
          p2: [0.0026966, 0.027846, 0.0]
          #p3:
          #p4: [-0.027701, 0.0023835, 0.0]
          #p5: [-0.027754, -0.0023835, 0.0]
          #p6:
          #p7: [0.0025323, 0.027675, 0.0]
          #p8:
          #p9:
          #p10: [0.038764, 0.060993, 0.0]
          p11: [-0.038839, 0.060991, 0.0]
          #p12:
          #p13:
          p14: [-0.039172, -0.060823, 0.0]
          p15: [0.039418, -0.060749, 0.0]
      custom_frame_2:
        offset: [0.0, 0.0, -0.032]
        points:
          #p0: [0.027805, -0.0023317, 0.0]
          #p1: [0.027942, 0.0023083, 0.0]
          p2: [0.0026966, 0.027846, 0.0]
          #p3:
          p4: [-0.027701, 0.0023835, 0.0]
          #p5: [-0.027754, -0.0023835, 0.0]
          #p6:
          p7: [0.0025323, -0.027675, 0.0]
          #p8:
          #p9:
          p10: [0.038764, 0.060993, 0.0]
          #p11: [-0.038839, 0.060991, 0.0]
          #p12:
          #p13:
          #p14: [-0.039172, -0.060823, 0.0]
          #p15: [0.039418, -0.060749, 0.0]

    dynamics_configurations:
      default:
        max_velocity: [2, 2, 3] # m/s
        max_angular_velocity: [20, 20, 10] # rad/s
        max_roll: 1.4 #rad
        max_pitch: 1.4 #rad
        max_fitness_score: 0.001

    # Rigid bodies will be automatically generated by the launch file
    # rigid_bodies:

[whoenig]

I suggest that you enable logging and then look with ros2 topic echo /cf2/pose if the on-board state estimate is correct (what rviz shows is the belief of the host computer, not the belief of the drone itself, so there could still be issues with the communication).

Does your config intentionally use the Qualisys software for rigid body tracking? If so, you don't need to define the custom_frames and you can even set motion_capture/enabled to false (the name is misleading, it just enables the custom rigid-body tracking, not the motion capture itself, which is done in a separate package). If you want to use our custom tracker, you should switch the tracking mode to libRigidBodyTracker.

[whoenig]

Interesting observation. At what update rate is your qualisys system running? We try to send a radio packet for every frame, so of the update rate is too high, this might be an issue.

[SkogsTomte]

Aha, that's the problem then, because qualisys is broadcasting at 200Hz which is then probably too high. I'll try to reduce the frequency and see if that helps.

[knmcguire]

@SkogsTomte did that solve it for you? We have a qualisy system at our office so if you still have problems I could check it out as well.

[SkogsTomte]

Yes, it solved the problem! Qualisys was simply broadcasting at too high of a frequency. I lowered it to 50Hz and then it worked just fine.

[knmcguire]

great! If you can, you can mention this in 'suggest an answer' on the bottom and mark that as an answer, so that others can more quickly find what the solution is.

[whoenig]

FYI, I did find many issues with the communication when having 3+ drones in Crazyswarm2. I have fixes for it, but it will take a few weeks before it gets cleaned up and merged.
Just make sure you test with a single drone first, since there are no known issues in that case.

[SkogsTomte]

The issue was solved by lowering the output frequency to 50 Hz, as the problem was that the update of the crazyflie's position was lagging behind, trying to send a radio packet for every time frame received from qualisys.