Guidance API#

Enumerations#

enum guidance::Trigger#

A Guidance Mode can specify at which rate their step() method should be called. This is done through the guidance::Mode::getTriggers method, which returns a timeout T, a period N and a bitmask build from the following values. Each enum value correspond to a possible event that triggers the step() method. TRIGGER_TIMER and TRIGGER_TICK cannot both be set.

Values:

enumerator TRIGGER_TIMER#

Fired after T ms after the last trigger, if no other trigger happen

enumerator TRIGGER_TICK#

Every N ticks as computed by Drone Controller

enum guidance::ChannelKind#

Each enum value identifies one predefined Channel

Values:

enumerator CHANNEL_KIND_GUIDANCE#

Server channel (normally the client is Flight Supervisor)

enumerator CHANNEL_KIND_DRONE_CONTROLLER#

Client channel connected to Drone Controller.

enum guidance::SenderKind#

Each enum identifies a Sender object, i.e. an interface through which protobuf messages can be sent (see libmsghub).

Values:

enumerator SENDER_KIND_DRONE_CONTROLLER#
enumerator SENDER_KIND_FRONT_CAM_CONTROLLER#
enumerator SENDER_KIND_FRONT_ZOOM_CAM_CONTROLLER#
enumerator SENDER_KIND_STEREO_CAM_CONTROLLER#
enumerator SENDER_KIND_GUIDANCE#

Classes#

class Guidance#

Guidance interface

This is the Guidance interface as seen from modes.

Subclassed by guidance::GuidanceImpl

Public Functions

inline Guidance()#
inline virtual ~Guidance()#
virtual pomp::Loop *getLoop() = 0#

Return the POMP loop owned by this guidance instance. This event base is used to aggregate events from external and internal messages (see libpomp.hpp).

virtual msghub::MessageHub *getMessageHub() = 0#

Return the current MessageHub instance, which is responsible for managing channels, handlers and senders for protocol buffer messages (see libmsghub).

virtual msghub::Channel *getChannel(ChannelKind kind) const = 0#

Get the instance of Channel associated with an identifier

Parameters:

kind – identifies the kind of channel to access.

virtual msghub::MessageSender *getSender(SenderKind kind) = 0#

Get the MessageSender associated with an identifier.

Parameters:

kind – identifies the kind of sender to access.

virtual cfgreader::ConfigReader *getConfig() const = 0#

Get the current ConfigReader instance, which holds static configuration values for the current drone hardware.

virtual const SpatialPerception *getSpatialPerception() const = 0#

Get the SpatialPerception instance owned by the guidance object. This gives access to the occupancy grid and the spherical map.

virtual bool addMode(Mode *mode) = 0#

Register a Mode The mode’s name is used as a key to identify it. Two modes cannot have the same name. If the name already exists, the current mode is NOT replaced by the new one. The mode name cannot be empty, or equal to “null”.

Parameters:

mode – non-null pointer to a Mode instance

Returns:

true if the operation succeeded.

virtual bool removeMode(Mode *mode) = 0#

Unregister a Mode

Parameters:

mode – non-null pointer to a Mode instance

Returns:

true if the mode was registered and is successfully unregistered.

virtual std::string getConfigDir() const = 0#

Return the configuration directory associated with the plugin currently being loaded (for example missions are all located in distinct directories and can use specific configuration files).

Calling this method is only valid while a plugin is being initialized. More precisely, it corresponds to the extent of a call to guidance::Plugin::init().

In practice, that means that Python modes can only access this path from inside Mode constructors, and C++ modes can only access it during the extent of guidance_plugin_init().

The resulting string is a copy, the caller owns the value. The string is empty if the method is called outside of its valid usage.

template<typename ModeType, class ...Args>
inline std::enable_if<std::is_base_of<Mode, ModeType>::value, ModeType*>::type registerMode(Args&&... args)#

Allocate a mode class, add the instance to guidance, and return a pointer to it

If the constructor does not throw and addMode succeeds, return a raw pointer to a freshly allocated mode. Otherwise, return nullptr.

Deprecated:

The registerMode method that returns a guidance mode pointer is deprecated. Use the registerMode(ModeType*&, …) method overload instead.

