Program Listing for File psdk_wrapper.hpp

↰ Return to documentation for file (include/psdk_wrapper/psdk_wrapper.hpp)

/*
 * Copyright (C) 2023 Unmanned Life
 * This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at http://mozilla.org/MPL/2.0/.
 */

#ifndef PSDK_WRAPPER_INCLUDE_PSDK_WRAPPER_PSDK_WRAPPER_HPP_
#define PSDK_WRAPPER_INCLUDE_PSDK_WRAPPER_PSDK_WRAPPER_HPP_

#include <dji_aircraft_info.h>
#include <dji_core.h>
#include <dji_logger.h>
#include <dji_platform.h>
#include <dji_typedef.h>
#include <hal_network.h>               //NOLINT
#include <hal_uart.h>                  //NOLINT
#include <hal_usb_bulk.h>              //NOLINT
#include <osal.h>                      //NOLINT
#include <osal_fs.h>                   //NOLINT
#include <osal_socket.h>               //NOLINT
#include <utils/dji_config_manager.h>  //NOLINT

#include <cmath>
#include <map>
#include <memory>
#include <nav_msgs/msg/odometry.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_lifecycle/lifecycle_node.hpp>
#include <string>

// PSDK wrapper interfaces

#include "psdk_wrapper/modules/camera.hpp"
#include "psdk_wrapper/modules/flight_control.hpp"
#include "psdk_wrapper/modules/gimbal.hpp"
#include "psdk_wrapper/modules/hms.hpp"
#include "psdk_wrapper/modules/liveview.hpp"
#include "psdk_wrapper/modules/perception.hpp"
#include "psdk_wrapper/modules/telemetry.hpp"
#include "psdk_wrapper/utils/psdk_wrapper_utils.hpp"

namespace psdk_ros2
{
class PSDKWrapper : public rclcpp_lifecycle::LifecycleNode
{
 public:
  using CallbackReturn =
      rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
  explicit PSDKWrapper(const std::string& node_name);

  ~PSDKWrapper();

  CallbackReturn on_configure(const rclcpp_lifecycle::State& state) override;

  CallbackReturn on_activate(const rclcpp_lifecycle::State& state) override;

  CallbackReturn on_deactivate(const rclcpp_lifecycle::State& state) override;

  CallbackReturn on_cleanup(const rclcpp_lifecycle::State& state) override;

  CallbackReturn on_shutdown(const rclcpp_lifecycle::State& state) override;

 private:
  struct PSDKParams
  {
    std::string app_name;
    std::string app_id;
    std::string app_key;
    std::string app_license;
    std::string developer_account;
    std::string baudrate;
    std::string link_config_file_path;
  };
  template <typename ModuleType>
  void create_module(bool is_mandatory,
                     std::shared_ptr<ModuleType>& module_ptr,         // NOLINT
                     std::unique_ptr<utils::NodeThread>& thread_ptr,  // NOLINT
                     const std::string& node_name,
                     std::shared_ptr<ModuleType>& global_ptr);  // NOLINT

  template <typename ModuleType>
  void create_module(bool is_mandatory,
                     std::shared_ptr<ModuleType>& module_ptr,         // NOLINT
                     std::unique_ptr<utils::NodeThread>& thread_ptr,  // NOLINT
                     const std::string& node_name);

  template <typename ModuleType>
  void stop_and_destroy_module(
      bool is_mandatory, std::shared_ptr<ModuleType>& module_ptr,  // NOLINT
      std::unique_ptr<utils::NodeThread>& thread_ptr);             // NOLINT

  template <typename ModuleType>
  bool initialize_module(bool is_mandatory,
                         std::shared_ptr<ModuleType>& module_ptr);  // NOLINT

  bool set_environment();

  bool set_user_info(T_DjiUserInfo* user_info);

  void load_parameters();

  bool init(T_DjiUserInfo* user_info);

  bool initialize_psdk_modules();

  void get_and_validate_frequency(const std::string& param_name,
                                  int& frequency,            // NOLINT
                                  const int max_frequency);  // NOLINT

  void get_non_mandatory_param(const std::string& param_name,  // NOLINT
                               std::string& param_string);     // NOLINT
  void get_mandatory_param(const std::string& param_name,  // NOLINT
                           std::string& param_string);     // NOLINT

  enum class LifecycleState
  {
    CONFIGURE,
    ACTIVATE,
    DEACTIVATE,
    CLEANUP,
    SHUTDOWN
  };

  bool transition_modules_to_state(LifecycleState state);

  /* Global variables */
  PSDKParams params_;
  rclcpp::Node::SharedPtr node_;
  T_DjiAircraftInfoBaseInfo aircraft_base_info_;

  int num_of_initialization_retries_{0};

  bool is_telemetry_module_mandatory_{true};
  bool is_camera_module_mandatory_{true};
  bool is_gimbal_module_mandatory_{true};
  bool is_flight_control_module_mandatory_{true};
  bool is_liveview_module_mandatory_{true};
  bool is_hms_module_mandatory_{true};
  bool is_perception_module_mandatory_{true};

  std::shared_ptr<FlightControlModule> flight_control_module_;
  std::shared_ptr<TelemetryModule> telemetry_module_;
  std::shared_ptr<CameraModule> camera_module_;
  std::shared_ptr<LiveviewModule> liveview_module_;
  std::shared_ptr<GimbalModule> gimbal_module_;
  std::shared_ptr<HmsModule> hms_module_;
  std::shared_ptr<PerceptionModule> perception_module_;

  std::unique_ptr<utils::NodeThread> flight_control_thread_;
  std::unique_ptr<utils::NodeThread> telemetry_thread_;
  std::unique_ptr<utils::NodeThread> camera_thread_;
  std::unique_ptr<utils::NodeThread> liveview_thread_;
  std::unique_ptr<utils::NodeThread> gimbal_thread_;
  std::unique_ptr<utils::NodeThread> hms_thread_;
  std::unique_ptr<utils::NodeThread> perception_thread_;

  bool is_core_initialized_{false};
};

}  // namespace psdk_ros2

#endif  // PSDK_WRAPPER_INCLUDE_PSDK_WRAPPER_PSDK_WRAPPER_HPP_