feat:add hms sample

OSDK-2808

CMakeLists.txt

@@ -161,9 +161,9 @@ add_service_files(
   GenerateWaypointV2Action.srv
   SetGlobalCruisespeed.srv
   GetGlobalCruisespeed.srv
+  SubscribeHMSInf.srv
   SubscribeWaypointV2Event.srv
   SubscribeWaypointV2State.srv
-  SubscribeHMSInf.srv
 
   Activation.srv
   CameraAction.srv

include/dji_osdk_ros/vehicle_wrapper.h

@@ -167,6 +167,7 @@ namespace dji_osdk_ros
       /*! Parts of hms */
       bool enableSubscribeHMSInfo(bool enable, uint32_t timeOutMs = 500);
       bool getHMSListInfo(HMSPushPacket& hmsPushPacket);
+      bool getHMSDeviceIndex(uint8_t& deviceIndex);
       /*! Parts of advanced_sendsing */
 #ifdef ADVANCED_SENSING
       /*! CameraStream */

src/dji_osdk_ros/dji_vehicle_node.cpp

@@ -1499,18 +1499,33 @@ bool VehicleNode::subscribeHMSInfoCallback(SubscribeHMSInf::Request& request,
   }
 
   dji_osdk_ros::HMSPushPacket hmsPushPacket;
-
-  response.result &= ptr_wrapper_->enableSubscribeHMSInfo(request.enable);
+  static uint8_t count = 0;
+
+  while(count < 1)
+  {
+     response.result &= ptr_wrapper_->enableSubscribeHMSInfo(request.enable);
+     count++;
+  }
+
   response.result &= ptr_wrapper_->getHMSListInfo(hmsPushPacket);
+  response.result &= ptr_wrapper_->getHMSDeviceIndex(response.deviceIndex);
   response.timeStamp = hmsPushPacket.timeStamp;
 
-  for (int i = 0; hmsPushPacket.hmsPushData.errList.size(); i++)
+  if (hmsPushPacket.hmsPushData.errList.size())
+  {
+    response.errList.clear();
+    response.errList.resize(hmsPushPacket.hmsPushData.errList.size());
+  }
+
+  for (int i = 0; i < hmsPushPacket.hmsPushData.errList.size(); i++)
   {
     response.errList[i].alarmID     = hmsPushPacket.hmsPushData.errList[i].alarmID;
-    response.errList[i].sensorIndex = hmsPushPacket.hmsPushData.errList[i].sensorIndex;
     response.errList[i].reportLevel = hmsPushPacket.hmsPushData.errList[i].reportLevel;
+    response.errList[i].sensorIndex = hmsPushPacket.hmsPushData.errList[i].sensorIndex;
+    DSTATUS("response.errList.size():%d,0x%08x,%d,%d", response.errList.size(),response.errList[i].alarmID, response.errList[i].sensorIndex,
+    response.errList[i].reportLevel);
   }
-
+ 
   return response.result;
 }
 

src/dji_osdk_ros/modules/vehicle_wrapper.cpp

@@ -2340,7 +2340,38 @@ bool VehicleWrapper::getHMSListInfo(HMSPushPacket& hmsPushPacket)
 
   DJI::OSDK::HMSPushPacket tempHMSPushPacket;
   tempHMSPushPacket = vehicle->djiHms->getHMSPushPacket();