Parameters:
  • ModeType – is a type of mode, which should be a subclass of Mode and define a constructor taking a Guidance pointer in parameter.

  • Args – is a parameter pack for additional arguments passed down to the mode constructor in addition to this Guidance instance.

template<typename ModeType, class ...Args>
inline std::enable_if<std::is_base_of<Mode, ModeType>::value, int>::type registerMode(ModeType *&mode, Args&&... args)#

Allocate a mode class, add the instance to guidance, and assign the mode output parameter to the newly created mode.

If the constructor does not throw and addMode() succeeds, return 0. Otherwise, return -EINVAL.

Parameters:
  • ModeType – is a type of mode, which should be a subclass of Mode and define a constructor taking a Guidance pointer in parameter.

  • args – is a parameter pack for additional arguments passed down to the mode constructor in addition to this Guidance instance.

template<typename ModeType>
inline void unregisterMode(ModeType *&mode)#

Remove a mode from guidance, delete it and assigns the pointer in reference to nullptr.

Parameters:
  • ModeType – type of mode, which should be a subclass of Mode. This template parameter is necessary because otherwise, the compiler here only knows about the incomplete Mode class and calling delete on such a type is undefined behavior (-Wdelete-incomplete warning)

  • mode – pointer to Mode instance, by reference, expected to be valid or nullptr on entry, and guaranteed to be nullptr on exit.

class Input#

Guidance input

The active mode has access to an instance of Input, which holds the current inputs at each step.

Public Types

using Pcmd = ::Guidance::Messages::Pcmd#

Public Functions

inline Input()#
inline void clearPcmds()#

Clear members

Public Members

Pcmd mPcmd#
class Output#

Guidance output

At each step of execution the active mode can update an instance of Output, which holds different drone and camera references expressed as protobuf messages.

Each output is associated with a boolean flag for that output that indicates if the output is present at this step or not. At the beginning of a step, all flags are false.

Public Types

using HorizontalReference = DroneController::Messages::HorizontalReference#
using VerticalReference = DroneController::Messages::VerticalReference#
using YawReference = DroneController::Messages::YawReference#
using FrontCamReference = AxesCamController::Messages::Reference#
using FrontCamZoom = ZoomCamController::Messages::Zoom#
using StereoCamReference = AxesCamController::Messages::Reference#
using FrontCamReferenceConfig = AxesCamController::Messages::Config#
using FrontCamZoomConfig = ZoomCamController::Messages::Config#
using StereoCamReferenceConfig = AxesCamController::Messages::Config#

Public Functions

inline Output()#
inline void clear()#

Clear members

Public Members

HorizontalReference mHorizontalReference#
bool mHasHorizontalReference#
VerticalReference mVerticalReference#
bool mHasVerticalReference#
YawReference mYawReference#
bool mHasYawReference#
FrontCamReference mFrontCamReference#
bool mHasFrontCamReference#
FrontCamZoom mFrontCamZoom#
bool mHasFrontCamZoom#
StereoCamReference mStereoCamReference#
bool mHasStereoCamReference#
FrontCamReferenceConfig mFrontCamReferenceConfig#
bool mHasFrontCamReferenceConfig#
FrontCamZoomConfig mFrontCamZoomConfig#
bool mHasFrontCamZoomConfig#
StereoCamReferenceConfig mStereoCamReferenceConfig#
bool mHasStereoCamReferenceConfig#
uint32_t mTickNum#
class Mode#

Guidance Mode

At each step (with registered triggers) the methods will be called in this order:

  • beginStep

  • generateDroneReference

  • correctDroneReference

  • generateAttitudeReferences

  • endStep

Subclassed by AbsoluteMoveMode, AcquisitionMode, CameraRotationMoveMode, CarpetMode, DescentMode, FlightPlanMode, guidance::NullMode, guidance::TrajectoryFcamMode, guidance::TrajectoryStcamMode, LookAtMode, ManualMode, PoiMode

Public Functions

inline Mode(Guidance *guidance)#
inline Mode(Guidance *guidance, ModeGroup *group)#
inline virtual ~Mode()#
virtual const std::string &getName() const = 0#

Get the name of the guidance mode (shall be unique in the system).

