How to control Crazyflies individually？

[K4IN1]

I use VRPN as motion_capture to fly crazyflies
The ros2 package motion_capture_tracking has not been updated with the latest submodule libmotioncapture to support 'fzmotion'. I set up the rigid body on the motion capture host with four Markers and named it something like 'cf1', the same as in my crazyflies.yaml file
VRPN is good in launch.py of motion_capture_tracking. Rigid body information can be displayed in rviz

When I fly just one drone, it behaves normally
But when I try to control two different drones separately, it looks like the different drones receive the same commands and they goTo the same location and crash. I tried the crazyfliesByName method mentioned in discussions#515
I am using code like:

    allcfs.crazyfliesByName['cf1']
    cf1.setParam('led.bitmask',255)
    cf1.takeoff(targetHeight=1.0, duration=TAKEOFF_DURATION)

I tried using the latest firmware built from source, witch include hlCommander.groupmask and using the code like:

    cf1 = swarm.allcfs.crazyfliesByName['cf1']
    cf1.set_GroupMask(0b00000001)
    allcfs.takeoff(targetHeight=0.5, duration=3.0, groupMask=1)

but the drones still don't behave as expected
To prevent collisions, I only use set led.bitmask and takeoff and landing
Sometimes only one of the two drones executes the command, sometimes the other drone executes the command in the exact opposite direction, and sometimes neither drone executes the command.

Another question in that when I start launch.py, not all drones are connected, I need to Ctrl+C out and restart launch.py. Only when I see the interface displayed in NiceGUI, I can be sure that both drones are connected.

How can I fix the above problem?

Here is my code

motion_capure.yaml

/motion_capture_tracking:
  ros__parameters:
    
    type: "vrpn"

    hostname: "169.254.127.188"

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

crazyflies.yaml

robots:
  cf1:
    enabled: true
    uri: radio://0/80/2M/E7E7E7E701
    initial_position: [-0.5, 0.0, 0.0]
    type: cf21_mocap_deck
  cf3:
    enabled: true
    uri: radio://0/80/2M/E7E7E7E703
    initial_position: [0.5, 0.0, 0.0]
    type: cf21_mocap_deck
robot_types:
  cf21:
    motion_capture:
      enabled: fasle
      marker: default_single_marker
      dynamics: default
    big_quad: false
    battery:
      voltage_warning: 3.8  # V
      voltage_critical: 3.7 # V
  cf21_mocap_deck:
    motion_capture:
      enabled: true
      marker: mocap_deck
      dynamics: default
    big_quad: false
    battery:
      voltage_warning: 3.8  
      voltage_critical: 3.7 
all:
  firmware_logging:
    enabled: false
    default_topics:
      pose:
        frequency: 10 # Hz
      status:
        frequency: 1 # Hz
  firmware_params:
    commander:
      enHighLevel: 1
    stabilizer:
      estimator: 2 # 1: complementary, 2: kalman
      controller: 1 # 1: PID, 2: mellinger
    locSrv:
      extPosStdDev: 1e-3
      extQuatStdDev: 0.5e-1
  broadcasts:
    num_repeats: 15 # number of times broadcast commands are repeated
    delay_between_repeats_ms: 1 # delay in milliseconds between individual repeats

I modified the script in the example to try my own script
group_mask.py

#!/usr/bin/env python

from crazyflie_py import Crazyswarm


def main():
    swarm = Crazyswarm()
    timeHelper = swarm.timeHelper
    allcfs = swarm.allcfs

    cf1 = swarm.allcfs.crazyfliesByName['cf1']
    cf3 = swarm.allcfs.crazyfliesByName['cf3']
    # set group mask to enable group 1 and 4 (one by one)
    
    cf1.setGroupMask(0b00000001)
    timeHelper.sleep(1)
    cf3.setGroupMask(0b00000010)
    timeHelper.sleep(1)

    print('Takeoff with a different mask (2) -> cf3(Green)should take off and land')
    allcfs.takeoff(targetHeight=0.5, duration=3.0, groupMask=2)
    timeHelper.sleep(3)
    allcfs.land(targetHeight=0.02, duration=3.0)
    timeHelper.sleep(3)

    print('Takeoff with correct mask (1) -> cf1(black) should take off and land')
    allcfs.takeoff(targetHeight=0.5, duration=3.0, groupMask=1)
    timeHelper.sleep(3)
    allcfs.land(targetHeight=0.02, duration=3.0)
    timeHelper.sleep(3)


