Interaction with objects in real time

[negvet]

Hi! I am creating an interactive application between a formation and a human.

For the first step, I simply need a crazyflies to follow the human. Just let's say the drone goal position is ((x-0.5),y,z), where (x,y,z) - human coordinates. You described in you paper (Crazyswarm, section VII.D Interactive Avoid-Obstacle Mode) that you compute the trajectories for the interaction purposes. For the very first and simple trial, I believe I do not need a smooth trajectories. But for the control input it is possible to use directly goal position of the drone. Let me know if I am wrong. So what I need is a simple "goto" command for the drone, which executes with every update of the human position.

I tried cf.hover(goal, yaw, duration), code is below, but the drone moving around the desired position with oscillation, becoming unstable. Could you please suggest me how to implement kind of interactive mode for the object following? Thanks a lot!

def callback(human_pose):
    x = human_pose.transform.translation.x
    y = human_pose.transform.translation.y
    z = human_pose.transform.translation.z
    if (z < 1.0):
        z = 1.0

    for cf in allcfs.crazyflies:
        pos = np.array([x-0.5, y, z])
        cf.hover(pos, 0, 1.0)

def human_listener():
    rospy.Subscriber('/vicon/human/human', TransformStamped, callback)
    rospy.spin()

if __name__ == "__main__":
    swarm = Crazyswarm()
    timeHelper = swarm.timeHelper
    allcfs = swarm.allcfs

    Z = 1.0
    allcfs.takeoff(targetHeight=Z, duration=1.0+Z)
    timeHelper.sleep(1.5+Z)

    try:
        human_listener()
    except KeyboardInterrupt:
        pass

    allcfs.land(targetHeight=0.02, duration=1.0+Z)
    timeHelper.sleep(1.0)

[negvet]

This will increase radio bandwidth, due to the fact, that more amount of information has to be delivered to the formation. Therefore, I have to use more radio modules, it is not a problem.

[whoenig]

hover actually plans a smooth trajectory on board from the current state (pos,vel,acc) to the goal position with zero velocity/acceleration. As such, it doesn't work very well with high-frequency updates.

In your case it is best to either: a) Do all the goal pose computation on the PC and send the goal pose instead of high-level commands, or b) Add a special flight mode in the firmware that does that computation and only send the Vicon data for the human to the CFs.

In the paper we did b) for best scalability; however a) is easier for prototyping or smaller swarms. The bad news is that a) doesn't work out of the box with the latest Crazyswarm package. We are working hard on integrating it, but it will take about two weeks to get there. You can, however, start with a single Crazyflie using crazyflie_ros master:

