flight_control_interface.hpp

/****************************************************************************
 * Copyright (c) 2018 John A. Dougherty. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name ATLFlight nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS LICENSE.
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * In addition Supplemental Terms apply.  See the SUPPLEMENTAL file.
 ****************************************************************************/


#ifndef SNAV_FCI_FLIGHT_CONTROL_INTERFACE_HPP_
#define SNAV_FCI_FLIGHT_CONTROL_INTERFACE_HPP_

#include <atomic>
#include <condition_variable>
#include <cstdbool>
#include <mutex>
#include <string>
#include <thread>

#include <Eigen/Geometry>

#include "snav/snapdragon_navigator.h"

#include "api/waypoint.hpp"
#include "api/config/landing_config.hpp"
#include "api/config/rx_config.hpp"
#include "api/config/takeoff_config.hpp"
#include "api/config/tx_config.hpp"
#include "api/config/waypoint_config.hpp"
#include "imp/inc/snav_cached_data_thread_safe.hpp"
#include "imp/inc/state_info.hpp"
#include "imp/inc/planner.hpp"
#include "imp/inc/transform.hpp"
#include "imp/inc/tx_command_thread_safe.hpp"

namespace snav_fci
{

class FlightControlInterface
{
public:
  enum class Permissions
  {
    READ_WRITE, 
    READ_ONLY,  
  };

  enum class Return
  {
    SUCCESS, 
    FAILURE, 
    NOT_CONNECTED, 
    ALREADY_CONNECTED, 
    TRAJ_NOT_OPTIMIZED, 
    TRAJ_NOT_FEASIBLE, 
    ACTION_PREEMPTED, 
    NO_LOCK, 
  };

  enum class Action
  {
    NONE, 
    TAKEOFF, 
    GO_TO_WAYPOINT, 
    EXECUTE_MISSION, 
    LAND, 
  };

  explicit FlightControlInterface(const Permissions& requested_access);

  ~FlightControlInterface() noexcept;

  Return configure_tx(const TxConfig& tx_config);

  Return configure_rx(const RxConfig& rx_config);

  void wait_for_configure() const noexcept;

  Return connect();

  Return disconnect() noexcept;

  Return start_props();

  Return stop_props();

  Return takeoff(const TakeoffConfig& takeoff_config);

  Return takeoff();


  Return takeoff_nb(const TakeoffConfig& takeoff_config);

  Return takeoff_nb();

  Return land(const LandingConfig& landing_config);

  Return land();

  Return land_nb(const LandingConfig& landing_config);

  Return land_nb();

  Return go_to_waypoint(const Waypoint& waypoint);

  Return go_to_waypoint_nb(const Waypoint& waypoint);

  Return configure_planner(const PlannerConfig& config);

  Return preload_waypoints(const std::vector<Waypoint>& waypoints);

  Return compute_trajectory(StateVector starting_state);

  Return execute_mission(const double t_start = 0);

  Return execute_mission_nb(const double t_start = 0);

  void wait_on_action() const noexcept;

  Permissions get_permissions() const noexcept;

  Return get_last_action_result() const noexcept;

  Action get_last_action() const noexcept;

  Action get_current_action() const noexcept;

  Return get_current_trajectory(std::vector<snav_traj_gen::SnavTrajectory>& traj) const;

  Return get_last_optimized_trajectory(std::vector<snav_traj_gen::SnavTrajectory>& traj) const;

  Return get_waypoints(std::vector<Waypoint>& waypoints) const;

  Return get_optimized_waypoints(std::vector<Waypoint>& waypoints) const;

  Planner::Status get_planner_status() const;

  float get_trajectory_time() const;

  Return set_waypoint_frame_tf(const Eigen::Quaternionf& q_pw,
      const Eigen::Vector3f& t_pw);

  Return get_waypoint_frame_tf(Eigen::Quaternionf& q_pw,
      Eigen::Vector3f& t_pw) const;

  Return set_tx_command(const TxCommand& command);

  SnavCachedData get_snav_cached_data() const;

  StateVector get_estimated_state(const SnavCachedData& snav_data) const;

  StateVector get_desired_state(const SnavCachedData& snav_data) const;

  Return convert_velocity_to_rc_command(const std::array<float, 3> velocity,
      const float yaw_rate, RcCommand& rc_command);

  static void preempt_current_action();

  inline static void set_verbosity(bool verbose)
  {
    verbose_ = verbose;
  }

  inline static bool tx_ok()
  {
    return tx_ok_;
  }

  inline static bool rx_ok()
  {
    return rx_ok_;
  }

  inline static bool ok()
  {
    return rx_ok_ && tx_ok_;
  }