virtual void getTriggers(uint32_t *triggers, uint32_t *timeout, uint32_t *period) = 0#

Get the triggers for the mode

Parameters:
  • triggers – mask of Trigger values. TRIGGER_TICK can not be combined with TRIGGER_TIMER.

  • timeout – if no frames received during this time (in ms), force a step.

  • period – if a timer is required, its period (in ms). If there is also a trigger on frame with a timeout specified, it will only be called when there is no frame. For tick trigger, this is the period of wakeup measured in ticks of the Drone Controller. For a 200Hz tick in drone controller this means a increment of 5ms per tick.

inline virtual void configure(const ::google::protobuf::Any &config, bool disableObstacleAvoidance, bool override_front_camera, bool override_stereo_camera)#

Called when the mode is set with a configuration

inline virtual bool hasObstacleAvoidance()#

Return whether the mode implements obstacle avoidance Unless this function is overriden, modes do not advertise they implement obstacle avoidance. TODO: Add to mode descriptor

inline virtual void enter()#

Called when the mode is activated and become the current one.

inline virtual void exit()#

Called when the mode is de-activated.

inline virtual void beginStep()#

Start a new step of the guidance The input data is valid at this point

inline virtual void endStep()#

Finish a step of the guidance Output data shall be valid after this point

inline virtual void generateDroneReference()#

Generate the drone trajectory reference (horizontal and vertical).

inline virtual void correctDroneReference()#

Correct the generated trajectory using obstacle avoidance data.

inline virtual void generateAttitudeReferences()#

Generate the front camera, stereo camera and drone attitudes references. The camera references do not depend on the drone ones, but will be saturated if not reachable.

inline const Input *getInput() const#
inline Output *getOutput()#
inline void setError(const std::string &message)#

Report an error in this mode. The error message is logged and sent back to the Flight Supervisor. Guidance will call the exit() method (in case we are not already in exit()), and deactivate the current mode.

inline Guidance *getGuidance()#

Protected Functions

template<typename T>
inline int unpackConfig(const ::google::protobuf::Any &any, T &config)#

unpack a mode configuration from an Any message

Parameters:
  • T – type of config (protobuf message)

  • any – message of type Any to unpack

  • config – (output) protobuf message of type T where message any is unpacked to

Returns:

0 on success, -EINVAL if message is empty (no configuration), -ENOMSG if the type_url does not match the type in T::descriptor(). @apiNote by convention, the type_url() of the any configuration message ends with a slash and the fully-qualified name of the type of message used as a configuration message. More precisely, this generic method infers the type T from its second argument, and calls T::descriptor()->full_name() to know the suffix type_url() the Any message should ends with.

class SpatialPerception#

Subclassed by guidance::SpatialPerceptionImpl

Public Types

typedef Eigen::Matrix<float, 3, -1, Eigen::RowMajor> PointsVector#

Public Functions

virtual const virtual moser::IGrid & getGrid () const =0
virtual void publishMiscDebugData(const PointsVector &nominalTraj, const std::array<float, 4> &nominalTrajColor, const PointsVector &referenceTraj, const std::array<float, 4> &referenceTrajColor) const = 0#
class TrajectoryFcamMode : public guidance::Mode#

Subclassed by RelativeMoveMode

Public Functions

TrajectoryFcamMode(Guidance *guidance, const std::string &name, ModeGroup *group = nullptr)#
virtual ~TrajectoryFcamMode() override#
void setDroneNominalPositionTrajectoryGlobal(const guidance::PositionTrajectoryPoint &point)#

Set nominal global drone trajectory (to be called from generateDroneReference) Will be adjusted in correctDroneReference if obstable avoidance is enabled.

void setDroneNominalPositionTrajectoryLocal(const guidance::PositionTrajectoryPoint &point)#

Set nominal local drone trajectory (to be called from generateDroneReference) Will be adjusted in correctDroneReference if obstable avoidance is enabled

void setFcamEulerAnglesZyxNed(const guidance::EulerAnglesZyx &angles, const guidance::EulerAnglesZyx &derivatives)#

Set front camera angles and derivative angles (to be called from generateFcamAttitudeReference) in NED

