[REP Index] [REP Source]

REP:147
Title:A Standard interface for Aerial Vehicles
Author:Tully Foote
Status:Deferred
Type:Informational
Content-Type:text/x-rst
Created:16-May-2016
Post-History:12-Jul-2016, 18-Jan-2018

Abstract

This REP is written to provide a common interface for aerial vehicles. This interface is designed to promote interoperability and reusability of core functionality between diverse aerial vehicles. By leveraging the interfaces defined in this REP we both tools and libraries can be designed to be reusable between different frameworks and vehicles. It is expected that aerial vehicles can and will implement additional interfaces in addition to these core interfaces. When choosing an interface these should be considered the default and only change away from them for a good reason.

These interfaces are designed to be relevant for all types of aerial vehicles whether multi-copter, fixed-wing, hybrid, transitional or other. Like ground robots, the core abstraction is the same for all vehicles regardless of their method of motivation.

Rationale

There are a lot of developers around the world using ROS on aerial vehicles. As such there are also many different implementations which are similiar but not exactly compatible. A short list of existing known bridges and implementations follows:

  • astec_mav_framework [1]
  • autopilot_bridge [8]
  • CRATES [5]
  • dji_sdk_dji2ros [10]
  • dji_sdk [11]
  • hector_quadrotor [7]
  • mav_tools [9]
  • mavlink2ros [2]
  • mavros [3]
  • roscopter [4]
  • rospilot [6]

Due to a lack of common interfaces the majority of the above packages cannot be used together. By implementing common interfaces elements of each package can be used in conjunction with elements of a different package. This will allow much better collaboration between projects and allow a reduction in duplicate efforts to implement the same functionality.

There are also a lot of interfaces already well developed from other domains which can be brought into the aerial vehicle domain. This REP seeks to also integrate as many of those reusable interfaces as well to provide the greatest compatibilty with those other domains.

Other interface definitions

There are a lot of messages already defined. These messages definitions were used to help define this set.

Reference Abstractions

For this document we will use the following abstractions of the architecture. These abstractions are used to define the interfaces, however it is not required that a given system implement all the interfaces. A given system may implement subsets of these interfaces.

Control Abstractions

For controlling an aerial vehicle we will use the following terms for the interfaces.

graph LR na[ ] tc[Trajectory Controller] ac[Attitude Controller] rc[Rate Controller] va[Vehicle Abstraction] na ==>|Trajectory|tc tc ==>|Pose|ac ac ==>|Rate|rc rc ==>|Acceleration|va va -.-> am[Allocation Matrix] va -.-> mixer am -.-> Actuators mixer -.-> Actuators

High Level Architecture

A higher level view of the system uses the following abstraction.

graph LR GCS[Ground Control Station] CS[Collision Sensors] OS[Odometry Sensors] C[Controller] L[ ] style L fill:#fff,stroke:#ff0,stroke-width:0px; L --> CS L --> GCS L --> OS subgraph Generic Planning Framework CM[Collision Mapping] MSC[Planner e.g.
Minimum Snap Trajectory Generator] CM ---|Shared State|MSC end CS -->|Obstacle Observations|CM OS -->|Odometry|MSC OS -->|Odometry|CM OS -->|Odometry|C GCS -->|Goals|MSC MSC -->|Control Abstraction|C %% Links to L linkStyle 0 fill:#fff,stroke:#ff0,stroke-width:0px; linkStyle 1 fill:#fff,stroke:#ff0,stroke-width:0px; linkStyle 2 fill:#fff,stroke:#ff0,stroke-width:0px;

Coordinate Frames

It is expected that users of this interface will also follow the coordinate frame conventions layed out in REP 105 [28].

Proposed Interfaces

Control Interfaces

Trajectory Control Interface

For sending trajectories in 3D space there already exists a message used for free space planning. This message used is trajectory_msgs/MultiDOFJointTrajectory [13] and is actively used by planning frameworks already.

This message is not the most straight forward to parse as it has a lot of optional fields which increases the complexity and requires consistent labeling and filling. The complexity of the message parsing is traded off against supporting a relatively compact representations.

  • Datatype: trajectory_msgs/MultiDOFJointTrajectory [13]
  • Topic: command_trajectory
# The header is used to specify the coordinate frame and the reference time for the trajectory durations
Header header

# A representation of a multi-dof joint trajectory (each point is a transformation)
# Each point along the trajectory will include an array of positions/velocities/accelerations
# that has the same length as the array of joint names, and has the same order of joints as
# the joint names array.

