Crazyswarm real-time trajectory update delay problem

[The-SS]

Hi guys,

We are working on a 3D formation control project using the Crazyswarm package for experimental validation. Our control law requires us to send velocity control commands to the Crazyflies in real-time (at least 10Hz). Things were fine with up to 8 drones on 3 radios, but as we increased the number of Crazyflies we started having serious delay issues.

We started off using the "hover" command (we're using an older fork of the package) and discretizing our control commands. But, when we tried using 16 drones, the total latency grew to almost 0.5 or 1 second. I also found the uploadTrajectory command to be slow as well (100ms for a single drone).

If I am not mistaken, both hover and uploadTrajectory/startTrajectory use request-response communication schemes which increase communication latency but guarantee that the messages are received. So, I started thinking about a broadcast communication scheme instead (similar to what is used for drone pose estimation / sending VICON pose to the drones).

My questions are:

• Are there any implemented functions that broadcast control commands to the Crazyflies?
• Have you tried doing anything similar to that?
• How complicated would it be to broadcast a control command in a similar way to the quadrotor pose estimation messages?

Thank you. -Sleiman

[whoenig]

Hi Sleiman,

For your case you should use the low-level commands instead of the high-level command mode. (Low-level: you evaluate the traj off-board and send commands; High-level: traj are generated/evaluated on-board). Low-level commands are not broadcasted, but also not repeated in case an ack is not received, so there should be barely any benefit of using broadcasts.

If you want to use the Mellinger controller, the best way is to send data to cmd_full_state (see https://github.com/whoenig/crazyflie_ros/blob/master/crazyflie_driver/msg/FullState.msg). This is currently not implemented in the crazyswarm_server, so you would either need to use crazyflie_ros, or change the crazyswarm_server in order to add the support for that mode.

Best, Wolfgang

[The-SS]

Are you referring to the crazyswarm/.../crazyflie.py services as high-level commands and to the crazyflie_ros/.../crazyflie_server.cpp functions as low-level commands?

If that is the case, are you suggesting that the best way would be for us to add a high-level command that uses cmdFullStateSetpoint to send the desired quadrotor state?

Thank you.

[whoenig]

No, I was referring to high- and low-level with respect to the firmware functions. The high-level commander (https://github.com/bitcraze/crazyflie-firmware/blob/master/src/modules/src/crtp_commander_high_level.c) provides "high-level" commands, and the generic commander (https://github.com/bitcraze/crazyflie-firmware/blob/master/src/modules/src/crtp_commander_generic.c) provides "low-level" commands.

I believe the best way forward is to send cmdFullStateSetpoint (instead of uploading trajectories or use hover/goTo high-level commands). This will increase the required bandwidth of course, and you will likely need more radios than with the Crazyswarm (I would guess that a ratio of 1:4 might be fine, but this needs to be tested and depends on the desired update rate).

[The-SS]

Thank you. I'll try that and update this issue.

[The-SS]

Hi again, I decided to use cmdFullStateSetpoint with Crazyswarm. I made the following modifications: In crazyswarm/src/crazyswarm_server.cpp I added the following to the CrazyflieROS class:

void cmdFullStateSetpoint(
      const crazyflie_driver::FullState::ConstPtr& msg)
    {
      float x = msg->pose.position.x;
      float y = msg->pose.position.y;
      float z = msg->pose.position.z;
      float vx = msg->twist.linear.x;
      float vy = msg->twist.linear.y;
      float vz = msg->twist.linear.z;
      float ax = msg->acc.x;
      float ay = msg->acc.y;
      float az = msg->acc.z;

      float qx = msg->pose.orientation.x;
      float qy = msg->pose.orientation.y;
      float qz = msg->pose.orientation.z;
      float qw = msg->pose.orientation.w;
      float rollRate = msg->twist.angular.x;
      float pitchRate = msg->twist.angular.y;
      float yawRate = msg->twist.angular.z;

      m_cf.sendFullStateSetpoint(
        x, y, z,
        vx, vy, vz,
        ax, ay, az,
        qx, qy, qz, qw,
        rollRate, pitchRate, yawRate);
    }

I also created a subscriber to the topic:

m_subscribeCmdFullState = n.subscribe(m_tf_prefix + "/cmd_full_state", 1, &CrazyflieROS::cmdFullStateSetpoint, this);

Then I added a method to the Crazyflie class in crazyswarm/scripts/pycrazyswarm/crazyflie.py that publishes to the same topic:

def cmdFullState(self, pose_data, twist_data, acc_data):
        '''
        pose_data: list/array with position and quaternion rotation (x, y, z, qx, qy, qz, qw)
        twist_data: list/array with linear and angular velocities (vx, vy, vz, roll_rate, pitch_rate, yaw_rate)
        acc_data: list/array with acceleration (ax, ay, az)
        '''
        state = FullState()
        pose = geometry_msgs.msg.Pose()
        twist = geometry_msgs.msg.Twist()
        acc = geometry_msgs.msg.Vector3()

        pose.position.x = pose_data[0]
        pose.position.y = pose_data[1]
        pose.position.z = pose_data[2]
        pose.orientation.x = pose_data[3]
        pose.orientation.y = pose_data[4]
        pose.orientation.z = pose_data[5]
        pose.orientation.w = pose_data[6]

        twist.linear.x = twist_data[0]
        twist.linear.y = twist_data[1]
        twist.linear.z = twist_data[2]
        twist.angular.x = twist_data[3]
        twist.angular.y = twist_data[4]
        twist.angular.z = twist_data[5]

        acc.x = acc_data[0]
        acc.y = acc_data[1]
        acc.z = acc_data[2]

        state.pose = pose
        state.twist = twist
        state.acc = acc
        state.header.seq +=1
        state.header.stamp = rospy.Time.now()
        state.header.frame_id = self.worldFrame

        self.pubCmdFullState.publish(state)

To test this, I wrote as script that moves a drone to a desired location. The script finds the desired state (position, velocity, acceleration, orientation, angular velocity) and issues the command.

while (True):
    # find state
    # publish state
    button_pressed = swarm.input.checkIfButtonIsPressed()
    if button_pressed == True:
        break 
cf.land(0.05, 3.0)
timeHelper.sleep(3.0)

The problem I am having is that once I stop publishing to that topic, the drone starts to drift. Then it stops the propellers and crash lands. I tried different things when the while loops ends. I tried using land, ending the script immediately, and publishing more states that would move the drone down then using cf.stop(). In all cases, the drone starts to drift then crashes.

Is there a reason for this to happen? Does it have something to do with how I implemented the publish/subscriber?

For example, when the crazyflie moves from (0.5, 0.0, 0.5) to (0.5, 0.5, 0.5), once I press the RB button, the drone starts to drift then stops and crashes.

[whoenig]

If no setpoint is send, the firmware will first try to hover and then stop the engine (see https://github.com/bitcraze/crazyflie-firmware/blob/master/src/modules/src/commander.c#L85-L104). The "high-level" commands (land and stop) only work if the high-level mode is enabled. Even if the high-level mode is enabled, the setpoints will take priority. Thus, I think you should enable the high-level mode at the beginning, then send your full_state commands and finally send the land command after you stopped issuing full_state commands.

[The-SS]

I'm using hover_swarm.launch to start up the crazyflies. The file already sets enableHighLevel to 1.

I made some modifications to my script and resolved the issue. This is how I'm landing the drones:

# go to desired goal
while (True):
    # find state
    # publish state
    button_pressed = swarm.input.checkIfButtonIsPressed()
    if button_pressed == True:
        break 
# once at desired goal update goal location to (x_position, y_position, 0.05)
while (True):
    # find state
    # publish state
    # get current position
    if z_position < 0.1 # once it gets to a low altitude 
        break 
cf.cmdStop() # cutoff power to motor by publishing  an Empty message to /cmd_stop
cf.land(0.05,0.5,0.0) # land crazyflie (although already landed)
timeHelper.sleep(2.0)

This is working for me now.

[yzhan407]

Hi@The-SS I have watched your 3D formation control project, in this video, it seems like you can control the speed very accurately. I have tried the recently implemented cmdFullState() function, it seems like that it is almost the same as your cmdFullState() function given above, but when I tried to give it a simple velocity like[-0.1,0,0] to one crazyflie , it did not go to the right direction, and I always have to give an offset (like [0,0,0.1]) position to make it not flying down, but after sometime, it will eventually goes higher and higher due to this offset. So my question is how did you keep the right speed and right position sent to crazyflie?