High-Level Commander Design

[whoenig]

This issue is to discuss the design of a high-level commander (@jpreiss , @mgreiff , @mikehamer).

Goal

The current commander allows for different high-frequency setpoints, such as attitude control, velocity control, or trajectory control. A high-level mode would allow to set high-level goals, while the low-level setpoints are generated on-board. Such a mode can, for example, include:

• Takeoff
• Landing
• GoTo waypoint (where a smooth trajectory is planned on-board)
• Execute Trajectory (that might be built into the firmware, uploaded on demand, or stored on an SD-card)

Challenges

• The current generic commander can only switch between low-level/high-frequency setpoints. A different layer is needed for high-level commanders.
• A backup pilot should be able to take over control at any point

Proposed Design

1. A new task for the high-level commander will be created. The task handles new CRTP commands using a new CRTP port. Commands can include takeoff, landing, etc. New commands can be added, similar to the current crtp_commander_generic. The task also provides a function to fill-in the current setpoint.
2. The logic of commanderGetSetpoint will be changed such that the high-level setpoint is returned if a) no low-level command was received for a specific time, and b) the high-level commander is enabled (a parameter and perhaps a compile-time flag). Check a) will allow backup pilots to take control. Also, once a low-level commander packet was received we should disable the high-level mode, until it gets explicitly enabled again (by setting a parameter).

We can discuss the actual CRTP packets later; the first step is to agree on how to integrate such a on-board setpoint generator properly. We already successfully use such a high-level commander for the Crazyswarm.

[jpreiss]

The interface of the Crazyswarm high-level commander on the firmware side is summarized in planner.h. The PC side is close to a 1:1 wrapper.

The manual backup is the main component missing from our implementation, I think.

[ataffanel]

Hi, thanks for starting a ticket about that!

1. Sounds good. I guess there also needs to be a 'fifo' mechanism to be able to send the next commands. Do you see that as a generic FIFO memory that holds raw packets and feed them to the decoding functions at regular interval?

2. The current low-level setpoint implementation has a 'priority' argument. The idea behind this argument was to be able to have multiple source for setpoint and automatically switch to the highest priority source. If the high priority setpoint has not been received for a while (0.5s currently), a lower priority setpoint can be used instead. If the high-level commander is set with a priority lower than the low level crtp_generic_commander setpoint, it seems to me that the existing design fulfills you point a), for b) a parameter or the high-level commander packet can be used to enable the high-level commander.

[whoenig]

1. So far we have not used any buffer. Instead, the client can send a new command any time to start execute it. For complex maneuvers, trajectories can be uploaded and executed.

2. I saw the priority argument. However, I want to avoid that the high-level commander generates low-level setpoints at a fixed rate: this will be less accurate in terms of latency and will make it much more complicated to run the control loop at a higher or lower frequency. Instead of letting the high-level commander actively push data, I want the stabilizer to query the setpoint whenever needed. In particular, the stabilizer calls commanderGetSetpoint periodically. In case the high-level setpoint is enabled (and no low-level setpoint is available), we compute the current setpoint (which in case of trajectories means evaluating a polynomial function).

Edit: An example implementation is available here.

[ataffanel]

Thanks for the PR @whoenig!

My understanding is that with the code already merged is that we have to send the commands when needed. How do you upload and execute a series of command, is there a separate module implemented for that?

[jpreiss]

@ataffanel in the Crazyswarm architecture, we have the python script that sends commands to the crazyflie-ros server. The commander messages are implemented as ROS services (aka remote procedure calls) that return immediately, even if the commanded action takes a long time. The script sleep()s for the appropriate amount of time before issuing another command. (it would be easy to write a wrapper library that does the correct sleep for you.)

Since we work with multi-robot teams, we have never wanted to upload any complex sequence because we need all the robots to stay coordinated. If we were to upload sequences, we would still need a mode where we can tell all the robots to advance to the next command at the same time, to ensure synchronization. Given this requirement, it is not any easier than just sending the commands at the right time.

However, I can see how a command queue would be useful for other kinds of research, or for operation without a PC.

[whoenig]

Just a few more comments:

• I just created another PR that supports uploading and executing trajectories. Trajectories can be seen as a sequence of commands that can be generated offline (e.g., using https://github.com/whoenig/uav_trajectories). The new design also allows to include takeoff and landing (which was not possible with the Crazyswarm design). A high-level python script (as described by James) can be found here: https://github.com/whoenig/crazyflie_ros/blob/master/crazyflie_demo/scripts/test_high_level.py.
• If we really want a command queue, I think that would be an additional layer on top of the high-level commander.

[whoenig]

Another update on the current status:

• With the outstanding PRs (HighLevelCmd: Add missing shift for traj. eval #308, HighLevelCmd:Broadcast support for startTrajectory #309, LocSrv: Add EXT_POS_PACKED CRTP packet #310) it is now possible to run a (modified) Crazyswarm with the official firmware. There are a few open minor issues:
  1. The official Kalman filter does not allow a full pose update (see Implement full pose measurement in EKF #246). This is an issue if there is not much movement in the xy-plane for a while (e.g., extended hovering).
  2. There are a few safety items missing (see e.g., Crash/tumble detection should not re-enable motors when flipped back #258) that prevent that the motors spin after a crash. This is in particular a concern for big quads. We have a solution in our custom firmware, but it is not nicely integrated with the situation awareness framework.
• I also tested the new high-level commander with a single CF and crazyflie_ros. It allows nice motion scripting, similar to the motion commander in the python-lib.
• It also works with the flow deck (and no VICON) without any changes. Although, the flight is shaky in the z-direction. I would guess that it will also work with the LPS, but didn't try it yet.