string[] joint_names
MultiDOFJointTrajectoryPoint[] points

Pose Interface

The message to send a goal pose is geometry_msgs/PoseStamped [14]. This is a very highly used standard message and contains precisely the information needed.

  • Datatype: geometry_msgs/PoseStamped [14]
  • Topic: command_pose
# A Pose with reference coordinate frame and timestamp
Header header
Pose pose

The reference position is the origin of the base_link as defined in [28]

Rate Interface

The message to send a velocity command is geometry_msgs/TwistStamped [27]. This is the standard way to command ROS vehicles. The datatype is not intended to be used in an open loop control system.

  • Datatype: geometry_msgs/TwistStamped [27]
  • Topic: command_velocity
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/Twist twist
  geometry_msgs/Vector3 linear
    float64 x
    float64 y
    float64 z
  geometry_msgs/Vector3 angular
    float64 x
    float64 y
    float64 z

The command is a body relative set of velocities in linear and angular space. This diverges from the common geometry_msgs/Twist [26] used by some ground robots. A pure Twist based interface could be provided for backwards compatibility as well. It was chosen to diverge here since this is a much more powerful inteface for the vehicles and does not require all commands to be reverse calculated for the instantaneous attitude of the vehicle. Furthermore, the timestamp field allows for improved timeout behavior due to stale command data.

This is replacing mav_msgs/AttitudeThrust [24] and mav_msgs/RollPitchYawrateThrust [25].

Acceleration Interface

The message to send a velocity command is geometry_msgs/TwistStamped [27]. The datatype is not intended to be used in an open loop control system.

  • Datatype: geometry_msgs/TwistStamped [27]
  • Topic: command_acceleration
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/Twist twist
  geometry_msgs/Vector3 linear
    float64 x
    float64 y
    float64 z
  geometry_msgs/Vector3 angular
    float64 x
    float64 y
    float64 z

The command is a body relative set of accelerations in linear and angular space. The reference position is the origin of the base_link as defined in [28]

This is replacing the mav_msgs/RateThrust [23] message for a more standard one with the specific call out of the base_link.

High Level Interfaces

Odometry Interface

To communicate odometry information the nav_msgs/Odometry message will be used. [22] This is the standard message used throughout ROS systems already, many modules already take advantage of it. However there are many use cases for aerial vehicles when the estimation of the acceleration is important. So an additional message will be defined which provides acceleration in addition to position and velocity already defined. This additional message will be published in parallel. For backwards compatibility all systems should publish both.

  • Datatype: nav_msgs/Odometry [22]
  • Topic: odom
# This represents an estimate of a position and velocity in free space.
# The pose in this message should be specified in the coordinate frame given by header.frame_id.
# The twist in this message should be specified in the coordinate frame given by the child_frame_id
std_msgs/Header header
string child_frame_id
geometry_msgs/PoseWithCovariance pose
geometry_msgs/TwistWithCovariance twist
  • Datatype: nav_msgs/OdometryWithAcceleration [proposed]
  • Topic: odometry
# This represents an estimate of a position, velocity, and acceleration in free space.
# The pose in this message should be specified in the coordinate frame given by header.frame_id.
# The twist and acceleration in this message should be specified in the coordinate frame given by the child_frame_id
std_msgs/Header header
string child_frame_id
geometry_msgs/PoseWithCovariance pose
geometry_msgs/TwistWithCovariance twist
geometry_msgs/TwistWithCovariance acceleration

Goal Interface

It is expected that there will be several high level ways to communicate goals which are application specific. A standard way to communicate goals will also be important following the mavlink protocol we can send the same messages over ROS using the following pair of messages.

  • SubDatatype: nav_msgs/GlobalPosition [proposed]
  • Topic: command_gps_pose
std_msgs/Header header

uint8 coordinate_frame
uint8 FRAME_GLOBAL_INT = 5
uint8 FRAME_GLOBAL_REL_ALT = 6
uint8 FRAME_GLOBAL_TERRAIN_ALT = 11

uint16 type_mask
uint16 IGNORE_LATITUDE = 1    # Position ignore flags
uint16 IGNORE_LONGITUDE = 2
uint16 IGNORE_ALTITUDE = 4
uint16 IGNORE_VX = 8  # Velocity vector ignore flags
uint16 IGNORE_VY = 16
uint16 IGNORE_VZ = 32
uint16 IGNORE_AFX = 64        # Acceleration/Force vector ignore flags
uint16 IGNORE_AFY = 128
uint16 IGNORE_AFZ = 256
uint16 FORCE = 512    # Force in af vector flag
uint16 IGNORE_YAW = 1024
uint16 IGNORE_YAW_RATE = 2048