if __name__ == '__main__':
    main()

hello_world.py

from crazyflie_py import Crazyswarm


TAKEOFF_DURATION = 2.0
HOVER_DURATION = 5.0


def main():
    swarm = Crazyswarm()
    timeHelper = swarm.timeHelper
    cf1 = swarm.allcfs.crazyfliesByName['cf1']
    cf3 = swarm.allcfs.crazyfliesByName['cf3']

    cf1.setParam('led.bitmask',255)
    cf1.takeoff(targetHeight=1.0, duration=TAKEOFF_DURATION)
    timeHelper.sleep(TAKEOFF_DURATION + HOVER_DURATION)
    cf1.goTo([1.0,1.0,1.0],0,3)
    timeHelper.sleep(HOVER_DURATION)
    cf1.land(targetHeight=0.04, duration=2.5)
    timeHelper.sleep(TAKEOFF_DURATION)
    cf1.setParam('led.bitmask',0)
    
    # cf3.setParam('led.bitmask',255)
    # cf3.takeoff(targetHeight=1.0, duration=TAKEOFF_DURATION)
    # timeHelper.sleep(TAKEOFF_DURATION + HOVER_DURATION)
    # cf3.goTo([1.0,1.0,1.0],0,3)
    # timeHelper.sleep(HOVER_DURATION)
    # cf3.land(targetHeight=0.04, duration=2.5)
    # timeHelper.sleep(TAKEOFF_DURATION)
    # cf3.setParam('led.bitmask',0)

if __name__ == '__main__':
    main()

Any kind of help and advice would be appreciated

[whoenig]

• Which radio are you using?
• Are you seeing the same issue with the led.bitmask code (i.e., the wrong drone is executing the LED changes?)
• Instead of the groupmask, you can use cf1.land and cf1.takeoff (although the behavior is expected to be the same; make sure you also use the groupmask for landing)

[K4IN1]

1.I'm using Crazyradio 2.0, and I flashed the 1.1 version of the firmware
The parameter settings for led.bitmask seem to be working fine in my previous scripts with no flying commands.
But I need to double check, I'll test let.bitmask scripts and cf1.land tomorrow.
I'll respond as soon as I can.

[K4IN1]

Test Results

According to your advice, I rewrote two Python scripts to test led.bitmask and takeoff.

led_test.py

from crazyflie_py import Crazyswarm
from crazyflie_py.crazyflie import Crazyflie

name1 = 'cf1'
name3 = 'cf3'

def main():
    swarm = Crazyswarm()
    timeHelper = swarm.timeHelper

    cf1:Crazyflie = swarm.allcfs.crazyfliesByName[name1]
    cf3:Crazyflie = swarm.allcfs.crazyfliesByName[name3]

    cf1.setParam('led.bitmask',128)
    timeHelper.sleep(3)
    cf3.setParam('led.bitmask',128)
    timeHelper.sleep(3)
    
    cf1.setParam('led.bitmask',191)
    timeHelper.sleep(3)
    cf3.setParam('led.bitmask',191)
    timeHelper.sleep(3)

    cf1.setParam('led.bitmask',0)
    timeHelper.sleep(3)
    cf3.setParam('led.bitmask',0)
    timeHelper.sleep(3)

if __name__ == '__main__':
    main()

takeoff_test.py

from crazyflie_py import Crazyswarm
from crazyflie_py.crazyflie import Crazyflie

name1 = 'cf1'
name3 = 'cf3'

TAKEOFF_DURATION = 2.0
HOVER_DURATION = 5.0
TARGET_HEIGHT = 0.5