Parameters:
  • angles – are euler angles

  • derivatives – are 1st derivatives of euler angles

void setFcamEulerAnglesZyxNedStart(const guidance::EulerAnglesZyx &angles, const guidance::EulerAnglesZyx &derivatives)#

Set front camera angles and derivative angles (to be called from generateFcamAttitudeReference) in NED Start

Parameters:
  • angles – are euler angles

  • derivatives – are 1st derivatives of euler angles

virtual void generateFcamAttitudeReference()#

Called at the beginning of generateAttitudeReference to let derived class fill mFcamEulerAnglesNedStartRef, mFcamEulerAnglesDerivativesNedStartRef, mFcamAcceleration and mFcamZoom.

virtual const guidance::PositionTrajectoryPoint &processObstacleAvoidance()#

Process obstacle avoidance algorithm. This method is usually called by correctDroneReference. It can be overrided to change obstacle avoidance behavior.

void enableObstacleAvoidance(bool enable)#

Enable or disable obstacle avoidance

Parameters:

enable – true to enable obstacle avoidance, false otherwise

bool isObstacleAvoidanceEnabled()#

Get obstacle avoidance status

Returns:

True if obstacle avoidance is enabled

inline uint32_t getTickPeriod() const#
inline float getTimePeriod() const#
inline const PositionTrajectoryPoint &getDroneNominalPositionTrajectoryLocal() const#
inline const PositionTrajectoryPoint &getDroneReferencePositionTrajectoryLocal() const#
float getMaxHorizontalSpeed() const#

Return the maximum admissible horizontal speed [m/s] (> 0)

float getMaxDescentSpeed() const#

Return the maximum admissible descent speed [m/s] (> 0)

float getMaxAscentSpeed() const#

Return the maximum admissible ascent speed [m/s] (> 0)

virtual bool hasObstacleAvoidance() override#

Return whether the mode implements obstacle avoidance Unless this function is overriden, modes do not advertise they implement obstacle avoidance. TODO: Add to mode descriptor

virtual void getTriggers(uint32_t *triggers, uint32_t *timeout, uint32_t *period) override#

Get the triggers for the mode

Parameters:
  • triggers – mask of Trigger values. TRIGGER_TICK can not be combined with TRIGGER_TIMER.

  • timeout – if no frames received during this time (in ms), force a step.

  • period – if a timer is required, its period (in ms). If there is also a trigger on frame with a timeout specified, it will only be called when there is no frame. For tick trigger, this is the period of wakeup measured in ticks of the Drone Controller. For a 200Hz tick in drone controller this means a increment of 5ms per tick.

virtual void configure(const ::google::protobuf::Any &config, bool disableObstacleAvoidance, bool overrideFrontCamera, bool overrideStereoCamera) override#

Called when the mode is set with a configuration

virtual void enter() override#

Called when the mode is activated and become the current one.

virtual void beginStep() override#

Start a new step of the guidance The input data is valid at this point

virtual void correctDroneReference() override#

Correct the generated trajectory using obstacle avoidance data.

virtual void generateAttitudeReferences() override#

Generate the front camera, stereo camera and drone attitudes references. The camera references do not depend on the drone ones, but will be saturated if not reachable.

virtual void endStep() override#

Finish a step of the guidance Output data shall be valid after this point

virtual void exit() override#

Called when the mode is de-activated.

Public Static Functions

static int init(Guidance *guidance)#
static void destroy()#

Protected Functions

void setFramer(AbsoluteFramer *framer)#

Set the Framer object.

Warning

setFramer is unsafe. Need to call setFramer(nullptr) in destructor of child class. Should be called only in constructor and destructor of child class

Parameters:

framer – New framer

void setPlanner(AbsolutePlanner *planner)#

Set the Planner object.

In order to compute vertical/horizontal ideal model and target reached for autonomous flight

Warning

setPlanner is unsafe. Need to call setPlanner(nullptr) in destructor of child class. Should be called only in constructor and destructor of child class

Parameters:

planner – New planner

bool updateResetStatus()#

Update the timer for checking if a mode should be reset.

This function should be called in each TrajectoryFcamMode mode

  • in the configure() function to decide wether the mode should be reset

  • in the beginStep() function to update the timer

Returns:

