feat:add get_whole_battery_info service

OSDK-2507

CMakeLists.txt

@@ -105,6 +105,8 @@ add_message_files(
   MissionHotpointTask.msg
   FlightAnomaly.msg
   VOPosition.msg
+  BatteryState.msg
+  BatteryWholeInfo.msg
 )
 
 ## Generate services in the 'srv' folder
@@ -130,6 +132,7 @@ add_service_files(
   CameraStartShootIntervalPhoto.srv
   CameraStopShootPhoto.srv
   CameraRecordVideoAction.srv
+  GetWholeBatteryInfo.srv
   MFIO.srv
   SetGoHomeAltitude.srv
   SetNewHomePoint.srv

include/dji_osdk_ros/dji_vehicle_node.h

@@ -96,6 +96,8 @@
 #include <dji_osdk_ros/MissionHpUpdateYawRate.h>
 #include <dji_osdk_ros/MissionHpResetYaw.h>
 #include <dji_osdk_ros/MissionHpUpdateRadius.h>
+//battery services
+#include <dji_osdk_ros/GetWholeBatteryInfo.h>
 
 //waypointV2.0 services
 #include <dji_osdk_ros/InitWaypointV2Setting.h>
@@ -207,6 +209,10 @@ namespace dji_osdk_ros
       ros::ServiceServer camera_control_start_shoot_interval_photo_server_;
       ros::ServiceServer camera_control_stop_shoot_photo_server_;
       ros::ServiceServer camera_control_record_video_action_server_;
+
+      /*! for battery */
+      ros::ServiceServer get_single_battery_dynamic_info_server_;
+      ros::ServiceServer get_whole_battery_info_server_;
       /*! for mfio */
       ros::ServiceServer mfio_control_server_;
       /*! for mobile device */
@@ -335,6 +341,8 @@ namespace dji_osdk_ros
       bool cameraStartShootIntervalPhotoCallback(CameraStartShootIntervalPhoto::Request& request, CameraStartShootIntervalPhoto::Response& response);
       bool cameraStopShootPhotoCallback(CameraStopShootPhoto::Request& request, CameraStopShootPhoto::Response& response);
       bool cameraRecordVideoActionCallback(CameraRecordVideoAction::Request& request, CameraRecordVideoAction::Response& response);
+      /*! for battery info */
+      bool getWholeBatteryInfoCallback(GetWholeBatteryInfo::Request& request,GetWholeBatteryInfo::Response& reponse);
       /*! for mfio conrol */
       bool mfioCtrlCallback(MFIO::Request& request, MFIO::Response& response);
       /*! for mobile device */

include/dji_osdk_ros/vehicle_wrapper.h

@@ -105,6 +105,9 @@ namespace dji_osdk_ros
       bool moveByPositionOffset(ACK::ErrorCode& ack, int timeout, MoveOffset& p_offset);
       bool obtainReleaseCtrlAuthority(bool enableObtain, int timeout);
 
+      /*! Parts of Battery */
+      bool getBatteryWholeInfo(DJI::OSDK::BatteryWholeInfo& batteryWholeInfo);
+
       /*! Parts of mfio */
       uint8_t outputMFIO(uint8_t mode, uint8_t channel, uint32_t init_on_time_us, uint16_t freq, bool block, uint8_t gpio_value);
       uint32_t inputMFIO(uint8_t mode, uint8_t channel, bool block);

launch/dji_vehicle_node.launch

@@ -3,8 +3,8 @@
     <!-- node parameters -->
     <param name="acm_name" type="string" value="/dev/ttyACM0"/>
     <param name="serial_name" type="string" value="/dev/ttyUSB0"/>
-    <param name="baud_rate" type="int" value="921600"/>
-    <param name="app_id" type="int" value="12345"/>
+    <param name="baud_rate" type="int" value="1000000"/>
+    <param name="app_id" type="int" value="10086"/>
     <param name="app_version" type="int" value="1"/>
     <param name="align_time" type="bool" value="false"/>
     <param name="enc_key" type="string" value="abc123"/>

msg/BatteryState.msg

@@ -0,0 +1,10 @@
+uint8 isFakeSingleBatteryMode
+uint8 isSingleBatteryMode
+uint8 batteryNotReady  # The battery is not ready (the battery has not communicated just after being powered on,
+                       # or has not passed the first battery certification).
+uint8 voltageNotSafety # Generally caused by low temperature, the battery has electricity,
+                       # but the battery voltage is too low.
+uint8 veryLowVoltageAlarm
+uint8 LowVoltageAlarm
+uint8 seriousLowCapacityAlarm
+uint8 LowCapacityAlarm

msg/BatteryWholeInfo.msg

@@ -0,0 +1,19 @@
+uint16  remainFlyTime
+uint16  goHomeNeedTime            # Time required for the gohome flight (s)
+uint16  landNeedTime              # Time required for the land flight (s).max value:100*/
+uint16  goHomeNeedCapacity        # Capacity required for the gohome flight (%).max value:100*/
+uint16  landNeedCapacity          # Capacity required for the land flight (%).max value:100*/
+float32 safeFlyRadius             # Safe flight area radius (m)*/
+float32 capacityConsumeSpeed      # (mAh/sec)*/
+BatteryState batteryState
+uint8   goHomeCountDownState      # Countdown status of smart battery gohome
+                                   # 0/2:not in gohome state
+                                   # 1  :in gohome state
+                                   #
+uint8   gohomeCountDownvalue      # uint:s.max value:10
+uint16  voltage                   # mv
+uint8   batteryCapacityPercentage # uint:%.max value:100
+uint8   lowBatteryAlarmThreshold
+uint8   lowBatteryAlarmEnable
+uint8   seriousLowBatteryAlarmThreshold
+uint8   seriousLowBatteryAlarmEnable

src/dji_osdk_ros/dji_vehicle_node.cpp

@@ -223,6 +223,12 @@ void VehicleNode::initService()
   camera_control_stop_shoot_photo_server_ = nh_.advertiseService("camera_stop_shoot_photo", &VehicleNode::cameraStopShootPhotoCallback, this);
   camera_control_record_video_action_server_ = nh_.advertiseService("camera_record_video_action", &VehicleNode::cameraRecordVideoActionCallback, this);
 
+  /* @brief
+   * get whole battery info server
+   * @platforms M210V2
+   */
+  get_whole_battery_info_server_ = nh_.advertiseService("get_whole_battery_info", &VehicleNode::getWholeBatteryInfoCallback, this);
+
   /*! @brief
    *  mfio control server
    *  @platforms null
@@ -1284,6 +1290,51 @@ bool VehicleNode::cameraRecordVideoActionCallback(CameraRecordVideoAction::Reque
   return response.result;
 }
 
+bool VehicleNode::getWholeBatteryInfoCallback(GetWholeBatteryInfo::Request& request,GetWholeBatteryInfo::Response& response)
+{
+  ROS_INFO_STREAM("get Whole Battery Info callback");
+  if (ptr_wrapper_ == nullptr)
+  {
+    ROS_ERROR_STREAM("Vehicle modules is nullptr");
+    return false;
+  }
+
+  DJI::OSDK::BatteryWholeInfo batteryWholeInfo;
+  if (ptr_wrapper_->getBatteryWholeInfo(batteryWholeInfo))
+  {
+    response.battery_whole_info.remainFlyTime  = batteryWholeInfo.remainFlyTime;
+    response.battery_whole_info.goHomeNeedTime = batteryWholeInfo.goHomeNeedTime ;
+    response.battery_whole_info.landNeedTime   = batteryWholeInfo.landNeedTime;
+    response.battery_whole_info.goHomeNeedCapacity = batteryWholeInfo.goHomeNeedCapacity;
+    response.battery_whole_info.landNeedCapacity = batteryWholeInfo.landNeedCapacity ;
+    response.battery_whole_info.safeFlyRadius = batteryWholeInfo.safeFlyRadius;
+    response.battery_whole_info.capacityConsumeSpeed = batteryWholeInfo.capacityConsumeSpeed;
+    response.battery_whole_info.goHomeCountDownState = batteryWholeInfo.goHomeCountDownState;
+    response.battery_whole_info.gohomeCountDownvalue = batteryWholeInfo.gohomeCountDownvalue;
+    response.battery_whole_info.voltage = batteryWholeInfo.voltage;
+    response.battery_whole_info.batteryCapacityPercentage = batteryWholeInfo.batteryCapacityPercentage;
+    response.battery_whole_info.lowBatteryAlarmThreshold = batteryWholeInfo.lowBatteryAlarmThreshold;
+    response.battery_whole_info.lowBatteryAlarmEnable = batteryWholeInfo.lowBatteryAlarmEnable;
+    response.battery_whole_info.seriousLowBatteryAlarmThreshold = batteryWholeInfo.seriousLowBatteryAlarmThreshold;
+    response.battery_whole_info.seriousLowBatteryAlarmEnable = batteryWholeInfo.seriousLowBatteryAlarmEnable;
+
+    response.battery_whole_info.batteryState.isFakeSingleBatteryMode = batteryWholeInfo.batteryState.isFakeSingleBatteryMode;
+    response.battery_whole_info.batteryState.isSingleBatteryMode     = batteryWholeInfo.batteryState.isSingleBatteryMode;
+    response.battery_whole_info.batteryState.batteryNotReady         = batteryWholeInfo.batteryState.batteryNotReady;
+    response.battery_whole_info.batteryState.voltageNotSafety        = batteryWholeInfo.batteryState.voltageNotSafety;
+    response.battery_whole_info.batteryState.veryLowVoltageAlarm     = batteryWholeInfo.batteryState.veryLowVoltageAlarm;
+    response.battery_whole_info.batteryState.LowVoltageAlarm         = batteryWholeInfo.batteryState.LowVoltageAlarm;
+    response.battery_whole_info.batteryState.seriousLowCapacityAlarm = batteryWholeInfo.batteryState.seriousLowCapacityAlarm;
+    response.battery_whole_info.batteryState.LowCapacityAlarm        = batteryWholeInfo.batteryState.LowCapacityAlarm;
+  }
+  else
+  {
+    DSTATUS("get Battery Whole Info false!");
+    return false;
+  }
+  return true;
+}
+
 bool VehicleNode::mfioCtrlCallback(MFIO::Request& request, MFIO::Response& response)
 {
   ROS_INFO_STREAM("MFIO Control callback");

src/dji_osdk_ros/modules/vehicle_wrapper.cpp

@@ -1896,6 +1896,21 @@ static T_OsdkOsalHandler osalHandler = {
     return true;
   }
 
+  bool VehicleWrapper::getBatteryWholeInfo(DJI::OSDK::BatteryWholeInfo& batteryWholeInfo)
+  {
+    if (!vehicle) {
+      DERROR("vehicle is a null value");
+      return false;
+    }
+    bool result = true;
+
+    result &= vehicle->djiBattery->subscribeBatteryWholeInfo(true);
+    OsdkOsal_TaskSleepMs(500);
+    result &= vehicle->djiBattery->getBatteryWholeInfo(batteryWholeInfo);
+
+    return result;
+  }
+
   uint8_t VehicleWrapper::outputMFIO(uint8_t mode, uint8_t channel, uint32_t init_on_time_us, uint16_t freq, bool block, uint8_t gpio_set_value)
   {
     int responseTimeout = 1;

src/dji_osdk_ros/samples/CMakeLists.txt

@@ -103,6 +103,12 @@ target_link_libraries(waypointV2_node
         )
 add_dependencies(waypointV2_node dji_osdk_ros_generate_messages_cpp)
 
+add_executable(battery_node battery_node.cpp)
+target_link_libraries(battery_node
+        ${PROJECT_NAME}
+        )
+add_dependencies(battery_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/battery_node.cpp

@@ -0,0 +1,77 @@
+/** @file battery_node.cpp
+ *  @version 4.1
+ *  @date Dec 2020
+ *
+ *  @brief sample node of battery.
+ *
+ *  @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/dji_vehicle_node.h>
+
+const uint8_t MAX_TIME_COUNT = 50;
+int main(int argc, char** argv)
+{
+    ros::init(argc, argv, "battery_node");
+    ros::NodeHandle nh;
+    uint8_t time_count = 0;
+
+    auto get_whole_battery_info = nh.serviceClient<dji_osdk_ros::GetWholeBatteryInfo>("get_whole_battery_info");
+
+    dji_osdk_ros::GetWholeBatteryInfo getWholeBatteryInfo;
+    while (time_count < MAX_TIME_COUNT)
+    {
+      get_whole_battery_info.call(getWholeBatteryInfo);
+      ROS_INFO("(It's valid only for M210V2)batteryCapacityPercentage is %d",getWholeBatteryInfo.response.battery_whole_info.batteryCapacityPercentage);
+      ROS_INFO("(It's valid only for M210V2)remainFlyTime is %d",getWholeBatteryInfo.response.battery_whole_info.remainFlyTime);
+      ROS_INFO("(It's valid only for M210V2)goHomeNeedTime is %d",getWholeBatteryInfo.response.battery_whole_info.goHomeNeedTime);
+      ROS_INFO("(It's valid only for M210V2)landNeedTime is %d",getWholeBatteryInfo.response.battery_whole_info.landNeedTime);
+      ROS_INFO("(It's valid only for M210V2)goHomeNeedCapacity is %d",getWholeBatteryInfo.response.battery_whole_info.goHomeNeedCapacity);
+      ROS_INFO("(It's valid only for M210V2)landNeedCapacity is %d",getWholeBatteryInfo.response.battery_whole_info.landNeedCapacity);
+      ROS_INFO("(It's valid only for M210V2)safeFlyRadius is %f",getWholeBatteryInfo.response.battery_whole_info.safeFlyRadius);
+      ROS_INFO("(It's valid only for M210V2)capacityConsumeSpeed is %f",getWholeBatteryInfo.response.battery_whole_info.capacityConsumeSpeed);
+      ROS_INFO("(It's valid only for M210V2)goHomeCountDownState is %d",getWholeBatteryInfo.response.battery_whole_info.goHomeCountDownState);
+      ROS_INFO("(It's valid only for M210V2)gohomeCountDownvalue is %d",getWholeBatteryInfo.response.battery_whole_info.gohomeCountDownvalue);
+      ROS_INFO("(It's valid only for M210V2)voltage is %ld",getWholeBatteryInfo.response.battery_whole_info.voltage);
+      ROS_INFO("(It's valid only for M210V2)lowBatteryAlarmThreshold is %d",getWholeBatteryInfo.response.battery_whole_info.lowBatteryAlarmThreshold);
+      ROS_INFO("(It's valid only for M210V2)lowBatteryAlarmEnable is %d",getWholeBatteryInfo.response.battery_whole_info.lowBatteryAlarmEnable);
+      ROS_INFO("(It's valid only for M210V2)seriousLowBatteryAlarmThreshold is %d",getWholeBatteryInfo.response.battery_whole_info.seriousLowBatteryAlarmThreshold);
+      ROS_INFO("(It's valid only for M210V2)seriousLowBatteryAlarmEnable is %d",getWholeBatteryInfo.response.battery_whole_info.seriousLowBatteryAlarmEnable);
+      ROS_INFO("(It's valid only for M210V2)isFakeSingleBatteryMode is %d",getWholeBatteryInfo.response.battery_whole_info.batteryState.isFakeSingleBatteryMode);
+      ROS_INFO("(It's valid only for M210V2)isSingleBatteryMode is %d",getWholeBatteryInfo.response.battery_whole_info.batteryState.isSingleBatteryMode);
+      ROS_INFO("(It's valid only for M210V2)batteryNotReady is %d",getWholeBatteryInfo.response.battery_whole_info.batteryState.batteryNotReady);
+      ROS_INFO("(It's valid only for M210V2)voltageNotSafety is %d",getWholeBatteryInfo.response.battery_whole_info.batteryState.voltageNotSafety);
+      ROS_INFO("(It's valid only for M210V2)veryLowVoltageAlarm is %d",getWholeBatteryInfo.response.battery_whole_info.batteryState.veryLowVoltageAlarm);
+      ROS_INFO("(It's valid only for M210V2)LowVoltageAlarm is %d",getWholeBatteryInfo.response.battery_whole_info.batteryState.LowVoltageAlarm);
+      ROS_INFO("(It's valid only for M210V2)seriousLowCapacityAlarm is %d",getWholeBatteryInfo.response.battery_whole_info.batteryState.seriousLowCapacityAlarm);
+      ROS_INFO("(It's valid only for M210V2)LowCapacityAlarm is %d",getWholeBatteryInfo.response.battery_whole_info.batteryState.LowCapacityAlarm);
+
+      ros::Duration(0.2).sleep();
+      time_count++;
+    }
+
+    ROS_INFO_STREAM("Finished. Press CTRL-C to terminate the node");
+
+    ros::spin();
+    return 0;
+}

srv/GetWholeBatteryInfo.srv

@@ -0,0 +1,4 @@
+#request
+---
+#response
+BatteryWholeInfo battery_whole_info