[REP Index] [REP Source]

REP:147
Title:A Standard interface for Aerial Vehicles
Author:Tully Foote
Status:Draft
Type:Informational
Content-Type:text/x-rst
Created:16-May-2016
Post-History:30-Aug-2002

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]
  • 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 defintions

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[Minimum Snap Controller] CM ---|Shared State|MSC end CS -->|Obstacle Observations|CM OS -->|Odometry|MSC OS -->|Odometry|CM OS -->|Odometry|C GCS -->|Goals|MSC MSC -->|Trajectory|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 [26].

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 [12] 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 labelling and filling. The complexity of the message parsing is traded off against supporting a relatively compact representations.

  • Datatype: trajectory_msgs/MultiDOFJointTrajectory [12]
  • 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 [13]. This is a very highly used standard message and contains precisely the information needed.

  • Datatype: geometry_msgs/PoseStamped [13]
  • 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 [26]

Rate Interface

The messge to send a velocity command is geometry_msgs/Twist [25]. This is the standard way to command ROS vehicles. The datatype is not timestamped or externally referenced as it is not intended to be used in an open loop control system.

  • Datatype: geometry_msgs/Twist [25]
  • Topic: cmd_vel
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. The reference position is the origin of the base_link as defined in [26]

Acceleration Interface

The messge to send a velocity command is geometry_msgs/Twist [25]. The datatype is not timestamped or externally referenced as it is not intended to be used in an open loop control system.

  • Datatype: geometry_msgs/Twist [25]
  • Topic: command_acceleration
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 [26]

High Level Interfaces

Odometry Interface

To communicate odometry information the nav_msgs/Odometry message will be used. [21] 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 will publish both.

  • Datatype: nav_msgs/Odometry [21]
  • 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
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
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.

TODO(tfoote): Maybe add a standard GPS waypoint list here? Things already in mavlink?

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. [14]

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.

Other Interfaces

Battery State

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

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

Summary

Existing interfaces

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

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](1, 2, 3) sensor_msgs/BatteryState (http://docs.ros.org/api/sensor_msgs/html/msg/BatteryState.html)
[12](1, 2, 3) (http://docs.ros.org/api/trajectory_msgs/html/msg/MultiDOFJointTrajectory.html)
[13](1, 2) (http://docs.ros.org/api/geometry_msgs/html/msg/PoseStamped.html)
[14](1, 2) (http://wiki.ros.org/sensor_msgs)
[16]trajectory_msgs (http://wiki.ros.org/trajectory_msgs)
[17]DroneKit (http://dronekit.io/)
[18]mav_msgs (http://wiki.ros.org/mav_msgs)
[20]Mavlink2 (https://github.com/diydrones/mavlink/blob/mavlink2-wip/doc/MAVLink2.md)
[21](1, 2, 3) odometry (http://www.ros.org/doc/api/nav_msgs/html/msg/Odometry.html)
[22]mav_msgs/RateThrust (http://docs.ros.org/jade/api/mav_msgs/html/msg/RateThrust.html)
[23]mav_msgs/AttitudeThrust (http://docs.ros.org/jade/api/mav_msgs/html/msg/AttitudeThrust.html)
[24]mav_msgs/RollPitchYawrateThrust (http://docs.ros.org/indigo/api/mav_msgs/html/msg/RollPitchYawrateThrust.html)
[25](1, 2, 3, 4) geometry_msgs/Twist (http://docs.ros.org/api/geometry_msgs/html/msg/Twist.html)
[26](1, 2, 3, 4) REP 105: Coordinate Frames for Mobile Platforms (http://www.ros.org/reps/rep-0105.html)