Multiple /tf frames and /poses from Optitrack and Motive

[jakeelkins]

Hey guys, apologies for the long issue.

My lab is using one crazyflie with a mocap deck and custom marker positioning with a crazyradio2, with Motive 2.2.0 to do the motion capture. I have bootloaded the latest firmware from the cfclient, which is labeled as 2024.02.

We have a Rigid Body asset defined in Motive (defined as "cf_model_new") on a host machine, streaming data to the Ubuntu system to run crazyswarm2. Motive is doing a great job of tracking the rigid body, so we want to simply use the data coming in from Motive directly, rather than use libRigidBodyTracker or something on the point cloud. The crazyflie is named "cf5" in crazyswarm2 config.

The summary of the issue is that two different poses are represented on the /tf topic and the /poses topic. This is seen in rviz2 here:

When unlabeled markers is turned off in Motive streaming, the /cf5 frame becomes static. When I turn it on again, it re-coincides with the /cf_model_new frame, though it is very shaky. We suspect this is the culprit of poor hello_world performance. The frame /cf_model_new looks great in rviz2.

How can we tell crazyswarm2 to use this data, instead of resolving the unlabeled point cloud? I have seen to set motion_capture/enabled to false in the config. However, when I do this, the motion_capture_tracking node dies due to an InvalidParameterException.

My current config files are below. Note that I use optitrack_closed_source due to the old Motive version (2.2.0).

motion_capture.yaml:

/motion_capture_tracking:
  ros__parameters:
    type: "optitrack_closed_source"
    hostname: "192.168.137.1"

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

    topics:
      poses:
        qos:
          mode: "sensor"
          deadline: 100.0 # Hz

    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.029, 0.0, 0.0] # front
          p1: [0.00, -0.029, 0.0] # right
          p2: [-0.014, 0.0, 0.0] # back
          p3: [0.00, 0.029, 0.0] # left
          p4: [0.038, -0.038, 0.01] # my side one
      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]

    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:

crazyflies.yaml:

# named list of all robots
robots:
  cf231:
    enabled: false
    uri: radio://0/80/2M/E7E7E7E7E7
    initial_position: [0.0, 0.0, 0.0]
    type: cf21  # 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"]
  cf5:
    enabled: true
    uri: radio://0/5/2M/E7E7E7E705
    initial_position: [0.0, 0.0, 0.0]
    type: cf21_mocap_deck  # 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:
  cf21:
    motion_capture:
      enabled: false
      # only if enabled; see motion_capture.yaml. there was something here about if it's an asset then dont enable this.
      marker: default_single_marker
      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"]

  cf21_mocap_deck:
    motion_capture:
      enabled: true
      # only if enabled; see motion_capture.yaml
      marker: mocap_deck
      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: true
    default_topics:
     # remove to disable default topic
      pose:
        frequency: 10 # Hz
      status:
        frequency: 1 # Hz
    custom_topics:
     position_jake:
       frequency: 10 # Hz
       vars: ["stateEstimateZ.x", "stateEstimateZ.y", "stateEstimateZ.z", "pm.vbat"]
     eulers_jake:
       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

Note that I also get low unicast and multicast warnings due to old firmware, though I have the latest from the cfclient. if there is a newer firmware that might be the culprit, any info on how to find that file is appreciated. Let me know if I need to add any other warnings, files, or Motive settings.

Thank you so much!
Jake

[whoenig]

• The asset in motive needs to have the same name as in crazyflies.yaml
• in crazyflies.yaml, for your robot type, set motion_capture: enabled to false (this is named confusingly - it should say use_librigidbodytracker)
• Later, you might run into a related issue RVIZ transform mocap pose conflict #235, i.e., try commenting out the pose default topic if that's the case.

[jakeelkins]

Thanks for the reply @whoenig, I really appreciate it.

When I set motion_capture: enabled to false in crazyflies.yaml (all other config the same as in original message), the motion capture node dies with the following output:

Any thoughts? When I commented out the pose default topic, the tracking is fine and should be good enough for our tests. I would still, however, like to use the Motive track, as our setup seems to work well.

[whoenig]

I have not seen this particular issue before.

One problem with these exceptions is that you only get useful information in a debugger. You could recompile with debugging enabled (see https://imrclab.github.io/crazyswarm2/howto.html#debugging), and then run the motion capture node with a debugger attached (using a prefix, see

crazyswarm2/crazyflie/launch/launch.py

Line 127 in d9064d9

prefix=PythonExpression(['"xterm -e gdb -ex run --args" if ', LaunchConfiguration('debug'), ' else ""']),

for an example).

[jakeelkins]

I added the debugger to the motion capture node and recompiled with debug enabled. I also commented out the default pose topic. When I start the server with debug:=true, the motion_capture node and server hang with the following outputs:

On Motive, this was still with streaming unlabeled markers on.

When I only streamed the rigid body points and run the server with debug:=true, the motion capture seems to work for a second, then fails to receive data:

When I run the server with debug:=false is off, the poses look fine on rviz2 (that is, looks the same as what I've defined on Motive), but the motion capture tracking node still prints that the pose isn't updated:

Thanks again!

[whoenig]

Two days ago the problem was that the motion_capture_tracking node essentially crashed. When you run with the debugger, the node doesn't crash anymore? It should be sufficient to only debug that node and keep the server running normally.

[jakeelkins]

Yes sir, I was having trouble reproducing the behavior last Friday, but today I can consistently reproduce the behavior when the node crashes. here's the xterm output, though I don't see much new information:

Let me know if there's anything else I can do to get more info. I might switch over to a vicon system in another lab for the data we need -- we're trying to flash a custom quadrotor controller onto the crazyflie for trajectory tracking control, so I think we do need the nice pose data.

Thanks again for the help!

[jakeelkins]

Update: I switched over to a Vicon system with Tracker, defined a rigid body in Tracker, and set all the motion_capture.enabled to false in my config files. Updated my ROS distro to Iron. I still get the InvalidParameterValueException from rclcpp, and the motion capture node crashes. It seems to only happen when the motion capture is not enabled for any robot in the config.