Get/set focus ring services

docs/documentation/ThePSDKWrapper.md

@@ -251,6 +251,9 @@ The following camera parameters can be set/get. Please note, that some of these
 | psdk_ros2/camera_get_focus_target           | Service   | 
 | psdk_ros2/camera_set_focus_mode             | Service   | 
 | psdk_ros2/camera_get_focus_mode             | Service   |
+| psdk_ros2/camera_get_focus_ring_range       | Service   |
+| psdk_ros2/camera_set_focus_ring_value       | Service   |
+| psdk_ros2/camera_get_focus_ring_value       | Service   |
 | psdk_ros2/camera_set_optical_zoom           | Service   | 
 | psdk_ros2/camera_get_optical_zoom           | Service   |      
 | psdk_ros2/camera_set_infrared_zoom          | Service   | 

psdk_interfaces/CMakeLists.txt

@@ -72,6 +72,9 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/CameraSetupStreaming.srv"
   "srv/CameraFormatSdCard.srv"
   "srv/CameraGetSDStorageInfo.srv"
+  "srv/CameraGetFocusRingRange.srv"
+  "srv/CameraGetFocusRingValue.srv"
+  "srv/CameraSetFocusRingValue.srv"
   "srv/GimbalSetMode.srv"
   "srv/GimbalReset.srv"
   "srv/SetHomeFromGPS.srv"

psdk_interfaces/srv/CameraGetFocusRingRange.srv

@@ -0,0 +1,8 @@
+#request
+uint8 payload_index 1       # see enum E_DjiMountPosition in dji_typedef.h
+
+---
+#response
+bool success
+int32 min_value
+int32 max_value

psdk_interfaces/srv/CameraGetFocusRingValue.srv

@@ -0,0 +1,7 @@
+#request
+uint8 payload_index 1       # see enum E_DjiMountPosition in dji_typedef.h
+
+---
+#response
+bool success
+int32 focus_ring_value

psdk_interfaces/srv/CameraSetFocusRingValue.srv

@@ -0,0 +1,7 @@
+#request
+uint8 payload_index 1            # see enum E_DjiMountPosition in dji_typedef.h
+uint16 value       # see enum E_DjiCameraManagerShutterSpeed in dji_camera_manager.h 
+                                 # potential inputs from 0 to 66 and 0xFF for unkown
+---
+#response
+bool success

psdk_wrapper/include/psdk_wrapper/modules/camera.hpp

@@ -45,6 +45,8 @@
 #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_focus_ring_range.hpp"
+#include "psdk_interfaces/srv/camera_get_focus_ring_value.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"
@@ -56,6 +58,7 @@
 #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_set_focus_ring_value.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"
@@ -100,6 +103,9 @@ class CameraModule : public rclcpp_lifecycle::LifecycleNode
   using CameraGetAperture = psdk_interfaces::srv::CameraGetAperture;
   using CameraFormatSdCard = psdk_interfaces::srv::CameraFormatSdCard;
   using CameraGetSDStorageInfo = psdk_interfaces::srv::CameraGetSDStorageInfo;
+  using CameraGetFocusRingRange = psdk_interfaces::srv::CameraGetFocusRingRange;
+  using CameraGetFocusRingValue = psdk_interfaces::srv::CameraGetFocusRingValue;
+  using CameraSetFocusRingValue = psdk_interfaces::srv::CameraSetFocusRingValue;
   /**
    * @brief Construct a new CameraModule object
    * @param node_name Name of the node
@@ -430,6 +436,33 @@ class CameraModule : public rclcpp_lifecycle::LifecycleNode
    * mounted position for which the request is made needs to be specified.
    * @param response CameraGetFileListInfo service response.
    */
+  void camera_get_focus_ring_range_cb(
+      const std::shared_ptr<CameraGetFocusRingRange::Request> request,
+      const std::shared_ptr<CameraGetFocusRingRange::Response> response);
+  /**
+   * @brief Request the range of the focus ring
+   * @param request CameraGetFocusRingRange service request. The camera
+   * mounted position for which the request is made needs to be specified.
+   * @param response CameraGetFocusRingRange service response.
+   */
+  void camera_get_focus_ring_value_cb(
+      const std::shared_ptr<CameraGetFocusRingValue::Request> request,
+      const std::shared_ptr<CameraGetFocusRingValue::Response> response);
+  /**
+   * @brief Request downloading of a file list
+   * @param request CameraGetFocusRingValue service request. The camera
+   * mounted position for which the request is made needs to be specified.
+   * @param response CameraGetFocusRingValue service response.
+   */
+  void camera_set_focus_ring_value_cb(
+      const std::shared_ptr<CameraSetFocusRingValue::Request> request,
+      const std::shared_ptr<CameraSetFocusRingValue::Response> response);
+  /**
+   * @brief Request downloading of a file list
+   * @param request CameraSetFocusRingValue service request. The camera
+   * mounted position for which the request is made needs to be specified.
+   * @param response CameraSetFocusRingValue service response.
+   */
   void camera_get_file_list_info_cb(
       const std::shared_ptr<CameraGetFileListInfo::Request> request,
       const std::shared_ptr<CameraGetFileListInfo::Response> response);
@@ -556,6 +589,12 @@ class CameraModule : public rclcpp_lifecycle::LifecycleNode
       camera_set_infrared_zoom_service_;
   rclcpp::Service<CameraSetAperture>::SharedPtr camera_set_aperture_service_;
   rclcpp::Service<CameraGetAperture>::SharedPtr camera_get_aperture_service_;
