Best way to integrate crazyflies and turtlebots with optitrack

[xiaoshan-lin]

High-level problem: We would like to move two turtlebots and two crazyflies together using crazyswarm2. While we have got a (mostly) working procedure, we want to check if this is indeed the best way.

What is currently working: We are using the firmware 2022.05 on the crazyflies and Crazyswarm2 commit 5bd08d8. We update the the motion_capture.yaml file to use optitrack-closed-source and libRigidBody, and set up the marker configurations (see motion_capture.yaml.txt). On the motive side, we are currently using Motive 2.3, and we have to define a rigidbody for each crazyflie and turtlebot, and then select labeled marker for streaming (see attached screenshot of the streaming settings)

Questions:

1. Our current (mostly working) approach requires defining rigid bodies on the Motive side for both the turtlebot and the crazyflie. Is that expected? We thought we should be sending unlabelled markers (as done in Crazyswarm1) for all the robots (crazyflies and turtlebots) and the ICP algorithm identifies the correct pose.

2. Occassional breakdown of the localization pipeline with one of the following errors. For each of the error, we have used sampleClient in the NatNetSDK package to confirm that the networking is still good (we still obtain the locations of markers). Our current fix when any of these errors happen is to relaunch the crazyflie launch.py repeatedly until the issue disappears (which is unsatisfactory since they happen unexpectedly).

   • Motion Capture did not receive data: This is the most common issue after launching crazyflie launch.py. No tf frames in rviz.

     [rviz2-5] [INFO] [1706304884.627673151] [rviz2]: Stereo is NOT SUPPORTED
     [crazyflie_server-4] [WARN] [1706304884.936016075] [all]: Motion capture did not receive data!
     [crazyflie_server-4] [WARN] [1706304884.955639368] [cf06]: last latency update: 1.002534 s
     [crazyflie_server-4] [WARN] [1706304885.936019074] [all]: Motion capture did not receive data!
     [crazyflie_server-4] [WARN] [1706304886.936029153] [all]: Motion capture did not receive data!
     [crazyflie_server-4] [WARN] 
     ...
   

   • NatNetSDK Error 3: After launching crazyflie launch.py, we get the following messages:

     terminate called after throwing an instance of 'std::runtime_error'
     [motion_capture_tracking_node-1]   what():  NatNetSDK Error 3
     [ERROR] [motion_capture_tracking_node-1]: process has died [pid 56586, exit code -6, cmd '/home/mobilerobot/merl_CA_Robots/ros2_ws/install/motion_capture_tracking/lib/motion_capture_tracking/motion_capture_tracking_node --ros-args -r __node:=motion_capture_tracking --params-file /tmp/launch_params_qho04dz1'].
   

   • Received unrequested data: After launching crazyflie launch.py, got this message:

     [cf08]: Received unrequested data for block: 0
     [cf08]: Received unrequested data for block: 1
     [cf08]: Received unrequested data for block: 0
     [cf08]: Received unrequested data for block: 1
     ...
   

3. Among these errors, the most common error is the error with "motion capture did not receive data". Also, when this error occurs, if we disable the turtlebots in the yaml file, we have confirmed that we can launch crazyflie launch.py successfully.

[whoenig]

1. We rely on the separate package motion_capture_tracking, which supports streaming assets as defined in motive, the custom frame-by-frame tracking, or a mixture. So this behavior is not expected, but depends on your configuration. Make sure that you disable all the assets in Motive and then check that you get data on the /pointCloud topic. Once that works, the tracking should be done by motion_capture_tracking, not Motive.
2. (motion capture did not receive data): This is certainly not normal. The NatNetSDK Error 3 means "network error" (whatever that means exactly). Have you tried using the "optitrack" (open source) backend? Have you tried using unicast rather than multicast? Are you using an Ethernet connection between the ROS computer and the Motive computer? Technically, you don't need to restart all nodes, you could also just restart motion_capture_tracking, although it would be better to find the root cause. We use optitrack (with the open source backend) for long periods of time in our lab and do not see these sort of connectivity issues to NatNet.
3. The "received unrequested data" should only happen if you didn't reboot your Crazyflies between launches. Either way, it shouldn't really affect the flights.

[xiaoshan-lin]

1. (Using optitrack-closed-source) After disabling all the assets in Motive, there is no pointcloud data published on the /pointCloud topic.
   ONLY when enabling the assets in Motive can we receive pointcloud data on the /pointCloud topic. Is this expected?

2. We tried using unicast but it doesn't work (didn't get positions of the markers). We use Ethernet connection between the ROS computer and Motive computer. We also tried using the optitrack open source but it doesn't work when our ROS PC is connected to our lab's internal network. Our experiment requires connecting the ROS PC to both motive PC and internal network via Ethernets.

Do you have any suggestions on how to properly set up the optitrack-closed-source and avoid the "motion capture did not receive data" issue?

3. Thanks for your clarifications.

[whoenig]

1. No, but I remember than changing the streaming settings in Motive has an effect. Best is to try the 3 combinations of streaming labeled markers on/unlabeled on; labeled off/unlabeled on; labeled on/unlabeled off. Note that optitrack-closed-source and optitrack behave differently with those settings.
2. Can you reproduce the issue with https://github.com/whoenig/NatNetSDKCrossplatform/tree/master, perhaps during longer sessions? If yes, and if you are using the closed source version, you could try reaching out directly to OptiTrack. The open-source version has a somewhat limited feature set. If it doesn't work, it usually means that some of the settings are non standard (e.g., port number), because not everything has been implemented. The assumptions are given in https://github.com/whoenig/NatNetSDKCrossplatform/tree/master?tab=readme-ov-file#notes. The closed-source version is feature-rich, but I've also seen crashes/instabilities with it.

[xiaoshan-lin]

1. We tried all combinations of labeled/unlabeled. To have the optitrack-close-source working, we need to

   • enable the assets in Motive
   • turn on unlabeled markers (labeled markers doesn't matter)

2. We are eventually able to switch to the open-source version and no longer have the "Motion Capture did not receive data" issue.
   We do noticed that when launching the crazyswarm2, we should only enable the Ethernet connection to Motive. If PC has multiple interfaces enabled (Ethernets, WIFIs), then crazyswarm2 won't start properly (no frames available on RVIZ)

   If the PC must connect to multiple internet, a workaround is:

   • Turn on the Ethernet connection to Motive and turn off any other internet connections
   • Launch crazyswarm2 and make sure frames are available on RVIZ
   • After that we can turn on other Ethernet connections without causing any issues to crazyswarm2

Thank you very much for your support!