true if the mode should be reset

void updateTelemetry()#

Update variables from latest telemetry.

Convert estimated position trajectory point from global to local. Convert estimated drone Euler angles from NED start to NED.

Protected Attributes

bool mFcamGroundConstraintEnabled#
float mFcamYawAccelerationNedStartRef#
float mFcamZoom#
::ColibryLite::Messages::HorizontalControlConfig::Enum mDroneHorizontalControlConfig#
::ColibryLite::Messages::VerticalControlConfig::Enum mDroneVerticalControlConfig#
::ColibryLite::Messages::YawControlConfig::Enum mDroneYawControlConfig#
::Guidance::Messages::msghub::EventSender mGuidanceEventSender#

Protected Static Attributes

static timespec sLastCallTimestamp#
static std::unique_ptr<boonta::IElcanoMpc> sReplannerLocal#
static unsigned int sReplannerReferenceStatus#
static bool sIsReplannerStuck = false#
static bool sHasReplannerFailed = false#
static Eigen::Vector3f sClosestObstaclePositionLocal#
static float sClosestObstacleSignedDistance#
static bool sIsInitialized = false#
static guidance::PositionTrajectoryPoint sDronePositionStateLocalEst#
static guidance::PositionTrajectoryPoint sDronePositionStateLocalNom#
static guidance::PositionTrajectoryPoint sDronePositionStateLocalRef#
static guidance::EulerAnglesZyx sDroneEulerAnglesNedEst#
static guidance::EulerAnglesZyx sDroneEulerAnglesNedStartEst#
static Eigen::Vector3f sDroneYawStateNedStartRef#
static float sDroneMaxHorizSpeed#
static float sDroneMinVerticalSpeed#
static float sDroneMaxVerticalSpeed#
static guidance::EulerAnglesZyx sStcamEulerAnglesNedStartRef#
class TrajectoryStcamMode : public guidance::Mode#

Public Functions

TrajectoryStcamMode(Guidance *guidance, const std::string &name, ModeGroup *group = nullptr)#
virtual ~TrajectoryStcamMode() override#
void setDroneNominalPositionTrajectoryGlobal(const guidance::PositionTrajectoryPoint &point)#

Set nominal global drone trajectory (to be called from generateDroneReference)

void setDroneNominalPositionTrajectoryLocal(const guidance::PositionTrajectoryPoint &point)#

Set nominal local drone trajectory (to be called from generateDroneReference)

void setFcamEulerAnglesZyxNed(const guidance::EulerAnglesZyx &angles, const guidance::EulerAnglesZyx &derivatives)#

Set front camera angles and derivative angles (to be called from generateFcamAttitudeReference) in NED

Parameters:
  • angles – are euler angles

  • derivatives – are 1st derivatives of euler angles

void setFcamEulerAnglesZyxNedStart(const guidance::EulerAnglesZyx &angles, const guidance::EulerAnglesZyx &derivatives)#

Set front camera angles and derivative angles (to be called from generateFcamAttitudeReference) in NED Start

Parameters:
  • angles – are euler angles

  • derivatives – are 1st derivatives of euler angles

void setStcamEulerAnglesZyxNed(const guidance::EulerAnglesZyx &angles, const guidance::EulerAnglesZyx &derivatives)#

Set stereo camera angles and derivative angles (to be called from generateStcamAttitudeReference) in NED

Parameters:
  • angles – are euler angles

  • derivatives – are 1st derivatives of euler angles

void setStcamEulerAnglesZyxNedStart(const guidance::EulerAnglesZyx &angles, const guidance::EulerAnglesZyx &derivatives)#

Set stereo camera angles and derivative angles (to be called from generateStcamAttitudeReference) in NED Start

Parameters:
  • angles – are euler angles

  • derivatives – are 1st derivatives of euler angles

float getMaxHorizontalSpeed() const#

Return the maximum admissible horizontal speed [m/s] (> 0)

float getMaxDescentSpeed() const#

Return the maximum admissible descent speed [m/s] (> 0)

float getMaxAscentSpeed() const#

Return the maximum admissible ascent speed [m/s] (> 0)

virtual void generateFcamAttitudeReference()#

