Integration of Health Monitoring System (HMS) as a module

debian/create_debians.sh

@@ -15,7 +15,7 @@ curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr
 echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
 
 apt update
-apt install -y --no-install-recommends python3-pip python3-bloom python3-catkin-pkg dpkg-dev debhelper dh-python
+apt install -y --no-install-recommends python3-pip python3-bloom python3-catkin-pkg dpkg-dev debhelper dh-python nlohmann-json3-dev
 pip3 install rosdep
 rosdep init
 cp psdk_ros2/debian/50-my-packages.list /etc/ros/rosdep/sources.list.d

docs/documentation/GettingStarted.md

@@ -53,9 +53,10 @@ The following parameters can be configured in the *psdk_wrapper/cfg/psdk_params.
 | mandatory_modules             |           |                                    |                                             |
 | - telemetry                   | Bool      |  True                              | Trigger node failure, if module not loaded  |
 | - flight_control              | Bool      |  True                              | Trigger node failure, if module not loaded  |
-| - camera                      | Bool      |  True                              | Trigger node failure, if module not loaded  |
-| - gimbal                      | Bool      |  True                              | Trigger node failure, if module not loaded  |
-| - liveview                    | Bool      |  True                              | Trigger node failure, if module not loaded  |
+| - camera                      | Bool      |  False                             | Trigger node failure, if module not loaded  |
+| - gimbal                      | Bool      |  False                             | Trigger node failure, if module not loaded  |
+| - liveview                    | Bool      |  False                             | Trigger node failure, if module not loaded  |
+| - hms                         | Bool      |  False                             | Trigger node failure, if module not loaded  |
 | data_frequency                | Object    | -                                  | Options are: 1, 5, 10, 50, 100, 200, 400 Hz |
 | - imu                         | Integer   | 100                                | -                                           |
 | - attitude                    | Integer   | 100                                | -                                           |
@@ -74,7 +75,7 @@ The following parameters can be configured in the *psdk_wrapper/cfg/psdk_params.
 | - control_information         | Integer   | 1                                  | -                                           |
 
 
-To configure the hardware connection type and to specify the exact ports that need to be used, please use the *psdk_wrapper/cfg/link_config.json* file. This file follows a similar strategy to the file one must configure before running the DJI PSDK samples. Thus, for simplicity, the psdk_ros2 wrapper follows the same approach. Please notice, that the App configuration (e.g. app_id, app_key) has been kept in the ros parameter file. 
+To configure the hardware connection type and to specify the exact ports that need to be used, please use the *psdk_wrapper/cfg/link_config.json* file. This file follows a similar strategy to the file one must configure before running the DJI PSDK samples. Thus, for simplicity, the psdk_ros2 wrapper follows the same approach. Please notice, that the App configuration (e.g. app_id, app_key) has been kept in the ros parameter file (cfg/psdk_params.yml). 
 
 ## Udev rules
 
@@ -107,7 +108,7 @@ The following ROS 2 packages are needed to successfully build the wrapper:
 
 ### Other libraries
 
-The following libraries are needded to enable the access to USB devices and handling the video streaming:
+The following libraries are needed to enable the access to USB devices and handling the video streaming:
 
 * libusb-1.0-0-dev
 * libopus-dev 
@@ -116,3 +117,5 @@ The following libraries are needded to enable the access to USB devices and hand
 * libavformat-dev 
 * libavfilter-dev
 
+The following library is used to work with JSON:
+* nlohmann-json-dev

docs/documentation/Introduction.md

@@ -28,7 +28,7 @@ The `psdk_ros2` wrapper currently supports the following features:
 
 ## Compatibility
 