  static std::string get_return_as_string(Return ret_code);

private:

  // Disallow copy and assign
  FlightControlInterface(const FlightControlInterface&) = delete;
  FlightControlInterface& operator=(const FlightControlInterface&) = delete;

  static void tx_loop();
  static void update_waypoint_frame_parent_transform();
  static void send_tx_command();
  static void rx_loop();
  static void check_tx_conditions();
  static float constrain_min_max(float input, const float min, const float max);
  static bool has_closed_loop_position_control(SnRcCommandType);
  static void print_error(const std::string& message);
  static void print_error(const std::string& origin, const std::string& message);
  static void print_warning(const std::string& message);
  static void print_warning(const std::string& origin, const std::string& message);
  static void print_info(const std::string& message);
  static void print_message(const std::string& prefix, const std::string& origin,
      const std::string& message);
  static void enter_action(Action action);
  static void exit_action(const Return& ret_code);

  void enforce_init_precondition() const;
  void enforce_connected_precondition() const;
  void enforce_write_precondition() const;
  void enforce_in_flight_precondition() const;
  void enforce_no_action_in_progress_precondition() const;
  void enforce_tx_connection_invariant() const;
  int infer_metadata_from_mode(const SnMode, const bool, TxCommand::Mode&, SnInputCommandType&,
      SnRcCommandType&);
  void enforce_props_state_constraint(const SnPropsState);
  void do_not_enforce_props_state_constraint();
  void update_tx_command_pos_ctrl();
  Return takeoff_impl(std::unique_lock<std::mutex>& lock, const TakeoffConfig& config);
  Return land_impl(std::unique_lock<std::mutex>& lock, const LandingConfig& config);
  Return go_to_waypoint_impl(std::unique_lock<std::mutex>& lock, const Waypoint& wp);
  Return execute_mission_impl(std::unique_lock<std::mutex>& lock, const double t_start);
  Return execute_planner(std::unique_lock<std::mutex>& lock, const double t_start = 0);

  // wrappers around functions to catch exceptions; safe for threads
  void takeoff_safe() noexcept;
  void takeoff_safe(const TakeoffConfig&) noexcept;
  void land_safe() noexcept;
  void land_safe(const LandingConfig&) noexcept;
  void go_to_waypoint_safe(const Waypoint&) noexcept;
  void execute_mission_safe(const double t_start) noexcept;

  static std::mutex connection_mutex_;
  static std::mutex command_mutex_;
  static TxConfig tx_config_;
  static bool configured_tx_;
  static RxConfig rx_config_;
  static bool configured_rx_;
  static std::condition_variable configured_rx_cv_;
  static TxCommandThreadSafe tcts_;
  static TransformThreadSafe tf_ep_;  // ESTIMATION <-> (parent of WAYPOINT)
  static TransformThreadSafe tf_pw_;  // (parent of WAYPOINT) <-> WAYPOINT
  static TransformThreadSafe tf_ev_;  // ESTIMATION <-> snav VIO frame
  static StateVector desired_setpoint_wrt_e_; // Desired state w.r.t. ESTIMATION
  static StateVector desired_setpoint_wrt_w_; // Desired state w.r.t. WAYPOINT
  static SnavCachedDataThreadSafe scdts_;
  static StateInfoThreadSafe sits_;
  static std::thread tx_thread_;
  static std::atomic_bool tx_ok_;
  static std::thread rx_thread_;
  static std::atomic_bool rx_ok_;
  static bool write_access_taken_;
  static int num_connections_;
  static std::atomic_bool verbose_;
  // system_clock is used here instead of steady_clock so that an absolute
  // timestamp relative to the wall clock can be provided to the execute
  // mission function, which is useful for synchronizing multiple robots. Also,
  // system_clock may be continually adjusted to maintain synchronization among
  // a multi-robot system where clock drift may be significant over the course
  // of a mission. However, stepping the system_clock during a mission could
  // cause undefined behavior. Be careful! It may make sense to make this
  // configurable and only use system_clock if the absolute time sync is
  // required.
  static std::chrono::time_point<std::chrono::system_clock> t0_;
  static std::mutex time_mutex_;
  static Planner planner_;
  static std::thread action_thread_;
  static std::atomic<Action> current_action_;
  static std::atomic<Action> last_action_;
  static std::atomic<Return> last_action_result_;
  static std::atomic_bool kill_current_action_;
  static std::condition_variable action_cv_;
  static std::mutex action_mutex_;

  std::atomic_bool connected_;
  std::atomic<Permissions> permissions_;

};

} // namespace snav_fci

#endif // SNAV_FCI_FLIGHT_CONTROL_INTERFACE_HPP_