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
-
enumerator TRIGGER_TIMER#
-
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.
-
enumerator CHANNEL_KIND_GUIDANCE#
-
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#
-
enumerator SENDER_KIND_DRONE_CONTROLLER#
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.
-
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.
-
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.
-
inline Guidance()#
-
class Input#
Guidance input
The active mode has access to an instance of Input, which holds the current inputs at each step.
-
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#
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#
-
using HorizontalReference = DroneController::Messages::HorizontalReference#
-
class 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 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.
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#
-
typedef Eigen::Matrix<float, 3, -1, Eigen::RowMajor> PointsVector#
-
class TrajectoryFcamMode : public guidance::Mode#
Subclassed by RelativeMoveMode
Public Functions
-
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.
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#
-
virtual ~TrajectoryFcamMode() override#
-
class TrajectoryStcamMode : public guidance::Mode#
Public Functions
-
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.
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
-
virtual ~TrajectoryStcamMode() override#
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)