+  rclcpp::Service<CameraGetFocusRingRange>::SharedPtr
+      camera_get_focus_ring_range_service_;
+  rclcpp::Service<CameraGetFocusRingValue>::SharedPtr
+      camera_get_focus_ring_value_service_;
+  rclcpp::Service<CameraSetFocusRingValue>::SharedPtr
+      camera_set_focus_ring_value_service_;
 
   // Action servers
   std::unique_ptr<utils::ActionServer<CameraDeleteFileByIndex>>

psdk_wrapper/src/modules/camera.cpp

@@ -176,6 +176,21 @@ CameraModule::on_configure(const rclcpp_lifecycle::State &state)
       std::bind(&CameraModule::camera_get_aperture_cb, this,
                 std::placeholders::_1, std::placeholders::_2),
       qos_profile_);
+  camera_get_focus_ring_range_service_ = create_service<CameraGetFocusRingRange>(
+      "psdk_ros2/camera_get_focus_ring_range",
+      std::bind(&CameraModule::camera_get_focus_ring_range_cb, this,
+                std::placeholders::_1, std::placeholders::_2),
+      qos_profile_);
+  camera_get_focus_ring_value_service_ = create_service<CameraGetFocusRingValue>(
+      "psdk_ros2/camera_get_focus_ring_value",
+      std::bind(&CameraModule::camera_get_focus_ring_value_cb, this,
+                std::placeholders::_1, std::placeholders::_2),
+      qos_profile_);
+  camera_set_focus_ring_value_service_ = create_service<CameraSetFocusRingValue>(
+      "psdk_ros2/camera_set_focus_ring_value",
+      std::bind(&CameraModule::camera_set_focus_ring_value_cb, this,
+                std::placeholders::_1, std::placeholders::_2),
+      qos_profile_);
 
   // Camera action servers
   camera_download_file_by_index_server_ =
@@ -249,6 +264,9 @@ CameraModule::on_cleanup(const rclcpp_lifecycle::State &state)
   camera_format_sd_card_service_.reset();
   camera_get_sd_storage_info_service_.reset();
   camera_get_aperture_service_.reset();
+  camera_get_focus_ring_range_service_.reset();
+  camera_get_focus_ring_value_service_.reset();
+  camera_set_focus_ring_value_service_.reset();
 
   return CallbackReturn::SUCCESS;
 }
@@ -923,6 +941,101 @@ CameraModule::camera_get_aperture_cb(
   }
 }
 
+void
+CameraModule::camera_get_focus_ring_range_cb(
+    const std::shared_ptr<CameraGetFocusRingRange::Request> request,
+    const std::shared_ptr<CameraGetFocusRingRange::Response> response)
+{
+  E_DjiMountPosition payload_index =
+      static_cast<E_DjiMountPosition>(request->payload_index);
+  T_DjiCameraManagerRangeList range_list;
+
+  T_DjiReturnCode return_code =
+      DjiCameraManager_GetFocusRingRange(payload_index, &range_list);
+  if (return_code != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
+  {
+    RCLCPP_ERROR(
+        get_logger(),
+        "Getting focus ring range for camera with payload index %d failed, error "
+        "code :%ld",
+        payload_index, return_code);
+    response->success = false;
+    return;
+  }
+  else
+  {
+    RCLCPP_INFO(get_logger(),
+                "Got minValue = %d, maxValue = %d, for camera with payload index %d.",
+                range_list.minValue, range_list.maxValue, payload_index);
+    response->success = true;
+    response->min_value = range_list.minValue;
+    response->max_value = range_list.maxValue;
+    return;
+  }
+}
+
+void
+CameraModule::camera_get_focus_ring_value_cb(
+    const std::shared_ptr<CameraGetFocusRingValue::Request> request,
+    const std::shared_ptr<CameraGetFocusRingValue::Response> response)
+{
+  E_DjiMountPosition payload_index =
+      static_cast<E_DjiMountPosition>(request->payload_index);
+  uint16_t value;
+
+  T_DjiReturnCode return_code =
+      DjiCameraManager_GetFocusRingValue(payload_index, &value);
+  if (return_code != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
+  {
+    RCLCPP_ERROR(
+        get_logger(),
+        "Getting ring value for camera with payload index %d failed, error "
+        "code :%ld",
+        payload_index, return_code);
+    response->success = false;
+    return;
+  }
+  else
+  {
+    RCLCPP_INFO(get_logger(),
+                "Got ring value = %d for camera with payload index %d.", value,
+                payload_index);
+    response->success = true;
+    response->focus_ring_value = value;
+    return;
+  }
+}
+
+void
+CameraModule::camera_set_focus_ring_value_cb(
+    const std::shared_ptr<CameraSetFocusRingValue::Request> request,
+    const std::shared_ptr<CameraSetFocusRingValue::Response> response)
+{
+  E_DjiMountPosition payload_index =
+      static_cast<E_DjiMountPosition>(request->payload_index);
+  uint16_t value = static_cast<uint16_t>(request->value);
+  T_DjiReturnCode return_code =
+      DjiCameraManager_SetFocusRingValue(payload_index, value);
+  if (return_code != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
+  {
+    RCLCPP_ERROR(
+        get_logger(),
+        "Setting ring value for camera with payload index %d failed, error "
+        "code :%ld",
+        payload_index, return_code);
+    response->success = false;
+    return;
+  }
+  else
+  {
+    RCLCPP_INFO(get_logger(),
+                "Set ring value = %d for camera with payload index %d.", value,
+                payload_index);
+    response->success = true;
+    return;
+  }
+}
+
 void
 CameraModule::camera_shoot_single_photo_cb(
     const std::shared_ptr<CameraShootSinglePhoto::Request> request,