fbenti commented 1 year ago

I cant't successfully launch crazyswarm and track with optitrack.

Here's my config files:

motin_capture.yaml

/motion_capture_tracking:
ros__parameters:
type: "optitrack"
# hostname: "169.254.250.11"
hostname: "192.168.1.130"

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

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.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]
  custom1:
    offset: [0.0, 0.0, -0.04]
    points:
      p0: [-0.034166, 8.161, 2.361]
      p1: [0.002376, -0.000792, 0.046405]
      p2: [0.033, -0.001288, 0.005244]
      p3: [-0.0013, -0.006, 0.0038]

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:
cf1:
enabled: true
uri: radio://0/80/2M/E7E7E7E70A
initial_position: [0, 0, 0]
type: cf11  # 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: false
uri: radio://0/80/2M/E7E7E7E705
initial_position: [0, -0.5, 0]
type: cf21  # 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: true
  # only if enabled; see motion_capture.yaml
  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: false
  # 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
#firmware_logging:
#   enabled: true
#   default_topics:
#   pose:
#     frequency: 1 # Hz
#   custom_topics:
#    topic_name3: 
#      frequency: 1
#      vars: ["acc.x", "acc.y", "acc.z"]

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

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

Where I've defined a custom marker configuration `custom1` which is used by `cf1`. However, when I run `ros2 launch crazyflie launch.py` this is what I get to terminal:

[INFO] [launch]: All log files can be found below /home/aut/.ros/log/2023-09-06-15-18-36-407728-aut-110872 [INFO] [launch]: Default logging verbosity is set to INFO [INFO] [motion_capture_tracking_node-1]: process started with pid [110873] [INFO] [crazyflie_server-2]: process started with pid [110875] [INFO] [rviz2-3]: process started with pid [110877] [rviz2-3] Warning: Ignoring XDG_SESSION_TYPE=wayland on Gnome. Use QT_QPA_PLATFORM=wayland to run on Wayland anyway. [crazyflie_server-2] [INFO] [1694006316.651714941] [cf1]: ESTKALMAN: State out of bounds, resetting [crazyflie_server-2] [INFO] [1694006316.654201422] [cf1]: ESTKALMAN: State out of bounds, resetting [crazyflie_server-2] [INFO] [1694006316.656473308] [cf1]: ESTKALMAN: State out of bounds, resetting [crazyflie_server-2] [INFO] [1694006316.658045578] [cf1]: ESTKALMAN: State out of bounds, resetting [crazyflie_server-2] [INFO] [1694006316.659682790] [cf1]: ESTKALMAN: State out of bounds, resetting [crazyflie_server-2] [INFO] [1694006316.661992239] [cf1]: ESTKALMAN: State out of bounds, resetting [crazyflie_server-2] [INFO] [1694006316.663638899] [cf1]: ESTKALMAN: State out of bounds, resetting [crazyflie_server-2] [INFO] [1694006316.664444885] [cf1]: Requesting parameters... [crazyflie_server-2] [INFO] [1694006316.670669360] [cf1]: Param TOC: 278 entries with CRC 3685482774 [crazyflie_server-2] [INFO] [1694006316.670725829] [cf1]: Param TOC: found cache. [crazyflie_server-2] [INFO] [1694006316.678435382] [cf1]: reqParamTOC: 0.037586 s (278 params) [crazyflie_server-2] [INFO] [1694006316.678540754] [cf1]: Update parameter "commander.enHighLevel" to 1 [crazyflie_server-2] [INFO] [1694006316.678588122] [cf1]: Update parameter "locSrv.extPosStdDev" to 0.001000 [crazyflie_server-2] [INFO] [1694006316.678626739] [cf1]: Update parameter "locSrv.extQuatStdDev" to 0.050000 [crazyflie_server-2] [INFO] [1694006316.678662104] [cf1]: Update parameter "stabilizer.controller" to 2 [crazyflie_server-2] [INFO] [1694006316.678707965] [cf1]: Update parameter "stabilizer.estimator" to 2 [crazyflie_server-2] [INFO] [1694006316.678748318] [cf1]: Requesting memories... [crazyflie_server-2] [INFO] [1694006316.685213245] [cf1]: Memories: 4

[rviz2-3] [INFO] [1694006316.898163513] [rviz2]: Stereo is NOT SUPPORTED [rviz2-3] [INFO] [1694006316.898283271] [rviz2]: OpenGl version: 4.6 (GLSL 4.6) [rviz2-3] [INFO] [1694006316.913504033] [rviz2]: Stereo is NOT SUPPORTED [crazyflie_server-2] [WARN] [1694006317.702892528] [all]: Motion capture did not receive data! [crazyflie_server-2] [WARN] [1694006318.702834451] [all]: Motion capture did not receive data! [crazyflie_server-2] [WARN] [1694006319.702830022] [all]: Motion capture did not receive data! [crazyflie_server-2] [WARN] [1694006320.702879948] [all]: Motion capture did not receive data! [crazyflie_server-2] [WARN] [1694006321.702818305] [all]: Motion capture did not receive data! [crazyflie_server-2] [WARN] [1694006322.702917879] [all]: Motion capture did not receive data! [crazyflie_server-2] [WARN] [1694006323.702830271] [all]: Motion capture did not receive data!



while instead the crazyflie is correctly tracked if I check the optitrack software

whoenig commented 1 year ago

The mode needs to be libRigidBodyTracker in your case.

fbenti commented 1 year ago

Thanks for the reply! But, how come is that? When one has to be used and when the other? According to https://crazyswarm.readthedocs.io/en/latest/configuration.html. it seems that motionCapture has to be used when there is an unique marker arrangement for every crazyflie, while libRigidBodyTracker when every crazyflie is identified by only one marker.

whoenig commented 1 year ago

Whenever you create objects/assets in your motion capture software, you use motionCapture, in all other cases libRigidBodyTracker. Most motion capture software packages have trouble with tracking small objects, causing axis flips. libRigidBodyTracker has a very simple first-order dynamics filter that prevents such tracking mistakes.

knmcguire commented 1 year ago

Transferring this to discussion as it is an support question.

IMRCLab / crazyswarm2

Motion capture did not receive data! #288

global settings for all robots

firmware logging for all drones (use robot_types/type_name to set per type, or

robots/drone_name to set per drone)

remove to disable default topic

firmware parameters for all drones (use robot_types/type_name to set per type, or

robots/drone_name to set per drone)

ring: