Controller Plugin

Write a motion controller plugin from scratch, build it in your own workspace and fly the simulated drone with it.

The motion controller keeps its control law in a plugin, so a new controller is a new plugin. This guide builds one from scratch, proportional_controller, and flies the simulated drone with it.

The plugin is kept small. It turns a position reference into a velocity proportional to the error, and passes speed references through as they are. That's enough to take off, fly to points and land.

Before you start

Aerostack2 to build against. Either an installation and a colcon workspace for your package, or a Pixi workspace.
The motion controller concepts. Motion controller explains control modes, how a mode is chosen and when the controller bypasses its plugin.

The controller_manager node loads the plugin with pluginlib, by the class name <plugin_name>::Plugin. The node does everything around the control law. It subscribes to the state and the references, converts them into the frames the plugin works in, agrees on control modes with the platform, and publishes the command. The plugin receives ready-made messages and fills in a command once per cycle, 100 times a second by default.

The base class

A plugin inherits from as2_motion_controller_plugin_base::ControllerBase, declared in controller_base.hpp. These are the methods it overrides.

Method	Required	What it does
`updateParameter`	Yes	Applies one of the plugin's parameters, named without the plugin's prefix. Called at start and on every change
`onUpdateState`	Yes	Receives the drone's pose and twist, already in the plugin's frames
`computeOutput`	Yes	Fills the command for this cycle. Returning false sends nothing
`requiredParameters`	No	Lists the parameters that must arrive before the plugin accepts a mode
`initParameters`	No	Lists the parameters read once, before `ownInitialize`. Later changes to them are ignored
`ownInitialize`	No	Runs once when the plugin is loaded, for setup such as debug publishers
`onUpdateReference`	No	Receives a pose, twist, trajectory or thrust reference. There's one overload per type, and each does nothing by default
`onSetMode`	No	Accepts or refuses a pair of input and output modes
`hoverMode`	No	Says which input mode the plugin hovers with. The default, unset, means it can't hover
`reset`	No	Clears the plugin's state, like integrators. An override calls `ControllerBase::reset()`

The base also gives the plugin a few helpers.

Helper	What it gives
`getNodePtr()`	The controller node, for logging, the clock and publishers
`getControlModeIn()`, `getControlModeOut()`	The pair of modes in use
`getDesiredPoseFrameId()`, `getDesiredTwistFrameId()`	The frames the state and references arrive in
`setDesiredPoseFrameId()`, `setDesiredTwistFrameId()`	Change those frames, usually from `onSetMode`
`getStatePose()`, `getStateTwist()`	The last state the plugin received
`param()`	A name with the plugin's prefix, like `proportional_controller.position_gain`
`createDebugPublisher()`	A publisher for an optional debug topic, which a parameter turns on

Write the plugin

Create the package

The package holds the plugin, two configuration files and what pluginlib needs to find it:

proportional_controller/
- CMakeLists.txt
- package.xml
- plugins.xmlTells pluginlib which class the library holds
- pixi.tomlOnly for a Pixi workspace
- config/
  - available_modes.yamlThe control modes the plugin takes and sends
  - controller_default.yamlThe plugin's default parameters
- src/
  - proportional_controller.cpp

Put it in the src folder of your colcon workspace, or next to the pixi.toml of your Pixi workspace. It's a package of your own, so it doesn't need the as2_ prefix. The plugins that ship with Aerostack2 split their class into a header and a source file. This one fits in a single file.

Write the control law

proportional_controller/src/proportional_controller.cpp

#include <algorithm>
#include <string>
#include <vector>

#include <as2_core/utils/frame_utils.hpp>
#include <as2_motion_controller/controller_base.hpp>
#include <pluginlib/class_list_macros.hpp>

namespace proportional_controller
{

using as2_msgs::msg::ControlMode;

class Plugin : public as2_motion_controller_plugin_base::ControllerBase
{
public:
  std::vector<std::string> requiredParameters() const override
  {
    return {"position_gain", "yaw_gain"};
  }

  void updateParameter(const std::string & name, const rclcpp::Parameter & param) override
  {
    if (name == "position_gain") {
      position_gain_ = param.as_double();
    } else if (name == "yaw_gain") {
      yaw_gain_ = param.as_double();
    }
  }

  ControlMode hoverMode() const override
  {
    ControlMode mode;
    mode.control_mode = ControlMode::POSITION;
    mode.yaw_mode = ControlMode::YAW_ANGLE;
    return mode;
  }