float64 latitude
float64 longitude
float32 altitude      # in meters, AMSL or above terrain depending on coordinate_frame value
geometry_msgs/Twist velocity
geometry_msgs/Twist acceleration_or_force
float32 yaw

To send a series of goals a list of GlobalPositions can be used.

  • Datatype: nav_msgs/GlobalWaypoints [proposed]
  • Topic: command_gps_pose
nav_msgs/GlobalPosition[] waypoints

Currently Unspecified

Obstacle Observations

The obstacle observations are expected to use common sensor messages. However since there are a large variety of potential sensors, this interface will not be defined in this REP. It is recommended to use the common messages such as LaserScan, DepthImage, and PointCloud2 from sensor_msgs whenever possible. [15]

Shared State

The shared state between the collision mapping and motion planning controller is expected to be very specific to the implementation. As such it is not proposed to be standardized.

Missions

Currently there are quite a few varieties of missions data formats that are available. The most prominent is the mavlink mission protocol. [30] These missions are heavily used and often customized. A ROS equivalent could be defined but the current mission protocol is both relatively complicated and only provides relatively simple primitive constructs with which to compose a mission. It is expected that mission planners can dynamically construct lower level controls for the vehicle that can then be executed upon by the vehicle controller. The simplest of these mission planners would generate a target waypoint for the vehicle to fly to and send a new one when there's a new position to fly to. A slightly more complex flight mission might send a series of waypoints which the vehicle will follow until the end. If a change in path is required the mission planner will deliver a whole new set of waypoints for the vehicle to follow. For more complicated missions the mission planner can generate the lower level trajectories for controlling the vehicles path more specifically, such as when doing an orbit or other more advanced mission operation. There are a lot more advanced missions that can be run with state machines and higher level logic. Until there is more development in this area we will not propose a standardized mission profile.

Other Interfaces

Battery State

Aerial vehicles should use the sensor_msgs/BatteryState message for reporting their battery status. [12] This is a recently standardized message which is much more broadly applicable than aerial vehicles and has already been standardized.

  • Datatype: sensor_msgs/BatteryState [12]
  • Topic: battery_state

Vehicle Status

Drones could have different configurations or types as well as define failsafes, engine failures or hardware in the loop setup. These are the fields proposed for this message

  • Datatype: proposed_aerial_msgs/VehicleStatus (proposed)
  • Service Name: status
uint8 HIL_STATE_OFF = 0
uint8 HIL_STATE_ON = 1

uint8 VEHICLE_TYPE_UNKNOWN = 0
uint8 VEHICLE_TYPE_ROTARY_WING = 1
uint8 VEHICLE_TYPE_FIXED_WING = 2
uint8 VEHICLE_TYPE_ROVER = 3
uint8 VEHICLE_TYPE_VTOL = 4

std_msgs/Header header

uint8 system_type     # system type
bool engine_failure   # Set to true if an engine failure is detected
bool failsafe         # true if system is in failsafe state
uint8 hil_state       # current hil state

Flight Modes

Drones have quite a few flight modes. For example the PX4 Flight Modes number between 9 and 12 for different configurations. [29] Many of these different modes are modifying which user inputs are used and/or how they are interpreted. For working with ROS it is expected that the user inputs will be modulated and received through the ROS system and that the system will be in control the whole time including implementing fallback and recovery behaviors. Without the need for these extra modes we will reduce the state diagram significantly. The autopilot itself is expected to be in Offboard control mode for most configurations using ROS. In theory all of the states could be eliminated and the arming, takeoff, landing, and return to land logic could be fully implemented, but we will keep those states which are related to safety system already implemented.

To that end the following modes for ROS flight operations are proposed.

graph LR DISARMED[Disarmed] ARMED[Armed] FLYING[Flying] LANDED[Landed] RTL[Return to Land] DISARMED --> |Arm| ARMED ARMED --> |Takeoff| FLYING FLYING --> |Land| LANDED FLYING --> | RTL
Lost Link | RTL LANDED --> |Auto Disarm| DISARMED RTL --> |Disarm| DISARMED FLYING --> |Disarm| DISARMED RTL --> | Auto Land | LANDED %% Links to DISARMED linkStyle 5 fill:#fff,stroke:#f00,stroke-width:3px; linkStyle 6 fill:#fff,stroke:#f00,stroke-width:3px;

