Crazyflie pose Drifting in RVIZ

[jackp24]

Hello,

Thank you for the reply in #322 ! The issues on that front have been resolved by changing the motion capture field to false as @whoenig suggested and updating the crazyradio2 to the latest release.

However, now I am experiencing a different issue. I am seeing that the pose of the Crazyflie is moving while the Crazyflie is sitting still on the ground when I launch the Crazyswarm 2 system. I am also seeing this reflectedin RVIZ as the Crazyflie visually appears to be drifting. I initially thought this was some kind of issue with the onboard IMU, but I tried this with multiple Crazyflie drones. I have the same configuration files as in #322 with the one change mentioned above. Below is the output of the terminal when I run the Crazyswarm 2 system. Do you have any advice on this?

jack@jacks-ThinkPad-T470s:~$ ros2 launch crazyflie launch.py backend:=cpp
[INFO] [launch]: All log files can be found below /home/...
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [motion_capture_tracking_node-1]: process started with pid [14714]
[INFO] [teleop-2]: process started with pid [14716]
[INFO] [joy_node-3]: process started with pid [14718]
[INFO] [crazyflie_server-4]: process started with pid [14720]
[INFO] [rviz2-5]: process started with pid [14722]
[rviz2-5] Warning: Ignoring XDG_SESSION_TYPE=wayland on Gnome. Use QT_QPA_PLATFORM=wayland to run on Wayland anyway.
[teleop-2] [INFO] [1699218728.127856711] [teleop]: Mode changed to cmd_vel_world
[crazyflie_server-4] [INFO] [1699218728.156211969] [cf5]: SYS: ----------------------------
[crazyflie_server-4] [INFO] [1699218728.159324807] [cf5]: SYS: Crazyflie 2.1 is up and running!
[crazyflie_server-4] [INFO] [1699218728.160976535] [cf5]: SYS: Build ... (2023.07) CLEAN
[crazyflie_server-4] [INFO] [1699218728.164173299] [cf5]: SYS: I am ... and I have 1024KB of flash!
[crazyflie_server-4] [INFO] [1699218728.165760945] [cf5]: CFGBLK: v1, verification [OK]
[crazyflie_server-4] [INFO] [1699218728.166626423] [cf5]: DECK_CORE: 0 deck(s) found
[crazyflie_server-4] [INFO] [1699218728.171416022] [cf5]: IMU: BMI088: Using I2C interface.
[crazyflie_server-4] [INFO] [1699218728.173054338] [cf5]: IMU: BMI088 Gyro connection [OK].
[crazyflie_server-4] [INFO] [1699218728.174660030] [cf5]: IMU: BMI088 Accel connection [OK]
[crazyflie_server-4] [INFO] [1699218728.176188794] [cf5]: IMU: BMP388 I2C connection [OK]
[crazyflie_server-4] [INFO] [1699218728.177765659] [cf5]: ESTIMATOR: Using Complementary (1) estimator
[crazyflie_server-4] [INFO] [1699218728.182701126] [cf5]: CONTROLLER: Using PID (1) controller
[crazyflie_server-4] [INFO] [1699218728.184332690] [cf5]: MTR-DRV: Using brushed motor driver
[crazyflie_server-4] [INFO] [1699218728.186714920] [cf5]: SYS: About to run tests in system.c.
[crazyflie_server-4] [INFO] [1699218728.187600462] [cf5]: SYS: NRF51 version: 2023.02
[crazyflie_server-4] [INFO] [1699218728.188465146] [cf5]: EEPROM: I2C connection [OK].
[crazyflie_server-4] [INFO] [1699218728.189312495] [cf5]: STORAGE: Storage check [OK].
[crazyflie_server-4] [INFO] [1699218728.190961568] [cf5]: IMU: BMI088 gyro self-test [OK]
[crazyflie_server-4] [INFO] [1699218728.191814843] [cf5]: SYS: Self test passed!
[crazyflie_server-4] [INFO] [1699218728.193477509] [cf5]: STAB: Wait for sensor calibration...
[crazyflie_server-4] [INFO] [1699218728.194310732] [cf5]: SYS: Free heap: 20496 bytes
[crazyflie_server-4] [INFO] [1699218728.195972048] [cf5]: STAB: Starting stabilizer loop
[crazyflie_server-4] [INFO] [1699218728.196779423] [cf5]: SUP: Can not fly
[crazyflie_server-4] [INFO] [1699218728.197610747] [cf5]: SUP: Ready to fly
[crazyflie_server-4] [INFO] [1699218728.198444624] [cf5]: Requesting parameters...
[crazyflie_server-4] [INFO] [1699218728.204122704] [cf5]: Param TOC: 279 entries with CRC 51380503
[crazyflie_server-4] [INFO] [1699218728.204179754] [cf5]: Param TOC: found cache.
[crazyflie_server-4] [INFO] [1699218728.246467409] [cf5]: reqParamTOC: 0.098088 s (279 params)
[crazyflie_server-4] [INFO] [1699218728.246589778] [cf5]: Update parameter "commander.enHighLevel" to 1
[crazyflie_server-4] [INFO] [1699218728.246646666] [cf5]: Update parameter "locSrv.extPosStdDev" to 0.001000
[crazyflie_server-4] [INFO] [1699218728.246696431] [cf5]: Update parameter "locSrv.extQuatStdDev" to 0.050000
[crazyflie_server-4] [INFO] [1699218728.246930904] [cf5]: Update parameter "ring.effect" to 16
[crazyflie_server-4] [INFO] [1699218728.246978304] [cf5]: Update parameter "stabilizer.controller" to 2
[crazyflie_server-4] [INFO] [1699218728.247026798] [cf5]: Update parameter "stabilizer.estimator" to 2
[crazyflie_server-4] [INFO] [1699218728.254537803] [cf5]: CONTROLLER: Using Mellinger (2) controller
[crazyflie_server-4] [INFO] [1699218728.257411526] [cf5]: ESTIMATOR: Using Kalman (2) estimator
[crazyflie_server-4] [INFO] [1699218728.258140163] [cf5]: Log TOC: 485 entries with CRC 3375487317
[crazyflie_server-4] [INFO] [1699218728.258164830] [cf5]: Log TOC: found cache.
[crazyflie_server-4] [INFO] [1699218728.258460801] [cf5]: Logging to accel at 10 Hz
[crazyflie_server-4] [INFO] [1699218728.263465692] [cf5]: Logging to gonogo at 10 Hz
[crazyflie_server-4] [INFO] [1699218728.268341962] [cf5]: Logging to misc at 10 Hz
[crazyflie_server-4] [INFO] [1699218728.273124719] [cf5]: Logging to state at 10 Hz
[crazyflie_server-4] [INFO] [1699218728.278758022] [cf5]: Logging to /pose at 10 Hz
[crazyflie_server-4] [INFO] [1699218728.288988696] [cf5]: Requesting memories...
[crazyflie_server-4] [INFO] [1699218728.291661562] [cf5]: Memories: 4
[crazyflie_server-4] [INFO] [1699218728.305914365] [all]: radiobroadcast://*/48/2M
[rviz2-5] [INFO] [1699218728.484049382] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-5] [INFO] [1699218728.484251319] [rviz2]: OpenGl version: 4.6 (GLSL 4.6)
[rviz2-5] [INFO] [1699218728.506147193] [rviz2]: Stereo is NOT SUPPORTED
[crazyflie_server-4] [INFO] [1699218755.400558553] [cf5]: ESTKALMAN: State out of bounds, resetting
[crazyflie_server-4] [INFO] [1699218782.441830166] [cf5]: ESTKALMAN: State out of bounds, resetting
[crazyflie_server-4] [INFO] [1699218809.805261981] [cf5]: ESTKALMAN: State out of bounds, resetting
[crazyflie_server-4] [INFO] [1699218837.212539090] [cf5]: ESTKALMAN: State out of bounds, resetting
[crazyflie_server-4] [INFO] [1699218864.547650845] [cf5]: ESTKALMAN: State out of bounds, resetting
[crazyflie_server-4] [INFO] [1699218892.211451222] [cf5]: ESTKALMAN: State out of bounds, resetting
[crazyflie_server-4] [INFO] [1699218919.917421270] [cf5]: ESTKALMAN: State out of bounds, resetting

