Program Listing for File camera.hpp

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

/*
 * Copyright (C) 2024 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_MODULES_CAMERA_HPP_
#define PSDK_WRAPPER_INCLUDE_PSDK_WRAPPER_MODULES_CAMERA_HPP_

#include <dji_camera_manager.h>  //NOLINT
#include <osal.h>                //NOLINT

#include <filesystem>
#include <fstream>
#include <iostream>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_lifecycle/lifecycle_node.hpp>
#include <shared_mutex>
#include <string>

#include "psdk_interfaces/action/camera_delete_file_by_index.hpp"
#include "psdk_interfaces/action/camera_download_file_by_index.hpp"
#include "psdk_interfaces/msg/file_attributes.hpp"
#include "psdk_interfaces/msg/file_info.hpp"
#include "psdk_interfaces/msg/sub_file_info.hpp"
#include "psdk_interfaces/srv/camera_format_sd_card.hpp"
#include "psdk_interfaces/srv/camera_get_aperture.hpp"
#include "psdk_interfaces/srv/camera_get_exposure_mode_ev.hpp"
#include "psdk_interfaces/srv/camera_get_file_list_info.hpp"
#include "psdk_interfaces/srv/camera_get_focus_mode.hpp"
#include "psdk_interfaces/srv/camera_get_focus_target.hpp"
#include "psdk_interfaces/srv/camera_get_iso.hpp"
#include "psdk_interfaces/srv/camera_get_laser_ranging_info.hpp"
#include "psdk_interfaces/srv/camera_get_optical_zoom.hpp"
#include "psdk_interfaces/srv/camera_get_sd_storage_info.hpp"
#include "psdk_interfaces/srv/camera_get_shutter_speed.hpp"
#include "psdk_interfaces/srv/camera_get_type.hpp"
#include "psdk_interfaces/srv/camera_record_video.hpp"
#include "psdk_interfaces/srv/camera_set_aperture.hpp"
#include "psdk_interfaces/srv/camera_set_exposure_mode_ev.hpp"
#include "psdk_interfaces/srv/camera_set_focus_mode.hpp"
#include "psdk_interfaces/srv/camera_set_focus_target.hpp"
#include "psdk_interfaces/srv/camera_set_infrared_zoom.hpp"
#include "psdk_interfaces/srv/camera_set_iso.hpp"
#include "psdk_interfaces/srv/camera_set_optical_zoom.hpp"
#include "psdk_interfaces/srv/camera_set_shutter_speed.hpp"
#include "psdk_interfaces/srv/camera_shoot_burst_photo.hpp"
#include "psdk_interfaces/srv/camera_shoot_interval_photo.hpp"
#include "psdk_interfaces/srv/camera_shoot_single_photo.hpp"
#include "psdk_interfaces/srv/camera_stop_shoot_photo.hpp"
#include "psdk_wrapper/utils/action_server.hpp"
#include "psdk_wrapper/utils/psdk_wrapper_utils.hpp"

namespace psdk_ros2
{

class CameraModule : public rclcpp_lifecycle::LifecycleNode
{
 public:
  using CameraShootSinglePhoto = psdk_interfaces::srv::CameraShootSinglePhoto;
  using CameraShootBurstPhoto = psdk_interfaces::srv::CameraShootBurstPhoto;
  using CameraShootIntervalPhoto =
      psdk_interfaces::srv::CameraShootIntervalPhoto;
  using CameraStopShootPhoto = psdk_interfaces::srv::CameraStopShootPhoto;
  using CameraRecordVideo = psdk_interfaces::srv::CameraRecordVideo;
  using CameraGetLaserRangingInfo =
      psdk_interfaces::srv::CameraGetLaserRangingInfo;
  using CameraGetFileListInfo = psdk_interfaces::srv::CameraGetFileListInfo;
  using CameraDownloadFileByIndex =
      psdk_interfaces::action::CameraDownloadFileByIndex;
  using CameraDeleteFileByIndex =
      psdk_interfaces::action::CameraDeleteFileByIndex;
  using CameraGetType = psdk_interfaces::srv::CameraGetType;
  using CameraSetExposureModeEV = psdk_interfaces::srv::CameraSetExposureModeEV;
  using CameraGetExposureModeEV = psdk_interfaces::srv::CameraGetExposureModeEV;
  using CameraSetShutterSpeed = psdk_interfaces::srv::CameraSetShutterSpeed;
  using CameraGetShutterSpeed = psdk_interfaces::srv::CameraGetShutterSpeed;
  using CameraSetISO = psdk_interfaces::srv::CameraSetISO;
  using CameraGetISO = psdk_interfaces::srv::CameraGetISO;
  using CameraSetFocusTarget = psdk_interfaces::srv::CameraSetFocusTarget;
  using CameraGetFocusTarget = psdk_interfaces::srv::CameraGetFocusTarget;
  using CameraSetFocusMode = psdk_interfaces::srv::CameraSetFocusMode;
  using CameraGetFocusMode = psdk_interfaces::srv::CameraGetFocusMode;
  using CameraSetOpticalZoom = psdk_interfaces::srv::CameraSetOpticalZoom;
  using CameraGetOpticalZoom = psdk_interfaces::srv::CameraGetOpticalZoom;
  using CameraSetInfraredZoom = psdk_interfaces::srv::CameraSetInfraredZoom;
  using CameraSetAperture = psdk_interfaces::srv::CameraSetAperture;
  using CameraGetAperture = psdk_interfaces::srv::CameraGetAperture;
  using CameraFormatSdCard = psdk_interfaces::srv::CameraFormatSdCard;
  using CameraGetSDStorageInfo = psdk_interfaces::srv::CameraGetSDStorageInfo;
  explicit CameraModule(const std::string& name);

  ~CameraModule();

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

  CallbackReturn on_activate(const rclcpp_lifecycle::State& state);
  CallbackReturn on_cleanup(const rclcpp_lifecycle::State& state);
  CallbackReturn on_deactivate(const rclcpp_lifecycle::State& state);
  CallbackReturn on_shutdown(const rclcpp_lifecycle::State& state);

  bool init();

  bool deinit();

  inline E_DjiCameraType
  get_attached_camera_type()
  {
    return attached_camera_type_;
  }

  std::string default_path_to_download_media_{"/logs/media"};

 private:
  friend T_DjiReturnCode c_camera_manager_download_file_data_callback(
      T_DjiDownloadFilePacketInfo packetInfo, const uint8_t* data,
      uint16_t len);
  bool get_camera_type(std::string* camera_type,
                       const E_DjiMountPosition index);

  /* ROS 2 Service callbacks */
  void camera_get_type_cb(
      const std::shared_ptr<CameraGetType::Request> request,
      const std::shared_ptr<CameraGetType::Response> response);
  void camera_set_exposure_mode_ev_cb(
      const std::shared_ptr<CameraSetExposureModeEV::Request> request,
      const std::shared_ptr<CameraSetExposureModeEV::Response> response);
  void camera_get_exposure_mode_ev_cb(
      const std::shared_ptr<CameraGetExposureModeEV::Request> request,
      const std::shared_ptr<CameraGetExposureModeEV::Response> response);
  void camera_set_shutter_speed_cb(
      const std::shared_ptr<CameraSetShutterSpeed::Request> request,
      const std::shared_ptr<CameraSetShutterSpeed::Response> response);
  void camera_get_shutter_speed_cb(
      const std::shared_ptr<CameraGetShutterSpeed::Request> request,
      const std::shared_ptr<CameraGetShutterSpeed::Response> response);
  void camera_set_iso_cb(
      const std::shared_ptr<CameraSetISO::Request> request,
      const std::shared_ptr<CameraSetISO::Response> response);
  void camera_get_iso_cb(
      const std::shared_ptr<CameraGetISO::Request> request,
      const std::shared_ptr<CameraGetISO::Response> response);
  void camera_set_focus_target_cb(
      const std::shared_ptr<CameraSetFocusTarget::Request> request,
      const std::shared_ptr<CameraSetFocusTarget::Response> response);
  void camera_get_focus_target_cb(
      const std::shared_ptr<CameraGetFocusTarget::Request> request,
      const std::shared_ptr<CameraGetFocusTarget::Response> response);
  void camera_set_focus_mode_cb(
      const std::shared_ptr<CameraSetFocusMode::Request> request,
      const std::shared_ptr<CameraSetFocusMode::Response> response);
  void camera_get_focus_mode_cb(
      const std::shared_ptr<CameraGetFocusMode::Request> request,
      const std::shared_ptr<CameraGetFocusMode::Response> response);
  void camera_set_optical_zoom_cb(
      const std::shared_ptr<CameraSetOpticalZoom::Request> request,
      const std::shared_ptr<CameraSetOpticalZoom::Response> response);
  void camera_get_optical_zoom_cb(
      const std::shared_ptr<CameraGetOpticalZoom::Request> request,
      const std::shared_ptr<CameraGetOpticalZoom::Response> response);
  void camera_set_infrared_zoom_cb(
      const std::shared_ptr<CameraSetInfraredZoom::Request> request,
      const std::shared_ptr<CameraSetInfraredZoom::Response> response);
  void camera_set_aperture_cb(
      const std::shared_ptr<CameraSetAperture::Request> request,
      const std::shared_ptr<CameraSetAperture::Response> response);
  void camera_get_aperture_cb(
      const std::shared_ptr<CameraGetAperture::Request> request,
      const std::shared_ptr<CameraGetAperture::Response> response);
  void camera_shoot_single_photo_cb(
      const std::shared_ptr<CameraShootSinglePhoto::Request> request,
      const std::shared_ptr<CameraShootSinglePhoto::Response> response);
  void camera_shoot_burst_photo_cb(
      const std::shared_ptr<CameraShootBurstPhoto::Request> request,
      const std::shared_ptr<CameraShootBurstPhoto::Response> response);
  void camera_shoot_interval_photo_cb(
      const std::shared_ptr<CameraShootIntervalPhoto::Request> request,
      const std::shared_ptr<CameraShootIntervalPhoto::Response> response);
  void camera_stop_shoot_photo_cb(
      const std::shared_ptr<CameraStopShootPhoto::Request> request,
      const std::shared_ptr<CameraStopShootPhoto::Response> response);
  void camera_record_video_cb(
      const std::shared_ptr<CameraRecordVideo::Request> request,
      const std::shared_ptr<CameraRecordVideo::Response> response);
  void camera_get_laser_ranging_info_cb(
      const std::shared_ptr<CameraGetLaserRangingInfo::Request> request,
      const std::shared_ptr<CameraGetLaserRangingInfo::Response> response);
  void camera_get_file_list_info_cb(
      const std::shared_ptr<CameraGetFileListInfo::Request> request,
      const std::shared_ptr<CameraGetFileListInfo::Response> response);
  void execute_download_file_by_index();
  void camera_format_sd_card_cb(
      const std::shared_ptr<CameraFormatSdCard::Request> request,
      const std::shared_ptr<CameraFormatSdCard::Response> response);
  void camera_get_sd_storage_info_cb(
      const std::shared_ptr<CameraGetSDStorageInfo::Request> request,
      const std::shared_ptr<CameraGetSDStorageInfo::Response> response);
  void execute_delete_file_by_index();
  psdk_interfaces::msg::FileInfo set_file_info(
      const T_DjiCameraManagerFileListInfo file_info);
  std::time_t get_unix_time(const T_DjiCameraManagerFileCreateTime& time);
  psdk_interfaces::msg::FileAttributes set_file_attributes(
      const T_DjiCameraManagerFileAttributeData& attributes);
  bool write_to_file(const uint8_t* data, uint16_t len);
  bool create_directory(const std::string& path);
  T_DjiReturnCode camera_manager_download_file_data_callback(
      T_DjiDownloadFilePacketInfo packetInfo, const uint8_t* data,
      uint16_t len);

  void register_file_data_callback(E_DjiMountPosition index);

  void obtain_downloader_rights(E_DjiMountPosition index);

  void release_downloader_rights(E_DjiMountPosition index);

  // Service servers
  rclcpp::Service<CameraShootSinglePhoto>::SharedPtr
      camera_shoot_single_photo_service_;
  rclcpp::Service<CameraShootBurstPhoto>::SharedPtr
      camera_shoot_burst_photo_service_;
  rclcpp::Service<CameraShootIntervalPhoto>::SharedPtr
      camera_shoot_interval_photo_service_;
  rclcpp::Service<CameraStopShootPhoto>::SharedPtr
      camera_stop_shoot_photo_service_;
  rclcpp::Service<CameraRecordVideo>::SharedPtr camera_record_video_service_;
  rclcpp::Service<CameraGetLaserRangingInfo>::SharedPtr
      camera_get_laser_ranging_info_service_;
  rclcpp::Service<CameraGetFileListInfo>::SharedPtr
      camera_get_file_list_info_service_;
  rclcpp::Service<CameraFormatSdCard>::SharedPtr camera_format_sd_card_service_;
  rclcpp::Service<CameraGetSDStorageInfo>::SharedPtr
      camera_get_sd_storage_info_service_;
  rclcpp::Service<CameraGetType>::SharedPtr camera_get_type_service_;
  rclcpp::Service<CameraSetExposureModeEV>::SharedPtr
      camera_set_exposure_mode_ev_service_;
  rclcpp::Service<CameraGetExposureModeEV>::SharedPtr
      camera_get_exposure_mode_ev_service_;
  rclcpp::Service<CameraSetShutterSpeed>::SharedPtr
      camera_set_shutter_speed_service_;
  rclcpp::Service<CameraGetShutterSpeed>::SharedPtr
      camera_get_shutter_speed_service_;
  rclcpp::Service<CameraSetISO>::SharedPtr camera_set_iso_service_;
  rclcpp::Service<CameraGetISO>::SharedPtr camera_get_iso_service_;
  rclcpp::Service<CameraSetFocusTarget>::SharedPtr
      camera_set_focus_target_service_;
  rclcpp::Service<CameraGetFocusTarget>::SharedPtr
      camera_get_focus_target_service_;
  rclcpp::Service<CameraSetFocusMode>::SharedPtr camera_set_focus_mode_service_;
  rclcpp::Service<CameraGetFocusMode>::SharedPtr camera_get_focus_mode_service_;
  rclcpp::Service<CameraSetOpticalZoom>::SharedPtr
      camera_set_optical_zoom_service_;
  rclcpp::Service<CameraGetOpticalZoom>::SharedPtr
      camera_get_optical_zoom_service_;
  rclcpp::Service<CameraSetInfraredZoom>::SharedPtr
      camera_set_infrared_zoom_service_;
  rclcpp::Service<CameraSetAperture>::SharedPtr camera_set_aperture_service_;
  rclcpp::Service<CameraGetAperture>::SharedPtr camera_get_aperture_service_;

  // Action servers
  std::unique_ptr<utils::ActionServer<CameraDeleteFileByIndex>>
      camera_delete_file_by_index_server_;
  std::unique_ptr<utils::ActionServer<CameraDownloadFileByIndex>>
      camera_download_file_by_index_server_;

  const rmw_qos_profile_t& qos_profile_{rmw_qos_profile_services_default};

  bool is_module_initialized_{false};
  E_DjiCameraType attached_camera_type_;
  int32_t file_index_to_download_{0};
  std::string file_name_to_download_;
  std::string file_path_to_download_;
  FILE* s_downloadMediaFile_ = NULL;

  mutable std::shared_mutex global_ptr_mutex_;
};

extern std::shared_ptr<CameraModule> global_camera_ptr_;

}  // namespace psdk_ros2

#endif  // PSDK_WRAPPER_INCLUDE_PSDK_WRAPPER_MODULES_CAMERA_HPP_