  bool onSetMode(const ControlMode & mode_in, const ControlMode & /*mode_out*/) override
  {
    if (mode_in.control_mode == ControlMode::POSITION && position_gain_ <= 0.0) {
      RCLCPP_ERROR(getNodePtr()->get_logger(), "position_gain must be above 0");
      return false;
    }
    // Take twists in the same frame as poses, odom by default
    setDesiredTwistFrameId(getDesiredPoseFrameId());
    return true;
  }

  void onUpdateState(
    const geometry_msgs::msg::PoseStamped & pose,
    const geometry_msgs::msg::TwistStamped & /*twist*/) override
  {
    position_ = pose.pose.position;
    yaw_ = as2::frame::getYawFromQuaternion(pose.pose.orientation);
  }

  void onUpdateReference(const geometry_msgs::msg::PoseStamped & ref) override
  {
    ref_position_ = ref.pose.position;
    ref_yaw_ = as2::frame::getYawFromQuaternion(ref.pose.orientation);
  }

  void onUpdateReference(const geometry_msgs::msg::TwistStamped & ref) override
  {
    ref_twist_ = ref.twist;
  }

  bool computeOutput(
    double /*dt*/,
    geometry_msgs::msg::PoseStamped & /*pose*/,
    geometry_msgs::msg::TwistStamped & twist,
    as2_msgs::msg::Thrust & /*thrust*/) override
  {
    twist.header.frame_id = getDesiredTwistFrameId();

    if (getControlModeIn().control_mode == ControlMode::SPEED) {
      // Pass speed references through as they are
      twist.twist = ref_twist_;
      return true;
    }

    // Position: a velocity proportional to the error, within the speed limit
    twist.twist.linear.x = command(ref_position_.x - position_.x, ref_twist_.linear.x);
    twist.twist.linear.y = command(ref_position_.y - position_.y, ref_twist_.linear.y);
    twist.twist.linear.z = command(ref_position_.z - position_.z, ref_twist_.linear.z);
    twist.twist.angular.z = yaw_gain_ * as2::frame::angleMinError(ref_yaw_, yaw_);
    return true;
  }

  void reset() override
  {
    ControllerBase::reset();
    ref_twist_ = geometry_msgs::msg::Twist();
  }

private:
  double command(double error, double limit) const
  {
    const double speed = position_gain_ * error;
    if (limit == 0.0) {
      return speed;
    }
    return std::clamp(speed, -std::abs(limit), std::abs(limit));
  }

  double position_gain_ = 0.0;
  double yaw_gain_ = 0.0;

  geometry_msgs::msg::Point position_;
  double yaw_ = 0.0;

  geometry_msgs::msg::Point ref_position_;
  double ref_yaw_ = 0.0;
  geometry_msgs::msg::Twist ref_twist_;
};

}  // namespace proportional_controller

PLUGINLIB_EXPORT_CLASS(
  proportional_controller::Plugin,
  as2_motion_controller_plugin_base::ControllerBase)

The class is called Plugin, inside a namespace named after the plugin. The controller builds the class name from plugin_name, so the namespace has to match it.

Parameters. The gains arrive through updateParameter, one call per parameter, without the proportional_controller. prefix. Both are in requiredParameters, so the controller refuses every mode until both have arrived, and logs the one that's missing.

Modes. onSetMode runs each time the controller settles on a new pair of modes. This plugin refuses position references while its gain is 0, and asks for twists in the pose frame. Poses arrive in odom by default and twists in base_link, so after this call both arrive in odom. The speed limit and the command then share a frame.

References. A behavior that sends a position, like go to, also sends a twist. With a position reference, each axis of that twist is the speed limit on that axis. With a speed reference, the twist is the velocity to fly, and its angular z is the yaw rate.

Output. computeOutput fills only the twist, because the plugin's only output mode is a velocity with a yaw rate. The controller ignores the pose and thrust in that mode. The twist carries its frame, and the platform converts it from there.

Hover. When a behavior asks for hover, the controller freezes the drone's current pose and sends it to the plugin as a position reference, with a zero twist. hoverMode tells the controller to do that in position mode with a yaw angle. A limit of 0 means no limit in command, so the hover isn't capped.

Reset. The controller calls reset after every mode change. The base's version clears the flag that says a reference has arrived, so the controller sends nothing until the next reference comes in.

List the control modes