1. Compile the latest official firmware using ESTIMATOR=kalman and CONTROLLER=mellinger
2. Create a catkin workspace with crazyflie_ros master branch (https://github.com/whoenig/crazyflie_ros).
3. Launch using this launchfile
4. Use the topics cmd_position or cmd_full_state to set the desired setpoints (for slow movements cmd_position is sufficient, for fast movements use the full_state version)
5. You can still use the high-level API for takeoff, landing, or goTo, see https://github.com/whoenig/crazyflie_ros/blob/master/crazyflie_demo/scripts/test_high_level.py. Note that trajectory support is not in the official firmware yet (but we do have active PRs for that).

As I said, we are planning to get the functionality into the Crazyswarm as well with our next big release (which will also support the latest official firmware).

[negvet]

Thanks for you answer!

I tried with a single Crazyflie using crazyflie_ros master, as you suggested. Actually, I implemented 1-5 steps, that you described, so that cf follows the human. Partly, the code is below.

I have several comments:

1. High-level API (standalone) works just fine, thanks for implementing it.
2. cmd_full_state topic works well for XY plane, but for movement along Z axis (for XY it also could happen but not that noticeable) Crazyflie becomes unstable if moving quite fast (or if the desired position is far from the current position), but with slow movements it flies in a smooth manner. I tried to tune PID along Z axis in controller_mellinger.c but with no success.
3. I am not sure that I understand how to switch between high level control and state control. For example, high level takeoff command works followed by a publishing to a cmd_full_state topic, but landing after does't work, even if I write cf.setParam("commander/enHighLevel", 0). cf just turning off during the flight.
4. I know how to switch to multiple CFs, but just to ask - all of this should work with multiple CFs?

Any updates about the new release of Crazyswarm which support the functionality? Do you have a guess when it is gonna be?

def tag_game(human, cf1):
    human_position = swarmlib.get_coord(human)
    drone1_position = swarmlib.get_coord(cf1)
    drone1_pose_goal = np.array([ human_position[0]-1, human_position[1], human_position[2] ])
    swarmlib.publish_goal_pos(drone1_pose_goal, 0, "crazyflie1")

def follower():
    human_sub = message_filters.Subscriber('/vicon/human/human', TransformStamped)
    cf1_sub = message_filters.Subscriber('/vicon/crazyflie1/crazyflie1', TransformStamped)
    ts = message_filters.TimeSynchronizer([human_sub, cf1_sub], 10)
    ts.registerCallback(tag_game)
    rospy.spin()

if __name__ == '__main__':
    rospy.init_node('test_high_level')
    cf = crazyflie.Crazyflie("crazyflie1", "/vicon/crazyflie1/crazyflie1")
    cf.setParam("commander/enHighLevel", 1)
    cf.takeoff(targetHeight = 0.5, duration = 2.0)
    time.sleep(3.0)

    print "\nfollowing human!"
    try:
        follower()
    except KeyboardInterrupt:
        pass

[whoenig]

Thanks for the update.

1. Great to hear!
2. How do you generate the full state setpoint? Ideally, you would generate a trajectory dynamically, for example by solving a quadratic program. You can then make sure that the generated trajectory you computed is within reasonable dynamic limits (max. velocity/acceleration) and only use it in that case.
3. The switching logic is here. Basically, "low-level" control always has priority (this allows a manual pilot to take over even if the high-level mode is enabled). High-level can only be used if a) it was enabled using the commander/enHighLevel parameter, and b) no "low-level" command arrived for some time period. If you want to use cmd_full_state, you can also execute takeoff and landing trajectories and not use the high-level mode at all.
4. Yes, multiple CFs should work just fine (although I didn't test this heavily). Of course, you'll be limited by radio bandwidth (more if you use low-level setpoints then if you use the high-level mode).
5. The Crazyswarm now uses the latest official crazyflie_ros and can operate with the latest official firmware. However, I am not sure if it helps in your use case, unless you want to implement your follower logic directly in a custom firmware logic. (The Crazyswarm is advantageous if you can operate using primarily broadcasts.)

[pete9936]

My question is for 'negvet' though it applies to anyone really. I was wondering if you were able to successfully get one or multiple crazyflie's to follow a human and if so would be willing to share the code you have to make that possible? I am attempting to follow a turtlebot and land autonomously on it. I have used the general outline you created in the first comment of this issue and modified it a bit but am having issues and am looking for some feedback. My code is below:

#!/usr/bin/env python

import numpy as np
import rospy
import tf
from geometry_msgs.msg import TransformStamped

from pycrazyswarm import *
import uav_trajectory

def callback(base_pose):
    x = base_pose.transform.translation.x
    y = base_pose.transform.translation.y
    z = base_pose.transform.translation.z
    if (z < 0.15):
        z = 0.15

    for cf in allcfs.crazyflies:
        pos = np.array([x, y, z+0.5])
        cf.hover(pos, 0, 1.0)

def base_listener():
    rospy.init_node('base_pose_vicon', anonymous=True) # this is a maybe
    rospy.Subscriber('/vicon/base/base', TransformStamped, callback)
    rospy.spin()

if __name__ == "__main__":
    swarm = Crazyswarm()
    timeHelper = swarm.timeHelper
    allcfs = swarm.allcfs

    Z = 1.0
    allcfs.takeoff(targetHeight=Z, duration=1.4+Z)
    timeHelper.sleep(1.5+Z)

    try:
        base_listener()
    except KeyboardInterrupt:
        pass

    allcfs.land(targetHeight=0.06, duration=1.0+Z)
    timeHelper.sleep(1.0)

For some reason I'm not getting the base_pose_vicon node to exist when I run the script. However, when I run it with --sim and check my nodes it comes up as /base_pose_vicon_####_######, some random numbers, and is actually subscribing to the frame /vicon/base/base as follows: I'm sure I'm just missing some simple things.

[whoenig]

• The Crazyswarm already creates a node internally, so don't call init_node yourself.

[negvet]

@pete9936, according to whoenig comment above ,for the interactive applications I succesfully use crazyflie_ros master (cmd_positionor cmd_full_state topics), rather than crazyswarm framework.