Mode Interface

The interface for the autopilot operating modes is defined in the autopilot package under the pegasus_autopilot/autopilot/include/autopilot/mode.hpp header file. The Mode class is an abstract class that defines the interface for the autopilot operating modes.

The autopilot operating modes must inherit from the Mode class and implement the methods highlighted in yellow (lines 42-47) in the code snippet below:

 1namespace autopilot {
 2
 3class Mode {
 4
 5public:
 6
 7    using SharedPtr = std::shared_ptr<Mode>;
 8    using UniquePtr = std::unique_ptr<Mode>;
 9    using WeakPtr = std::weak_ptr<Mode>;
10
11    // Configuration for the operation mode
12    struct Config {
13        rclcpp::Node::SharedPtr node;                                           // ROS 2 node ptr (in case the mode needs to create publishers, subscribers, etc.)
14        std::function<State()> get_vehicle_state;                               // Function pointer to get the current state of the vehicle      
15        std::function<VehicleStatus()> get_vehicle_status;                      // Function pointer to get the current status of the vehicle  
16        std::function<VehicleConstants()> get_vehicle_constants;                // Function pointer to get the current dynamical constants of the vehicle    
17        std::function<void()> signal_mode_finished;                             // Function pointer to signal that the mode has finished operating
18        Controller::SharedPtr controller;                                       // Controller to be used by the mode
19        TrajectoryManager::SharedPtr trajectory_manager;                        // Trajectory manager that can be used by some modes
20    };
21
22    // Custom constructor like function - as we must have the default constructor for the pluginlib
23    inline void initialize_mode(const Mode::Config & config) {
24
25        // Initialize the base class
26        node_ = config.node;
27        get_vehicle_state = config.get_vehicle_state;
28        get_vehicle_status = config.get_vehicle_status;
29        get_vehicle_constants = config.get_vehicle_constants;        
30        signal_mode_finished = config.signal_mode_finished;
31
32        // Initialize the controller
33        controller_ = config.controller;
34
35        // Initialize the trajectory manager
36        trajectory_manager_ = config.trajectory_manager;
37
38        // Initialize the derived class
39        initialize();
40    }
41
42    // Methods that can be implemented by derived classes
43    // that are executed by the state machine when entering, exiting or updating the mode
44    virtual void initialize() = 0;
45    virtual bool enter() = 0;
46    virtual bool exit() = 0;
47    virtual void update(double dt) = 0;
48
49protected:
50
51    // The ROS 2 node
52    rclcpp::Node::SharedPtr node_{nullptr};
53
54    // The controller object that can be used by each mode to track references
55    Controller::SharedPtr controller_{nullptr};
56
57    // The trajectory manager object that can be used by each mode to generate references to track
58    TrajectoryManager::SharedPtr trajectory_manager_{nullptr};
59
60    // Function pointer which will be instantiated with the function pointers passed in the configuration
61    std::function<State()> get_vehicle_state{nullptr};
62    std::function<VehicleStatus()> get_vehicle_status{nullptr};
63    std::function<VehicleConstants()> get_vehicle_constants{nullptr};
64
65    // Function pointer to signal that the mode has finished operating
66    std::function<void()> signal_mode_finished{nullptr};
67};
68
69} // namespace autopilot

Additionally, the Mode class provides methods to get:

  1. The current State of the vehicle can be accessed by calling get_vehicle_state() . It returns the following struct:

1namespace autopilot {
2// State of the vehicle
3struct State {
4    Eigen::Vector3d position{Eigen::Vector3d::Zero()};           // Position of the vehicle (FRD) in the world frame NED
5    Eigen::Vector3d velocity{Eigen::Vector3d::Zero()};           // Velocity of the vehicle (FRD) with respect to the world frame NED expressed in the world frame NED
6    Eigen::Quaterniond attitude{1.0, 0.0, 0.0, 0.0};             // Attitude of the vehicle (FRD) with respect to the world frame NED expressed in the world frame NED
7    Eigen::Vector3d angular_velocity{Eigen::Vector3d::Zero()};   // Angular velocity of the vehicle (FRD) with respect to the world frame NED expressed in the body frame FRD
8};

  1. The Status of the vehicle can be accessed by calling get_vehicle_status() . It returns the following struct:

1namespace autopilot {
2// Status of the vehicle
3struct VehicleStatus {
4    bool armed{false};      // Whether the vehicle is armed
5    bool flying{false};     // Whether the vehicle is flying
6    bool offboard{false};   // Whether the vehicle is in offboard mode
7};

  1. The Constants such as mass or thrust curve can be accessed by calling get_vehicle_constants . It returns the following struct:

1namespace autopilot {
2// Dynamical constants of the vehicle
3struct VehicleConstants {
4    int id{0};                                                                // ID of the vehicle         
5    double mass{0.0};                                                         // Mass of the vehicle (in Kg)
6    std::string thrust_curve_id{"None"};                                      // Thrust curve of the vehicle
7    std::vector<std::string> thurst_curve_params{std::vector<std::string>()}; // Thrust curve parameters
8    std::vector<double> thrust_curve_values{std::vector<double>()};           // Thrust curve values associated with the parameters

This means that to access these values, you do not need to manually subscribe to the ROS 2 topics. The autopilot takes care of that for you. However, if you are required to access other values that are not provided by the Mode class, or if you need to publish values to the ROS 2 topics, you can do so by using the node_ shared pointer that is a member of the Mode class.

4. The Mode class also provides a shared pointer to a controller_ object that can be used to access the controller interface. To check the available methods in the controller interface, please refer to the Controllers page. This is useful if you want to send control commands to the vehicle without having to publish to the ROS 2 topics directly. The autopilot takes care of that for you.

5. The Mode class also provides a shared pointer to a trajectory_manager_ object that can be used to access the trajectory manager interface. To check the available methods in the trajectory manager interface, please refer to the Trajectory Manager page. This is useful if your mode needs to follow a trajectory and you don’t want to manually subscribe to the ROS 2 topics or hard-code the trajectory in the mode.