available_modes.yaml lists the modes the plugin takes from the behaviors and the modes it sends to the platform:

proportional_controller/config/available_modes.yaml

# Each mode is 0b<control mode, 4 bits><yaw mode, 2 bits><reserved, 2 bits>
input_control_modes:
  - 0b01000100  # SPEED with YAW_SPEED
  - 0b01100000  # POSITION with YAW_ANGLE

output_control_modes:
  - 0b01000100  # SPEED with YAW_SPEED

The first four bits are the control mode, such as 0110 for a position and 0100 for a speed. The next two are the yaw mode, 00 for an angle and 01 for a rate. The list of the PID speed controller has every code. The controller tries the output modes in the order they're listed.

These two inputs cover the standard motion behaviors. Take off, go to and follow path send a position with a yaw angle, and land sends a speed with a yaw rate. Leave hover out of the lists, since hoverMode covers it.

Set the default parameters

The plugin's parameters sit under its name:

proportional_controller/config/controller_default.yaml

/**:
  ros__parameters:
    proportional_controller:
      position_gain: 1.0
      yaw_gain: 1.0

Write them as decimals. as_double throws on a whole number like 1.

Export the plugin

plugins.xml names the library and the class inside it:

proportional_controller/plugins.xml

<library path="proportional_controller">
  <class type="proportional_controller::Plugin"
    base_class_type="as2_motion_controller_plugin_base::ControllerBase">
    <description>Proportional position controller that sends velocities.</description>
  </class>
</library>

The package depends on the motion controller, which holds the base class:

proportional_controller/package.xml

<?xml version="1.0"?>
<package format="3">
  <name>proportional_controller</name>
  <version>0.1.0</version>
  <description>A proportional position controller for the Aerostack2 motion controller.</description>
  <maintainer email="you@example.com">You</maintainer>
  <license>BSD-3-Clause</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <depend>rclcpp</depend>
  <depend>pluginlib</depend>
  <depend>geometry_msgs</depend>
  <depend>as2_core</depend>
  <depend>as2_msgs</depend>
  <depend>as2_motion_controller</depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

CMakeLists.txt builds the plugin as a shared library and registers it:

proportional_controller/CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(proportional_controller)

set(CMAKE_CXX_STANDARD 17)

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(pluginlib REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(as2_core REQUIRED)
find_package(as2_msgs REQUIRED)
find_package(as2_motion_controller REQUIRED)

add_library(${PROJECT_NAME} SHARED src/proportional_controller.cpp)
ament_target_dependencies(${PROJECT_NAME}
  rclcpp pluginlib geometry_msgs as2_core as2_msgs as2_motion_controller)

# Register the plugin with the motion controller
pluginlib_export_plugin_description_file(as2_motion_controller plugins.xml)

install(TARGETS ${PROJECT_NAME}
  LIBRARY DESTINATION lib
  ARCHIVE DESTINATION lib
  RUNTIME DESTINATION bin)
install(DIRECTORY config DESTINATION share/${PROJECT_NAME})

ament_package()

The first argument of pluginlib_export_plugin_description_file is the package that owns the base class, not yours. That's how the motion controller finds plugins in other packages. The library's name has to match the path in plugins.xml.

Build it

From the root of your workspace, with Aerostack2 sourced:

cd ~/aerostack2_ws
colcon build --symlink-install --packages-select proportional_controller
source install/setup.bash

Run it

Start the controller with your plugin

The controller's launch file only takes the stock plugins

controller_launch.py refuses a plugin_name that isn't one of the plugins that ship with Aerostack2, and looks for the plugin's files in its own package. Start the node directly instead, as below.

Run this in a shell where ros2 finds your package. That's a terminal where you sourced install/setup.bash, or one opened with pixi shell in your Pixi workspace:

ros2 run as2_motion_controller as2_motion_controller_node --ros-args \
  -r __ns:=/drone0 \
  --params-file $(ros2 pkg prefix as2_motion_controller)/share/as2_motion_controller/config/motion_controller_default.yaml \
  --params-file $(ros2 pkg prefix proportional_controller)/share/proportional_controller/config/controller_default.yaml \
  -p plugin_name:=proportional_controller \
  -p plugin_available_modes_config_file:=$(ros2 pkg prefix proportional_controller)/share/proportional_controller/config/available_modes.yaml \
  -p use_bypass:=false

The first file holds the controller's own parameters and the second the plugin's gains. plugin_name picks the plugin, and plugin_available_modes_config_file points to its modes. In a simulator with its own clock, such as Gazebo, add -p use_sim_time:=true.

use_bypass:=false sends every reference through the plugin. With bypass on, the controller skips the plugin whenever the platform takes the behavior's mode as it is. The multirotor simulator takes positions and speeds, so with bypass on your plugin would never run there.

The controller logs the gains it read and the modes the plugin offers:

[INFO] [drone0.controller_manager]: Parameter proportional_controller.position_gain := 1.000000
[INFO] [drone0.controller_manager]: Parameter proportional_controller.yaw_gain := 1.000000
[INFO] [drone0.controller_manager]: PLUGIN LOADED [proportional_controller::Plugin])
[INFO] [drone0.controller_manager]: ==========================================================
[INFO] [drone0.controller_manager]: AVAILABLE INPUT MODES:
[INFO] [drone0.controller_manager]: 	 - SPEED YAW_SPEED
[INFO] [drone0.controller_manager]: 	 - POSITION YAW_ANGLE
[INFO] [drone0.controller_manager]: AVAILABLE OUTPUT MODES:
[INFO] [drone0.controller_manager]: 	 -SPEED YAW_SPEED
[INFO] [drone0.controller_manager]: ==========================================================

Fly a mission

Start the rest of the drone as usual, with the platform, the state estimator and the motion behaviors, and leave out the stock controller. The multirotor simulator project has all of them. Its take off, go to and follow path behaviors use their position plugins, and land uses its speed plugin, which matches the modes above.

Then fly a short mission:

fly.py

import rclpy
from as2_python_api.drone_interface import DroneInterface

rclpy.init()
drone = DroneInterface(drone_id='drone0')
drone.arm()
drone.offboard()
print('takeoff', drone.takeoff(height=1.5, speed=0.5))
print('go_to', drone.go_to(2.0, 1.0, 1.5, speed=1.0))
print('at', [round(v, 2) for v in drone.position])
print('go_to', drone.go_to(0.0, 0.0, 1.5, speed=1.0))
print('at', [round(v, 2) for v in drone.position])
print('land', drone.land(speed=0.5))
drone.manual()
drone.shutdown()
rclpy.shutdown()

python3 fly.py

takeoff True
go_to True
at [1.81, 0.96, 1.49]
go_to True
at [0.17, 0.04, 1.5]
land True

Go to reports success once the drone is close enough to the goal, so the position it prints falls a little short. A proportional controller slows down as the error shrinks, and the plugin keeps closing the gap afterwards.

Check what it did

The controller logs each pair of modes it settles on. Take off picks position, and land switches to speed:

[INFO] [drone0.controller_manager]: Twist frame set to 'drone0/odom'
[INFO] [drone0.controller_manager]: input_mode:[POSITION YAW_ANGLE ]
[INFO] [drone0.controller_manager]: output_mode:[SPEED YAW_SPEED ]
[INFO] [drone0.controller_manager]: input_pose_frame_id:[drone0/odom]
[INFO] [drone0.controller_manager]: input_twist_frame_id:[drone0/odom]

The command your plugin sends is on actuator_command/twist. During take off it's the speed limit of 0.5 m/s, straight up:

ros2 topic echo --once /drone0/actuator_command/twist

header:
  stamp:
    sec: 1793666298
    nanosec: 782957521
  frame_id: drone0/odom
twist:
  linear:
    x: 0.0
    y: 0.0
    z: 0.5
  angular:
    x: 0.0
    y: 0.0
    z: 0.0

The gains can change in flight. The controller passes the new value to updateParameter and logs it:

ros2 param set /drone0/controller_manager proportional_controller.position_gain 1.5

[INFO] [drone0.controller_manager]: Parameter proportional_controller.position_gain := 1.500000

Going further

The plugins that ship with Aerostack2 use the rest of the interface.

pid_speed_controller also takes trajectories, reading the first setpoint of each one. It publishes a debug topic made with createDebugPublisher, and picks its hover mode from the gains it has.
differential_flatness_controller sends body rates and a thrust, so it fills both the twist and the thrust in computeOutput.
geometric_controller has two output modes, body rates and speed. It refuses body rates in onSetMode when its mass isn't set, and the controller then falls back to speed.

Motion controller

Control modes, bypass, hover and frames.

Motion controller plugins

The stock plugins and every gain they take.