-  memcpy(&hmsPushPacket, &tempHMSPushPacket, sizeof(hmsPushPacket));
+
+  hmsPushPacket.timeStamp = tempHMSPushPacket.timeStamp;
+  hmsPushPacket.hmsPushData.msgVersion = tempHMSPushPacket.hmsPushData.msgVersion;
+  hmsPushPacket.hmsPushData.msgIndex = tempHMSPushPacket.hmsPushData.msgIndex;
+  hmsPushPacket.hmsPushData.msgEnd = tempHMSPushPacket.hmsPushData.msgEnd;
+  hmsPushPacket.hmsPushData.globalIndex = tempHMSPushPacket.hmsPushData.globalIndex;
+  if (tempHMSPushPacket.hmsPushData.errList.size())
+  {
+    hmsPushPacket.hmsPushData.errList.clear();
+    hmsPushPacket.hmsPushData.errList.resize(tempHMSPushPacket.hmsPushData.errList.size());
+
+    for (int i = 0; i < hmsPushPacket.hmsPushData.errList.size(); i++)
+    {
+      hmsPushPacket.hmsPushData.errList[i].alarmID     = tempHMSPushPacket.hmsPushData.errList[i].alarmID;
+      hmsPushPacket.hmsPushData.errList[i].reportLevel = tempHMSPushPacket.hmsPushData.errList[i].reportLevel;
+      hmsPushPacket.hmsPushData.errList[i].sensorIndex = tempHMSPushPacket.hmsPushData.errList[i].sensorIndex;
+      DSTATUS("0x%08x,%d,%d", hmsPushPacket.hmsPushData.errList[i].alarmID, hmsPushPacket.hmsPushData.errList[i].sensorIndex,
+      hmsPushPacket.hmsPushData.errList[i].reportLevel);
+    }
+  }
+  return true;
+}
+
+bool VehicleWrapper::getHMSDeviceIndex(uint8_t& deviceIndex)
+{
+  if (!vehicle)
+  {
+    std::cout << "Vehicle is a null value!" << std::endl;
+    return false;
+  }
+
+  deviceIndex = vehicle->djiHms->getDeviceIndex();
   return true;
 }
 

src/dji_osdk_ros/samples/CMakeLists.txt

@@ -109,6 +109,12 @@ target_link_libraries(battery_node
         )
 add_dependencies(battery_node dji_osdk_ros_generate_messages_cpp)
 
+add_executable(hms_node hms_node.cpp)
+target_link_libraries(hms_node
+        ${PROJECT_NAME}
+        )
+add_dependencies(hms_node dji_osdk_ros_generate_messages_cpp)
+
 if (OPEN_CV_3_3_0_INSTALLED AND FOUND_OPENCV_VIZ)
 
     message(STATUS "Found OpenCV ${OpenCV_VERSION}, Viz3d, and advanced sensing module, stereo vision depth perception node will be compiled")

src/dji_osdk_ros/samples/hms_node.cpp

@@ -0,0 +1,71 @@
+/** @file hms_node.cpp
+ *  @version 4.1
+ *  @date Dec 2020
+ *
+ *  @brief sample node of hms.
+ *
+ *  @Copyright (c) 2020 DJI
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ */
+
+#include <ros/ros.h>
+#include <dji_osdk_ros/SubscribeHMSInf.h>
+
+int main(int argc, char** argv)
+{
+    ros::init(argc, argv, "hms_node");
+    ros::NodeHandle nh;
+
+    auto subscribe_hms_info_client = nh.serviceClient<dji_osdk_ros::SubscribeHMSInf>("/subscribe_hms_info");
+    dji_osdk_ros::SubscribeHMSInf subscribe_hms_info;
+    subscribe_hms_info.request.enable = true;
+
+    ros::Duration(1).sleep();
+    ros::AsyncSpinner spinner(1);
+    spinner.start();
+
+    ros::Rate rate(10);
+    while(ros::ok())
+    {
+        subscribe_hms_info_client.call(subscribe_hms_info);
+
+        for (int i = 0; i < subscribe_hms_info.response.errList.size(); i++)
+        {
+            ROS_INFO("hmsErrListNum: %d, alarm_id: 0x%08x, sensorIndex: %d, level: %d",
+                      i, subscribe_hms_info.response.errList[i].alarmID,
+                         subscribe_hms_info.response.errList[i].sensorIndex,
+                         subscribe_hms_info.response.errList[i].reportLevel);
+            if (subscribe_hms_info.response.deviceIndex != 0xff)
+            {
+                ROS_INFO("%d Camera/Gimbal has trouble!", subscribe_hms_info.response.deviceIndex);
+            }
+        }
+
+        rate.sleep();
+    }
+
+    ROS_INFO_STREAM("Finished. Press CTRL-C to terminate the node");
+
+    ros::waitForShutdown();
+    return 0;
+}
+
+

srv/SubscribeHMSInf.srv

@@ -4,4 +4,6 @@ bool enable
 #response
 bool result
 uint32 timeStamp
-HMSPushInfo[] errList     #/*! error code list in each pushing*/
+uint8 deviceIndex         # When the error code is related to camera or gimbal device,
+                          # it will tell you which device it is.
+HMSPushInfo[] errList     # error code list in each pushing