def main():
    swarm = Crazyswarm()
    timeHelper = swarm.timeHelper

    cf1:Crazyflie = swarm.allcfs.crazyfliesByName[name1]
    cf3:Crazyflie = swarm.allcfs.crazyfliesByName[name3]

    cf1.setGroupMask(0b00000001)
    timeHelper.sleep(0.5)
    cf3.setGroupMask(0b00000010)
    timeHelper.sleep(0.5)

    cf1.takeoff(TARGET_HEIGHT, TAKEOFF_DURATION, groupMask=0b00000001)
    timeHelper.sleep(3)
    cf3.takeoff(TARGET_HEIGHT, TAKEOFF_DURATION, groupMask=0b00000010)
    timeHelper.sleep(3)
    
    cf1.land(0.04, 2.5, groupMask=0b00000001)
    timeHelper.sleep(3)
    cf3.land(0.04, 2.5, groupMask=0b00000001)
    timeHelper.sleep(3)

    swarm.allcfs.land(0.04, 2.5, 0)
    timeHelper.sleep(3)

if __name__ == '__main__':
    main()

When I tried, I found that changing enabled: true for motion_capture in crazyflie.yaml was necessary to correctly use the positional information provided by the motion capture system, contrary to what is described in #537 and #506 .

I am using the FZmotion motion capture system, where I configured rigid assets named cf1 and cf3 in their software and transmitted the positional information of these assets via VRPN. This was confirmed in the rviz window of ros2 launch motion_capture_tracking launch.py.

With enabled: false, I tried running led_test with the CPP backend, and it behaved as expected. However, the positions displayed in NiceGUI did not match those shown by the motion capture system. When I attempted takeoff_test, the drones immediately flew into my safety net, indicating they did not receive correct positional information. Therefore, I modified crazyflie.yaml as follows:

robot_types:
  cf21:
    ...

  cf21_mocap_deck:
    motion_capture:
      enabled: true # This line was changed
      marker: mocap_deck
      dynamics: default
    big_quad: false
    battery:
      voltage_warning: 3.8  # V
      voltage_critical: 3.7 # V

Afterwards, I tested in both backends:

CPP Backend

When I used ros2 launch crazyflie launch.py to start the node, NiceGUI launched in my browser, but the page was often empty. I had to interrupt with Ctrl+C in the terminal and restart launch.py three or four times before I could see the NiceGUI interface in the browser. The NiceGUI interface updated slowly, and the drones consistently showed red. I manually launched rviz, which displayed normally.

At this point, the positions in the interface matched those shown by the motion capture software. I tried led_test, but it exhibited unexpected behavior. It seemed some scripts were sent to the wrong objects or ignored altogether, with results appearing random each time the script ran. However, the script behaved correctly when enabled: false.

I also tested takeoff_test.py, and as before, it was unpredictable which drone executed which command. Often, a drone would hover in the air after the script finished, requiring me to manually power it off. (I haven't thoroughly read the Topic and Server code, so I'm not sure if there's a command-line tool similar to ros2 topic pub for sending landing commands.)

cflib Backend

Following the installation guide for Crazyswarm2, I installed the corresponding dependencies and used ros2 launch crazyflie launch.py backend:=cflib to start the node (using enabled: true).

Now, led_test.py performed normally, and the positions in NiceGUI matched those of the motion capture system.

I then continued testing take_off.py. Due to previous crashes, the supports for the motors were damaged. I removed the propellers from the motors and judged whether the script was running correctly based on the order in which the drones started. The result was normal, with the order and timing of the motor noises matching the commands in the script.

I hope you can tell me how to resolve the issues with the CPP backend and why my motion_capture: enabled: behaves opposite to others.
THANKS @whoenig

[whoenig]

Do you have a Crazyradio PA by any chance? I've seen weird issues with Crazyradio 2 (and the cpp backend) as well and didn't investigate, yet.
If not, is there any particular feature you are missing in the cflib backend?

[K4IN1]

Thank you very much for your reply.

I will try to buy a Crazyradio PA and get it working using the cpp backend. Or I can use the cflib backend, which I think is powerful enough for my research(at least for now).

I will mark your response as an answer.

[knmcguire]

if you are only using 2 crazyflies as I see in your code, then I think that the cflib should be sufficient. However the mocap integration is not very thoroughly tested so make sure to let us know if you encounter any issues