Called at the beginning of generateAttitudeReferences to let derived class fill mFcamEulerAnglesNedStartRef, mFcamEulerAnglesDerivativesNedStartRef, mFcamAcceleration and mFcamZoom.

virtual void generateStcamAttitudeReference() = 0#

Called at the beginning of generateAttitudeReferences to let derived class fill mStcamEulerAnglesNedStartRef, mStcamEulerAnglesDerivativesNedStartRef and mStcamAcceleration.

inline uint32_t getTickPeriod() const#
inline float getTimePeriod() const#
inline const PositionTrajectoryPoint &getDroneNominalPositionTrajectoryLocal() const#
inline const PositionTrajectoryPoint &getDroneReferencePositionTrajectoryLocal() const#
virtual bool hasObstacleAvoidance() override#

Return whether the mode implements obstacle avoidance Unless this function is overriden, modes do not advertise they implement obstacle avoidance. TODO: Add to mode descriptor

virtual void getTriggers(uint32_t *triggers, uint32_t *timeout, uint32_t *period) override#

Get the triggers for the mode

Parameters:
  • triggers – mask of Trigger values. TRIGGER_TICK can not be combined with TRIGGER_TIMER.

  • timeout – if no frames received during this time (in ms), force a step.

  • period – if a timer is required, its period (in ms). If there is also a trigger on frame with a timeout specified, it will only be called when there is no frame. For tick trigger, this is the period of wakeup measured in ticks of the Drone Controller. For a 200Hz tick in drone controller this means a increment of 5ms per tick.

virtual void configure(const ::google::protobuf::Any &config, bool disableObstacleAvoidance, bool overrideFrontCamera, bool overrideStereoCamera) override#

Called when the mode is set with a configuration

virtual void beginStep() override#

Start a new step of the guidance The input data is valid at this point

virtual void correctDroneReference() override#

Correct the generated trajectory using obstacle avoidance data.

virtual void generateAttitudeReferences() override#

Generate the front camera, stereo camera and drone attitudes references. The camera references do not depend on the drone ones, but will be saturated if not reachable.

virtual void endStep() override#

Finish a step of the guidance Output data shall be valid after this point

Protected Functions

void setFramer(AbsoluteFramer *framer)#

Set the Framer object.

In order to compute yaw ideal model and target reached for autonomous flight

Warning

setFramer is unsafe. Need to call setFramer(nullptr) in destructor of child class. Should be called only in constructor and destructor of child class

Parameters:

framer – New framer

void setPlanner(AbsolutePlanner *planner)#

Set the Planner object.

In order to compute vertical/horizontal ideal model and target reached for autonomous flight

Warning

setPlanner is unsafe. Need to call setPlanner(nullptr) in destructor of child class. Should be called only in constructor and destructor of child class

Parameters:

planner – New planner

Protected Attributes

bool mStcamGroundConstraintEnabled#
float mStcamYawAccelerationNedStartRef#
float mFcamZoom#

Native specific#

It is possible to declare from one to several modes in a .cpp file. The files where modes are implemented must contain two functions that allow to use them:

static FirstMode *sFirstMode;
static SecondMode *sSecondMode;

void guidance_plugin_init(guidance::Guidance *guidance)
{
    sFirstMode = guidance->registerMode<FirstMode>();;
    sSecondMode = guidance->registerMode<SecondMode>();
}

void guidance_plugin_shutdown(guidance::Guidance *guidance)
{
    guidance->unregisterMode(sFirstMode);
    guidance->unregisterMode(sSecondMode);
}

Python specific#

It is possible to declare from one to several modes in a .py file. The files where modes are implemented must contain a dictionary that allows to use them.

GUIDANCE_MODES = {
    'first_mode': FirstModeClass,
    'second_mode': SecondModeClass
}

All methods from guidance::Mode class use snake case instead of camel case, except for the guidance::Mode::getName() function which does not exist in Python.

Example:

Native method

void getTriggers(uint32_t *triggers, uint32_t *timeout, uint32_t *period)
{
    *triggers = guidance::TRIGGER_TICK;
    *timeout = 0;
    *period = 6;
}

becomes

def get_triggers(self):
    return (gdnc_core.Trigger.TICK, 0, 6)