[knmcguire]

Hi! If you see it drifting away, this means that it is not achieving a proper postiion estimate and it is diverging (that is also what this part indicates: ESTKALMAN: State out of bounds, resetting).

So something is still going on... and I don't see the 'motion_capture_tracking_node' being started at all. @whoenig, perhaps you have a better view of what is missing still.

[jackp24]

I am not sure what is causing the Kalman filter estimate to diverge either. Would it be worth changing to the complementary filter or tuning the Kalman Gain?
Also, isn't the 4th line the 'motion_capture_tracking_node' starting? Or does this line mean something else?
[INFO] [motion_capture_tracking_node-1]: process started with pid [14714]

[whoenig]

This error happens if the motion capture data is not broadcast correctly. How does your /poses topic look like (using ros2 topic echo)?

[jackp24]

The /poses topic shows the drone as drifting as well.

[Eric-Ewing]

So just to confirm, the /poses topic isn't matching the positions in Vicon? If you select the crazyflie object and look at the position under graphs in Tracker, the Tx/y/z values don't match the data in /poses when it's drifting in Rviz? You don't see the object flipping or doing anything weird in Tracker?

[jackp24]

I will check the position graphs in Vicon tomorrow afternoon, but visually, the crazyflie object is stationary in the perspective view in vicon. Which does not match the crazyflie pose echo or Rviz. To clarify, when the crazyflie is stationary, both rviz and the pose echo reflect the crazyflie drifting while the vicon system reflects the object as stationary. However, I can inspect the graphs in Tracker tomorrow to make sure the vicon system data is what it should be.

[whoenig]

There is a difference between /poses (the vicon data) and cf<x>/pose (the data from the on-board state estimator). What Eric was asking is if you see a difference between those two topics.

[jackp24]

@whoenig @knmcguire @Eric-Ewing
I am still having the same issue. I had the system working towards the middle of November, but it is no longer working. The /poses topic has no content at all (when I echo it, nothing outputs) and the /cf5/pose topic shows that the drone is drifting. Do you have any advice on how I can troubleshoot this or any insight as to why I am getting this error?

[whoenig]

If there is no output on /poses, you are not getting any data from the motion capture. That typically means that your IP address i incorrect or the network is not working properly. Try the standalone version https://github.com/IMRCLab/libmotioncapture.