Note that the red disarm transitions are potentially unsafe as they could lead to the vehicle falling.

  • Datatype: mav_msgs/FlightMode (proposed)
uint8 FLIGHT_MODE_DISARMED=0
uint8 FLIGHT_MODE_ARMED=1
uint8 FLIGHT_MODE_FLYING=2
uint8 FLIGHT_MODE_LANDED=3
uint8 FLIGHT_MODE_RTL=4

uint8 flight_mode # The flight mode indicated
  • Datatype: mav_msgs/SetFlightMode (proposed)
  • Service Name: set_flight_mode
mav_msgs/FlightMode goal # The desired flight mode
---
mav_msgs/FlightMode result # The resultant state after the attempted transition.
bool success # True if desired change was executed False otherwise.

Summary

Existing interfaces

  • Battery State via sensor_msgs/BatteryState [12]
  • Trajectory via trajectory_msgs/MultiDOFJointTrajectory [13]
  • Rates via mav_msgs/RollPitchYawrateThrust [25] or mav_msgs/AttitudeThrust [24]
  • Accelerations via mav_msgs/RateThrust [23]
  • Obstacle information from common sensor_msgs [15]
  • Odometry via``nav_msgs/Odometry`` [22]

Proposed New Interfaces

  • Extended Odometry via proposed nav_msgs/OdometryWithAcceleration [propoised]

References

[1]astec_mav_framework (http://wiki.ros.org/asctec_mav_framework)
[2]mavlink2ros (https://github.com/posilva/mav2rosgenerator)
[3](1, 2) mavros (http://wiki.ros.org/mavros)
[4]roscopter (https://code.google.com/p/roscopter/)
[5]CRATES (https://bitbucket.org/asymingt/crates)
[6]rospilot (http://wiki.ros.org/rospilot)
[7]hector_quadrotor (http://wiki.ros.org/hector_quadrotor)
[8]autopilot_bridge (https://github.com/mikeclement/autopilot_bridge)
[9]mav_tools (http://wiki.ros.org/mav_tools)
[10]dji_sdk_dji2ros (http://www.ros.org/browse/details.php?distro=indigo&name=dji_sdk_dji2mav)
[11]dji_sdk (http://wiki.ros.org/dji_sdk)
[12](1, 2, 3) sensor_msgs/BatteryState (http://docs.ros.org/api/sensor_msgs/html/msg/BatteryState.html)
[13](1, 2, 3) (http://docs.ros.org/api/trajectory_msgs/html/msg/MultiDOFJointTrajectory.html)
[14](1, 2) (http://docs.ros.org/api/geometry_msgs/html/msg/PoseStamped.html)
[15](1, 2, 3) (http://wiki.ros.org/sensor_msgs)
[17]trajectory_msgs (http://wiki.ros.org/trajectory_msgs)
[18]DroneKit (http://dronekit.io/)
[19]mav_msgs (http://wiki.ros.org/mav_msgs)
[21]Mavlink2 (https://github.com/diydrones/mavlink/blob/mavlink2-wip/doc/MAVLink2.md)
[22](1, 2, 3) odometry (http://www.ros.org/doc/api/nav_msgs/html/msg/Odometry.html)
[23](1, 2) mav_msgs/RateThrust (http://docs.ros.org/jade/api/mav_msgs/html/msg/RateThrust.html)
[24](1, 2) mav_msgs/AttitudeThrust (http://docs.ros.org/jade/api/mav_msgs/html/msg/AttitudeThrust.html)
[25](1, 2) mav_msgs/RollPitchYawrateThrust (http://docs.ros.org/indigo/api/mav_msgs/html/msg/RollPitchYawrateThrust.html)
[26]geometry_msgs/Twist (http://docs.ros.org/api/geometry_msgs/html/msg/Twist.html)
[27](1, 2, 3, 4) geometry_msgs/TwistStamped (http://docs.ros.org/api/geometry_msgs/html/msg/TwistStamped.html)
[28](1, 2, 3) REP 105: Coordinate Frames for Mobile Platforms (http://www.ros.org/reps/rep-0105.html)
[29]PX4 Flight Modes (https://dev.px4.io/en/concept/flight_modes.html)
[30]Mavlink Mission protocol (https://mavlink.io/en/services/mission.html)
[31]Mavlink Common.xml (https://mavlink.io/en/messages/common.html)
[32]sensor_msgs/Range (http://docs.ros.org/melodic/api/sensor_msgs/html/msg/Range.html)
[33]std_srvs/Trigger (http://docs.ros.org/melodic/api/std_srvs/html/srv/Trigger.html)