-The current version supports **PSDK v3.5** and it has been tested and built for **ROS 2 Humble**. For firmware and product compatibility, please check the page [DJI Developer - PSDK](https://developer.dji.com/doc/payload-sdk-tutorial/en/).
+The current version supports **PSDK v3.8** and it has been tested and built for **ROS 2 Humble**. For firmware and product compatibility, please check the page [DJI Developer - PSDK](https://developer.dji.com/doc/payload-sdk-tutorial/en/).
 
 
 ## Important Notice 

docs/documentation/ThePSDKWrapper.md

@@ -137,6 +137,12 @@ Given the high number of topics available, these have been grouped under differe
 | psdk_ros2/relative_obstacle_info |  100 Hz   |
 
 
+* **Health Management System (HMS)**
+
+| ROS 2 Topic               | Max. freq |
+| ------------------------- |-----------|
+| psdk_ros2/hms_info_table  |  1 Hz     |
+
 The user can set a specific publishing frequency to each category within the `psdk_params.yaml` file and this will be applied to all topics contained within it. The possible frequencies that can be set for any given category are: 1, 5, 10, 50, 100, 200, 400 Hz.  If the frequency set by the user is higher than the maximum one handled internally by the PSDK libraries, an error message will appear. 
 
 <div style="background-color: #D6EAF8; padding: 10px; border: 1px solid ##FBFAFA;">
@@ -259,7 +265,7 @@ The following services can be called to start shooting photos or record videos w
 
 #### Camera streaming
 
-The camera streaming is managed by a ROS 2 service. Calling the `psdk_ros2/camera_setup_streaming` you can select the payload index of the camera you want to stream, the camera source (e.g. zoom camera/wide camera) and whether to start or stop the streaming with a boolean. Once the streaming is started, you can see the images either on the `psdk_ros2/main_camera_stream`  if the main camera has been selected or the `psdk_ros2/fpv_camera_stream`  if the FPV camera has been selected. 
+The camera streaming is managed by a ROS 2 service. Calling the `psdk_ros2/camera_setup_streaming` you can select the payload index of the camera you want to stream, the camera source (e.g. zoom camera/wide camera) and whether to start or stop the streaming with a boolean. Moreover, one can set if the stream should be published decoded or encoded. Once the streaming is started, you can see the images either on the `psdk_ros2/main_camera_stream`  if the main camera has been selected or the `psdk_ros2/fpv_camera_stream`  if the FPV camera has been selected. 
 
 Please notice that the frequency of the streaming will depend on the computational resources available on the board where the `psdk_ros2 wrapper` is launched. 
 

psdk_interfaces/CMakeLists.txt

@@ -36,6 +36,8 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/FlightStatus.msg"
   "msg/RCConnectionStatus.msg"
   "msg/ControlMode.msg"
+  "msg/HmsInfoMsg.msg"
+  "msg/HmsInfoTable.msg"
   "msg/SingleBatteryInfo.msg"
   "srv/CameraGetType.srv"
   "srv/CameraSetExposureModeEV.srv"

psdk_interfaces/msg/HmsInfoMsg.msg

@@ -0,0 +1,10 @@
+uint32 error_code           # Error code associated to HMS message
+uint8 component_index       # Related to "payloadId" in the camera module
+uint8 error_level
+string ground_info          # Description of the error code if/when the drone is on the ground
+string fly_info             # Description of the error code if/when the drone is in the air
+
+uint8 MIN_HMS_ERROR_LEVEL = 0
+uint8 MID_HMS_ERROR_LEVEL = 3
+uint8 MAX_HMS_ERROR_LEVEL = 6
+

psdk_interfaces/msg/HmsInfoTable.msg

@@ -0,0 +1,3 @@
+uint32 num_msg          # Length of 'table' array
+HmsInfoMsg[] table      # Array of HmsInfoMsg
+

psdk_wrapper/CMakeLists.txt

@@ -31,6 +31,7 @@ find_package(tf2 REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(psdk_interfaces REQUIRED)
 find_package(std_srvs REQUIRED)
+find_package(nlohmann_json REQUIRED)
 
 # FetchContent module
 include(FetchContent)
@@ -64,6 +65,7 @@ set(DJI_HAL_PATH ${PSDK_PATH}/samples/sample_c++/platform/linux/manifold2/hal)
 set(DJI_STREAMING_PATH ${PSDK_PATH}/samples/sample_c++/module_sample/liveview)
 set(DJI_CONFIG_MANAGER_PATH ${PSDK_PATH}/samples/sample_c++/platform/linux/manifold2/application)
 set(DJI_UTILS_PATH ${PSDK_PATH}/samples/sample_c/module_sample)
+set(DJI_HMS_RETURN_CODES_PATH ${PSDK_PATH}/samples/sample_c/module_sample/hms/data)
 
 # Find toolchain name needed to select the right PSDK lib
 execute_process(COMMAND uname -m
@@ -86,7 +88,7 @@ if (PSDK_LIB)
   message(STATUS "PSDK library found: ${PSDK_LIB}")
 else()
   message(FATAL_ERROR "PSDK library NOT found")
-endif()                                                                                                     
+endif()
 
 if (FFMPEG_FOUND)
     message(STATUS "Found FFMPEG installed in the system")
@@ -159,6 +161,7 @@ add_library(psdk_wrapper_libs SHARED
   src/modules/camera.cpp
   src/modules/gimbal.cpp
   src/modules/liveview.cpp
+  src/modules/hms.cpp
   ${DJI_OSAL_PATH}/osal_fs.c
   ${DJI_OSAL_PATH}/osal_socket.c
   ${DJI_OSAL_PATH}/osal.c
@@ -180,6 +183,7 @@ ament_target_dependencies(psdk_wrapper_libs
 target_link_libraries(
   psdk_wrapper_libs
   ${PSDK_LIB}
+  nlohmann_json::nlohmann_json
 )
 
 target_link_libraries(
@@ -219,6 +223,11 @@ install(DIRECTORY
   DESTINATION share/${PROJECT_NAME}/
 )
 
+install(DIRECTORY
+  ${DJI_HMS_RETURN_CODES_PATH}/
+  DESTINATION share/${PROJECT_NAME}/cfg
+)
+
 ament_export_dependencies(
   "rclcpp"
 )

psdk_wrapper/cfg/psdk_params.yaml

@@ -16,8 +16,8 @@
       camera_frame: "psdk_camera_link"
       publish_transforms: true
 
-      # Mandatory modules to be initialized. Mark with a true those which you 
-      # consider mandatory for your application, false otherwise. Be aware that 
+      # Mandatory modules to be initialized. Mark with a true those which you
+      # consider mandatory for your application, false otherwise. Be aware that
       # some modules might have inter-dependencies. Non mandatory modules will
       # still be initialized but if case of failure, the node will continue to run.
       mandatory_modules:
@@ -26,8 +26,9 @@
         camera: false
         gimbal: false
         liveview: false
-
-      data_frequency: # Options are: 1, 5, 10, 50, 100, 200, 400 Hz 
+        hms: false
+
+      data_frequency: # Options are: 1, 5, 10, 50, 100, 200, 400 Hz
         imu: 100
         attitude: 100
         acceleration: 50
@@ -44,5 +45,3 @@
         flight_status: 1
         battery_level: 1
         control_information: 1
-
-

psdk_wrapper/include/psdk_wrapper/json_utils.hpp

@@ -0,0 +1,47 @@
+/*
+ * 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/.
+ */
+
+/**
+ * @file json_utils.hpp
+ *
+ * @brief Header file containing utility functions for C++ library nlohmann_json
+ *
+ * @authors Alejandro Mora
+ * Contact: [email protected]
+ *
+ */
+#ifndef PSDK_WRAPPER_INCLUDE_PSDK_WRAPPER_JSON_UTILS_HPP_
+#define PSDK_WRAPPER_INCLUDE_PSDK_WRAPPER_JSON_UTILS_HPP_
+
+#include <iostream>
+#include <nlohmann/json.hpp>
+
+namespace psdk_ros2
+{
+namespace json_utils
+{
+inline nlohmann::json
+parse_file(const std::string& path)
+{
+  std::ifstream file(path);
+  return nlohmann::json::parse(file);
+}
+
+template <typename T>
+inline std::string
+to_hex_str(const T& value, const bool& is_lower_case = true)
+{
+  std::stringstream stream;
+  stream << "0x" << std::setfill('0') << std::setw(sizeof(T) * 2) << std::hex
+         << (is_lower_case ? std::nouppercase : std::uppercase) << value;
+  return stream.str();
+}
+
+}  // namespace json_utils
+}  // namespace psdk_ros2
+
+#endif  // PSDK_WRAPPER_INCLUDE_PSDK_WRAPPER_JSON_UTILS_HPP_

psdk_wrapper/include/psdk_wrapper/psdk_wrapper.hpp

@@ -21,6 +21,7 @@
 #include <dji_aircraft_info.h>
 #include <dji_core.h>
 #include <dji_flight_controller.h>
+#include <dji_hms.h>
 #include <dji_liveview.h>
 #include <dji_logger.h>
 #include <dji_platform.h>
@@ -36,6 +37,7 @@
 #include <map>
 #include <memory>
 #include <nav_msgs/msg/odometry.hpp>
+#include <nlohmann/json.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_lifecycle/lifecycle_node.hpp>
 #include <sensor_msgs/msg/battery_state.hpp>
@@ -70,6 +72,8 @@
 #include "psdk_interfaces/msg/gimbal_rotation.hpp"
 #include "psdk_interfaces/msg/gimbal_status.hpp"
 #include "psdk_interfaces/msg/gps_details.hpp"
+#include "psdk_interfaces/msg/hms_info_msg.hpp"
+#include "psdk_interfaces/msg/hms_info_table.hpp"
 #include "psdk_interfaces/msg/home_position.hpp"
 #include "psdk_interfaces/msg/position_fused.hpp"
 #include "psdk_interfaces/msg/rc_connection_status.hpp"
@@ -231,6 +235,7 @@ class PSDKWrapper : public rclcpp_lifecycle::LifecycleNode
     std::string map_frame;
     std::string gimbal_frame;
     std::string camera_frame;
+    std::string hms_return_codes_path;
     bool publish_transforms;
     int imu_frequency;
     int attitude_frequency;
@@ -322,7 +327,6 @@ class PSDKWrapper : public rclcpp_lifecycle::LifecycleNode
    * @return true/false
    */
   bool deinit_gimbal_manager();
-
   /**
    * @brief Initialize the liveview streaming module
    * @return true/false
@@ -333,6 +337,19 @@ class PSDKWrapper : public rclcpp_lifecycle::LifecycleNode
    * @return true/false
    */
   bool deinit_liveview();
+  /**
+   * @brief Initialize the health monitoring system (HMS) module
+   * @note Since the HMS module callback function involves a ROS2
+   * publisher, this init method should be invoked **after** ROS2
+   * elements have been initialized.
+   * @return true/false
+   */
+  bool init_hms();
+  /**
+   * @brief Deinitialize the health monitoring system (HMS) module
+   * @return true/false
+   */
+  bool deinit_hms();
 
   /**
    * @brief Get the DJI frequency object associated with a certain frequency
@@ -496,6 +513,7 @@ class PSDKWrapper : public rclcpp_lifecycle::LifecycleNode
   friend T_DjiReturnCode c_home_point_altitude_callback(
       const uint8_t* data, uint16_t data_size,
       const T_DjiDataTimestamp* timestamp);
+  friend T_DjiReturnCode c_hms_callback(T_DjiHmsInfoTable hms_info_table);
   /* Streaming */
   friend void c_publish_main_streaming_callback(CameraRGBImage img,
                                                 void* user_data);
@@ -1062,6 +1080,15 @@ class PSDKWrapper : public rclcpp_lifecycle::LifecycleNode
       const uint8_t* data, uint16_t data_size,
       const T_DjiDataTimestamp* timestamp);
 
+  /**
+   * @brief Callback function registered to retrieve HMS information.
+   * DJI pushes data at a fixed frequency of 1Hz.
+   * @param hms_info_table  Array of HMS info messages
+   * @return T_DjiReturnCode error code indicating whether there have been any
+   * issues processing the HMS info table
+   */
+  T_DjiReturnCode hms_callback(T_DjiHmsInfoTable hms_info_table);
+
   /* ROS 2 Subscriber callbacks */
   /**
    * @brief Callback function to control aircraft position and yaw. This
@@ -1780,6 +1807,8 @@ class PSDKWrapper : public rclcpp_lifecycle::LifecycleNode
       altitude_barometric_pub_;
   rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::Float32>::SharedPtr
       home_point_altitude_pub_;
+  rclcpp_lifecycle::LifecyclePublisher<
+      psdk_interfaces::msg::HmsInfoTable>::SharedPtr hms_info_table_pub_;
   rclcpp_lifecycle::LifecyclePublisher<sensor_msgs::msg::Image>::SharedPtr
       main_camera_stream_pub_;
   rclcpp_lifecycle::LifecyclePublisher<sensor_msgs::msg::Image>::SharedPtr
@@ -2007,6 +2036,20 @@ class PSDKWrapper : public rclcpp_lifecycle::LifecycleNode
    */
   bool initialize_psdk_modules();
 
+  /**
+   * @brief Create a 'psdk_interfaces::msg::HmsInfoTable' from a
+   * PSDK HMS message of type 'T_DjiHmsInfoTable', given a JSON
+   * with all known return codes and a language to retrieve
+   * the return code messages in.
+   * @param hms_info_table HMS message from PSDK.
+   * @param codes JSON containing known return codes.
+   * @param language Language to fetch the return codes in.
+   * @return psdk_interfaces::msg::HmsInfoTable
+   */
+  psdk_interfaces::msg::HmsInfoTable
+  to_ros2_msg(const T_DjiHmsInfoTable& hms_info_table,
+              const nlohmann::json& codes, const char* language = "en");
+
   /* Global variables */
   PSDKParams params_;
   rclcpp::Node::SharedPtr node_;
@@ -2052,6 +2095,7 @@ class PSDKWrapper : public rclcpp_lifecycle::LifecycleNode
   T_DjiAircraftInfoBaseInfo aircraft_base_info_;
   E_DjiCameraType attached_camera_type_;
   E_DjiLiveViewCameraSource selected_camera_source_;
+  nlohmann::json hms_return_codes_json_;
   bool publish_camera_transforms_{false};
   bool decode_stream_{true};
   int num_of_initialization_retries_{0};
@@ -2061,6 +2105,7 @@ class PSDKWrapper : public rclcpp_lifecycle::LifecycleNode
   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};
 };
 
 /**