diff --git a/.github/workflows/package.yml b/.github/workflows/package.yml new file mode 100644 index 00000000..05d6e115 --- /dev/null +++ b/.github/workflows/package.yml @@ -0,0 +1,38 @@ +name: Release Candidate +on: + push: + branches-ignore: + - master +jobs: + package-cuda: + runs-on: [arm64,self-hosted,linux, cuda] + steps: + - uses: AutoModality/action-clean@v1.1.0 + - uses: actions/checkout@v2 + - uses: rlespinasse/github-slug-action@3.1.0 # for GITHUB_REF_SLUG + - name: Package + run: | + AM_PLATFORM=dji_m300 amros packages install -y --password=${{ secrets.BUILD_PASSWORD }} + echo ${{ secrets.BUILD_PASSWORD }} | sudo -S whoami && source /opt/ros/melodic/setup.bash && amros dev build deb --clean --version="0.0.${{ github.run_number }}" + - name: Deploy + id: deploy + uses: AutoModality/action-cloudsmith@0.2.0 + with: + api-key: ${{ secrets.CLOUDSMITH_API_KEY }} + command: 'push' + format: 'deb' + owner: 'automodality' + repo: 'dev' + distro: 'ubuntu' + release: 'bionic' + file: 'ros-melodic-dji-osdk-ros_0.0.${{ github.run_number }}-0bionic_arm64.deb' # version must match package phase + draft-pr: + runs-on: ubuntu-18.04 + needs: package-cuda + steps: + - uses: actions/checkout@v2 + - name: Draft a Pull Request + uses: repo-sync/pull-request@v2 + with: + pr_draft: true + github_token: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml new file mode 100644 index 00000000..f5c4d04d --- /dev/null +++ b/.github/workflows/release.yml @@ -0,0 +1,34 @@ +name: Release +on: + push: + branches: + - master +jobs: + release-package: + runs-on: ${{ matrix.os }} + strategy: + matrix: + os: [ubuntu-18.04, [arm64,self-hosted,linux, build]] + fail-fast: false + name: Package + steps: + - uses: AutoModality/action-clean@v1.1.0 + - uses: actions/checkout@v2 + - name: Package + id: package + uses: AutoModality/action-package-debian-ros@v3 + with: + version: 4.0.0-AM${{ github.run_number }} + release-repo-entitlement: ${{ secrets.CLOUDSMITH_READ_RELEASE_ENTITLEMENT }} + - name: Deploy + id: deploy + uses: AutoModality/action-cloudsmith@0.2.0 + with: + api-key: ${{ secrets.CLOUDSMITH_API_KEY }} + command: 'push' + format: 'deb' + owner: 'automodality' + repo: 'release' + distro: 'ubuntu' + release: 'bionic' + file: '${{ steps.package.outputs.artifact-path }}' diff --git a/.gitignore b/.gitignore index 2b4540a9..27dc7bdf 100644 --- a/.gitignore +++ b/.gitignore @@ -1,7 +1,18 @@ ./CMakeLists.txt +*~ *.swn *.swo *.swp *sdk.launch build.sh .idea/ +.history +debian/ros-melodic-am-osdk-ros* +debian/ros-melodic-dji-onboard-sdk-ros.debhelper.log +ros-melodic-dji-onboard-sdk-build-deps_*_all.deb +build +catkin_ws +.vscode + +debian/ros-*-dji-onboard-sdk-ros* +debian/ros-*-dji-osdk-ros* diff --git a/CMakeLists.txt b/CMakeLists.txt index 8288b131..1e937ee8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -73,6 +73,7 @@ add_message_files( FILES FCTimeInUTC.msg + JoystickParams.msg GPSUTC.msg Gimbal.msg Waypoint.msg @@ -96,7 +97,7 @@ add_message_files( WaypointV2.msg WaypointV2MissionEventPush.msg WaypointV2MissionStatePush.msg - + RelPosition.msg MobileData.msg PayloadData.msg MissionWaypointAction.msg @@ -105,6 +106,11 @@ add_message_files( MissionHotpointTask.msg FlightAnomaly.msg VOPosition.msg + BatteryState.msg + BatteryWholeInfo.msg + SmartBatteryState.msg + SmartBatteryDynamicInfo.msg + HMSPushInfo.msg ) ## Generate services in the 'srv' folder @@ -114,6 +120,11 @@ add_service_files( GetDroneType.srv GetM300StereoParams.srv FlightTaskControl.srv + SetJoystickMode.srv + JoystickAction.srv + ObtainControlAuthority.srv + KillSwitch.srv + EmergencyBrake.srv GimbalAction.srv CameraEV.srv CameraShutterSpeed.srv @@ -121,6 +132,7 @@ add_service_files( CameraISO.srv CameraFocusPoint.srv CameraTapZoomPoint.srv + CameraSetZoomPara.srv CameraZoomCtrl.srv CameraStartShootSinglePhoto.srv CameraStartShootBurstPhoto.srv @@ -128,11 +140,16 @@ add_service_files( CameraStartShootIntervalPhoto.srv CameraStopShootPhoto.srv CameraRecordVideoAction.srv + GetWholeBatteryInfo.srv + GetSingleBatteryDynamicInfo.srv MFIO.srv SetGoHomeAltitude.srv - SetNewHomePoint.srv + GetGoHomeAltitude.srv + SetCurrentAircraftLocAsHomePoint.srv + SetHomePoint.srv SetupCameraH264.srv - AvoidEnable.srv + SetAvoidEnable.srv + GetAvoidEnable.srv InitWaypointV2Setting.srv UploadWaypointV2Mission.srv UploadWaypointV2Action.srv @@ -144,6 +161,7 @@ add_service_files( GenerateWaypointV2Action.srv SetGlobalCruisespeed.srv GetGlobalCruisespeed.srv + GetHMSData.srv SubscribeWaypointV2Event.srv SubscribeWaypointV2State.srv @@ -315,11 +333,19 @@ add_subdirectory(src) # ) ## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h*" + PATTERN ".svn" EXCLUDE +) + +install(DIRECTORY include/dji_osdk_ros_obsoleted + DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) + +## Install launch files into share directory +install(DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) ## Mark other files for installation (e.g. launch and bag files, etc.) # install(FILES diff --git a/ReadMe.md b/ReadMe.md index bfc864e9..81667cb7 100644 --- a/ReadMe.md +++ b/ReadMe.md @@ -1,30 +1,41 @@ -# DJI Onboard SDK ROS 4.0.1 +# DJI Onboard SDK ROS 4.1.0 ## Latest Update -OSDK-ROS 4.0.1 was released on 24 August 2020.You need to read newest update below to get update information. Please see the [release notes](https://developer.dji.com/onboard-sdk/downloads/) and [ROS sample setup](https://developer.dji.com/cn/onboard-sdk/documentation/development-workflow/environment-setup.html#linux-with-ros) for more information.And We will update [ROS Wiki](http://wiki.ros.org/dji_sdk/) later. +OSDK-ROS 4.1.0 was released on 20 January 2021.You need to read newest update below to get update information. Please see the [release notes](https://developer.dji.com/onboard-sdk/downloads/) and [ROS sample setup](https://developer.dji.com/cn/onboard-sdk/documentation/development-workflow/environment-setup.html#linux-with-ros) for more information.And We will update [ROS Wiki](http://wiki.ros.org/dji_sdk/) later. ### 1. feature -This 4.0.1 version releases a feature package: dji_osdk_ros. The package contains two different framework's interface. OSDK-ROS-obsoleted kept ros3.8.1's interface. +This 4.1.0 version releases a feature package: dji_osdk_ros. The package contains two different framework's interface. OSDK-ROS-obsoleted kept ros3.8.1's interface. (__note:We will cancel support for the OSDK-ROS-obsoleted's interface in the next version.__) -| **OSDK-ROS4.0.1 interface** | **OSDK-ROS-obsoleted interface** | +| **OSDK-ROS4.1.0 interface** | **OSDK-ROS-obsoleted interface** | |--------------------------------------|---------------------------------------------| |files below in dji_osdk_ros folder | files below in dji_osdk_ros_obsoleted folder| -This update mainly includes: -1. Data subscription interface and sample; -2. Mobile device interface and sample; -3. Payload device interface and sample; -4. Add the top obstacle avoidance enable interface; -5. Waypoint1.0/2.0 interface and sample; -6. Advanced sensing interface and sample perfection; - include: - > camera-h264 - > camera-stream - > stereo-vision-depth-perception(rectified stereo images．disparity mapfiltered disparity map (Optional)．point cloud) - (__note: If you roslaunch dji_vehicle_node.launch to get m300_stereo_param.yaml, the yaml file will be prouduced in .ros folder(/home/${user}/.ros).And you need to copy it to the directory which you need to rosrun.__) -7. we also kept all services and topics of osdk-ros 3.8.1. If you want to use these interfaces,you need to run dji_sdk_node and use it's services and topics. +This update mainly includes: +1. Battery information interface and sample; +2. hms interface and sample; +3. update flight-control interface and sample: + include: + >set_joystick_mode + >joystick_action + >get/set_go_home_altitude + >set_home_point + >rename 'set_current_point_as_home' to 'set_current_aircraft_point_as_home' + >rename 'enable_avoid' to 'set_horizon_avoid_enable' + >rename 'enable_upwards_avoid' to 'set_upwards_avoid_enable' + >get_acoid_enable_status + >kill_switch + >emergency_brake + >update flight_task_control,include: + a.add velocity and yaw rate control action + b.add turn on/off motor action + c.add force landing and confirm landing action + d.add cancel landing and cancel go home action + +4. fixed telemetry_node problem:displayMode and rcConnection is zero. + +5. we also kept all services and topics of osdk-ros 3.8.1. If you want to use these interfaces,you need to run dji_sdk_node and use it's services and topics. (__note: These interfaces are not fully compatible with onboard-sdk4.0.1.And they will not be supported in next osdk-ros version.__) | **nodes** | **services's name** | **topics's name** | @@ -32,9 +43,18 @@ This update mainly includes: |dji_vehicle_node |get_drone_type | | |flight_control_node |flight_task_control | | | |set_go_home_altitude | | -| |set_current_point_as_home | | -| |enable_avoid | | -| |enable_upwards_avoid | | +| |get_go_home_altitude | | +| |set_current_aircraft_point_as_home | | +| |set_horizon_avoid_enable | | +| |set_upwards_avoid_enable | | +| |set_local_pos_reference | | +| |joystick_action | | +| |set_joystick_mode | | +| |set_home_point | | +| |get_avoid_enable_status | | +| |obtain_release_control_authority | | +| |kill_switch | | +| |emergency_brake | | |gimbal_camera_control_node |gimbal_task_control | | | |camera_task_set_EV | | | |camera_task_set_shutter_speed | | @@ -42,6 +62,7 @@ This update mainly includes: | |camera_task_set_ISO | | | |camera_task_set_focus_point | | | |camera_task_tap_zoom_point | | +| |camera_task_set_zoom_para | | | |camera_task_zoom_ctrl | | | |camera_start_shoot_single_photo | | | |camera_start_shoot_aeb_photo | | @@ -78,7 +99,10 @@ This update mainly includes: | | |dji_osdk_ros/stereo_240p_front_depth_images| | | |dji_osdk_ros/stereo_vga_front_left_images | | | |dji_osdk_ros/stereo_vga_front_right_images | -|time_sync_node | | | +|time_sync_node | |dji_osdk_ros/time_sync_nmea_msg | +| | |dji_osdk_ros/time_sync_gps_utc | +| | |dji_osdk_ros/time_sync_fc_time_utc | +| | |dji_osdk_ros/time_sync_pps_source | |mission_node |dji_osdk_ros/mission_waypoint_upload | | | |dji_osdk_ros/mission_waypoint_action | | | |dji_osdk_ros/mission_waypoint_getInfo | | @@ -112,6 +136,9 @@ This update mainly includes: | |dji_osdk_ros/waypointV2_getGlobalCruisespeed | | | |dji_osdk_ros/waypointV2_subscribeMissionEvent |dji_osdk_ros/waypointV2_mission_event | | |dji_osdk_ros/waypointV2_subscribeMissionState |dji_osdk_ros/swaypointV2_mission_state | +|battery_node |get_whole_battery_info | | +| |get_single_battery_dynamic_info | | +|hms_node |get_hms_data | | ### 2. Prerequisites The system environment we have tested is in the table below. @@ -120,17 +147,18 @@ The system environment we have tested is in the table below. | **system version** | ubuntu 16.04 | | **processor architecture** | x86(mainfold2-c),armv8(mainfold2-g) | #### Firmware Compatibility -OSDK-ROS 4.0.1's firmware compatibility depends on onboard-sdk 4.0.1's. you can get more information [here](https://developer.dji.com/cn/document/0c2b2d75-d019-480c-9241-8c8e7209692d); +OSDK-ROS 4.1.0's firmware compatibility depends on onboard-sdk 4.1.0's. you can get more information [here](https://developer.dji.com/cn/document/0c2b2d75-d019-480c-9241-8c8e7209692d); #### Ros you need to install ros first.Install instruction can be found at: http://wiki.ros.org/ROS/Installation. We just tested ROS kinetic version. #### C++11 Compiler We compile with C + + 11 Standard. #### onboard-sdk -you need to download onboard-sdk4.0.1,and install it. +you need to download onboard-sdk4.1.0,and install it. >$mkdir build >$cd build ->$cmake.. ->$sudo make -j7 install +>$cmake .. +>$make -j7 +>$sudo make install #### nema-comms > $sudo apt install ros-{release}-nmea-comms @@ -139,6 +167,8 @@ __note:we only test on kinetic,but it should be support on other version.__ > $sudo apt install ffmpeg #### libusb-1.0-0-dev > $sudo apt install libusb-1.0-0-dev +#### libsdl2-dev +> $sudo apt install libsdl2-dev #### opencv3.x We use OpenCV to show images from camera stream. Download and install instructions can be found at: http://opencv.org. Tested with OpenCV 3.3.0.Suggest using 3.3.0+. #### stereo-vision function @@ -157,7 +187,7 @@ You will need to add an udev file to allow your system to obtain permission and >$sudo vi DJIDevice.rules Then add these content into DJIDevice.rules. ->$SUBSYSTEM=="usb", ATTRS{idVendor}=="2ca3", MODE="0666" +>SUBSYSTEM=="usb", ATTRS{idVendor}=="2ca3", MODE="0666" At last,you need to reboot your computer to make sure it works. @@ -169,8 +199,8 @@ If you don't have a catkin workspace, create one as follows: >$mkdir src >$cd src >$catkin_init_workspace -#### add osdk-ros 4.0.1 -Download osdk-ros 4.0.1 and put it into src. +#### add osdk-ros 4.1.0 +Download osdk-ros 4.1.0 and put it into src. #### Build the dji_osdk_ros ROS package >$cd .. >$catkin_make @@ -181,7 +211,7 @@ Download osdk-ros 4.0.1 and put it into src. 2.Edit the launch file and enter your App ID, Key, Baudrate and Port name in the designated places. (__note:there are two launch file. dji_sdk_node.launch is for dji_sdk_node.(3.8.1's interface) -dji_vehicle_node is for dji_vehicle_node(4.0.1's interface)__) +dji_vehicle_node is for dji_vehicle_node(4.1.0's interface)__) > $rosed dji_osdk_ros dji_sdk_node.launch > $rosed dji_osdk_ros dji_vehicle_node.launch @@ -190,7 +220,7 @@ dji_vehicle_node is for dji_vehicle_node(4.0.1's interface)__) >dji_vehicle_node.launch does not need UserConfig.txt. #### Running the Samples 1.Start up the dji_osdk_ros ROS node. -if you want to use OSDK ROS 4.0.1's services and topics: +if you want to use OSDK ROS 4.1.0's services and topics: >$roslaunch dji_osdk_ros dji_vehicle_node.launch if you want to adapt to OSDK ROS 3.8.1's services and topics: @@ -207,3 +237,4 @@ __note:if you want to rosrun dji_sdk_node,you need to put UserConfig.txt into cu You can get support from DJI and the community with the following methods: * Email to dev@dji.com * Report issue on github + diff --git a/debian/bloom-generate.txt b/debian/bloom-generate.txt new file mode 100644 index 00000000..68010a0a --- /dev/null +++ b/debian/bloom-generate.txt @@ -0,0 +1 @@ +Debian dir generated by bloom using AMROS cli diff --git a/debian/changelog b/debian/changelog new file mode 100644 index 00000000..6b8f5bc8 --- /dev/null +++ b/debian/changelog @@ -0,0 +1,7 @@ +ros-melodic-dji-osdk-ros (4.0.1002-0bionic) bionic; urgency=high + + * Autogenerated, no changelog for this version found in CHANGELOG.rst. + + -- kevin.hoo Mon, 11 Jan 2021 22:41:01 -0000 + + diff --git a/debian/compat b/debian/compat new file mode 100644 index 00000000..ec635144 --- /dev/null +++ b/debian/compat @@ -0,0 +1 @@ +9 diff --git a/debian/control b/debian/control new file mode 100644 index 00000000..47bfc700 --- /dev/null +++ b/debian/control @@ -0,0 +1,12 @@ +Source: ros-melodic-dji-osdk-ros +Section: misc +Priority: optional +Maintainer: kevin.hoo +Build-Depends: debhelper (>= 9.0.0), opencv, ros-melodic-actionlib, ros-melodic-actionlib-msgs, ros-melodic-catkin, ros-melodic-geometry-msgs, ros-melodic-message-filters, ros-melodic-message-generation, ros-melodic-nav-msgs, ros-melodic-nmea-msgs, ros-melodic-roscpp, ros-melodic-rospy, ros-melodic-std-msgs +Homepage: http://developer.dji.com/onboard-sdk/documentation/github-platform-docs/ROS/README.html +Standards-Version: 3.9.2 + +Package: ros-melodic-dji-osdk-ros +Architecture: any +Depends: ${shlibs:Depends}, ${misc:Depends}, opencv, ros-melodic-actionlib, ros-melodic-actionlib-msgs, ros-melodic-geometry-msgs, ros-melodic-message-filters, ros-melodic-message-runtime, ros-melodic-nav-msgs, ros-melodic-nmea-msgs, ros-melodic-roscpp, ros-melodic-rospy, ros-melodic-std-msgs +Description: A ROS package using DJI Onboard SDK diff --git a/debian/rules b/debian/rules new file mode 100755 index 00000000..43390e96 --- /dev/null +++ b/debian/rules @@ -0,0 +1,69 @@ +#!/usr/bin/make -f +# -*- makefile -*- +# Sample debian/rules that uses debhelper. +# This file was originally written by Joey Hess and Craig Small. +# As a special exception, when this file is copied by dh-make into a +# dh-make output file, you may use that output file without restriction. +# This special exception was added by Craig Small in version 0.37 of dh-make. + +# Uncomment this to turn on verbose mode. +export DH_VERBOSE=1 +# TODO: remove the LDFLAGS override. It's here to avoid esoteric problems +# of this sort: +# https://code.ros.org/trac/ros/ticket/2977 +# https://code.ros.org/trac/ros/ticket/3842 +export LDFLAGS= +export PKG_CONFIG_PATH=/opt/ros/melodic/lib/pkgconfig +# Explicitly enable -DNDEBUG, see: +# https://github.com/ros-infrastructure/bloom/issues/327 +export DEB_CXXFLAGS_MAINT_APPEND=-DNDEBUG + +%: + dh $@ -v --buildsystem=cmake + +override_dh_auto_configure: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/melodic/setup.sh" ]; then . "/opt/ros/melodic/setup.sh"; fi && \ + dh_auto_configure -- \ + -DCATKIN_BUILD_BINARY_PACKAGE="1" \ + -DCMAKE_INSTALL_PREFIX="/opt/ros/melodic" \ + -DCMAKE_PREFIX_PATH="/opt/ros/melodic" + +override_dh_auto_build: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/melodic/setup.sh" ]; then . "/opt/ros/melodic/setup.sh"; fi && \ + dh_auto_build + +override_dh_auto_test: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + echo -- Running tests. Even if one of them fails the build is not canceled. + if [ -f "/opt/ros/melodic/setup.sh" ]; then . "/opt/ros/melodic/setup.sh"; fi && \ + dh_auto_test || true + +override_dh_shlibdeps: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/melodic/setup.sh" ]; then . "/opt/ros/melodic/setup.sh"; fi && \ + dh_shlibdeps -l$(CURDIR)/debian/ros-melodic-dji-osdk-ros//opt/ros/melodic/lib/ + +override_dh_auto_install: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/melodic/setup.sh" ]; then . "/opt/ros/melodic/setup.sh"; fi && \ + dh_auto_install + + +override_dh_builddeb: + dh_builddeb --destdir=. + +override_dh_auto_clean: + rm -f *.*deb + rm -rf obj-* \ No newline at end of file diff --git a/debian/source/format b/debian/source/format new file mode 100644 index 00000000..163aaf8d --- /dev/null +++ b/debian/source/format @@ -0,0 +1 @@ +3.0 (quilt) diff --git a/debian/source/options b/debian/source/options new file mode 100644 index 00000000..8bc9182a --- /dev/null +++ b/debian/source/options @@ -0,0 +1,5 @@ +# Automatically add upstream changes to the quilt overlay. +# http://manpages.ubuntu.com/manpages/trusty/man1/dpkg-source.1.html +# This supports reusing the orig.tar.gz for debian increments. +auto-commit + diff --git a/include/dji_osdk_ros/common_type.h b/include/dji_osdk_ros/common_type.h index 4d59eba3..1ff84c92 100644 --- a/include/dji_osdk_ros/common_type.h +++ b/include/dji_osdk_ros/common_type.h @@ -30,6 +30,7 @@ // Header include #include +#include // Declaration namespace dji_osdk_ros @@ -53,6 +54,50 @@ namespace dji_osdk_ros double yaw_threshold; }; + struct JoystickMode + { + uint8_t horizontalLogic; + uint8_t verticalLogic; + uint8_t yawLogic; + uint8_t horizontalCoordinate; + uint8_t stableMode; + }; + + struct JoystickCommand + { + DJI::OSDK::float32_t x; /*!< Control with respect to the x axis of the + DJI::OSDK::Control::HorizontalCoordinate.*/ + DJI::OSDK::float32_t y; /*!< Control with respect to the y axis of the + DJI::OSDK::Control::HorizontalCoordinate.*/ + DJI::OSDK::float32_t z; /*!< Control with respect to the z axis, up is positive. */ + DJI::OSDK::float32_t yaw; /*!< Yaw position/velocity control w.r.t. the ground frame.*/ + }; + +#pragma pack(1) + /*! the type of error code list in HMS's raw pushing data*/ + typedef struct ErrList{ + uint32_t alarmID; /*! error code*/ + uint8_t sensorIndex; /*! fault sensor's index*/ + uint8_t reportLevel; /*! fault level ,0-4,0 is no error,4 is highest*/ + } ErrList; + + /*! the type of HMS's raw pushing data*/ + typedef struct HMSPushData { + uint8_t msgVersion; /*! version of message.default:0*/ + uint8_t globalIndex; /*! cycle message sequence number*/ + uint8_t msgEnd : 1; /*! end flag bit of message subcontract push. last package of subcontracting push is 1.*/ + uint8_t msgIndex : 7; /*! Message serial number*/ + std::vector errList; /*! error code list in each pushing*/ + } HMSPushData; +#pragma pack() + + /*! the type of HMS's pushing data with a time stamp*/ + typedef struct HMSPushPacket + { + HMSPushData hmsPushData; /*! HMS's raw pushing data*/ + uint32_t timeStamp; /*! timestamp of the packet*/ + } HMSPushPacket; + struct RotationAngle { DJI::OSDK::float32_t roll; diff --git a/include/dji_osdk_ros/dji_vehicle_node.h b/include/dji_osdk_ros/dji_vehicle_node.h index c6a5ff54..4f07fe53 100644 --- a/include/dji_osdk_ros/dji_vehicle_node.h +++ b/include/dji_osdk_ros/dji_vehicle_node.h @@ -58,18 +58,32 @@ #include /*! services */ +//flight control services +#include #include +#include +#include #include -#include +#include +#include +#include #include -#include +#include +#include +#include +#include +#include +//Gimbal control services #include + +//Camera control services #include #include #include #include #include #include +#include #include #include #include @@ -77,10 +91,14 @@ #include #include #include + +//HMS services +#include +//mfio services #include +//MOP services #include #include -#include //mission services #include #include @@ -94,6 +112,9 @@ #include #include #include +//battery services +#include +#include //waypointV2.0 services #include @@ -149,6 +170,10 @@ #include #include +#include + +#include + #define C_EARTH (double)6378137.0 #define C_PI (double)3.141592653589793 #define DEG2RAD(DEG) ((DEG) * ((C_PI) / (180.0))) @@ -176,17 +201,29 @@ namespace dji_osdk_ros bool initTopic(); bool initDataSubscribeFromFC(); bool cleanUpSubscribeFromFC(); +#ifdef ADVANCED_SENSING + void publishCameraInfo(const std_msgs::Header &header); +#endif protected: /*! services */ /*! for general */ ros::ServiceServer get_drone_type_server_; /*! for flight control */ + ros::ServiceServer obtain_releae_control_authority_server_; ros::ServiceServer task_control_server_; + ros::ServiceServer set_joystick_mode_server_; + ros::ServiceServer joystick_action_server_; ros::ServiceServer set_home_altitude_server_; - ros::ServiceServer set_current_point_as_home_server_; + ros::ServiceServer get_home_altitude_server_; + ros::ServiceServer set_home_point_server_; + ros::ServiceServer set_current_aircraft_point_as_home_server_; ros::ServiceServer set_local_pos_reference_server_; - ros::ServiceServer avoid_enable_server_; - ros::ServiceServer upwards_avoid_enable_server_; + ros::ServiceServer set_horizon_avoid_enable_server_; + ros::ServiceServer get_avoid_enable_status_server_; + ros::ServiceServer set_upwards_avoid_enable_server_; + ros::ServiceServer kill_switch_server_; + ros::ServiceServer emergency_brake_action_server_; + /*! for gimbal */ ros::ServiceServer gimbal_control_server_; /*! for camera */ @@ -196,6 +233,7 @@ namespace dji_osdk_ros ros::ServiceServer camera_control_set_ISO_server_; ros::ServiceServer camera_control_set_focus_point_server_; ros::ServiceServer camera_control_set_tap_zoom_point_server_; + ros::ServiceServer camera_control_set_zoom_para_server_; ros::ServiceServer camera_control_zoom_ctrl_server_; ros::ServiceServer camera_control_start_shoot_single_photo_server_; ros::ServiceServer camera_control_start_shoot_burst_photo_server_; @@ -203,6 +241,12 @@ 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 hms */ + ros::ServiceServer get_hms_data_server_; /*! for mfio */ ros::ServiceServer mfio_control_server_; /*! for mobile device */ @@ -295,6 +339,9 @@ namespace dji_osdk_ros ros::Publisher stereo_240p_front_depth_publisher_; ros::Publisher stereo_vga_front_left_publisher_; ros::Publisher stereo_vga_front_right_publisher_; + ros::Publisher left_camera_info_pub_; + ros::Publisher right_camera_info_pub_; + ros::Timer info_timer_; #endif //waypointV2 @@ -302,17 +349,29 @@ namespace dji_osdk_ros ros::Publisher waypointV2_mission_event_publisher_; protected: + + void initTimer(); /*! for general */ bool getDroneTypeCallback(dji_osdk_ros::GetDroneType::Request &request, dji_osdk_ros::GetDroneType::Response &response); /*! for flight control */ bool taskCtrlCallback(FlightTaskControl::Request& request, FlightTaskControl::Response& response); + bool setJoystickModeCallback(SetJoystickMode::Request& request, SetJoystickMode::Response& response); + bool JoystickActionCallback(JoystickAction::Request& request, JoystickAction::Response& response); bool setGoHomeAltitudeCallback(SetGoHomeAltitude::Request& request, SetGoHomeAltitude::Response& response); - bool setHomeCallback(SetNewHomePoint::Request& request, SetNewHomePoint::Response& response); + bool getGoHomeAltitudeCallback(GetGoHomeAltitude::Request& request, GetGoHomeAltitude::Response& response); + bool setCurrentAircraftLocAsHomeCallback(SetCurrentAircraftLocAsHomePoint::Request& request, + SetCurrentAircraftLocAsHomePoint::Response& response); + bool setHomePointCallback(SetHomePoint::Request& request, SetHomePoint::Response& response); bool setLocalPosRefCallback(dji_osdk_ros::SetLocalPosRef::Request &request, dji_osdk_ros::SetLocalPosRef::Response &response); - bool setAvoidCallback(AvoidEnable::Request& request, AvoidEnable::Response& response); - bool setUpwardsAvoidCallback(AvoidEnable::Request& request, AvoidEnable::Response& response); + bool setHorizonAvoidCallback(SetAvoidEnable::Request& request, SetAvoidEnable::Response& response); + bool setUpwardsAvoidCallback(SetAvoidEnable::Request& request, SetAvoidEnable::Response& response); + bool getAvoidEnableStatusCallback(GetAvoidEnable::Request& request, GetAvoidEnable::Response& response); + bool obtainReleaseControlAuthorityCallback(ObtainControlAuthority::Request& request, + ObtainControlAuthority::Response& reponse); + bool killSwitchCallback(KillSwitch::Request& request, KillSwitch::Response& response); + bool emergencyBrakeCallback(EmergencyBrake::Request& request, EmergencyBrake::Response& response); /*! for gimbal control */ bool gimbalCtrlCallback(GimbalAction::Request& request, GimbalAction::Response& response); /*! for camera conrol */ @@ -322,19 +381,34 @@ namespace dji_osdk_ros bool cameraSetISOCallback(CameraISO::Request& request, CameraISO::Response& response); bool cameraSetFocusPointCallback(CameraFocusPoint::Request& request, CameraFocusPoint::Response& response); bool cameraSetTapZoomPointCallback(CameraTapZoomPoint::Request& request, CameraTapZoomPoint::Response& response); + bool cameraSetZoomParaCallback(CameraSetZoomPara::Request& request, CameraSetZoomPara::Response& response); bool cameraZoomCtrlCallback(CameraZoomCtrl::Request& request, CameraZoomCtrl::Response& response); - bool cameraStartShootSinglePhotoCallback(CameraStartShootSinglePhoto::Request& request, CameraStartShootSinglePhoto::Response& response); - bool cameraStartShootAEBPhotoCallback(CameraStartShootAEBPhoto::Request& request, CameraStartShootAEBPhoto::Response& response); - bool cameraStartShootBurstPhotoCallback(CameraStartShootBurstPhoto::Request& request, CameraStartShootBurstPhoto::Response& response); - bool cameraStartShootIntervalPhotoCallback(CameraStartShootIntervalPhoto::Request& request, CameraStartShootIntervalPhoto::Response& response); - bool cameraStopShootPhotoCallback(CameraStopShootPhoto::Request& request, CameraStopShootPhoto::Response& response); - bool cameraRecordVideoActionCallback(CameraRecordVideoAction::Request& request, CameraRecordVideoAction::Response& response); + bool cameraStartShootSinglePhotoCallback(CameraStartShootSinglePhoto::Request& request, + CameraStartShootSinglePhoto::Response& response); + bool cameraStartShootAEBPhotoCallback(CameraStartShootAEBPhoto::Request& request, + CameraStartShootAEBPhoto::Response& response); + bool cameraStartShootBurstPhotoCallback(CameraStartShootBurstPhoto::Request& request, + CameraStartShootBurstPhoto::Response& response); + 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); + bool getSingleBatteryDynamicInfoCallback(GetSingleBatteryDynamicInfo::Request& request, + GetSingleBatteryDynamicInfo::Response& response); + /*! for hms info */ + bool getHMSDataCallback(GetHMSData::Request& request, GetHMSData::Response& response); /*! for mfio conrol */ bool mfioCtrlCallback(MFIO::Request& request, MFIO::Response& response); /*! for mobile device */ - bool sendToMobileCallback(dji_osdk_ros::SendMobileData::Request& request,dji_osdk_ros::SendMobileData::Response& response); + bool sendToMobileCallback(dji_osdk_ros::SendMobileData::Request& request, + dji_osdk_ros::SendMobileData::Response& response); /*! for payload device */ - bool sendToPayloadCallback(dji_osdk_ros::SendPayloadData::Request& request,dji_osdk_ros::SendPayloadData::Response& response); + bool sendToPayloadCallback(dji_osdk_ros::SendPayloadData::Request& request, + dji_osdk_ros::SendPayloadData::Response& response); /*! for advanced sensing conrol */ #ifdef ADVANCED_SENSING bool setupCameraStreamCallback(dji_osdk_ros::SetupCameraStream::Request& request, @@ -351,6 +425,10 @@ namespace dji_osdk_ros bool getM300StereoParamsCallback(dji_osdk_ros::GetM300StereoParams::Request& request, dji_osdk_ros::GetM300StereoParams::Response& response); void publishAdvancedSeningData(); + + //void infoTimerCB(const ros::TimerEvent &even); + + #endif /*! for mission service callback*/ // mission manager diff --git a/include/dji_osdk_ros/mobile_device_node.h b/include/dji_osdk_ros/mobile_device_node.h index 20860b5f..a5de3549 100644 --- a/include/dji_osdk_ros/mobile_device_node.h +++ b/include/dji_osdk_ros/mobile_device_node.h @@ -47,6 +47,7 @@ ros::ServiceClient flight_control_client; ros::ServiceClient gimbal_control_client; ros::ServiceClient camera_start_shoot_single_photo_client; ros::ServiceClient camera_record_video_action_client; +ros::ServiceClient obtain_ctrl_authority_client; /*! Subscriber */ ros::Subscriber fromMobileDataSub; @@ -67,5 +68,6 @@ bool resetGimbal(); bool Rotategimbal(); bool takePicture(); bool recordVideo(); +bool obtainJoystickControlAuthority(); #endif //SRC_MOBILE_DEVICE_NODE_H diff --git a/include/dji_osdk_ros/stereo_utility/m300_stereo_param_tool.hpp b/include/dji_osdk_ros/stereo_utility/m300_stereo_param_tool.hpp index 316a9ed9..6d54e16b 100755 --- a/include/dji_osdk_ros/stereo_utility/m300_stereo_param_tool.hpp +++ b/include/dji_osdk_ros/stereo_utility/m300_stereo_param_tool.hpp @@ -4,6 +4,7 @@ #include "dji_vehicle.hpp" #include "dji_perception.hpp" #include "dji_osdk_ros/stereo_utility/config.hpp" +#include using namespace M210_STEREO; using namespace DJI; @@ -23,9 +24,13 @@ class M300StereoParamTool double rotaionLeftInRight[9]; double translationLeftInRight[3]; } DoubleCamParamType; - + + /*!*/ Perception::CamParamType getM300stereoParams(Perception::DirectionType direction); + + void getM300stereoCameraInfo(Perception::CamParamType param, sensor_msgs::CameraInfo &left, + sensor_msgs::CameraInfo &right); bool createStereoParamsYamlFile(std::string fileName, Perception::CamParamType param); diff --git a/include/dji_osdk_ros/vehicle_wrapper.h b/include/dji_osdk_ros/vehicle_wrapper.h index 576f2cc1..42e5c5ba 100644 --- a/include/dji_osdk_ros/vehicle_wrapper.h +++ b/include/dji_osdk_ros/vehicle_wrapper.h @@ -60,14 +60,28 @@ namespace dji_osdk_ros virtual ~VehicleWrapper() = default; public: + /*! Parts of environment */ void setupEnvironment(bool enable_advanced_sensing); bool initVehicle(); ErrorCode::ErrorCodeType initGimbalModule(dji_osdk_ros::PayloadIndex index, const char* name); ErrorCode::ErrorCodeType initCameraModule(dji_osdk_ros::PayloadIndex index, const char* name); + Vehicle::ActivateData *getActivateData() + { + return &this->activate_data_; + } + + Vehicle *getVehicle() + { + return vehicle; + } + + Linker *getLinker() + { + return linker; + } - public: /*! Parts of Camera */ bool setEV(const PayloadIndex& payloadIndex, const ExposureCompensation& exposureCompensation); bool setExposureMode(const PayloadIndex& index, const ExposureMode& dataTarget); @@ -76,6 +90,7 @@ namespace dji_osdk_ros bool setAperture(const PayloadIndex& payloadIndex, const Aperture& aperture); bool setFocusPoint(const PayloadIndex& payloadIndex,const float& x, const float& y); bool setTapZoomPoint(const PayloadIndex& payloadIndex,const uint8_t& multiplier, const float& x,const float& y); + bool setZoom(const PayloadIndex& payloadIndex, float factor); bool startZoom(const PayloadIndex& payloadIndex,const uint8_t& direction, const uint8_t& speed); bool stopZoom(const PayloadIndex& payloadIndex); bool startShootSinglePhoto(const PayloadIndex& payloadIndex); @@ -92,16 +107,37 @@ namespace dji_osdk_ros /*! Parts of Flight Control*/ bool checkActionStarted(uint8_t mode); - bool setNewHomeLocation(int timeout = 1); + bool setCurrentAircraftLocAsHomePoint(int timeout = 1); + bool setHomePoint(float64_t latitude, float64_t longitude, int timeout = 1); bool setHomeAltitude(uint16_t altitude, int timeout = 1); - bool goHome(ACK::ErrorCode& ack, int timeout); + bool getHomeAltitude(uint16_t& altitude, int timeout = 1); + bool goHome(int timeout); + bool cancelGoHome(int timeout); bool goHomeAndConfirmLanding(int timeout); - bool setAvoid(bool enable); + bool setCollisionAvoidance(bool enable); + bool getCollisionAvoidance(uint8_t& enable); bool setUpwardsAvoidance(bool enable); + bool getUpwardsAvoidance(uint8_t& enable); - bool monitoredTakeoff(ACK::ErrorCode& ack, int timeout); - bool monitoredLanding(ACK::ErrorCode& ack, int timeout); - bool moveByPositionOffset(ACK::ErrorCode& ack, int timeout, MoveOffset& p_offset); + bool monitoredTakeoff(int timeout); + bool monitoredLanding(int timeout); + bool startForceLanding(int timeout); + bool startConfirmLanding(int timeout); + bool cancelLanding(int timeout); + bool moveByPositionOffset(const JoystickCommand &JoystickCommand,int timeout, + float posThresholdInM = 0.8,float yawThresholdInDeg = 1.0); + void velocityAndYawRateCtrl(const JoystickCommand &JoystickCommand, int timeMs); + bool setJoystickMode(const JoystickMode &joystickMode); + bool JoystickAction(const JoystickCommand &JoystickCommand); + bool obtainReleaseCtrlAuthority(bool enableObtain, int timeout); + bool turnOnOffMotors(bool OnOff,int timeOut = 1); + bool killSwitch(bool enable, char msg[10], int timeOut = 1); + bool emergencyBrake(); + + /*! Parts of Battery */ + bool getBatteryWholeInfo(DJI::OSDK::BatteryWholeInfo& batteryWholeInfo); + bool getSingleBatteryDynamicInfo(const DJI::OSDK::DJIBattery::RequestSmartBatteryIndex batteryIndex, + DJI::OSDK::SmartBatteryDynamicInfo& batteryDynamicInfo); /*! 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); @@ -128,6 +164,10 @@ namespace dji_osdk_ros bool RegisterMissionEventCallback(void *userData, PushCallback cb); bool RegisterMissionStateCallback(void *userData, PushCallback cb); + /*! 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 */ @@ -204,8 +244,8 @@ namespace dji_osdk_ros /*! Parts of data subscription*/ bool setUpSubscription(int pkgIndex, int freq,TopicName* topicList, - uint8_t topicSize, int timeout); - bool teardownSubscription(const int pkgIndex, int timeout); + uint8_t topicSize, int timeout = 1); + bool teardownSubscription(const int pkgIndex, int timeout = 1); ACK::ErrorCode verify(int timeout); bool initPackageFromTopicList(int packageID, int numberOfTopics,TopicName* topicList, bool sendTimeStamp, uint16_t freq); @@ -215,27 +255,7 @@ namespace dji_osdk_ros UserData userData); Version::FirmWare getFwVersion() const; char* getHwVersion() const; - protected: - bool startGlobalPositionBroadcast(); - Telemetry::Vector3f toEulerAngle(void* quaternionData); - void localOffsetFromGpsOffset(Telemetry::Vector3f& deltaNed, void* target, void* origin); - - public: - Vehicle::ActivateData *getActivateData() - { - return &this->activate_data_; - } - - Vehicle *getVehicle() - { - return vehicle; - } - - Linker *getLinker() - { - return linker; - } - + private: uint32_t timeout_; Vehicle::ActivateData activate_data_; @@ -246,6 +266,22 @@ namespace dji_osdk_ros unsigned int baudrate_; std::string sample_case_; const static unsigned int default_acm_baudrate = 230400; + + template + static int signOfData(Type type); + Vector3f vector3FSub(const Vector3f& vectorA,const Vector3f& vectorB); + float32_t vectorNorm(Vector3f v); + void horizCommandLimit(float speedFactor, float& commandX,float& commandY); + Vector3f quaternionToEulerAngle(const Telemetry::Quaternion& quat); + static Vector3f localOffsetFromGpsAndFusedHeightOffset(const Telemetry::GPSFused& target, + const Telemetry::GPSFused& origin, + const float32_t& targetHeight, + const float32_t& originHeight); + bool startGlobalPositionBroadcast(); + bool motorStartedCheck(); + bool takeOffInAirCheck(); + bool takeoffFinishedCheck(); + bool landFinishedCheck(); }; } diff --git a/include/dji_osdk_ros_obsoleted/dji_linux_environment.hpp b/include/dji_osdk_ros_obsoleted/dji_linux_environment.hpp index 093a01e2..a8e4b632 100644 --- a/include/dji_osdk_ros_obsoleted/dji_linux_environment.hpp +++ b/include/dji_osdk_ros_obsoleted/dji_linux_environment.hpp @@ -43,6 +43,11 @@ class DJI_Environment ~DJI_Environment(); static std::string findFile(std::string file); + /** + * @return a string of the path to a file, if found. Empty otherwise. + */ + static std::string findFileInDir(std::string file, std::string dir); + int getApp_id() const; const std::string& getEnc_key() const; const std::string& getDevice() const; diff --git a/include/dji_osdk_ros_obsoleted/dji_sdk/dji_sdk_node.h b/include/dji_osdk_ros_obsoleted/dji_sdk/dji_sdk_node.h index 886f08b2..f1210780 100644 --- a/include/dji_osdk_ros_obsoleted/dji_sdk/dji_sdk_node.h +++ b/include/dji_osdk_ros_obsoleted/dji_sdk/dji_sdk_node.h @@ -37,6 +37,7 @@ #include #include #include +#include #include #include @@ -76,6 +77,8 @@ #include #include #include +#include +#include #endif //! SDK library @@ -228,6 +231,9 @@ class DJISDKNode dji_osdk_ros::StereoVGASubscription::Response& response); bool setupCameraStreamCallback(dji_osdk_ros::SetupCameraStream::Request& request, dji_osdk_ros::SetupCameraStream::Response& response); + void publishCameraInfo(const std_msgs::Header &header); + + sensor_msgs::CameraInfo getCameraInfo(int camera_select, bool isLeftRequired); #endif //! data broadcast callback @@ -293,9 +299,16 @@ class DJISDKNode static void publishMainCameraImage(CameraRGBImage img, void* userData); static void publishFPVCameraImage(CameraRGBImage img, void* userData); + + static void processRosImage(sensor_msgs::Image &img, int camera_select); #endif private: + +#ifdef ADVANCED_SENSING + int latest_camera_ = {-1}; +#endif + //! OSDK core Vehicle* vehicle; LinuxSetup* linuxEnvironment; @@ -342,6 +355,8 @@ class DJISDKNode ros::ServiceServer subscribe_stereo_depth_server; ros::ServiceServer subscribe_stereo_vga_server; ros::ServiceServer camera_stream_server; + ros::Publisher left_camera_info_pub_; + ros::Publisher right_camera_info_pub_; #endif //! flight control subscribers @@ -372,6 +387,7 @@ class DJISDKNode ros::Publisher gimbal_angle_publisher; ros::Publisher displaymode_publisher; ros::Publisher rc_publisher; + ros::Publisher device_status_publisher; ros::Publisher rc_connection_status_publisher; ros::Publisher rtk_position_publisher; ros::Publisher rtk_velocity_publisher; @@ -382,6 +398,7 @@ class DJISDKNode ros::Publisher flight_anomaly_publisher; //! Local Position Publisher (Publishes local position in ENU frame) ros::Publisher local_position_publisher; + ros::Publisher relative_position_publisher; ros::Publisher local_frame_ref_publisher; ros::Publisher time_sync_nmea_publisher; ros::Publisher time_sync_gps_utc_publisher; @@ -415,6 +432,7 @@ class DJISDKNode std::string app_bundle_id; // reserved int uart_or_usb; double gravity_const; + bool enable_advanced_sensing; //! use broadcast or subscription to get telemetry data TELEMETRY_TYPE telemetry_from_fc; diff --git a/launch/UserConfig.txt b/launch/UserConfig.txt new file mode 100644 index 00000000..e0d76260 --- /dev/null +++ b/launch/UserConfig.txt @@ -0,0 +1,5 @@ +app_id : 1023987 +app_key : afe96210ea022d24e17808d30229be0cfa5d41d75bb37f4965f1502b6d3cc62b +device : /dev/ttyUSB0 +baudrate : 921600 +acm_port : /dev/ttyACM0 diff --git a/launch/dji_sdk_node.launch b/launch/dji_sdk_node.launch index d468b97d..8cebcd4d 100644 --- a/launch/dji_sdk_node.launch +++ b/launch/dji_sdk_node.launch @@ -4,10 +4,10 @@ - + - + diff --git a/launch/dji_vehicle_node.launch b/launch/dji_vehicle_node.launch index 50f804aa..d3b1a15e 100644 --- a/launch/dji_vehicle_node.launch +++ b/launch/dji_vehicle_node.launch @@ -4,10 +4,11 @@ - + - + + diff --git a/launch/stereo_disparity.launch b/launch/stereo_disparity.launch new file mode 100644 index 00000000..a78907ef --- /dev/null +++ b/launch/stereo_disparity.launch @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/launch/stereo_pointcloud.launch b/launch/stereo_pointcloud.launch new file mode 100644 index 00000000..db458909 --- /dev/null +++ b/launch/stereo_pointcloud.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + diff --git a/msg/BatteryState.msg b/msg/BatteryState.msg new file mode 100644 index 00000000..2ce239c8 --- /dev/null +++ b/msg/BatteryState.msg @@ -0,0 +1,6 @@ +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 \ No newline at end of file diff --git a/msg/BatteryWholeInfo.msg b/msg/BatteryWholeInfo.msg new file mode 100644 index 00000000..18ba98da --- /dev/null +++ b/msg/BatteryWholeInfo.msg @@ -0,0 +1,19 @@ +uint16 remainFlyTime # remain fly time(s) +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 \ No newline at end of file diff --git a/msg/HMSPushInfo.msg b/msg/HMSPushInfo.msg new file mode 100644 index 00000000..5125b065 --- /dev/null +++ b/msg/HMSPushInfo.msg @@ -0,0 +1,3 @@ +uint32 alarmID #/*! error code*/ +uint8 sensorIndex #/*! fault sensor's index*/ +uint8 reportLevel #/*! fault level ,0-4,0 is no error,4 is highest*/ \ No newline at end of file diff --git a/msg/JoystickParams.msg b/msg/JoystickParams.msg new file mode 100644 index 00000000..aaa077e5 --- /dev/null +++ b/msg/JoystickParams.msg @@ -0,0 +1,6 @@ +float32 x # Control with respect to the x axis of the + # DJI::OSDK::Control::HorizontalCoordinate. +float32 y # Control with respect to the y axis of the + # DJI::OSDK::Control::HorizontalCoordinate. +float32 z # Control with respect to the z axis, up is positive. +float32 yaw #Yaw position/velocity control w.r.t. the ground frame. \ No newline at end of file diff --git a/msg/RelPosition.msg b/msg/RelPosition.msg new file mode 100644 index 00000000..25d44f5e --- /dev/null +++ b/msg/RelPosition.msg @@ -0,0 +1,13 @@ +Header header +float32 down +float32 front +float32 right +float32 back +float32 left +float32 up +uint8 downHealth +uint8 frontHealth +uint8 rightHealth +uint8 backHealth +uint8 leftHealth +uint8 upHealth \ No newline at end of file diff --git a/msg/SmartBatteryDynamicInfo.msg b/msg/SmartBatteryDynamicInfo.msg new file mode 100644 index 00000000..ded9afb9 --- /dev/null +++ b/msg/SmartBatteryDynamicInfo.msg @@ -0,0 +1,10 @@ +uint8 batteryIndex +int32 currentVoltage # uint:mV +int32 currentElectric # uint:mA +uint32 fullCapacity # uint:mAh +uint32 remainedCapacity # uint:mAh +int16 batteryTemperature # uint:0.1℃ +uint8 cellCount +uint8 batteryCapacityPercent # uint:% +SmartBatteryState batteryState +uint8 SOP # Relative power percentage \ No newline at end of file diff --git a/msg/SmartBatteryState.msg b/msg/SmartBatteryState.msg new file mode 100644 index 00000000..0a2ee459 --- /dev/null +++ b/msg/SmartBatteryState.msg @@ -0,0 +1,8 @@ +uint8 cellBreak # 0:normal;other:Undervoltage core index(0x01-0x1F) +uint8 selfCheckError # enum-type: DJISmartBatterySelfCheck +uint8 batteryClosedReason # enum-type: DJI_BETTERY_CLOSED_REASON +uint8 batSOHState # enum-type: DJISmartBatterySohState*/ +uint8 maxCycleLimit # APP:cycle_limit*10*/ +uint8 batteryCommunicationAbnormal +uint8 hasCellBreak +uint8 heatState # enum-type: DJISmartBatteryHeatState \ No newline at end of file diff --git a/msg/WaypointV2Action.msg b/msg/WaypointV2Action.msg index 7cdf796d..bdafe0c0 100644 --- a/msg/WaypointV2Action.msg +++ b/msg/WaypointV2Action.msg @@ -1,11 +1,6 @@ # This class represents an action for ``DJIWaypointV2Mission``. It # determines how action is performed when a waypoint mission is executed. -#contant for waypointV2ActionTriggerType -# The action will be trigger when the aircraft reach the waypoint point. -# The parameters should be setting by ``DJIWaypointV2Action_DJIWaypointV2ReachPointTriggerParam``. -uint8 DJIWaypointV2ActionTriggerTypeReachPoint = 1 - # The action will be triggered when action associated executes. # The parameters should be defined by ``DJIWaypointV2Action_DJIWaypointV2AssociateTriggerParam``. uint8 DJIWaypointV2ActionTriggerTypeActionAssociated = 2 @@ -33,7 +28,7 @@ uint8 DJIWaypointV2ActionActuatorTypeGimbal = 2 # The action will executes by control aircraft. # The parameters should be setting by ``DJIWaypointV2Action_DJIWaypointV2CameraActuatorParam``. -uint8 DJIWaypointV2ActionActuatorTypeAircraftControl = 3 +uint8 DJIWaypointV2ActionActuatorTypeAircraftControl = 4 # Unknown actuator type. uint8 DJIWaypointV2ActionActuatorTypeUnknown = 255 @@ -44,7 +39,6 @@ uint8 waypointV2ActionTriggerType uint8 waypointV2ACtionActuatorType # The trigger of action.You can only choose one to config -WaypointV2ReachpointTrigger waypointV2ReachpointTrigger WaypointV2AssociateTrigger waypointV2AssociateTrigger WaypointV2IntervalTrigger waypointV2IntervalTrigger WaypointV2TrajectoryTrigger waypointV2TrajectoryTrigger diff --git a/msg/WaypointV2AircraftControlActuator.msg b/msg/WaypointV2AircraftControlActuator.msg index ceaa3bb5..6a75797a 100644 --- a/msg/WaypointV2AircraftControlActuator.msg +++ b/msg/WaypointV2AircraftControlActuator.msg @@ -11,6 +11,6 @@ uint8 DJIWaypointV2ActionActuatorAircraftControlOperationTypeUnknown = 255 uint8 actuatorIndex # The index of actuator. It is valid when the diagnostics is related # to camera or gimbal and the connected product has multiple gimbals and cameras. -uint8 DJIWaypointV2ActionActuatorAircraftControlOperationType +uint16 DJIWaypointV2ActionActuatorAircraftControlOperationType WaypointV2AircraftControlActuatorFlying waypointV2AircraftControlActuatorFlying WaypointV2AircraftControlActuatorRotateHeading waypointV2AircraftControlActuatorRotateHeading \ No newline at end of file diff --git a/msg/WaypointV2CameraActuatorFocusParam.msg b/msg/WaypointV2CameraActuatorFocusParam.msg index 84f731aa..d5ecea9c 100644 --- a/msg/WaypointV2CameraActuatorFocusParam.msg +++ b/msg/WaypointV2CameraActuatorFocusParam.msg @@ -4,7 +4,9 @@ # out area if the focus assistant is enabled for the manual mode. # The range for x and y is from 0.0 to 1.0. The point [0.0, 0.0] represents the top-left angle of the screen. - uint16 x # x axis focus point value.range: [0,10000] - uint16 y # y axis focus point value.range: [0,10000] - uint8 retryTimes = 1 - uint8 focusDelayTime = 0 \ No newline at end of file + float32 x # x axis focus point value.range: [0,1] + float32 y # y axis focus point value.range: [0,1] + uint8 regionType #focus type:0:point focus,1:rectangle focus + float32 width #Normalized focus area width(0,1) + float32 height # Normalized focus area height(0,1) + uint8 retryTimes = 1 \ No newline at end of file diff --git a/msg/WaypointV2GimbalActuator.msg b/msg/WaypointV2GimbalActuator.msg index 73d68ba6..10e8105e 100644 --- a/msg/WaypointV2GimbalActuator.msg +++ b/msg/WaypointV2GimbalActuator.msg @@ -7,6 +7,6 @@ uint8 DJIWaypointV2ActionActuatorGimbalOperationTypeRotateGimbal = 1 uint8 DJIWaypointV2ActionActuatorGimbalOperationTypeUnknown = 255 uint8 DJIWaypointV2ActionActuatorGimbalOperationType -uint8 actuatorIndex # The index of actuator. It is valid when the diagnostics is related +uint16 actuatorIndex # The index of actuator. It is valid when the diagnostics is related # to camera or gimbal and the connected product has multiple gimbals and cameras. WaypointV2GimbalActuatorRotationParam waypointV2GimbalActuatorRotationParam # The operation type of gimbal actuator. \ No newline at end of file diff --git a/msg/WaypointV2GimbalActuatorRotationParam.msg b/msg/WaypointV2GimbalActuatorRotationParam.msg index 39cbf54c..cd2a7a5a 100644 --- a/msg/WaypointV2GimbalActuatorRotationParam.msg +++ b/msg/WaypointV2GimbalActuatorRotationParam.msg @@ -1,5 +1,5 @@ - int16 x # gimbal roll angle, unit: 0.1 deg,range:[-200, 200]*/ - int16 y # gimbal pitch angle, unit: 0.1 deg,range:[-1300, 1300]*/ + int16 x # gimbal roll angle, unit: 0.1 deg,range:[-3600, 3600]*/ + int16 y # gimbal pitch angle, unit: 0.1 deg,range:[-3600, 3600]*/ int16 z # gimbal yaw angle, unit: 0.1 deg,range:[-3600, 3600]*/ uint8 ctrl_mode # 0: absolute position control, 1:relative position control*/ uint8 rollCmdIgnore # 0: roll command normal, 1: roll command ignore*/ diff --git a/package.xml b/package.xml index 03a3dd27..5da5dfa5 100644 --- a/package.xml +++ b/package.xml @@ -1,54 +1,53 @@ - dji_osdk_ros - 4.0.0 + 4.0.1002 A ROS package using DJI Onboard SDK - - - + + + kevin.hoo - - - + + + MIT - - - + + + http://developer.dji.com/onboard-sdk/documentation/github-platform-docs/ROS/README.html - - - - + + + + Arjun Menon - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + catkin roscpp @@ -61,7 +60,8 @@ actionlib_msgs nmea_msgs message_filters - + cv_bridge + roscpp rospy message_runtime @@ -72,10 +72,10 @@ actionlib actionlib_msgs message_filters - - + cv_bridge + - + diff --git a/src/dji_osdk_ros/dji_vehicle_node.cpp b/src/dji_osdk_ros/dji_vehicle_node.cpp index a3b1e15d..ead7360a 100644 --- a/src/dji_osdk_ros/dji_vehicle_node.cpp +++ b/src/dji_osdk_ros/dji_vehicle_node.cpp @@ -30,11 +30,16 @@ #include #include -#ifdef OPEN_CV_INSTALLED +/*#ifdef OPEN_CV_INSTALLED +#include +#endif*/ #include +#include + +#ifdef ADVANCED_SENSING +#include #endif -#include //CODE using namespace dji_osdk_ros; #define M300_FRONT_STEREO_PARAM_YAML_NAME "m300_front_stereo_param.yaml" @@ -83,6 +88,7 @@ VehicleNode::VehicleNode():telemetry_from_fc_(TelemetryType::USE_ROS_BROADCAST), initCameraModule(); initService(); initTopic(); + initTimer(); } VehicleNode::~VehicleNode() @@ -104,7 +110,7 @@ bool VehicleNode::initGimbalModule() if(ptr_wrapper_ == nullptr) { ROS_ERROR_STREAM("Vehicle modules is nullptr"); - return true; + return false; } /*! init gimbal modules for gimbalManager */ @@ -144,7 +150,7 @@ bool VehicleNode::initCameraModule() if(ptr_wrapper_ == nullptr) { ROS_ERROR_STREAM("Vehicle modules is nullptr"); - return true; + return false; } /*! init camera modules for cameraManager */ @@ -191,12 +197,21 @@ void VehicleNode::initService() * @platforms M210V2, M300 */ task_control_server_ = nh_.advertiseService("flight_task_control", &VehicleNode::taskCtrlCallback, this); + joystick_action_server_ = nh_.advertiseService("joystick_action", &VehicleNode::JoystickActionCallback, this); + set_joystick_mode_server_ = nh_.advertiseService("set_joystick_mode", &VehicleNode::setJoystickModeCallback, this); set_home_altitude_server_ = nh_.advertiseService("set_go_home_altitude", &VehicleNode::setGoHomeAltitudeCallback,this); - set_current_point_as_home_server_ = nh_.advertiseService("set_current_point_as_home", &VehicleNode::setHomeCallback,this); + get_home_altitude_server_ = nh_.advertiseService("get_go_home_altitude", &VehicleNode::getGoHomeAltitudeCallback,this); + set_current_aircraft_point_as_home_server_ = nh_.advertiseService("set_current_aircraft_point_as_home", + &VehicleNode::setCurrentAircraftLocAsHomeCallback,this); + set_home_point_server_ = nh_.advertiseService("set_home_point", &VehicleNode::setHomePointCallback, this); set_local_pos_reference_server_ = nh_.advertiseService("set_local_pos_reference", &VehicleNode::setLocalPosRefCallback,this); - avoid_enable_server_ = nh_.advertiseService("enable_avoid", &VehicleNode::setAvoidCallback,this); - upwards_avoid_enable_server_ = nh_.advertiseService("enable_upwards_avoid", &VehicleNode::setUpwardsAvoidCallback, this); - + set_horizon_avoid_enable_server_ = nh_.advertiseService("set_horizon_avoid_enable", &VehicleNode::setHorizonAvoidCallback,this); + set_upwards_avoid_enable_server_ = nh_.advertiseService("set_upwards_avoid_enable", &VehicleNode::setUpwardsAvoidCallback, this); + get_avoid_enable_status_server_ = nh_.advertiseService("get_avoid_enable_status", &VehicleNode::getAvoidEnableStatusCallback, this); + obtain_releae_control_authority_server_ = nh_.advertiseService("obtain_release_control_authority", + &VehicleNode::obtainReleaseControlAuthorityCallback, this); + kill_switch_server_ = nh_.advertiseService("kill_switch", &VehicleNode::killSwitchCallback, this); + emergency_brake_action_server_ = nh_.advertiseService("emergency_brake", &VehicleNode::emergencyBrakeCallback, this); /*! @brief * gimbal control server * @platforms M210V2, M300 @@ -213,6 +228,7 @@ void VehicleNode::initService() camera_control_set_ISO_server_ = nh_.advertiseService("camera_task_set_ISO", &VehicleNode::cameraSetISOCallback, this); camera_control_set_focus_point_server_ = nh_.advertiseService("camera_task_set_focus_point", &VehicleNode::cameraSetFocusPointCallback, this); camera_control_set_tap_zoom_point_server_ = nh_.advertiseService("camera_task_tap_zoom_point", &VehicleNode::cameraSetTapZoomPointCallback, this); + camera_control_set_zoom_para_server_ = nh_.advertiseService("camera_task_set_zoom_para", &VehicleNode::cameraSetZoomParaCallback, this); camera_control_zoom_ctrl_server_ = nh_.advertiseService("camera_task_zoom_ctrl", &VehicleNode::cameraZoomCtrlCallback, this); camera_control_start_shoot_single_photo_server_ = nh_.advertiseService("camera_start_shoot_single_photo", &VehicleNode::cameraStartShootSinglePhotoCallback, this); camera_control_start_shoot_AEB_photo_server_ = nh_.advertiseService("camera_start_shoot_aeb_photo", &VehicleNode::cameraStartShootAEBPhotoCallback, this); @@ -221,6 +237,18 @@ 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); + get_single_battery_dynamic_info_server_ = nh_.advertiseService("get_single_battery_dynamic_info", &VehicleNode::getSingleBatteryDynamicInfoCallback, this); + + /*! @brief + * mfio control server + * @platforms M300 + */ + get_hms_data_server_ = nh_.advertiseService("get_hms_data", &VehicleNode::getHMSDataCallback, this); /*! @brief * mfio control server * @platforms null @@ -249,6 +277,7 @@ void VehicleNode::initService() subscribe_stereo_240p_server_ = nh_.advertiseService("stereo_240p_subscription", &VehicleNode::stereo240pSubscriptionCallback, this); subscribe_stereo_depth_server_ = nh_.advertiseService("stereo_depth_subscription", &VehicleNode::stereoDepthSubscriptionCallback,this); subscribe_stereo_vga_server_ = nh_.advertiseService("stereo_vga_subscription", &VehicleNode::stereoVGASubscriptionCallback, this); + get_m300_stereo_params_server_ = nh_.advertiseService("get_m300_stereo_params", &VehicleNode::getM300StereoParamsCallback, this); #ifdef OPEN_CV_INSTALLED /*! @brief * get m300 stereo params server @@ -300,6 +329,14 @@ void VehicleNode::initService() ROS_INFO_STREAM("Services startup!"); } +void VehicleNode::initTimer() +{ +#ifdef ADVANCED_SENSING + //info_timer_ = nh_.createTimer(ros::Duration(0.05), &VehicleNode::infoTimerCB, this); +#endif +} + + bool VehicleNode::initTopic() { attitude_publisher_ = nh_.advertise("dji_osdk_ros/attitude", 10); @@ -319,7 +356,7 @@ bool VehicleNode::initTopic() * - Fused attitude (duplicated from attitude topic) * - Raw linear acceleration (body frame: FLU, m/s^2) * Z value is +9.8 when placed on level ground statically - * - Raw angular velocity (body frame: FLU, rad/s^2) + * - Raw angular velocity (body frame: FLU, rad/s) */ imu_publisher_ = nh_.advertise("dji_osdk_ros/imu", 10); // Refer to dji_sdk.h for different enums for M100 and A3/N3 @@ -374,8 +411,11 @@ bool VehicleNode::initTopic() stereo_240p_down_front_publisher_ = nh_.advertise("dji_osdk_ros/stereo_240p_down_front_images", 10); stereo_240p_down_back_publisher_ = nh_.advertise("dji_osdk_ros/stereo_240p_down_back_images", 10); stereo_240p_front_depth_publisher_ = nh_.advertise("dji_osdk_ros/stereo_240p_front_depth_images", 10); - stereo_vga_front_left_publisher_ = nh_.advertise("dji_osdk_ros/stereo_vga_front_left_images", 10); - stereo_vga_front_right_publisher_ = nh_.advertise("dji_osdk_ros/stereo_vga_front_right_images", 10); + stereo_vga_front_left_publisher_ = nh_.advertise("dji_osdk_ros/stereo_vga_front/left/image_raw", 10); + stereo_vga_front_right_publisher_ = nh_.advertise("dji_osdk_ros/stereo_vga_front/right/image_raw", 10); + left_camera_info_pub_ = nh_.advertise("/dji_osdk_ros/stereo_vga_front/left/camera_info",10); + right_camera_info_pub_ = nh_.advertise("/dji_osdk_ros/stereo_vga_front/right/camera_info",10); + #endif waypointV2_mission_state_publisher_ = nh_.advertise("dji_osdk_ros/waypointV2_mission_state", 10); @@ -384,7 +424,7 @@ bool VehicleNode::initTopic() if(ptr_wrapper_ == nullptr) { ROS_ERROR_STREAM("Vehicle modules is nullptr"); - return true; + return false; } Vehicle* vehicle = ptr_wrapper_->getVehicle(); @@ -478,7 +518,7 @@ bool VehicleNode::initDataSubscribeFromFC() if(ptr_wrapper_ == nullptr) { ROS_ERROR_STREAM("Vehicle modules is nullptr"); - return true; + return false; } ACK::ErrorCode ack = ptr_wrapper_->verify(WAIT_TIMEOUT); @@ -756,7 +796,7 @@ bool VehicleNode::stereo240pSubscriptionCallback(dji_osdk_ros::Stereo240pSubscri if(ptr_wrapper_ == nullptr) { ROS_ERROR_STREAM("Vehicle modules is nullptr"); - return true; + return false; } if (request.unsubscribe_240p == 1) @@ -809,7 +849,7 @@ VehicleNode::stereoDepthSubscriptionCallback(dji_osdk_ros::StereoDepthSubscripti if(ptr_wrapper_ == nullptr) { ROS_ERROR_STREAM("Vehicle modules is nullptr"); - return true; + return false; } if (request.unsubscribe_240p == 1) @@ -854,7 +894,7 @@ bool VehicleNode::stereoVGASubscriptionCallback(dji_osdk_ros::StereoVGASubscript if(ptr_wrapper_ == nullptr) { ROS_ERROR_STREAM("Vehicle modules is nullptr"); - return true; + return false; } if (request.unsubscribe_vga == 1) @@ -892,9 +932,35 @@ bool VehicleNode::stereoVGASubscriptionCallback(dji_osdk_ros::StereoVGASubscript return true; } +bool VehicleNode::getM300StereoParamsCallback(dji_osdk_ros::GetM300StereoParams::Request& request, + dji_osdk_ros::GetM300StereoParams::Response& response) +{ + ROS_INFO("called getM300StereoParamsCallback"); + + if(ptr_wrapper_ == nullptr) + { + ROS_ERROR_STREAM("Vehicle modules is nullptr"); + response.result = false; + } + + Vehicle* vehicle = ptr_wrapper_->getVehicle(); + M300StereoParamTool *tool = new M300StereoParamTool(vehicle); + Perception::CamParamType stereoParam = + tool->getM300stereoParams(Perception::DirectionType::RECTIFY_UP); + if (tool->createStereoParamsYamlFile(M300_FRONT_STEREO_PARAM_YAML_NAME, stereoParam)) + { + tool->setParamFileForM300(M300_FRONT_STEREO_PARAM_YAML_NAME); + response.result = true; + } + else + { + response.result = false; + } + return response.result; +} #ifdef OPEN_CV_INSTALLED -bool VehicleNode::getM300StereoParamsCallback(dji_osdk_ros::GetM300StereoParams::Request& request, +/*bool VehicleNode::getM300StereoParamsCallback(dji_osdk_ros::GetM300StereoParams::Request& request, dji_osdk_ros::GetM300StereoParams::Response& response) { ROS_INFO("called getM300StereoParamsCallback"); @@ -920,7 +986,7 @@ bool VehicleNode::getM300StereoParamsCallback(dji_osdk_ros::GetM300StereoParams: } return response.result; -} +}*/ #endif #endif @@ -932,7 +998,7 @@ bool VehicleNode::getDroneTypeCallback(dji_osdk_ros::GetDroneType::Request &requ if(ptr_wrapper_ == nullptr) { ROS_ERROR_STREAM("Vehicle modules is nullptr"); - return true; + return false; } if (ptr_wrapper_->isM100()) @@ -971,7 +1037,7 @@ bool VehicleNode::taskCtrlCallback(FlightTaskControl::Request& request, FlightT if(ptr_wrapper_ == nullptr) { ROS_ERROR_STREAM("Vehicle modules is nullptr"); - return true; + return false; } switch (request.task) @@ -979,7 +1045,7 @@ bool VehicleNode::taskCtrlCallback(FlightTaskControl::Request& request, FlightT case FlightTaskControl::Request::TASK_GOHOME: { ROS_INFO_STREAM("call go home service"); - if (ptr_wrapper_->goHome(ack, FLIGHT_CONTROL_WAIT_TIMEOUT)) + if (ptr_wrapper_->goHome(FLIGHT_CONTROL_WAIT_TIMEOUT)) { response.result = true; } @@ -994,22 +1060,17 @@ bool VehicleNode::taskCtrlCallback(FlightTaskControl::Request& request, FlightT } break; } - case FlightTaskControl::Request::TASK_GO_LOCAL_POS: + case FlightTaskControl::Request::TASK_POSITION_AND_YAW_CONTROL: { - ROS_INFO_STREAM("call move local position service"); - if(request.pos_offset.size() < 3 && request.yaw_params.size() < 3) - { - ROS_INFO_STREAM("Local Position Service Params Error"); - break; - } - MoveOffset tmp_offset{request.pos_offset[0], - request.pos_offset[1], - request.pos_offset[2], - request.yaw_params[0], - request.yaw_params[1], - request.yaw_params[2] - }; - if (ptr_wrapper_->moveByPositionOffset(ack, FLIGHT_CONTROL_WAIT_TIMEOUT, tmp_offset)) + ROS_INFO_STREAM("call move local position offset service"); + dji_osdk_ros::JoystickCommand joystickCommand; + joystickCommand.x = request.joystickCommand.x; + joystickCommand.y = request.joystickCommand.y; + joystickCommand.z = request.joystickCommand.z; + joystickCommand.yaw = request.joystickCommand.yaw; + + if (ptr_wrapper_->moveByPositionOffset(joystickCommand, FLIGHT_CONTROL_WAIT_TIMEOUT, + request.posThresholdInM, request.yawThresholdInDeg)) { response.result = true; } @@ -1018,21 +1079,90 @@ bool VehicleNode::taskCtrlCallback(FlightTaskControl::Request& request, FlightT case FlightTaskControl::Request::TASK_TAKEOFF: { ROS_INFO_STREAM("call takeoff service"); - if (ptr_wrapper_->monitoredTakeoff(ack, FLIGHT_CONTROL_WAIT_TIMEOUT)) + if (ptr_wrapper_->monitoredTakeoff(FLIGHT_CONTROL_WAIT_TIMEOUT)) { response.result = true; } + break; + } + case FlightTaskControl::Request::TASK_VELOCITY_AND_YAWRATE_CONTROL: + { + ROS_INFO_STREAM("call velocity and yaw rate service"); + + dji_osdk_ros::JoystickCommand joystickCommand; + joystickCommand.x = request.joystickCommand.x; + joystickCommand.y = request.joystickCommand.y; + joystickCommand.z = request.joystickCommand.z; + joystickCommand.yaw = request.joystickCommand.yaw; + + ptr_wrapper_->velocityAndYawRateCtrl(joystickCommand, request.velocityControlTimeMs); + response.result = true; + break; } case FlightTaskControl::Request::TASK_LAND: { ROS_INFO_STREAM("call land service"); - if (ptr_wrapper_->monitoredLanding(ack, FLIGHT_CONTROL_WAIT_TIMEOUT)) + if (ptr_wrapper_->monitoredLanding(FLIGHT_CONTROL_WAIT_TIMEOUT)) + { + response.result = true; + } + break; + } + case FlightTaskControl::Request::START_MOTOR: + { + ROS_INFO_STREAM("call start motor service"); + if (ptr_wrapper_->turnOnOffMotors(true)) + { + response.result = true; + } + break; + } + case FlightTaskControl::Request::STOP_MOTOR: + { + ROS_INFO_STREAM("call stop motor service"); + if (ptr_wrapper_->turnOnOffMotors(false)) + { + response.result = true; + } + break; + } + case FlightTaskControl::Request::TASK_EXIT_GO_HOME: + { + ROS_INFO_STREAM("call cancel go home service"); + if (ptr_wrapper_->cancelGoHome(FLIGHT_CONTROL_WAIT_TIMEOUT)) { response.result = true; } break; } + case FlightTaskControl::Request::TASK_EXIT_LANDING: + { + ROS_INFO_STREAM("call cancel landing service"); + if (ptr_wrapper_->cancelLanding(FLIGHT_CONTROL_WAIT_TIMEOUT)) + { + response.result = true; + } + break; + } + case FlightTaskControl::Request::TASK_FORCE_LANDING_AVOID_GROUND: + { + ROS_INFO_STREAM("call confirm landing service"); + if (ptr_wrapper_->startConfirmLanding(FLIGHT_CONTROL_WAIT_TIMEOUT)) + { + response.result = true; + } + break; + } + case FlightTaskControl::Request::TASK_FORCE_LANDING: + { + ROS_INFO_STREAM("call force landing service"); + if (ptr_wrapper_->startForceLanding(FLIGHT_CONTROL_WAIT_TIMEOUT)) + { + response.result = true; + } + break; + } default: { ROS_INFO_STREAM("No recognized task"); @@ -1040,12 +1170,6 @@ bool VehicleNode::taskCtrlCallback(FlightTaskControl::Request& request, FlightT break; } } - ROS_DEBUG("ack.info: set=%i id=%i", ack.info.cmd_set, ack.info.cmd_id); - ROS_DEBUG("ack.data: %i", ack.data); - - response.cmd_set = (int)ack.info.cmd_set; - response.cmd_id = (int)ack.info.cmd_id; - response.ack_data = (unsigned int)ack.data; if (response.result) { @@ -1057,12 +1181,54 @@ bool VehicleNode::taskCtrlCallback(FlightTaskControl::Request& request, FlightT } } +bool VehicleNode::setJoystickModeCallback(SetJoystickMode::Request& request, SetJoystickMode::Response& response) +{ + ROS_DEBUG("called setJoystickModeCallback"); + if(ptr_wrapper_ == nullptr) + { + ROS_ERROR_STREAM("Vehicle modules is nullptr"); + return false; + } + + dji_osdk_ros::JoystickMode joystickMode; + joystickMode.horizontalLogic = request.horizontal_mode; + joystickMode.verticalLogic = request.vertical_mode; + joystickMode.yawLogic = request.yaw_mode; + joystickMode.horizontalCoordinate = request.horizontal_coordinate; + joystickMode.stableMode = request.stable_mode; + + ptr_wrapper_->setJoystickMode(joystickMode); + + response.result = true; + return true; +} + +bool VehicleNode::JoystickActionCallback(JoystickAction::Request& request, JoystickAction::Response& response) +{ + if(ptr_wrapper_ == nullptr) + { + ROS_ERROR_STREAM("Vehicle modules is nullptr"); + return false; + } + + dji_osdk_ros::JoystickCommand joystickCommand; + joystickCommand.x = request.joystickCommand.x; + joystickCommand.y = request.joystickCommand.y; + joystickCommand.z = request.joystickCommand.z; + joystickCommand.yaw = request.joystickCommand.yaw; + + ptr_wrapper_->JoystickAction(joystickCommand); + + response.result = true; + return true; +} + bool VehicleNode::gimbalCtrlCallback(GimbalAction::Request& request, GimbalAction::Response& response) { if(ptr_wrapper_ == nullptr) { ROS_ERROR_STREAM("Vehicle modules is nullptr"); - return true; + return false; } response.result = false; ROS_INFO("Current gimbal %d, angle (p,r,y) = (%0.2f, %0.2f, %0.2f)", static_cast(request.payload_index), @@ -1099,20 +1265,14 @@ bool VehicleNode::cameraSetEVCallback(CameraEV::Request& request, CameraEV::Resp if(ptr_wrapper_ == nullptr) { ROS_ERROR_STREAM("Vehicle modules is nullptr"); - return true; - } - response.result = false; - if (ptr_wrapper_->setExposureMode(static_cast(request.payload_index), static_cast(request.exposure_mode))) - { - return false; - } - if (ptr_wrapper_->setEV(static_cast(request.payload_index), - static_cast(request.exposure_compensation))) - { return false; } response.result = true; - return true; + response.result &= ptr_wrapper_->setExposureMode(static_cast(request.payload_index), + static_cast(request.exposure_mode)); + response.result &= ptr_wrapper_->setEV(static_cast(request.payload_index), + static_cast(request.exposure_compensation)); + return response.result; } bool VehicleNode::cameraSetShutterSpeedCallback(CameraShutterSpeed::Request& request, CameraShutterSpeed::Response& response) @@ -1120,19 +1280,14 @@ bool VehicleNode::cameraSetShutterSpeedCallback(CameraShutterSpeed::Request& req if(ptr_wrapper_ == nullptr) { ROS_ERROR_STREAM("Vehicle modules is nullptr"); - return true; - } - response.result = false; - if (ptr_wrapper_->setExposureMode(static_cast(request.payload_index), static_cast(request.exposure_mode))) - { - return false; - } - if (ptr_wrapper_->setShutterSpeed(static_cast(request.payload_index), static_cast(request.shutter_speed))) - { return false; } response.result = true; - return true; + response.result &= ptr_wrapper_->setExposureMode(static_cast(request.payload_index), + static_cast(request.exposure_mode)); + response.result &= ptr_wrapper_->setShutterSpeed(static_cast(request.payload_index), + static_cast(request.shutter_speed)); + return response.result; } bool VehicleNode::cameraSetApertureCallback(CameraAperture::Request& request, CameraAperture::Response& response) @@ -1140,19 +1295,14 @@ bool VehicleNode::cameraSetApertureCallback(CameraAperture::Request& request, Ca if(ptr_wrapper_ == nullptr) { ROS_ERROR_STREAM("Vehicle modules is nullptr"); - return true; - } - response.result = false; - if (ptr_wrapper_->setExposureMode(static_cast(request.payload_index), static_cast(request.exposure_mode))) - { - return false; - } - if (ptr_wrapper_->setAperture(static_cast(request.payload_index), static_cast(request.aperture))) - { return false; } response.result = true; - return true; + response.result &= ptr_wrapper_->setExposureMode(static_cast(request.payload_index), + static_cast(request.exposure_mode)); + response.result &= ptr_wrapper_->setAperture(static_cast(request.payload_index), + static_cast(request.aperture)); + return response.result; } bool VehicleNode::cameraSetISOCallback(CameraISO::Request& request, CameraISO::Response& response) @@ -1160,19 +1310,14 @@ bool VehicleNode::cameraSetISOCallback(CameraISO::Request& request, CameraISO::R if(ptr_wrapper_ == nullptr) { ROS_ERROR_STREAM("Vehicle modules is nullptr"); - return true; - } - response.result = false; - if (ptr_wrapper_->setExposureMode(static_cast(request.payload_index), static_cast(request.exposure_mode))) - { - return false; - } - if (ptr_wrapper_->setISO(static_cast(request.payload_index), static_cast(request.iso_data))) - { return false; } response.result = true; - return true; + response.result &= ptr_wrapper_->setExposureMode(static_cast(request.payload_index), + static_cast(request.exposure_mode)); + response.result &= ptr_wrapper_->setISO(static_cast(request.payload_index), + static_cast(request.iso_data)); + return response.result; } bool VehicleNode::cameraSetFocusPointCallback(CameraFocusPoint::Request& request, CameraFocusPoint::Response& response) @@ -1180,7 +1325,7 @@ bool VehicleNode::cameraSetFocusPointCallback(CameraFocusPoint::Request& request if(ptr_wrapper_ == nullptr) { ROS_ERROR_STREAM("Vehicle modules is nullptr"); - return true; + return false; } response.result = ptr_wrapper_->setFocusPoint(static_cast(request.payload_index), request.x, request.y); return response.result; @@ -1191,18 +1336,30 @@ bool VehicleNode::cameraSetTapZoomPointCallback(CameraTapZoomPoint::Request& req if(ptr_wrapper_ == nullptr) { ROS_ERROR_STREAM("Vehicle modules is nullptr"); - return true; + return false; } response.result = ptr_wrapper_->setTapZoomPoint(static_cast(request.payload_index),request.multiplier, request.x, request.y); return response.result; } +bool VehicleNode::cameraSetZoomParaCallback(CameraSetZoomPara::Request& request, CameraSetZoomPara::Response& response) +{ + if(ptr_wrapper_ == nullptr) + { + ROS_ERROR_STREAM("Vehicle modules is nullptr"); + return false; + } + + response.result = ptr_wrapper_->setZoom(static_cast(request.payload_index), request.factor); + return response.result; +} + bool VehicleNode::cameraZoomCtrlCallback(CameraZoomCtrl::Request& request, CameraZoomCtrl::Response& response) { if(ptr_wrapper_ == nullptr) { ROS_ERROR_STREAM("Vehicle modules is nullptr"); - return true; + return false; } if (request.start_stop == 1) { @@ -1220,7 +1377,7 @@ bool VehicleNode::cameraStartShootSinglePhotoCallback(CameraStartShootSinglePhot if(ptr_wrapper_ == nullptr) { ROS_ERROR_STREAM("Vehicle modules is nullptr"); - return true; + return false; } response.result = ptr_wrapper_->startShootSinglePhoto(static_cast(request.payload_index)); return response.result; @@ -1231,7 +1388,7 @@ bool VehicleNode::cameraStartShootAEBPhotoCallback(CameraStartShootAEBPhoto::Req if(ptr_wrapper_ == nullptr) { ROS_ERROR_STREAM("Vehicle modules is nullptr"); - return true; + return false; } response.result = ptr_wrapper_->startShootAEBPhoto(static_cast(request.payload_index), static_cast(request.photo_aeb_count)); return response.result; @@ -1242,7 +1399,7 @@ bool VehicleNode::cameraStartShootBurstPhotoCallback(CameraStartShootBurstPhoto: if(ptr_wrapper_ == nullptr) { ROS_ERROR_STREAM("Vehicle modules is nullptr"); - return true; + return false; } response.result = ptr_wrapper_->startShootBurstPhoto(static_cast(request.payload_index),static_cast(request.photo_burst_count)); return response.result; @@ -1253,7 +1410,7 @@ bool VehicleNode::cameraStartShootIntervalPhotoCallback(CameraStartShootInterval if(ptr_wrapper_ == nullptr) { ROS_ERROR_STREAM("Vehicle modules is nullptr"); - return true; + return false; } PhotoIntervalData photoIntervalData; photoIntervalData.photoNumConticap = request.photo_num_conticap; @@ -1267,7 +1424,7 @@ bool VehicleNode::cameraStopShootPhotoCallback(CameraStopShootPhoto::Request& re if(ptr_wrapper_ == nullptr) { ROS_ERROR_STREAM("Vehicle modules is nullptr"); - return true; + return false; } response.result = ptr_wrapper_->shootPhotoStop(static_cast(request.payload_index)); return response.result; @@ -1278,7 +1435,7 @@ bool VehicleNode::cameraRecordVideoActionCallback(CameraRecordVideoAction::Reque if(ptr_wrapper_ == nullptr) { ROS_ERROR_STREAM("Vehicle modules is nullptr"); - return true; + return false; } if(request.start_stop == 1) { @@ -1291,13 +1448,141 @@ 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.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 failed!"); + return false; + } + return true; +} + +bool VehicleNode::getSingleBatteryDynamicInfoCallback(GetSingleBatteryDynamicInfo::Request& request, + GetSingleBatteryDynamicInfo::Response& response) +{ + ROS_INFO_STREAM("get Single Battery Dynamic Info callback"); + if(ptr_wrapper_ == nullptr) + { + ROS_ERROR_STREAM("Vehicle modules is nullptr"); + return false; + } + + DJI::OSDK::SmartBatteryDynamicInfo SmartBatteryDynamicInfo; + if (ptr_wrapper_->getSingleBatteryDynamicInfo(static_cast(request.batteryIndex), + SmartBatteryDynamicInfo)) + { + response.smartBatteryDynamicInfo.batteryIndex = SmartBatteryDynamicInfo.batteryIndex; + response.smartBatteryDynamicInfo.currentVoltage = SmartBatteryDynamicInfo.currentVoltage; + response.smartBatteryDynamicInfo.currentElectric = SmartBatteryDynamicInfo.currentElectric; + response.smartBatteryDynamicInfo.fullCapacity = SmartBatteryDynamicInfo.fullCapacity; + response.smartBatteryDynamicInfo.remainedCapacity = SmartBatteryDynamicInfo.remainedCapacity; + response.smartBatteryDynamicInfo.batteryTemperature = SmartBatteryDynamicInfo.batteryTemperature; + response.smartBatteryDynamicInfo.cellCount = SmartBatteryDynamicInfo.cellCount; + response.smartBatteryDynamicInfo.batteryCapacityPercent = SmartBatteryDynamicInfo.batteryCapacityPercent; + response.smartBatteryDynamicInfo.SOP = SmartBatteryDynamicInfo.SOP; + + response.smartBatteryDynamicInfo.batteryState.cellBreak = SmartBatteryDynamicInfo.batteryState.cellBreak; + response.smartBatteryDynamicInfo.batteryState.selfCheckError = SmartBatteryDynamicInfo.batteryState.selfCheckError; + response.smartBatteryDynamicInfo.batteryState.batteryClosedReason = SmartBatteryDynamicInfo.batteryState.batteryClosedReason; + response.smartBatteryDynamicInfo.batteryState.batSOHState = SmartBatteryDynamicInfo.batteryState.batSOHState; + response.smartBatteryDynamicInfo.batteryState.maxCycleLimit = SmartBatteryDynamicInfo.batteryState.maxCycleLimit; + response.smartBatteryDynamicInfo.batteryState.batteryCommunicationAbnormal = SmartBatteryDynamicInfo.batteryState.batteryCommunicationAbnormal; + response.smartBatteryDynamicInfo.batteryState.hasCellBreak = SmartBatteryDynamicInfo.batteryState.hasCellBreak; + response.smartBatteryDynamicInfo.batteryState.heatState = SmartBatteryDynamicInfo.batteryState.heatState; + } + else + { + DSTATUS("get Single Battery Dynamic Info failed!"); + return false; + } + + return true; +} + +bool VehicleNode::getHMSDataCallback(GetHMSData::Request& request, GetHMSData::Response& response) +{ + //ROS_INFO_STREAM("Get HMS Data callback"); + if(ptr_wrapper_ == nullptr) + { + ROS_ERROR_STREAM("Vehicle modules is nullptr"); + return false; + } + response.result = true; + + static uint8_t count = 0; + while (count < 1) + { + response.result = ptr_wrapper_->enableSubscribeHMSInfo(request.enable); + count++; + } + + if (request.enable == true) + { + dji_osdk_ros::HMSPushPacket hmsPushPacket; + ptr_wrapper_->getHMSListInfo(hmsPushPacket); + ptr_wrapper_->getHMSDeviceIndex(response.deviceIndex); + response.timeStamp = hmsPushPacket.timeStamp; + + 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].reportLevel = hmsPushPacket.hmsPushData.errList[i].reportLevel; + response.errList[i].sensorIndex = hmsPushPacket.hmsPushData.errList[i].sensorIndex; + // DSTATUS("%ld, response.errList.size():%d,0x%08x,%d,%d",response.timeStamp,response.errList.size(),response.errList[i].alarmID, + // response.errList[i].sensorIndex, + // response.errList[i].reportLevel); + } + } + + return response.result; +} + bool VehicleNode::mfioCtrlCallback(MFIO::Request& request, MFIO::Response& response) { ROS_INFO_STREAM("MFIO Control callback"); if(ptr_wrapper_ == nullptr) { ROS_ERROR_STREAM("Vehicle modules is nullptr"); - return true; + return false; } if(request.mode == MFIO::Request::MODE_PWM_OUT || @@ -1331,7 +1616,7 @@ bool VehicleNode::setGoHomeAltitudeCallback(SetGoHomeAltitude::Request& request, if(ptr_wrapper_ == nullptr) { ROS_ERROR_STREAM("Vehicle modules is nullptr"); - return true; + return false; } if(request.altitude < 5) { @@ -1350,16 +1635,37 @@ bool VehicleNode::setGoHomeAltitudeCallback(SetGoHomeAltitude::Request& request, return true; } -bool VehicleNode::setHomeCallback(SetNewHomePoint::Request& request, SetNewHomePoint::Response& response) +bool VehicleNode::getGoHomeAltitudeCallback(GetGoHomeAltitude::Request& request, GetGoHomeAltitude::Response& response) { - ROS_INFO_STREAM("Set new home point callback"); + ROS_INFO_STREAM("Get go home altitude callback"); if(ptr_wrapper_ == nullptr) { ROS_ERROR_STREAM("Vehicle modules is nullptr"); - return true; + return false; + } + + uint16_t altitude = 0; + if (!(ptr_wrapper_->getHomeAltitude(altitude))) + { + response.result = false; + return false; + } + + response.altitude = altitude; + response.result = true; + return true; +} + +bool VehicleNode::setCurrentAircraftLocAsHomeCallback(SetCurrentAircraftLocAsHomePoint::Request& request, SetCurrentAircraftLocAsHomePoint::Response& response) +{ + ROS_INFO_STREAM("Set current aircraft location as new home point callback"); + if(ptr_wrapper_ == nullptr) + { + ROS_ERROR_STREAM("Vehicle modules is nullptr"); + return false; } - if(ptr_wrapper_->setNewHomeLocation() == true) + if(ptr_wrapper_->setCurrentAircraftLocAsHomePoint() == true) { response.result = true; } @@ -1371,10 +1677,29 @@ bool VehicleNode::setHomeCallback(SetNewHomePoint::Request& request, SetNewHomeP return true; } +bool VehicleNode::setHomePointCallback(SetHomePoint::Request& request, SetHomePoint::Response& response) +{ + ROS_INFO_STREAM("Set home point callback"); + if(ptr_wrapper_ == nullptr) + { + ROS_ERROR_STREAM("Vehicle modules is nullptr"); + return false; + } + + if (ptr_wrapper_->setHomePoint(request.latitude, request.longitude)) + { + response.result = true; + return true; + } + + response.result = false; + return false; +} + bool VehicleNode::setLocalPosRefCallback(dji_osdk_ros::SetLocalPosRef::Request &request, dji_osdk_ros::SetLocalPosRef::Response &response) { - printf("Currrent GPS health is %d \n",current_gps_health_ ); + ROS_INFO("Currrent GPS health is %d",current_gps_health_ ); if (current_gps_health_ > 3) { local_pos_ref_latitude_ = current_gps_latitude_; @@ -1403,48 +1728,124 @@ bool VehicleNode::setLocalPosRefCallback(dji_osdk_ros::SetLocalPosRef::Request & return true; } -bool VehicleNode::setAvoidCallback(AvoidEnable::Request& request, AvoidEnable::Response& response) +bool VehicleNode::setHorizonAvoidCallback(SetAvoidEnable::Request& request, SetAvoidEnable::Response& response) { - ROS_INFO_STREAM("Set avoid function callback"); + ROS_INFO_STREAM("Set horizon avoid function callback"); if(ptr_wrapper_ == nullptr) { ROS_ERROR_STREAM("Vehicle modules is nullptr"); - return true; + return false; } - if(ptr_wrapper_->setAvoid(request.enable) == true) + if (!(ptr_wrapper_->setCollisionAvoidance(request.enable))) { - response.result = true; + response.result = false; + return false; } - else + + response.result = true; + return true; +} + +bool VehicleNode::setUpwardsAvoidCallback(SetAvoidEnable::Request& request, SetAvoidEnable::Response& response) +{ + ROS_INFO_STREAM("Set upwards avoid function callback"); + if(ptr_wrapper_ == nullptr) + { + ROS_ERROR_STREAM("Vehicle modules is nullptr"); + return false; + } + + if(!(ptr_wrapper_->setUpwardsAvoidance(request.enable))) { response.result = false; + return false; } + + response.result = true; return true; } -bool VehicleNode::setUpwardsAvoidCallback(AvoidEnable::Request& request, AvoidEnable::Response& response) +bool VehicleNode::getAvoidEnableStatusCallback(GetAvoidEnable::Request& request, GetAvoidEnable::Response& response) { ROS_INFO_STREAM("Set upwards avoid function callback"); if(ptr_wrapper_ == nullptr) { ROS_ERROR_STREAM("Vehicle modules is nullptr"); - return true; + return false; } - if(ptr_wrapper_->setUpwardsAvoidance(request.enable) == true) + uint8_t get_horizon_avoid_enable_status = 0xF; + uint8_t get_upwards_avoid_enable_status = 0xF; + + if (!(ptr_wrapper_->getCollisionAvoidance(get_horizon_avoid_enable_status))) { - response.result = true; + response.result = false; + return false; } - else + response.horizon_avoid_enable_status = get_horizon_avoid_enable_status; + + if(!(ptr_wrapper_->getUpwardsAvoidance(get_upwards_avoid_enable_status))) { response.result = false; + return false; } + response.upwards_avoid_enable_status = get_horizon_avoid_enable_status; + + response.result = true; return true; } +bool VehicleNode::obtainReleaseControlAuthorityCallback(ObtainControlAuthority::Request& request, ObtainControlAuthority::Response& response) +{ + if(request.enable_obtain) + { + ROS_INFO_STREAM("Obtain Control Authority Callback"); + } + else + { + ROS_INFO_STREAM("release Control Authority Callback"); + } + + if(ptr_wrapper_ == nullptr) + { + ROS_ERROR_STREAM("Vehicle modules is nullptr"); + return false; + } + + response.result = ptr_wrapper_->obtainReleaseCtrlAuthority(request.enable_obtain, FLIGHT_CONTROL_WAIT_TIMEOUT); + + return response.result; +} + +bool VehicleNode::killSwitchCallback(KillSwitch::Request& request, KillSwitch::Response& response) +{ + if(ptr_wrapper_ == nullptr) + { + ROS_ERROR_STREAM("Vehicle modules is nullptr"); + return false; + } + char msg[10] = "StopFLy"; + response.result = ptr_wrapper_->killSwitch(request.enable, msg); + + return response.result; +} + +bool VehicleNode::emergencyBrakeCallback(EmergencyBrake::Request& request, EmergencyBrake::Response& response) +{ + if(ptr_wrapper_ == nullptr) + { + ROS_ERROR_STREAM("Vehicle modules is nullptr"); + return false; + } + + response.result = ptr_wrapper_->emergencyBrake(); + + return response.result; +} + int main(int argc, char** argv) { ros::init(argc, argv, "vehicle_node"); diff --git a/src/dji_osdk_ros/modules/CMakeLists.txt b/src/dji_osdk_ros/modules/CMakeLists.txt index 3f76e9b8..2ac75ce9 100644 --- a/src/dji_osdk_ros/modules/CMakeLists.txt +++ b/src/dji_osdk_ros/modules/CMakeLists.txt @@ -77,12 +77,12 @@ elseif() message(STATUS "Did not find CUDA in the system") endif() -find_package(darknet_ros QUIET) -if(darknet_ros_FOUND) - message(STATUS "Found darknet_ros package, will use it for object depth perception sample.") - add_definitions(-DUSE_DARKNET_ROS) - set(ENABLE_DARKNET_ROS 1 CACHE INTERNAL "ENABLE_DARKNET_ROS") -endif() +#find_package(darknet_ros QUIET) +#if(darknet_ros_FOUND) +# message(STATUS "Found darknet_ros package, will use it for object depth perception sample.") +# add_definitions(-DUSE_DARKNET_ROS) +# set(ENABLE_DARKNET_ROS 1 CACHE INTERNAL "ENABLE_DARKNET_ROS") +#endif() if (OPEN_CV_3_3_0_INSTALLED AND FOUND_OPENCV_VIZ) FILE(GLOB SOURCES *.cc *.cpp *.c *.cxx stereo_utility/*cpp) @@ -90,7 +90,9 @@ if (OPEN_CV_3_3_0_INSTALLED AND FOUND_OPENCV_VIZ) else() FILE(GLOB SOURCES *.cc *.cpp *.c *.cxx) endif() - +FILE(GLOB SOURCES *.cc *.cpp *.c *.cxx stereo_utility/*cpp) +include_directories(../../../include/dji_osdk_ros/stereo_utility) + ADD_LIBRARY(${PROJECT_NAME} SHARED ${HEADERS} ${SOURCES}) MESSAGE(STATUS "PROJECT_NAME: ${PROJECT_NAME}") MESSAGE(STATUS "DJIOSDK_LIB: ${DJIOSDK_LIBRARIES}") @@ -114,4 +116,4 @@ target_include_directories(${PROJECT_NAME} PRIVATE ${OpenCV_INCLUDE_DIRS}) if (OpenCV_FOUND) target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBS}) -endif () \ No newline at end of file +endif () diff --git a/src/dji_osdk_ros/modules/dji_vehicle_node_mission_services.cpp b/src/dji_osdk_ros/modules/dji_vehicle_node_mission_services.cpp index 825134af..4837b943 100644 --- a/src/dji_osdk_ros/modules/dji_vehicle_node_mission_services.cpp +++ b/src/dji_osdk_ros/modules/dji_vehicle_node_mission_services.cpp @@ -726,20 +726,6 @@ bool VehicleNode::waypointV2GenerateActionsCallback( { switch(request.actions[i].waypointV2ActionTriggerType) { - case dji_osdk_ros::WaypointV2Action::DJIWaypointV2ActionTriggerTypeReachPoint: - { - DJI::OSDK::DJIWaypointV2ReachPointTriggerParam param; - param.startIndex = request.actions[i].waypointV2ReachpointTrigger.startIndex; - param.endIndex = request.actions[i].waypointV2ReachpointTrigger.endIndex; - param.intervalWPNum = request.actions[i].waypointV2ReachpointTrigger.intervalWPNum; - param.waypointCountToTerminate = request.actions[i].waypointV2ReachpointTrigger.waypointCountToTerminate; - trigger = new DJI::OSDK::DJIWaypointV2Trigger( - DJI::OSDK::DJIWaypointV2ActionTriggerTypeReachPoint, ¶m); - response.result = true; - - break; - } - case dji_osdk_ros::WaypointV2Action::DJIWaypointV2ActionTriggerTypeActionAssociated: { DJI::OSDK::DJIWaypointV2AssociateTriggerParam param; @@ -811,8 +797,10 @@ bool VehicleNode::waypointV2GenerateActionsCallback( DJI::OSDK::DJIWaypointV2CameraFocusParam focusParam; focusParam.focusTarget.x = request.actions[i].waypointV2CameraActuator.focusParam.x; focusParam.focusTarget.y = request.actions[i].waypointV2CameraActuator.focusParam.y; + focusParam.regionType = request.actions[i].waypointV2CameraActuator.focusParam.regionType; + focusParam.width = request.actions[i].waypointV2CameraActuator.focusParam.width; + focusParam.height = request.actions[i].waypointV2CameraActuator.focusParam.height; focusParam.retryTimes = request.actions[i].waypointV2CameraActuator.focusParam.retryTimes; - focusParam.focusDelayTime = request.actions[i].waypointV2CameraActuator.focusParam.focusDelayTime; cameraActuatorParam = new DJI::OSDK::DJIWaypointV2CameraActuatorParam(DJI::OSDK::DJIWaypointV2ActionActuatorCameraOperationTypeFocus, &focusParam); } else if(request.actions[i].waypointV2CameraActuator.DJIWaypointV2ActionActuatorCameraOperationType == diff --git a/src/dji_osdk_ros/modules/dji_vehicle_node_publisher.cpp b/src/dji_osdk_ros/modules/dji_vehicle_node_publisher.cpp index b6a15d2c..b7de10b8 100644 --- a/src/dji_osdk_ros/modules/dji_vehicle_node_publisher.cpp +++ b/src/dji_osdk_ros/modules/dji_vehicle_node_publisher.cpp @@ -815,8 +815,42 @@ void VehicleNode::publishVGAStereoImage(Vehicle* vehicle, img.header.frame_id = "vga_right"; memcpy((char*)(&img.data[0]), recvFrame.recvData.stereoVGAImgData->img_vec[1], 480*640); node_ptr->stereo_vga_front_right_publisher_.publish(img); + node_ptr->publishCameraInfo(img.header); + } +#ifdef ADVANCED_SENSING +void VehicleNode::publishCameraInfo(const std_msgs::Header &header) +{ + static bool isFirstTime = true; + + static sensor_msgs::CameraInfo left_camera_info; + static sensor_msgs::CameraInfo right_camera_info; + + if(isFirstTime) + { + left_camera_info.distortion_model = right_camera_info.distortion_model = "plumb_bob"; + left_camera_info.width = right_camera_info.width = 640; + left_camera_info.height = right_camera_info.height = 480; + + + Vehicle* vehicle = ptr_wrapper_->getVehicle(); + M300StereoParamTool *tool = new M300StereoParamTool(vehicle); + Perception::CamParamType stereoParam = tool->getM300stereoParams(Perception::DirectionType::RECTIFY_DOWN); + + tool->getM300stereoCameraInfo(stereoParam, left_camera_info, right_camera_info); + isFirstTime = false; + } + + left_camera_info.header = right_camera_info.header = header; + left_camera_info.header.frame_id = "left_camera"; + right_camera_info.header.frame_id = "right_camera"; + left_camera_info_pub_.publish(left_camera_info); + right_camera_info_pub_.publish(right_camera_info); + +} +#endif + void VehicleNode::publishMainCameraImage(CameraRGBImage rgbImg, void* userData) { VehicleNode *node_ptr = (VehicleNode *)userData; @@ -856,6 +890,7 @@ void VehicleNode::publishCameraH264(uint8_t* buf, int bufLen, void* userData) VehicleNode *node_ptr = reinterpret_cast(userData); std::vector tempRawData(buf, buf+bufLen); sensor_msgs::Image img; + img.header.stamp = ros::Time::now(); img.data = tempRawData; node_ptr->camera_h264_publisher_.publish(img); } @@ -865,4 +900,4 @@ void VehicleNode::publishCameraH264(uint8_t* buf, int bufLen, void* userData) } } -#endif \ No newline at end of file +#endif diff --git a/src/dji_osdk_ros/modules/stereo_utility/m300_stereo_param_tool.cpp b/src/dji_osdk_ros/modules/stereo_utility/m300_stereo_param_tool.cpp index cc0d7a7d..781f5a40 100755 --- a/src/dji_osdk_ros/modules/stereo_utility/m300_stereo_param_tool.cpp +++ b/src/dji_osdk_ros/modules/stereo_utility/m300_stereo_param_tool.cpp @@ -51,13 +51,78 @@ void M300StereoParamTool::PerceptionCamParamCB( if ((pack.directionNum > 0) && (pack.directionNum <= IMAGE_MAX_DIRECTION_NUM)) for (int i = 0; i < pack.directionNum; i++) { if ((userData) - && (pack.cameraParam[i].direction == Perception::RECTIFY_FRONT)) { + && (pack.cameraParam[i].direction == Perception::RECTIFY_UP)) { auto camParam = (Perception::CamParamType *) userData; *camParam = pack.cameraParam[i]; } } } +void M300StereoParamTool::getM300stereoCameraInfo(Perception::CamParamType param, sensor_msgs::CameraInfo &left, + sensor_msgs::CameraInfo &right) +{ + DoubleCamParamType doubleCameraParams; + doubleCameraParams.direction = param.direction; + + /*! Convert float camera parameters matrix to be double matrix */ + for (int i = 0; i < sizeof(param.leftIntrinsics); i++) + { + doubleCameraParams.leftIntrinsics[i] = (double)param.leftIntrinsics[i]; + doubleCameraParams.rightIntrinsics[i] = (double)param.rightIntrinsics[i]; + doubleCameraParams.rotaionLeftInRight[i] = (double)param.rotaionLeftInRight[i]; + } + for (int i = 0; i < sizeof(param.translationLeftInRight); i++) + doubleCameraParams.translationLeftInRight[i] = (double)param.translationLeftInRight[i]; + + cv::Mat leftCameraIntrinsicMatrix(3, 3, CV_64F, doubleCameraParams.leftIntrinsics); + cv::Mat rightCameraIntrinsicMatrix(3, 3, CV_64F, doubleCameraParams.rightIntrinsics); + + + //cv::Mat leftDistCoeffs = cv::Mat::zeros(5, 1, CV_64F); + //cv::Mat rightDistCoeffs = cv::Mat::zeros(5, 1, CV_64F); + //loading the distortion matrix + left.D = std::vector(5,0.0); + right.D = std::vector(5,0.0); + + cv::Mat leftRectificationMatrix = cv::Mat::eye(3, 3, CV_64F); + cv::Mat rightRectificationMatrix = cv::Mat::eye(3, 3, CV_64F); + cv::Mat translationLeftInRight(3, 1, CV_64F, doubleCameraParams.translationLeftInRight); + cv::Mat leftProjectionMatrix(leftCameraIntrinsicMatrix.rows, leftCameraIntrinsicMatrix.cols + translationLeftInRight.cols, CV_64F); + cv::hconcat(leftCameraIntrinsicMatrix, translationLeftInRight, leftProjectionMatrix); + cv::Mat rightProjectionMatrix(rightCameraIntrinsicMatrix.rows, rightCameraIntrinsicMatrix.cols + translationLeftInRight.cols, CV_64F); + cv::hconcat(rightCameraIntrinsicMatrix, translationLeftInRight, rightProjectionMatrix); + + //loading the rectification matrix + for(int r = 0; r < leftRectificationMatrix.rows; r++) + { + for(int c = 0; c < leftRectificationMatrix.cols; c++) + { + left.R[r*leftCameraIntrinsicMatrix.cols + c] = leftRectificationMatrix.at(r,c); + right.R[r*rightRectificationMatrix.cols + c] = rightRectificationMatrix.at(r,c); + } + } + + //loading the camera intrinsic matrix + for(int r = 0; r < leftCameraIntrinsicMatrix.rows; r++) + { + for(int c = 0; c < leftCameraIntrinsicMatrix.cols; c++) + { + left.K[r*leftCameraIntrinsicMatrix.cols + c] = leftCameraIntrinsicMatrix.at(r,c); + right.K[r*rightCameraIntrinsicMatrix.cols + c] = rightCameraIntrinsicMatrix.at(r,c); + } + } + + //loading the projection matrix + for(int r = 0; r < leftProjectionMatrix.rows; r++) + { + for(int c = 0; c < leftProjectionMatrix.cols; c++) + { + left.P[r*leftProjectionMatrix.cols + c] = leftProjectionMatrix.at(r,c); + right.P[r*rightProjectionMatrix.cols + c] = rightProjectionMatrix.at(r,c); + } + } +} + bool M300StereoParamTool::createStereoParamsYamlFile(std::string fileName, Perception::CamParamType param) { DoubleCamParamType doubleCameraParams; diff --git a/src/dji_osdk_ros/modules/vehicle_wrapper.cpp b/src/dji_osdk_ros/modules/vehicle_wrapper.cpp index 2457cc39..c48ea2e4 100644 --- a/src/dji_osdk_ros/modules/vehicle_wrapper.cpp +++ b/src/dji_osdk_ros/modules/vehicle_wrapper.cpp @@ -35,7 +35,6 @@ #include #include - //CODE namespace dji_osdk_ros { @@ -189,14 +188,7 @@ static T_OsdkOsalHandler osalHandler = { ACK::getErrorCodeMessage(ack, __func__); return false; } - ack = vehicle->control->obtainCtrlAuthority(timeout_); - if (ACK::getError(ack)) - { - ACK::getErrorCodeMessage(ack, __func__); - return false; - } - - return true; + return true; } bool VehicleWrapper::setEV(const PayloadIndex& payloadIndex, const ExposureCompensation& exposureCompensation) @@ -213,26 +205,23 @@ static T_OsdkOsalHandler osalHandler = { retCode = pm->getExposureCompensationSync(index, evGet, 1); if (retCode == ErrorCode::SysCommonErr::Success) { DSTATUS("Get ev = %d", evGet); - if (dataTarget != evGet) { - DSTATUS("Set evTarget = %d", dataTarget); - retCode = pm->setExposureCompensationSync(index, dataTarget, 1); - if (retCode == ErrorCode::SysCommonErr::Success) { - DSTATUS("Set ev value successfully."); - } else { - DERROR("Set ev parameter error. Error code : 0x%lX", retCode); - ErrorCode::printErrorCodeMsg(retCode); - DERROR( - "In order to use this function, the camera exposure mode should be " - "set to be PROGRAM_AUTO, SHUTTER_PRIORITY or APERTURE_PRIORITY " - "first"); - return false; - } - } else { + if (dataTarget == evGet) { DSTATUS("The ev value is already %d.", dataTarget); + return true; } + } + + DSTATUS("Set evTarget = %d", dataTarget); + retCode = pm->setExposureCompensationSync(index, dataTarget, 1); + if (retCode == ErrorCode::SysCommonErr::Success) { + DSTATUS("Set ev value successfully."); } else { - DERROR("Get ev error. Error code : 0x%lX", retCode); + DERROR("Set ev parameter error. Error code : 0x%lX", retCode); ErrorCode::printErrorCodeMsg(retCode); + DERROR( + "In order to use this function, the camera exposure mode should be " + "set to be PROGRAM_AUTO, SHUTTER_PRIORITY or APERTURE_PRIORITY " + "first"); return false; } @@ -251,24 +240,22 @@ static T_OsdkOsalHandler osalHandler = { PayloadIndexType index = static_cast(payloadIndex); CameraModule::ExposureMode dataTarget = static_cast(exposureMode); + retCode = pm->getExposureModeSync(index, exposureModeGet, 1); if (retCode == ErrorCode::SysCommonErr::Success) { DSTATUS("Get exposure mode = %d", exposureModeGet); - if (dataTarget != exposureModeGet) { - DSTATUS("Set exposure mode = %d", dataTarget); - retCode = pm->setExposureModeSync(index, dataTarget, 1); - if (retCode == ErrorCode::SysCommonErr::Success) { - DSTATUS("Set exposure mode successfully."); - } else { - DERROR("Set exposure mode error. Error code : 0x%lX", retCode); - ErrorCode::printErrorCodeMsg(retCode); - return false; - } - } else { + if (dataTarget == exposureModeGet) { DSTATUS("The exposure mode is already %d.", dataTarget); + return true; } + } + + DSTATUS("Set exposure mode = %d", dataTarget); + retCode = pm->setExposureModeSync(index, dataTarget, 1); + if (retCode == ErrorCode::SysCommonErr::Success) { + DSTATUS("Set exposure mode successfully."); } else { - DERROR("Get exposure mode error. Error code : 0x%lX", retCode); + DERROR("Set exposure mode error. Error code : 0x%lX", retCode); ErrorCode::printErrorCodeMsg(retCode); return false; } @@ -292,26 +279,23 @@ static T_OsdkOsalHandler osalHandler = { retCode = pm->getISOSync(index, isoGet, 1); if (retCode == ErrorCode::SysCommonErr::Success) { DSTATUS("Get iso = %d", isoGet); - if (dataTarget != isoGet) { - DSTATUS("Set iso = %d", dataTarget); - retCode = pm->setISOSync(index, dataTarget, 1); - if (retCode == ErrorCode::SysCommonErr::Success) { - DSTATUS("Set iso successfully"); - } else { - DERROR("Set ISO parameter error. Error code : 0x%lX", retCode); - ErrorCode::printErrorCodeMsg(retCode); - DERROR( - "For the X5, X5R, X4S and X5S, the ISO value can be set for all " - "modes. For the other cameras, the ISO value can only be set when " - "the camera exposure mode is in Manual mode."); - return false; - } - } else { + if (dataTarget == isoGet) { DSTATUS("The iso parameter is already %d.", dataTarget); + return true; } + } + + DSTATUS("Set iso = %d", dataTarget); + retCode = pm->setISOSync(index, dataTarget, 1); + if (retCode == ErrorCode::SysCommonErr::Success) { + DSTATUS("Set iso successfully"); } else { - DERROR("Get iso error. Error code : 0x%lX", retCode); + DERROR("Set ISO parameter error. Error code : 0x%lX", retCode); ErrorCode::printErrorCodeMsg(retCode); + DERROR( + "For the X5, X5R, X4S and X5S, the ISO value can be set for all " + "modes. For the other cameras, the ISO value can only be set when " + "the camera exposure mode is in Manual mode."); return false; } @@ -334,27 +318,24 @@ static T_OsdkOsalHandler osalHandler = { retCode = pm->getShutterSpeedSync(index, shutterSpeedGet, 1); if (retCode == ErrorCode::SysCommonErr::Success) { DSTATUS("Get shutterSpeed = %d", shutterSpeedGet); - if (dataTarget != shutterSpeedGet) { - DSTATUS("Set shutterSpeed = %d", dataTarget); - retCode = pm->setShutterSpeedSync(index, dataTarget, 1); - if (retCode == ErrorCode::SysCommonErr::Success) { - DSTATUS("Set iso successfully"); - } else { - DERROR("Set shutterSpeed parameter error. Error code : 0x%lX", retCode); - ErrorCode::printErrorCodeMsg(retCode); - DERROR( - "The shutter speed can be set only when the camera exposure mode " - "is Shutter mode or Manual mode. The shutter speed should not be " - "set slower than the video frame rate when the camera's mode is " - "RECORD_VIDEO."); - return false; - } - } else { + if (dataTarget == shutterSpeedGet) { DSTATUS("The shutterSpeed is already %d.", dataTarget); + return true; } + } + + DSTATUS("Set shutterSpeed = %d", dataTarget); + retCode = pm->setShutterSpeedSync(index, dataTarget, 1); + if (retCode == ErrorCode::SysCommonErr::Success) { + DSTATUS("Set iso successfully"); } else { - DERROR("Get shutterSpeed error. Error code : 0x%lX", retCode); + DERROR("Set shutterSpeed parameter error. Error code : 0x%lX", retCode); ErrorCode::printErrorCodeMsg(retCode); + DERROR( + "The shutter speed can be set only when the camera exposure mode " + "is Shutter mode or Manual mode. The shutter speed should not be " + "set slower than the video frame rate when the camera's mode is " + "RECORD_VIDEO."); return false; } @@ -376,27 +357,24 @@ static T_OsdkOsalHandler osalHandler = { retCode = pm->getApertureSync(index, apertureGet, 1); if (retCode == ErrorCode::SysCommonErr::Success) { DSTATUS("Get aperture = %d", apertureGet); - if (dataTarget != apertureGet) { - DSTATUS("Set aperture = %d", dataTarget); - retCode = pm->setApertureSync(index, dataTarget, 1); - if (retCode == ErrorCode::SysCommonErr::Success) { - DSTATUS("Set aperture successfully"); - } else { - DERROR("Set aperture parameter error. Error code : 0x%lX", retCode); - ErrorCode::printErrorCodeMsg(retCode); - DERROR( - "In order to use this function, the exposure mode ExposureMode " - "must be in MANUAL or APERTURE_PRIORITY. Supported only by the X5, " - "X5R, X4S, X5S camera."); - return false; - } - } else { + if (dataTarget == apertureGet) { DSTATUS("The aperture is already %d.", dataTarget); + return true; } + } + + DSTATUS("Set aperture = %d", dataTarget); + retCode = pm->setApertureSync(index, dataTarget, 1); + if (retCode == ErrorCode::SysCommonErr::Success) { + DSTATUS("Set aperture successfully"); } else { - DERROR("Get aperture error. Error code : 0x%lX", retCode); + DERROR("Set aperture parameter error. Error code : 0x%lX", retCode); ErrorCode::printErrorCodeMsg(retCode); - return false; + DERROR( + "In order to use this function, the exposure mode ExposureMode " + "must be in MANUAL or APERTURE_PRIORITY. Supported only by the X5, " + "X5R, X4S, X5S camera."); + return false; } return true; @@ -434,6 +412,7 @@ static T_OsdkOsalHandler osalHandler = { "It's should be attention that X4S will keep focus point as (0.5,0.5) " "all the time, the setting of focus point to X4S will quickly replaced " "by (0.5, 0.5)."); + return false; } } else { DERROR("Set focus mode parameter error. Error code : 0x%lX", retCode); @@ -480,7 +459,7 @@ static T_OsdkOsalHandler osalHandler = { if (retCode != ErrorCode::SysCommonErr::Success) { DERROR("Set tap zoom multiplier fail. Error code : 0x%lX", retCode); ErrorCode::printErrorCodeMsg(retCode); - DERROR("It is only supported Z30 camera."); + DERROR("It is only supported Z30 camera or H20/H20T zoom mode."); return false; } @@ -490,7 +469,7 @@ static T_OsdkOsalHandler osalHandler = { if (retCode != ErrorCode::SysCommonErr::Success) { DERROR("Set tap zoom target fail. Error code : 0x%lX", retCode); ErrorCode::printErrorCodeMsg(retCode); - DERROR("It is only supported Z30 camera."); + DERROR("It is only supported Z30 camera or H20/H20T zoom mode."); return false; } else { DSTATUS( @@ -502,6 +481,48 @@ static T_OsdkOsalHandler osalHandler = { return true; } + bool VehicleWrapper::setZoom(const PayloadIndex& payloadIndex, float factor) + { + if (!vehicle || !vehicle->cameraManager) { + DERROR("vehicle or cameraManager is a null value."); + return ErrorCode::SysCommonErr::InstInitParamInvalid; + } + ErrorCode::ErrorCodeType retCode; + CameraManager *pm = vehicle->cameraManager; + + DSTATUS("Attention : It is only supported by X5, X5R and X5S camera on Osmo with" + "lens Olympus M.Zuiko ED 14-42mm f/3.5-5.6 EZ, Z3 camera, Z30 camera."); + + float curFactor = 0; + PayloadIndexType index = static_cast(payloadIndex); + retCode = pm->getOpticalZoomFactorSync(index, curFactor, 1); + if (retCode != ErrorCode::SysCommonErr::Success) { + DERROR("Get zoom parameter fail. Error code : 0x%lX", retCode); + ErrorCode::printErrorCodeMsg(retCode); + DSTATUS( + "Attention : It is only supported by X5, X5R and X5S camera on Osmo with" + "lens Olympus M.Zuiko ED 14-42mm f/3.5-5.6 EZ, Z3 camera, Z30 camera."); + return false; + } + DSTATUS("Got the current optical zoom factor : %0.1f", curFactor); + if (curFactor != factor) { + DSTATUS("Set the current optical zoom factor as %0.1f", factor); + retCode = pm->setOpticalZoomFactorSync(index, factor, 1); + + if (retCode != ErrorCode::SysCommonErr::Success) { + DERROR("Set zoom parameter fail. Error code : 0x%lX", retCode); + ErrorCode::printErrorCodeMsg(retCode); + return false; + } + return true; + } else { + DSTATUS("The current zoom factor is already : %0.2f", factor); + return true; + } + + return true; + } + bool VehicleWrapper::startZoom(const PayloadIndex& payloadIndex,const uint8_t& direction, const uint8_t& speed) { if (!vehicle || !vehicle->cameraManager) { @@ -567,20 +588,18 @@ static T_OsdkOsalHandler osalHandler = { return false; } - /*! @TODO XT* and Z30 don't support set shoot-photo mode. To fix it in the - * future */ - /*!< set shoot-photo mode + /*! @TODO XT* and Z30 don't support set shoot-photo mode. To fix it in the + * future */ + /*!< set shoot-photo mode */ DSTATUS("set shoot-photo mode as SINGLE"); - retCode = - pm->setShootPhotoModeSync(index, CameraModule::ShootPhotoMode::SINGLE, 1); + retCode = pm->setShootPhotoModeSync(index, CameraModule::ShootPhotoMode::SINGLE, 1); if (retCode != ErrorCode::SysCommonErr::Success) { DERROR("Set shoot-photo mode as SINGLE fail. Error code : 0x%lX", retCode); ErrorCode::printErrorCodeMsg(retCode); DERROR("If the camera is XT, XT2, or XTS, set shoot-photo mode interface is" - "temporarily not supported."); - return retCode; + "temporarily not supported."); + return false; } - */ /*! wait the APP change the shoot-photo mode display */ Platform::instance().taskSleepMs(500); @@ -843,335 +862,270 @@ static T_OsdkOsalHandler osalHandler = { return true; } - bool VehicleWrapper::monitoredTakeoff(ACK::ErrorCode& ack, int timeout) + bool VehicleWrapper::motorStartedCheck() { - //@todo: remove this once the getErrorCode function signature changes - char func[50]; - // Start takeoff - ack = vehicle->control->takeoff(timeout); - if (ACK::getError(ack) != ACK::SUCCESS) + if (!vehicle) { - ACK::getErrorCodeMessage(ack, func); + std::cout << "Vehicle is a null value!" << std::endl; return false; } - // First check: Motors started int motorsNotStarted = 0; - int timeoutCycles = 20; - - if (!vehicle->isM100() && !vehicle->isLegacyM600()) - { - while (vehicle->subscribe->getValue() != - VehicleStatus::FlightStatus::ON_GROUND && - vehicle->subscribe->getValue() != - VehicleStatus::DisplayMode::MODE_ENGINE_START && - motorsNotStarted < timeoutCycles) - { - motorsNotStarted++; - usleep(100000); - } - - if (motorsNotStarted == timeoutCycles) - { - std::cout << "Takeoff failed. Motors are not spinning." << std::endl; - return false; - } - else - { - std::cout << "Motors spinning...\n"; - } + int timeoutCycles = 20; + while (vehicle->subscribe->getValue() != + VehicleStatus::FlightStatus::ON_GROUND && + vehicle->subscribe->getValue() != + VehicleStatus::DisplayMode::MODE_ENGINE_START && + motorsNotStarted < timeoutCycles) { + motorsNotStarted++; + usleep(100000); } - else if (vehicle->isLegacyM600()) - { - while ((vehicle->broadcast->getStatus().flight < - DJI::OSDK::VehicleStatus::FlightStatus::ON_GROUND) && - motorsNotStarted < timeoutCycles) - { - motorsNotStarted++; - usleep(100000); - } + return motorsNotStarted != timeoutCycles ? true : false; + } - if (motorsNotStarted < timeoutCycles) - { - std::cout << "Successful TakeOff!" << std::endl; - } - } - else // M100 + bool VehicleWrapper::takeOffInAirCheck() + { + if (!vehicle) { - while ((vehicle->broadcast->getStatus().flight < - DJI::OSDK::VehicleStatus::M100FlightStatus::TAKEOFF) && - motorsNotStarted < timeoutCycles) - { - motorsNotStarted++; - usleep(100000); - } - - if (motorsNotStarted < timeoutCycles) - { - std::cout << "Successful TakeOff!" << std::endl; - } + std::cout << "Vehicle is a null value!" << std::endl; + return false; } - // Second check: In air int stillOnGround = 0; - timeoutCycles = 110; + int timeoutCycles = 110; + while (vehicle->subscribe->getValue() != + VehicleStatus::FlightStatus::IN_AIR && + (vehicle->subscribe->getValue() != + VehicleStatus::DisplayMode::MODE_ASSISTED_TAKEOFF || + vehicle->subscribe->getValue() != + VehicleStatus::DisplayMode::MODE_AUTO_TAKEOFF) && + stillOnGround < timeoutCycles) { + stillOnGround++; + usleep(100000); + } - if (!vehicle->isM100() && !vehicle->isLegacyM600()) - { - while (vehicle->subscribe->getValue() != - VehicleStatus::FlightStatus::IN_AIR && - (vehicle->subscribe->getValue() != - VehicleStatus::DisplayMode::MODE_ASSISTED_TAKEOFF || - vehicle->subscribe->getValue() != - VehicleStatus::DisplayMode::MODE_AUTO_TAKEOFF) && - stillOnGround < timeoutCycles) - { - stillOnGround++; - usleep(100000); - } + return stillOnGround != timeoutCycles ? true : false; + } - if (stillOnGround == timeoutCycles) - { - std::cout << "Takeoff failed. Aircraft is still on the ground, but the " - "motors are spinning." - << std::endl; - return false; - } - else - { - std::cout << "Ascending...\n"; - } - } - else if (vehicle->isLegacyM600()) + bool VehicleWrapper::takeoffFinishedCheck() + { + if (!vehicle) { - while ((vehicle->broadcast->getStatus().flight < - DJI::OSDK::VehicleStatus::FlightStatus::IN_AIR) && - stillOnGround < timeoutCycles) - { - stillOnGround++; - usleep(100000); - } + std::cout << "Vehicle is a null value!" << std::endl; + return false; + } - if (stillOnGround < timeoutCycles) - { - std::cout << "Aircraft in air!" << std::endl; - } + while (vehicle->subscribe->getValue() == + VehicleStatus::DisplayMode::MODE_ASSISTED_TAKEOFF || + vehicle->subscribe->getValue() == + VehicleStatus::DisplayMode::MODE_AUTO_TAKEOFF) { + sleep(1); } - else // M100 - { - while ((vehicle->broadcast->getStatus().flight != - DJI::OSDK::VehicleStatus::M100FlightStatus::IN_AIR_STANDBY) && - stillOnGround < timeoutCycles) - { - stillOnGround++; - usleep(100000); - } + return ((vehicle->subscribe->getValue() == + VehicleStatus::DisplayMode::MODE_P_GPS) || + (vehicle->subscribe->getValue() == + VehicleStatus::DisplayMode::MODE_ATTITUDE)) + ? true + : false; + } - if (stillOnGround < timeoutCycles) - { - std::cout << "Aircraft in air!" << std::endl; - } + bool VehicleWrapper::landFinishedCheck() + { + if (!vehicle) + { + std::cout << "Vehicle is a null value!" << std::endl; + return false; } - // Final check: Finished takeoff - if (!vehicle->isM100() && !vehicle->isLegacyM600()) + while(vehicle->subscribe->getValue() == + VehicleStatus::DisplayMode::MODE_AUTO_LANDING && + vehicle->subscribe->getValue() == + VehicleStatus::FlightStatus::IN_AIR) { - while (vehicle->subscribe->getValue() == - VehicleStatus::DisplayMode::MODE_ASSISTED_TAKEOFF || - vehicle->subscribe->getValue() == - VehicleStatus::DisplayMode::MODE_AUTO_TAKEOFF) - { - sleep(1); - } + Platform::instance().taskSleepMs(1000); + } - if (!vehicle->isM100() && !vehicle->isLegacyM600()) - { - if (vehicle->subscribe->getValue() != + return ((vehicle->subscribe->getValue() != VehicleStatus::DisplayMode::MODE_P_GPS || vehicle->subscribe->getValue() != - VehicleStatus::DisplayMode::MODE_ATTITUDE) - { - std::cout << "Successful takeoff!\n"; - } - else - { - std::cout - << "Takeoff finished, but the aircraft is in an unexpected mode. " - "Please connect DJI GO.\n"; - return false; - } - } - } - else - { - float32_t delta; - Telemetry::GlobalPosition currentHeight; - Telemetry::GlobalPosition deltaHeight = - vehicle->broadcast->getGlobalPosition(); + VehicleStatus::DisplayMode::MODE_ATTITUDE)) ? true:false; + } - do - { - sleep(4); - currentHeight = vehicle->broadcast->getGlobalPosition(); - delta = fabs(currentHeight.altitude - deltaHeight.altitude); - deltaHeight.altitude = currentHeight.altitude; - } while (delta >= 0.009); + bool VehicleWrapper::monitoredTakeoff(int timeout) + { + //! Start takeoff + //ErrorCode::ErrorCodeType takeoffStatus = + vehicle->flightController->startTakeoffSync(timeout); - std::cout << "Aircraft hovering at " << currentHeight.altitude << "m!\n"; + //! Motors start check + if (!motorStartedCheck()) { + std::cout << "Takeoff failed. Motors are not spinning." << std::endl; + return false; + } else { + std::cout << "Motors spinning...\n"; + } + //! In air check + if (!takeOffInAirCheck()) { + std::cout << "Takeoff failed. Aircraft is still on the ground, but the " + "motors are spinning." + << std::endl; + return false; + } else { + std::cout << "Ascending...\n"; } + //! Finished takeoff check + if (takeoffFinishedCheck()) { + std::cout << "Successful takeoff!\n"; + } else { + std::cout << "Takeoff finished, but the aircraft is in an unexpected mode. " + "Please connect DJI GO.\n"; + return false; + } return true; } - bool VehicleWrapper::monitoredLanding(ACK::ErrorCode& ack, int timeout) - { - //@todo: remove this once the getErrorCode function signature changes - char func[50]; - // Start landing - ack = vehicle->control->land(timeout); - if (ACK::getError(ack) != ACK::SUCCESS) + bool VehicleWrapper::monitoredLanding(int timeout) + { + if (!vehicle) { - ACK::getErrorCodeMessage(ack, func); + std::cout << "Vehicle is a null value!" << std::endl; return false; } - // First check: Landing started - int landingNotStarted = 0; - int timeoutCycles = 20; - - if (!vehicle->isM100() && !vehicle->isLegacyM600()) - { - while (vehicle->subscribe->getValue() != - VehicleStatus::DisplayMode::MODE_AUTO_LANDING && - landingNotStarted < timeoutCycles) - { - landingNotStarted++; - usleep(100000); - } - } - else if (vehicle->isM100()) + /*! Step 1: Start landing */ + DSTATUS("Start landing action"); + ErrorCode::ErrorCodeType landingErrCode = vehicle->flightController->startLandingSync(timeout); + if (landingErrCode != ErrorCode::SysCommonErr::Success) { - while (vehicle->broadcast->getStatus().flight != - DJI::OSDK::VehicleStatus::M100FlightStatus::LANDING && - landingNotStarted < timeoutCycles) - { - landingNotStarted++; - usleep(100000); - } + DERROR( "Fail to execute landing action! Error code: " + "%llx\n ",landingErrCode); + return false; } - if (landingNotStarted == timeoutCycles) + /*! Step 2: check Landing start*/ + if (!checkActionStarted(VehicleStatus::DisplayMode::MODE_AUTO_LANDING)) { - std::cout << "Landing failed. Aircraft is still in the air." << std::endl; + DERROR("Fail to execute Landing action!"); return false; - } + } else { - std::cout << "Landing...\n"; - } - - // Second check: Finished landing - if (!vehicle->isM100() && !vehicle->isLegacyM600()) - { - while (vehicle->subscribe->getValue() == - VehicleStatus::DisplayMode::MODE_AUTO_LANDING && - vehicle->subscribe->getValue() == - VehicleStatus::FlightStatus::IN_AIR) + /*! Step 3: check Landing finished*/ + if (this->landFinishedCheck()) { - sleep(1); - } - - if (vehicle->subscribe->getValue() != - VehicleStatus::DisplayMode::MODE_P_GPS || - vehicle->subscribe->getValue() != - VehicleStatus::DisplayMode::MODE_ATTITUDE) - { - std::cout << "Successful landing!\n"; + DSTATUS("Successful landing!"); } else { - std::cout - << "Landing finished, but the aircraft is in an unexpected mode. " - "Please connect DJI GO.\n"; + DERROR("Landing finished, but the aircraft is in an unexpected mode. " + "Please connect DJI Assistant."); return false; } } - else if (vehicle->isLegacyM600()) + + return true; + } + + bool VehicleWrapper::startForceLanding(int timeout) + { + if (!vehicle) + { + std::cout << "Vehicle is a null value!" << std::endl; + return false; + } + ErrorCode::ErrorCodeType errCode = vehicle->flightController->startForceLandingSync(timeout); + if (errCode != ErrorCode::SysCommonErr::Success) { - while (vehicle->broadcast->getStatus().flight > - DJI::OSDK::VehicleStatus::FlightStatus::STOPED) - { - sleep(1); - } + DERROR( "Fail to execute force landing action! Error code: " + "%llx\n ",errCode); + return false; + } - Telemetry::GlobalPosition gp; - do - { - sleep(2); - gp = vehicle->broadcast->getGlobalPosition(); - } while (gp.altitude != 0); + return true; + } - if (gp.altitude != 0) - { - std::cout - << "Landing finished, but the aircraft is in an unexpected mode. " - "Please connect DJI GO.\n"; - return false; - } - else - { - std::cout << "Successful landing!\n"; - } + bool VehicleWrapper::startConfirmLanding(int timeout) + { + if (!vehicle) + { + std::cout << "Vehicle is a null value!" << std::endl; + return false; } - else // M100 + ErrorCode::ErrorCodeType errCode = vehicle->flightController->startConfirmLandingSync(timeout); + if (errCode != ErrorCode::SysCommonErr::Success) { - while (vehicle->broadcast->getStatus().flight == - DJI::OSDK::VehicleStatus::M100FlightStatus::FINISHING_LANDING) - { - sleep(1); - } + DERROR( "Fail to execute confirm landing action! Error code: " + "%llx\n ",errCode); + return false; + } - Telemetry::GlobalPosition gp; - do - { - sleep(2); - gp = vehicle->broadcast->getGlobalPosition(); - } while (gp.altitude != 0); + return true; + } - if (gp.altitude != 0) - { - std::cout - << "Landing finished, but the aircraft is in an unexpected mode. " - "Please connect DJI GO.\n"; - return false; - } - else - { - std::cout << "Successful landing!\n"; - } + bool VehicleWrapper::cancelLanding(int timeout) + { + if (!vehicle) + { + std::cout << "Vehicle is a null value!" << std::endl; + return false; + } + ErrorCode::ErrorCodeType errCode = vehicle->flightController->cancelLandingSync(timeout); + if (errCode != ErrorCode::SysCommonErr::Success) + { + DERROR( "Fail to execute cancel landing action! Error code: " + "%llx\n ",errCode); + return false; } return true; } - bool VehicleWrapper::goHome(ACK::ErrorCode& ack, int timeout) + bool VehicleWrapper::goHome( int timeout) { - ack = vehicle->control->goHome(timeout); - if (ACK::getError(ack)) + if (!vehicle) { - ACK::getErrorCodeMessage(ack, __func__); + std::cout << "Vehicle is a null value!" << std::endl; return false; } - else + ErrorCode::ErrorCodeType goHomeErrCode = vehicle->flightController->startGoHomeSync(timeout); + if (goHomeErrCode != ErrorCode::SysCommonErr::Success) { - return true; + DERROR( "Fail to execute gohome action! Error code: " + "%llx\n ",goHomeErrCode); + return false; } + + return true; } - bool VehicleWrapper::setUpSubscription(int pkgIndex, int freq, TopicName* topicList, - uint8_t topicSize, int timeout) + bool VehicleWrapper::cancelGoHome( int timeout) { + if (!vehicle) + { + std::cout << "Vehicle is a null value!" << std::endl; + return false; + } + ErrorCode::ErrorCodeType goHomeErrCode = vehicle->flightController->cancelGoHomeSync(timeout); + if (goHomeErrCode != ErrorCode::SysCommonErr::Success) + { + DERROR( "Fail to execute cancel go home action! Error code: " + "%llx\n ",goHomeErrCode); + return false; + } + + return true; + } + + bool VehicleWrapper::setUpSubscription(int pkgIndex, int freq, TopicName* topicList, + uint8_t topicSize, int timeout) + { + if (!vehicle) + { + std::cout << "Vehicle is a null value!" << std::endl; + return false; + } if (vehicle) { /*! Telemetry: Verify the subscription*/ ACK::ErrorCode subscribeStatus; @@ -1210,6 +1164,11 @@ static T_OsdkOsalHandler osalHandler = { bool VehicleWrapper::teardownSubscription(const int pkgIndex, int timeout) { + if (!vehicle) + { + std::cout << "Vehicle is a null value!" << std::endl; + return false; + } ACK::ErrorCode ack = vehicle->subscribe->removePackage(pkgIndex, timeout); if (ACK::getError(ack)) { DERROR( @@ -1222,6 +1181,11 @@ static T_OsdkOsalHandler osalHandler = { bool VehicleWrapper::checkActionStarted(uint8_t mode) { + if (!vehicle) + { + std::cout << "Vehicle is a null value!" << std::endl; + return false; + } int actionNotStarted = 0; int timeoutCycles = 20; while (vehicle->subscribe->getValue() != mode && @@ -1241,6 +1205,11 @@ static T_OsdkOsalHandler osalHandler = { bool VehicleWrapper::goHomeAndConfirmLanding(int timeout) { + if (!vehicle) + { + std::cout << "Vehicle is a null value!" << std::endl; + return false; + } /*! Step 1: Verify and setup the subscription */ const int pkgIndex = static_cast(SubscribePackgeIndex::TEMP_SUB_PACKAGE_INDEX); int freq = 10; @@ -1333,8 +1302,13 @@ static T_OsdkOsalHandler osalHandler = { return true; } - bool VehicleWrapper::setNewHomeLocation(int timeout) + bool VehicleWrapper::setCurrentAircraftLocAsHomePoint(int timeout) { + if (!vehicle) + { + std::cout << "Vehicle is a null value!" << std::endl; + return false; + } HomeLocationSetStatus homeLocationSetStatus; HomeLocationData originHomeLocation; ErrorCode::ErrorCodeType ret = ErrorCode::FlightControllerErr::SetHomeLocationErr::Fail; @@ -1403,6 +1377,11 @@ static T_OsdkOsalHandler osalHandler = { bool VehicleWrapper::setHomeAltitude(uint16_t altitude, int timeout) { + if (!vehicle) + { + std::cout << "Vehicle is a null value!" << std::endl; + return false; + } ErrorCode::ErrorCodeType ret = vehicle->flightController->setGoHomeAltitudeSync(altitude, timeout); if (ret != ErrorCode::SysCommonErr::Success) { @@ -1416,8 +1395,56 @@ static T_OsdkOsalHandler osalHandler = { } } - bool VehicleWrapper::setAvoid(bool enable) + bool VehicleWrapper::getHomeAltitude(uint16_t& altitude, int timeout) + { + if (!vehicle) + { + std::cout << "Vehicle is a null value!" << std::endl; + return false; + } + ErrorCode::ErrorCodeType ret = vehicle->flightController->getGoHomeAltitudeSync(altitude, timeout); + if (ret != ErrorCode::SysCommonErr::Success) + { + DSTATUS("Get go home altitude failed, ErrorCode is:%8x", ret); + return false; + } + else + { + DSTATUS("Get go home altitude successfully,altitude is: %d", altitude); + return true; + } + } + + bool VehicleWrapper::setHomePoint(float64_t latitude, float64_t longitude, int timeout) { + if (!vehicle) + { + std::cout << "Vehicle is a null value!" << std::endl; + return false; + } + DJI::OSDK::FlightController::HomeLocation homeLocation; + homeLocation.latitude = latitude; + homeLocation.longitude = longitude; + ErrorCode::ErrorCodeType ret = vehicle->flightController->setHomeLocationSync(homeLocation, timeout); + if (ret != ErrorCode::SysCommonErr::Success) + { + DSTATUS("Set home point failed, ErrorCode is:%8x", ret); + return false; + } + else + { + DSTATUS("Set home point successfully,latitude: %f rad, longitude:%f rad", latitude, longitude); + return true; + } + } + + bool VehicleWrapper::setCollisionAvoidance(bool enable) + { + if (!vehicle) + { + std::cout << "Vehicle is a null value!" << std::endl; + return false; + } auto enum_enable = enable ? FlightController::AvoidEnable::AVOID_ENABLE : FlightController::AvoidEnable::AVOID_DISABLE; ErrorCode::ErrorCodeType ack = vehicle->flightController->setCollisionAvoidanceEnabledSync(enum_enable, 1); if (ack == ErrorCode::SysCommonErr::Success) @@ -1427,9 +1454,33 @@ static T_OsdkOsalHandler osalHandler = { return false; } + bool VehicleWrapper::getCollisionAvoidance(uint8_t& enable) + { + if (!vehicle) + { + std::cout << "Vehicle is a null value!" << std::endl; + return false; + } + FlightController::AvoidEnable enum_enable; + ErrorCode::ErrorCodeType ack = vehicle->flightController->getCollisionAvoidanceEnabledSync(enum_enable, 1); + enable = static_cast(enum_enable); + if (ack == ErrorCode::SysCommonErr::Success) + { + return true; + } + + enable = 0xF; + return false; + } + bool VehicleWrapper::setUpwardsAvoidance(bool enable) { - auto enum_enable = enable ? FlightController::UpwardsAvoidEnable::UPWARDS_AVOID_DISABLE : FlightController::UpwardsAvoidEnable::UPWARDS_AVOID_ENABLE; + if (!vehicle) + { + std::cout << "Vehicle is a null value!" << std::endl; + return false; + } + auto enum_enable = enable ? FlightController::UpwardsAvoidEnable::UPWARDS_AVOID_ENABLE : FlightController::UpwardsAvoidEnable::UPWARDS_AVOID_DISABLE; ErrorCode::ErrorCodeType ack = vehicle->flightController->setUpwardsAvoidanceEnabledSync(enum_enable, 1); if (ack == ErrorCode::SysCommonErr::Success) { @@ -1438,230 +1489,365 @@ static T_OsdkOsalHandler osalHandler = { return false; } - bool VehicleWrapper::moveByPositionOffset(ACK::ErrorCode& ack, int timeout, MoveOffset& p_offset) + bool VehicleWrapper::getUpwardsAvoidance(uint8_t& enable) { - using namespace Telemetry; - auto xOffsetDesired = p_offset.x; - auto yOffsetDesired = p_offset.y; - auto zOffsetDesired = p_offset.z; - auto yawDesired = p_offset.yaw; - auto posThresholdInM = p_offset.pos_threshold; - auto yawThresholdInDeg = p_offset.yaw_threshold; - - // Set timeout: this timeout is the time you allow the drone to take to finish - // the - // mission - int timeoutInMilSec = 40000; - int controlFreqInHz = 50; // Hz - int cycleTimeInMs = 1000 / controlFreqInHz; - int outOfControlBoundsTimeLimit = 10 * cycleTimeInMs; // 10 cycles - int withinControlBoundsTimeReqmt = 50 * cycleTimeInMs; // 50 cycles - - //@todo: remove this once the getErrorCode function signature changes - char func[50]; - - startGlobalPositionBroadcast(); - // Wait for data to come in - sleep(1); - - // Get data - - // Global position retrieved via subscription - Telemetry::TypeMap::type currentSubscriptionGPS; - Telemetry::TypeMap::type originSubscriptionGPS; - // Global position retrieved via broadcast - Telemetry::GlobalPosition currentBroadcastGP; - Telemetry::GlobalPosition originBroadcastGP; - - // Convert position offset from first position to local coordinates - Telemetry::Vector3f localOffset; - - if (!vehicle->isM100() && !vehicle->isLegacyM600()) + if (!vehicle) { - currentSubscriptionGPS = vehicle->subscribe->getValue(); - originSubscriptionGPS = currentSubscriptionGPS; - localOffsetFromGpsOffset(localOffset, - static_cast(¤tSubscriptionGPS), - static_cast(&originSubscriptionGPS)); - - // Get the broadcast GP since we need the height for zCmd - currentBroadcastGP = vehicle->broadcast->getGlobalPosition(); + std::cout << "Vehicle is a null value!" << std::endl; + return false; } - else + FlightController::UpwardsAvoidEnable enum_enable; + ErrorCode::ErrorCodeType ack = vehicle->flightController->getUpwardsAvoidanceEnabledSync(enum_enable, 1); + enable = static_cast(enum_enable); + if (ack == ErrorCode::SysCommonErr::Success) { - currentBroadcastGP = vehicle->broadcast->getGlobalPosition(); - originBroadcastGP = currentBroadcastGP; - localOffsetFromGpsOffset(localOffset, - static_cast(¤tBroadcastGP), - static_cast(&originBroadcastGP)); + return true; } - // Get initial offset. We will update this in a loop later. - double xOffsetRemaining = xOffsetDesired - localOffset.x; - double yOffsetRemaining = yOffsetDesired - localOffset.y; - double zOffsetRemaining = zOffsetDesired - localOffset.z; + enable = 0xF; + return false; + } - // Conversions - double yawDesiredRad = DEG2RAD * yawDesired; - double yawThresholdInRad = DEG2RAD * yawThresholdInDeg; + bool VehicleWrapper::setJoystickMode(const JoystickMode &joystickMode) + { + if (!vehicle) + { + std::cout << "Vehicle is a null value!" << std::endl; + return false; + } + DJI::OSDK::FlightController::JoystickMode interJoystickMode; + interJoystickMode.horizontalLogic = static_cast(joystickMode.horizontalLogic); + interJoystickMode.verticalLogic = static_cast(joystickMode.verticalLogic); + interJoystickMode.yawLogic = static_cast(joystickMode.yawLogic); + interJoystickMode.horizontalCoordinate = static_cast(joystickMode.horizontalCoordinate); + interJoystickMode.stableMode = static_cast(joystickMode.stableMode); - //! Get Euler angle + vehicle->flightController->setJoystickMode(interJoystickMode); - // Quaternion retrieved via subscription - Telemetry::TypeMap::type subscriptionQ; - // Quaternion retrieved via broadcast - Telemetry::Quaternion broadcastQ; + return true; + } - double yawInRad; - if (!vehicle->isM100() && !vehicle->isLegacyM600()) - { - subscriptionQ = vehicle->subscribe->getValue(); - yawInRad = toEulerAngle((static_cast(&subscriptionQ))).z / DEG2RAD; - } - else + bool VehicleWrapper::JoystickAction(const JoystickCommand &JoystickCommand) + { + if (!vehicle) { - broadcastQ = vehicle->broadcast->getQuaternion(); - yawInRad = toEulerAngle((static_cast(&broadcastQ))).z / DEG2RAD; + std::cout << "Vehicle is a null value!" << std::endl; + return false; } + DJI::OSDK::FlightController::JoystickCommand interJoystickCommand; + memcpy(&interJoystickCommand, &JoystickCommand, sizeof(interJoystickCommand)); + vehicle->flightController->setJoystickCommand(interJoystickCommand); + vehicle->flightController->joystickAction(); - int elapsedTimeInMs = 0; - int withinBoundsCounter = 0; - int outOfBounds = 0; - int brakeCounter = 0; - int speedFactor = 2; - float xCmd, yCmd, zCmd; + return true; + } - /*! Calculate the inputs to send the position controller. We implement basic - * receding setpoint position control and the setpoint is always 1 m away - * from the current position - until we get within a threshold of the goal. - * From that point on, we send the remaining distance as the setpoint. - */ - if (xOffsetDesired > 0) - xCmd = (xOffsetDesired < speedFactor) ? xOffsetDesired : speedFactor; - else if (xOffsetDesired < 0) - xCmd = - (xOffsetDesired > -1 * speedFactor) ? xOffsetDesired : -1 * speedFactor; - else - xCmd = 0; + Vector3f VehicleWrapper::quaternionToEulerAngle(const Telemetry::Quaternion& quat) + { + Telemetry::Vector3f eulerAngle; + double q2sqr = quat.q2 * quat.q2; + double t0 = -2.0 * (q2sqr + quat.q3 * quat.q3) + 1.0; + double t1 = 2.0 * (quat.q1 * quat.q2 + quat.q0 * quat.q3); + double t2 = -2.0 * (quat.q1 * quat.q3 - quat.q0 * quat.q2); + double t3 = 2.0 * (quat.q2 * quat.q3 + quat.q0 * quat.q1); + double t4 = -2.0 * (quat.q1 * quat.q1 + q2sqr) + 1.0; + t2 = (t2 > 1.0) ? 1.0 : t2; + t2 = (t2 < -1.0) ? -1.0 : t2; + eulerAngle.x = asin(t2); + eulerAngle.y = atan2(t3, t4); + eulerAngle.z = atan2(t1, t0); + return eulerAngle; + } - if (yOffsetDesired > 0) - yCmd = (yOffsetDesired < speedFactor) ? yOffsetDesired : speedFactor; - else if (yOffsetDesired < 0) - yCmd = - (yOffsetDesired > -1 * speedFactor) ? yOffsetDesired : -1 * speedFactor; - else - yCmd = 0; + Vector3f VehicleWrapper::localOffsetFromGpsAndFusedHeightOffset( + const Telemetry::GPSFused& target, const Telemetry::GPSFused& origin, + const float32_t& targetHeight, const float32_t& originHeight) + { + Telemetry::Vector3f deltaNed; + double deltaLon = target.longitude - origin.longitude; + double deltaLat = target.latitude - origin.latitude; + deltaNed.x = deltaLat * C_EARTH; + deltaNed.y = deltaLon * C_EARTH * cos(target.latitude); + deltaNed.z = targetHeight - originHeight; + return deltaNed; + } + + Vector3f VehicleWrapper::vector3FSub(const Vector3f& vectorA, + const Vector3f& vectorB) + { + Telemetry::Vector3f result; + result.x = vectorA.x - vectorB.x; + result.y = vectorA.y - vectorB.y; + result.z = vectorA.z - vectorB.z; + return result; + } + + template + int VehicleWrapper::signOfData(Type type) + { + return type < 0 ? -1 : 1; + } + + void VehicleWrapper::horizCommandLimit(float speedFactor, float& commandX, + float& commandY) + { + if (fabs(commandX) > speedFactor) + commandX = signOfData(commandX) * speedFactor; + if (fabs(commandY) > speedFactor) + commandY = signOfData(commandY) * speedFactor; + } - if (!vehicle->isM100() && !vehicle->isLegacyM600()) + float32_t VehicleWrapper::vectorNorm(Vector3f v) + { + return sqrt(pow(v.x, 2) + pow(v.y, 2) + pow(v.z, 2)); + } + + bool VehicleWrapper::moveByPositionOffset(const JoystickCommand &JoystickCommand,int timeout, + float posThresholdInM,float yawThresholdInDeg) + { + if (!vehicle) { - zCmd = currentBroadcastGP.height + zOffsetDesired; //Since subscription cannot give us a relative height, use broadcast. + std::cout << "Vehicle is a null value!" << std::endl; + return false; } - else + + using namespace Telemetry; + Vector3f offsetDesired; + offsetDesired.x = JoystickCommand.x; + offsetDesired.y = JoystickCommand.y; + offsetDesired.z = JoystickCommand.z; + float yawDesiredInDeg = JoystickCommand.yaw; + + int responseTimeout = 1; + int timeoutInMilSec = 40000; + int controlFreqInHz = 50; // Hz + int cycleTimeInMs = 1000 / controlFreqInHz; + int outOfControlBoundsTimeLimit = 10 * cycleTimeInMs; // 10 cycles + int withinControlBoundsTimeReqmt = 100 * cycleTimeInMs; // 100 cycles + int elapsedTimeInMs = 0; + int withinBoundsCounter = 0; + int outOfBounds = 0; + int brakeCounter = 0; + int speedFactor = 2; + + /* now we need position-height broadcast to obtain the real-time altitude of the aircraft, + * which is consistent with the altitude closed-loop data of flight control internal position control + * TO DO:the data will be replaced by new data subscription. + */ + if (!startGlobalPositionBroadcast()) { - zCmd = currentBroadcastGP.height + zOffsetDesired; + return false; } + sleep(1); - //! Main closed-loop receding setpoint position control - while (elapsedTimeInMs < timeoutInMilSec) - { - vehicle->control->positionAndYawCtrl(xCmd, yCmd, zCmd, - yawDesiredRad / DEG2RAD); + //! get origin position and relative height(from home point)of aircraft. + Telemetry::TypeMap::type originGPSPosition = + vehicle->subscribe->getValue(); + Telemetry::GlobalPosition currentBroadcastGP = vehicle->broadcast->getGlobalPosition(); + float32_t originHeightBaseHomepoint = currentBroadcastGP.height; + FlightController::JoystickMode joystickMode = { + FlightController::HorizontalLogic::HORIZONTAL_POSITION, + FlightController::VerticalLogic::VERTICAL_POSITION, + FlightController::YawLogic::YAW_ANGLE, + FlightController::HorizontalCoordinate::HORIZONTAL_GROUND, + FlightController::StableMode::STABLE_ENABLE, + }; + vehicle->flightController->setJoystickMode(joystickMode); - usleep(cycleTimeInMs * 1000); - elapsedTimeInMs += cycleTimeInMs; + while (elapsedTimeInMs < timeoutInMilSec) { + Telemetry::TypeMap::type currentGPSPosition = + vehicle->subscribe->getValue(); + Telemetry::TypeMap::type currentQuaternion = + vehicle->subscribe->getValue(); + currentBroadcastGP = vehicle->broadcast->getGlobalPosition(); + float yawInRad = quaternionToEulerAngle(currentQuaternion).z; + //! get the vector between aircraft and origin point. - //! Get current position in required coordinates and units - if (!vehicle->isM100() && !vehicle->isLegacyM600()) - { - subscriptionQ = vehicle->subscribe->getValue(); - yawInRad = toEulerAngle((static_cast(&subscriptionQ))).z; - currentSubscriptionGPS = vehicle->subscribe->getValue(); - localOffsetFromGpsOffset(localOffset, - static_cast(¤tSubscriptionGPS), - static_cast(&originSubscriptionGPS)); - - // Get the broadcast GP since we need the height for zCmd - currentBroadcastGP = vehicle->broadcast->getGlobalPosition(); - } - else - { - broadcastQ = vehicle->broadcast->getQuaternion(); - yawInRad = toEulerAngle((static_cast(&broadcastQ))).z; - currentBroadcastGP = vehicle->broadcast->getGlobalPosition(); - localOffsetFromGpsOffset(localOffset, - static_cast(¤tBroadcastGP), - static_cast(&originBroadcastGP)); - } + Vector3f localOffset = localOffsetFromGpsAndFusedHeightOffset(currentGPSPosition, originGPSPosition, + currentBroadcastGP.height, originHeightBaseHomepoint); + //! get the vector between aircraft and target point. + Vector3f offsetRemaining = vector3FSub(offsetDesired, localOffset); - //! See how much farther we have to go - xOffsetRemaining = xOffsetDesired - localOffset.x; - yOffsetRemaining = yOffsetDesired - localOffset.y; - zOffsetRemaining = zOffsetDesired - localOffset.z; + Vector3f positionCommand = offsetRemaining; + horizCommandLimit(speedFactor, positionCommand.x, positionCommand.y); - //! See if we need to modify the setpoint - if (std::abs(xOffsetRemaining) < speedFactor) - { - xCmd = xOffsetRemaining; - } - if (std::abs(yOffsetRemaining) < speedFactor) - { - yCmd = yOffsetRemaining; - } + FlightController::JoystickCommand joystickCommand = { + positionCommand.x, positionCommand.y, + offsetDesired.z + originHeightBaseHomepoint, yawDesiredInDeg}; - if (vehicle->isM100() && std::abs(xOffsetRemaining) < posThresholdInM && - std::abs(yOffsetRemaining) < posThresholdInM && - std::abs(yawInRad - yawDesiredRad) < yawThresholdInRad) - { - //! 1. We are within bounds; start incrementing our in-bound counter - withinBoundsCounter += cycleTimeInMs; - } - else if (std::abs(xOffsetRemaining) < posThresholdInM && - std::abs(yOffsetRemaining) < posThresholdInM && - std::abs(zOffsetRemaining) < posThresholdInM && - std::abs(yawInRad - yawDesiredRad) < yawThresholdInRad) - { + vehicle->flightController->setJoystickCommand(joystickCommand); + + vehicle->flightController->joystickAction(); + + if (vectorNorm(offsetRemaining) < posThresholdInM && + std::fabs(yawInRad / DEG2RAD - yawDesiredInDeg) < yawThresholdInDeg) { //! 1. We are within bounds; start incrementing our in-bound counter withinBoundsCounter += cycleTimeInMs; - } - else - { - if (withinBoundsCounter != 0) - { + } else { + if (withinBoundsCounter != 0) { //! 2. Start incrementing an out-of-bounds counter outOfBounds += cycleTimeInMs; } } //! 3. Reset withinBoundsCounter if necessary - if (outOfBounds > outOfControlBoundsTimeLimit) - { + if (outOfBounds > outOfControlBoundsTimeLimit) { withinBoundsCounter = 0; - outOfBounds = 0; + outOfBounds = 0; } //! 4. If within bounds, set flag and break - if (withinBoundsCounter >= withinControlBoundsTimeReqmt) - { + if (withinBoundsCounter >= withinControlBoundsTimeReqmt) { break; } + usleep(cycleTimeInMs * 1000); + elapsedTimeInMs += cycleTimeInMs; + } + + while (brakeCounter < withinControlBoundsTimeReqmt) { + //! TODO: remove emergencyBrake + vehicle->flightController->emergencyBrakeAction(); + usleep(cycleTimeInMs * 1000); + brakeCounter += cycleTimeInMs; } - //! Set velocity to zero, to prevent any residual velocity from position - //! command - if (!vehicle->isM100() && !vehicle->isLegacyM600()) + if (elapsedTimeInMs >= timeoutInMilSec) { + std::cout << "Task timeout!\n"; + return false; + } + return true; + } + + void VehicleWrapper::velocityAndYawRateCtrl(const JoystickCommand &JoystickCommand, int timeMs) + { + + Vector3f offsetDesired; + offsetDesired.x = JoystickCommand.x; + offsetDesired.y = JoystickCommand.y; + offsetDesired.z = JoystickCommand.z; + float yawRate = JoystickCommand.yaw; + + uint32_t originTime = 0; + uint32_t currentTime = 0; + uint32_t elapsedTimeInMs = 0; + OsdkOsal_GetTimeMs(&originTime); + OsdkOsal_GetTimeMs(¤tTime); + elapsedTimeInMs = currentTime - originTime; + + FlightController::JoystickMode joystickMode = { + FlightController::HorizontalLogic::HORIZONTAL_VELOCITY, + FlightController::VerticalLogic::VERTICAL_VELOCITY, + FlightController::YawLogic::YAW_RATE, + FlightController::HorizontalCoordinate::HORIZONTAL_GROUND, + FlightController::StableMode::STABLE_ENABLE, + }; + + vehicle->flightController->setJoystickMode(joystickMode); + FlightController::JoystickCommand joystickCommand = {offsetDesired.x, offsetDesired.y, offsetDesired.z,yawRate}; + vehicle->flightController->setJoystickCommand(joystickCommand); + + while(elapsedTimeInMs <= timeMs) { - while (brakeCounter < withinControlBoundsTimeReqmt) + vehicle->flightController->joystickAction(); + usleep(20000); + OsdkOsal_GetTimeMs(¤tTime); + elapsedTimeInMs = currentTime - originTime; + } + } + + bool VehicleWrapper::obtainReleaseCtrlAuthority(bool enableObtain, int timeout) + { + if (!vehicle) + { + std::cout << "Vehicle is a null value!" << std::endl; + return false; + } + + ACK::ErrorCode initAck; + + if (enableObtain) + { + initAck = vehicle->control->obtainCtrlAuthority(timeout); + } + else + { + initAck = vehicle->control->releaseCtrlAuthority(timeout); + } + + if (ACK::getError(initAck)) + { + ACK::getErrorCodeMessage(initAck, __func__); + return false; + } + + return true; + } + + bool VehicleWrapper::turnOnOffMotors(bool OnOff,int timeOut) + { + if (!vehicle) + { + std::cout << "Vehicle is a null value!" << std::endl; + return false; + } + + if (OnOff) + { + ErrorCode::ErrorCodeType ack = vehicle->flightController->turnOnMotorsSync(timeOut); + if (ack == ErrorCode::SysCommonErr::Success) { - vehicle->control->emergencyBrake(); - usleep(cycleTimeInMs * 10); - brakeCounter += cycleTimeInMs; + return true; } } + else + { + ErrorCode::ErrorCodeType ack = vehicle->flightController->turnOffMotorsSync(timeOut); + if (ack == ErrorCode::SysCommonErr::Success) + { + return true; + } + } + + return false; + } - if (elapsedTimeInMs >= timeoutInMilSec) + bool VehicleWrapper::killSwitch(bool enable, char msg[10], int timeOut) + { + if (!vehicle) { - std::cout << "Task timeout!\n"; - return ACK::FAIL; + std::cout << "Vehicle is a null value!" << std::endl; + return false; } - return ACK::SUCCESS; + + ErrorCode::ErrorCodeType ack; + if (enable) + { + ack = vehicle->flightController->killSwitch(DJI::OSDK::FlightActions::KillSwitch::ENABLE, + timeOut, msg); + } + else + { + ack = vehicle->flightController->killSwitch(DJI::OSDK::FlightActions::KillSwitch::DISABLE, + timeOut, msg); + } + + if (ack == ErrorCode::SysCommonErr::Success) + { + return true; + } + + return false; + } + + bool VehicleWrapper::emergencyBrake() + { + if (!vehicle) + { + std::cout << "Vehicle is a null value!" << std::endl; + return false; + } + + vehicle->flightController->emergencyBrakeAction(); + + return true; } bool VehicleWrapper::startGlobalPositionBroadcast() @@ -1710,62 +1896,6 @@ static T_OsdkOsalHandler osalHandler = { } } - void VehicleWrapper::localOffsetFromGpsOffset(Telemetry::Vector3f& deltaNed, void* target, void* origin) - { - Telemetry::GPSFused* subscriptionTarget; - Telemetry::GPSFused* subscriptionOrigin; - Telemetry::GlobalPosition* broadcastTarget; - Telemetry::GlobalPosition* broadcastOrigin; - double deltaLon; - double deltaLat; - - if (!vehicle->isM100() && !vehicle->isLegacyM600()) - { - subscriptionTarget = (Telemetry::GPSFused*)target; - subscriptionOrigin = (Telemetry::GPSFused*)origin; - deltaLon = subscriptionTarget->longitude - subscriptionOrigin->longitude; - deltaLat = subscriptionTarget->latitude - subscriptionOrigin->latitude; - deltaNed.x = deltaLat * C_EARTH; - deltaNed.y = deltaLon * C_EARTH * cos(subscriptionTarget->latitude); - deltaNed.z = subscriptionTarget->altitude - subscriptionOrigin->altitude; - } - else - { - broadcastTarget = (Telemetry::GlobalPosition*)target; - broadcastOrigin = (Telemetry::GlobalPosition*)origin; - deltaLon = broadcastTarget->longitude - broadcastOrigin->longitude; - deltaLat = broadcastTarget->latitude - broadcastOrigin->latitude; - deltaNed.x = deltaLat * C_EARTH; - deltaNed.y = deltaLon * C_EARTH * cos(broadcastTarget->latitude); - deltaNed.z = broadcastTarget->altitude - broadcastOrigin->altitude; - } - } - - Telemetry::Vector3f VehicleWrapper::toEulerAngle(void* quaternionData) - { - Telemetry::Vector3f ans; - Telemetry::Quaternion* quaternion = (Telemetry::Quaternion*)quaternionData; - - double q2sqr = quaternion->q2 * quaternion->q2; - double t0 = -2.0 * (q2sqr + quaternion->q3 * quaternion->q3) + 1.0; - double t1 = - +2.0 * (quaternion->q1 * quaternion->q2 + quaternion->q0 * quaternion->q3); - double t2 = - -2.0 * (quaternion->q1 * quaternion->q3 - quaternion->q0 * quaternion->q2); - double t3 = - +2.0 * (quaternion->q2 * quaternion->q3 + quaternion->q0 * quaternion->q1); - double t4 = -2.0 * (quaternion->q1 * quaternion->q1 + q2sqr) + 1.0; - - t2 = (t2 > 1.0) ? 1.0 : t2; - t2 = (t2 < -1.0) ? -1.0 : t2; - - ans.x = asin(t2); - ans.y = atan2(t3, t4); - ans.z = atan2(t1, t0); - - return ans; - } - GimbalSingleData VehicleWrapper::getGimbalData(const PayloadIndex& payloadIndex) { GimbalSingleData gimbalSingleData; @@ -1848,6 +1978,32 @@ 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; + } + + bool VehicleWrapper::getSingleBatteryDynamicInfo(const DJI::OSDK::DJIBattery::RequestSmartBatteryIndex batteryIndex, + DJI::OSDK::SmartBatteryDynamicInfo& batteryDynamicInfo) + { + if (!vehicle) { + DERROR("vehicle is a null value"); + return false; + } + + return vehicle->djiBattery->getSingleBatteryDynamicInfo(batteryIndex, batteryDynamicInfo); + } + 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; @@ -2163,6 +2319,62 @@ bool VehicleWrapper::RegisterMissionStateCallback(void *userData, PushCallback c return true; } +bool VehicleWrapper::enableSubscribeHMSInfo(bool enable, uint32_t timeOutMs) +{ + if (!vehicle) + { + std::cout << "Vehicle is a null value!" << std::endl; + return false; + } + + return vehicle->djiHms->subscribeHMSInf(enable, timeOutMs); +} + +bool VehicleWrapper::getHMSListInfo(HMSPushPacket& hmsPushPacket) +{ + if (!vehicle) + { + std::cout << "Vehicle is a null value!" << std::endl; + return false; + } + + DJI::OSDK::HMSPushPacket tempHMSPushPacket; + tempHMSPushPacket = vehicle->djiHms->getHMSPushPacket(); + + 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; +} + #ifdef ADVANCED_SENSING bool VehicleWrapper::startFPVCameraStream(CameraImageCallback cb, void * cbParam) { diff --git a/src/dji_osdk_ros/samples/CMakeLists.txt b/src/dji_osdk_ros/samples/CMakeLists.txt index 99e70eb9..2cd88a05 100644 --- a/src/dji_osdk_ros/samples/CMakeLists.txt +++ b/src/dji_osdk_ros/samples/CMakeLists.txt @@ -14,9 +14,9 @@ if(ENABLE_GPU) add_definitions(-DUSE_GPU) endif() -if(ENABLE_DARKNET_ROS) -add_definitions(-DUSE_DARKNET_ROS) -endif() +#if(ENABLE_DARKNET_ROS) +#add_definitions(-DUSE_DARKNET_ROS) +#endif() add_executable(flight_control_node flight_control_node.cpp) diff --git a/src/dji_osdk_ros/samples/CMakeLists.txt.ignore b/src/dji_osdk_ros/samples/CMakeLists.txt.ignore new file mode 100644 index 00000000..57906f38 --- /dev/null +++ b/src/dji_osdk_ros/samples/CMakeLists.txt.ignore @@ -0,0 +1,142 @@ +FILE(GLOB EXAMPLES *.cc *.c *.cpp) + +include_directories(stereo_utility) + +if(ENABLE_OPEN_CV_CONTRIB) +add_definitions(-DUSE_OPEN_CV_CONTRIB) +endif() + +if(ENABLE_OPEN_CV_INSTALLED OR OPEN_CV_3_3_0_INSTALLED) +add_definitions(-DOPEN_CV_INSTALLED) +endif() + +if(ENABLE_GPU) +add_definitions(-DUSE_GPU) +endif() + +if(ENABLE_DARKNET_ROS) +add_definitions(-DUSE_DARKNET_ROS) +endif() + +add_executable(flight_control_node + flight_control_node.cpp) +target_link_libraries(flight_control_node + ${PROJECT_NAME} + ) +add_dependencies(flight_control_node dji_osdk_ros_generate_messages_cpp) + +add_executable(gimbal_camera_control_node + gimbal_camera_control_node.cpp ) +target_link_libraries(gimbal_camera_control_node + ${PROJECT_NAME} + ) +add_dependencies(gimbal_camera_control_node dji_osdk_ros_generate_messages_cpp) + +add_executable(mobile_device_node + mobile_device_node.cpp) +target_link_libraries(mobile_device_node + ${PROJECT_NAME} + ) +add_dependencies(mobile_device_node dji_osdk_ros_generate_messages_cpp) + +add_executable(payload_device_node + payload_device_node.cpp) + +target_link_libraries(payload_device_node + ${PROJECT_NAME} + ) +add_dependencies(payload_device_node dji_osdk_ros_generate_messages_cpp) + +add_executable(telemetry_node telemetry_node.cpp) + +target_link_libraries(telemetry_node + ${PROJECT_NAME} + ) +add_dependencies(telemetry_node dji_osdk_ros_generate_messages_cpp) + +add_executable(time_sync_node time_sync_node.cpp) + +target_link_libraries(time_sync_node + ${PROJECT_NAME} + ) +add_dependencies(time_sync_node dji_osdk_ros_generate_messages_cpp) + +find_package(SDL2 REQUIRED) +if (SDL2_FOUND) + message(STATUS "Found SDL2:") + message(STATUS " - Includes: ${SDL2_INCLUDE_DIRS}") + message(STATUS " - Libraries: ${SDL2_LIBRARIES}") + add_definitions(-DSDL2_INSTALLED) + set(SDL2_LIB_DEPEND_FLAG -lSDL2) +else () +message(STATUS "--Recommendation : It is found that using \"cv::imshow\" on some platforms will cause more + CPU resources and more processing time in camera_stream_node. Using SDL to display images can improve this + situation. \n--Install SDL2 library in shell : \"sudo apt-get install libsdl2-dev\".") +endif() +if(ENABLE_OPEN_CV_INSTALLED OR OPEN_CV_3_3_0_INSTALLED) +message(STATUS "Find OPENCV, camera stream node will be compiled.") +add_executable(camera_stream_node camera_stream_node.cpp) +target_link_libraries(camera_stream_node + ${PROJECT_NAME} + ${SDL2_LIB_DEPEND_FLAG} + ) +add_dependencies(camera_stream_node dji_osdk_ros_generate_messages_cpp) +else() +message(STATUS "Did not find OPENCV, camera stream node will not be compiled.") +endif() + +add_executable(camera_h264_node camera_h264_node.cpp) +target_link_libraries(camera_h264_node + ${PROJECT_NAME} + ) +add_dependencies(camera_h264_node dji_osdk_ros_generate_messages_cpp) + +add_executable(mission_node mission_node.cpp) +target_link_libraries(mission_node + ${PROJECT_NAME} + ) +add_dependencies(mission_node dji_osdk_ros_generate_messages_cpp) + +add_executable(waypointV2_node waypointV2_node.cpp) +target_link_libraries(waypointV2_node + ${PROJECT_NAME} + ) +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) + +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") + add_definitions(-DOPEN_CV_INSTALLED) + add_executable(stereo_vision_depth_perception_node + stereo_vision_depth_perception_node.cpp) + target_include_directories(${PROJECT_NAME} PRIVATE ${OpenCV_INCLUDE_DIRS}) + + if (OPEN_CV_3_3_0_INSTALLED) + target_link_libraries(stereo_vision_depth_perception_node + ${catkin_LIBRARIES} + ${PROJECT_NAME} + ${OpenCV_LIBRARIES} + ) + else() + target_link_libraries(stereo_vision_depth_perception_node + ${catkin_LIBRARIES} + ${PROJECT_NAME} + ) + endif() + add_dependencies(stereo_vision_depth_perception_node dji_osdk_ros_generate_messages_cpp) +else() + message(STATUS "Did not find required libraries, stereo vision depth perception node will not be compiled.") +endif () + diff --git a/src/dji_osdk_ros/samples/battery_node.cpp b/src/dji_osdk_ros/samples/battery_node.cpp new file mode 100644 index 00000000..b91f9d9c --- /dev/null +++ b/src/dji_osdk_ros/samples/battery_node.cpp @@ -0,0 +1,100 @@ +/** @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 +#include + +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("get_whole_battery_info"); + auto get_single_battery_dynamic_info = nh.serviceClient("get_single_battery_dynamic_info"); + + dji_osdk_ros::GetWholeBatteryInfo getWholeBatteryInfo; + dji_osdk_ros::GetSingleBatteryDynamicInfo firstBatteryDynamicInfo; + dji_osdk_ros::GetSingleBatteryDynamicInfo secondBatteryDynamicInfo; + + while (time_count < MAX_TIME_COUNT) + { + get_whole_battery_info.call(getWholeBatteryInfo); + ROS_INFO("(It's valid only for M210V2)Whole battery Info:"); + 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(s)",getWholeBatteryInfo.response.battery_whole_info.remainFlyTime); + ROS_INFO("(It's valid only for M210V2)goHomeNeedTime is %d(s)",getWholeBatteryInfo.response.battery_whole_info.goHomeNeedTime); + ROS_INFO("(It's valid only for M210V2)landNeedTime is %d(s)",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(m)",getWholeBatteryInfo.response.battery_whole_info.safeFlyRadius); + ROS_INFO("(It's valid only for M210V2)capacityConsumeSpeed is %f(mAh/sec)",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 %d(mv)",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)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_INFO("---------------------------------------------------"); + ros::Duration(0.2).sleep(); + ROS_INFO("Single Battery Info:"); + ROS_INFO("The First Battery Info:"); + firstBatteryDynamicInfo.request.batteryIndex = dji_osdk_ros::GetSingleBatteryDynamicInfo::Request::first_smart_battery; + get_single_battery_dynamic_info.call(firstBatteryDynamicInfo); + ROS_INFO("battery index %d batteryCapacityPercent is %d(%)",firstBatteryDynamicInfo.response.smartBatteryDynamicInfo.batteryIndex, + firstBatteryDynamicInfo.response.smartBatteryDynamicInfo.batteryCapacityPercent); + ROS_INFO("battery index %d currentVoltage is %d(mV)",firstBatteryDynamicInfo.response.smartBatteryDynamicInfo.batteryIndex, + firstBatteryDynamicInfo.response.smartBatteryDynamicInfo.currentVoltage); + ROS_INFO("battery index %d batteryTemperature is %d",firstBatteryDynamicInfo.response.smartBatteryDynamicInfo.batteryIndex, + firstBatteryDynamicInfo.response.smartBatteryDynamicInfo.batteryTemperature/10); + ROS_INFO("The Second Battery Info:"); + secondBatteryDynamicInfo.request.batteryIndex = dji_osdk_ros::GetSingleBatteryDynamicInfo::Request::second_smart_battery; + get_single_battery_dynamic_info.call(secondBatteryDynamicInfo); + ROS_INFO("battery index %d batteryCapacityPercent is %d(%)",secondBatteryDynamicInfo.response.smartBatteryDynamicInfo.batteryIndex, + secondBatteryDynamicInfo.response.smartBatteryDynamicInfo.batteryCapacityPercent); + ROS_INFO("battery index %d currentVoltage is %d(mV)",secondBatteryDynamicInfo.response.smartBatteryDynamicInfo.batteryIndex, + secondBatteryDynamicInfo.response.smartBatteryDynamicInfo.currentVoltage); + ROS_INFO("battery index %d batteryTemperature is %d",secondBatteryDynamicInfo.response.smartBatteryDynamicInfo.batteryIndex, + secondBatteryDynamicInfo.response.smartBatteryDynamicInfo.batteryTemperature/10); + ROS_INFO("---------------------------------------------------"); + + time_count++; + } + + ROS_INFO_STREAM("Finished. Press CTRL-C to terminate the node"); + + ros::spin(); + return 0; +} diff --git a/src/dji_osdk_ros/samples/camera_stream_node.cpp b/src/dji_osdk_ros/samples/camera_stream_node.cpp index 5aeb15c7..d9d1e434 100644 --- a/src/dji_osdk_ros/samples/camera_stream_node.cpp +++ b/src/dji_osdk_ros/samples/camera_stream_node.cpp @@ -29,7 +29,8 @@ //INCLUDE #include #include - +#include +#include #include #include @@ -44,7 +45,6 @@ extern "C"{ #include "opencv2/highgui/highgui.hpp" #endif - //CODE using namespace dji_osdk_ros; @@ -96,13 +96,65 @@ bool ffmpeg_init() pCodecCtx->flags2 |= AV_CODEC_FLAG2_SHOW_ALL; } -void show_rgb(uint8_t *rawData, int height, int width) +#ifdef SDL2_INSTALLED +#include +void sdl_show_rgb(uint8_t *rgb24Buf, int width, int height) { + static SDL_Renderer* sdlRenderer = NULL; + static SDL_Texture* sdlTexture = NULL; + static SDL_Window *screen = NULL; + static int initFlag = 0; + + if (initFlag == 0) { + initFlag = 1; + if(SDL_Init(SDL_INIT_VIDEO)) { + printf( "Could not initialize SDL - %s\n", SDL_GetError()); + return; + } + + + //SDL 2.0 Support for multiple windows + screen = SDL_CreateWindow("camera_stream_node", SDL_WINDOWPOS_UNDEFINED, SDL_WINDOWPOS_UNDEFINED, + 100, 100,SDL_WINDOW_OPENGL|SDL_WINDOW_SHOWN|SDL_WINDOW_RESIZABLE); + if(!screen) { + printf("SDL: could not create window - exiting:%s\n",SDL_GetError()); + return; + } + sdlRenderer = SDL_CreateRenderer(screen, -1, 0); + uint32_t pixformat = SDL_PIXELFORMAT_RGB24; + sdlTexture = SDL_CreateTexture(sdlRenderer,pixformat, SDL_TEXTUREACCESS_STREAMING, width, height); + } + + if (!sdlRenderer || !sdlTexture || !screen) return; + SDL_SetWindowSize(screen, width, height); + + SDL_Event event; + event.type = (SDL_USEREVENT + 1); + SDL_PushEvent(&event); + if (SDL_WaitEventTimeout(&event, 5)) { + SDL_Rect sdlRect; + SDL_UpdateTexture(sdlTexture, NULL, rgb24Buf, width * 3); + sdlRect.x = 0; + sdlRect.y = 0; + sdlRect.w = width; + sdlRect.h = height; + + SDL_RenderClear(sdlRenderer); + SDL_RenderCopy(sdlRenderer, sdlTexture, NULL, &sdlRect); + SDL_RenderPresent(sdlRenderer); + } +} +#endif + +void show_rgb(CameraRGBImage img, char* name) { -#ifdef OPEN_CV_INSTALLED - cv::Mat mat(height, width, CV_8UC3, rawData, width*3); - cv::cvtColor(mat, mat, cv::COLOR_RGB2BGR); - cv::imshow("camera_stream_node", mat); - cv::waitKey(1); + std::cout << "#### Got image from:\t" << std::string(name) << std::endl; +#ifdef SDL2_INSTALLED + sdl_show_rgb(img.rawData.data(), img.width, img.height); +#elif defined(OPEN_CV_INSTALLED) + cv::Mat mat(img.height, img.width, CV_8UC3, img.rawData.data(), img.width*3); + cvtColor(mat, mat, cv::COLOR_RGB2BGR); + imshow(name,mat); + cv::waitKey(1); #endif } @@ -161,8 +213,9 @@ void decodeToDisplay(uint8_t *buf, int bufLen) pFrameRGB->height = h; pFrameRGB->width = w; - -#ifdef OPEN_CV_INSTALLED +#ifdef SDL2_INSTALLED + sdl_show_rgb(pFrameRGB->data[0], pFrameRGB->width, pFrameRGB->height); +#elif defined(OPEN_CV_INSTALLED) cv::Mat mat(pFrameRGB->height, pFrameRGB->width, CV_8UC3, pFrameRGB->data[0], pFrameRGB->width * 3); cv::cvtColor(mat, mat, cv::COLOR_RGB2BGR); cv::imshow("camera_stream_node", mat); @@ -175,6 +228,32 @@ void decodeToDisplay(uint8_t *buf, int bufLen) av_free_packet(&pkt); } + +void fpvCameraStreamCallBack(const sensor_msgs::Image& msg) +{ + CameraRGBImage img; + img.rawData = msg.data; + img.height = msg.height; + img.width = msg.width; + char Name[] = "FPV_CAM"; + show_rgb(img, Name); + std::cout<<"height is"<("setup_camera_h264"); auto fpv_camera_h264_sub = nh.subscribe("dji_osdk_ros/camera_h264_stream", 10, cameraH264CallBack); dji_osdk_ros::SetupCameraH264 setupCameraH264_; ffmpeg_init(); + /*! RGB flow init */ + auto setup_camera_stream_client = nh.serviceClient("setup_camera_stream"); + auto fpv_camera_stream_sub = nh.subscribe("dji_osdk_ros/fpv_camera_images", 10, fpvCameraStreamCallBack); + auto main_camera_stream_sub = nh.subscribe("dji_osdk_ros/main_camera_images", 10, mainCameraStreamCallBack); + dji_osdk_ros::SetupCameraStream setupCameraStream_; + +#ifndef SDL2_INSTALLED + std::cout + << "--Recommandation : It is found that using \"cv::imshow\" will cause more CPU resources and more processing " + "time. Using SDL to display images can improve this situation. At present, SDL display is supported in this " + "node, which can be used by installing SDL2 library and recompiling. \n" + << "--Install SDL2 library in shell : \"sudo apt-get install libsdl2-dev\"." + << std::endl; +#endif +#ifdef SDL2_INSTALLED + std::cout << "Using SDL2 lib to display the images." << std::endl; +#elif defined(OPEN_CV_INSTALLED) + std::cout << "Using Opencv to display the images." << std::endl; +#endif char inputChar = 0; std::cout << std::endl; std::cout - << "| Available commands: |" + << "| Input 'm' or 'f' to view the camera stream from the RGB flow |\n" + "| of the topic \"dji_osdk_ros/fpv_camera_images\" and |\n" + "| \"dji_osdk_ros/main_camera_images\" : |" + << std::endl + << "| [m] Start to display main camera stream |" + << std::endl + << "| [f] Start to display fpv stream |" + << std::endl; + std::cout + << "| |" + << std::endl; + std::cout + << "| Input 'a' ~ 'b' to view the camera stream decoded by the h264|\n" + "| flow from the topic \"dji_osdk_ros/camera_h264_stream\" : |" << std::endl << "| [a] Start to display FPV stream sample |" << std::endl @@ -205,14 +318,24 @@ int main(int argc, char** argv) std::cin >> inputChar; - struct timeval tv; - struct tm tm; - gettimeofday(&tv, NULL); - localtime_r(&tv.tv_sec, &tm); - - switch (inputChar) { + case 'f': + { + setupCameraStream_.request.cameraType = setupCameraStream_.request.FPV_CAM; + setupCameraStream_.request.start = 1; + setup_camera_stream_client.call(setupCameraStream_); + + break; + } + case 'm': + { + setupCameraStream_.request.cameraType = setupCameraStream_.request.MAIN_CAM; + setupCameraStream_.request.start = 1; + setup_camera_stream_client.call(setupCameraStream_); + + break; + } case 'a': { setupCameraH264_.request.request_view = setupCameraH264_.request.FPV_CAMERA; @@ -249,11 +372,26 @@ int main(int argc, char** argv) ros::AsyncSpinner spinner(1); spinner.start(); - ROS_INFO("Wait 10 second to record stream"); - ros::Duration(10).sleep(); + ros::Duration(20).sleep(); switch (inputChar) { + case 'f': + { + setupCameraStream_.request.cameraType = setupCameraStream_.request.FPV_CAM; + setupCameraStream_.request.start = 0; + setup_camera_stream_client.call(setupCameraStream_); + + break; + } + case 'm': + { + setupCameraStream_.request.cameraType = setupCameraStream_.request.MAIN_CAM; + setupCameraStream_.request.start = 0; + setup_camera_stream_client.call(setupCameraStream_); + + break; + } case 'a': { setupCameraH264_.request.request_view = setupCameraH264_.request.FPV_CAMERA; @@ -292,4 +430,4 @@ int main(int argc, char** argv) ros::waitForShutdown(); return 0; -} \ No newline at end of file +} diff --git a/src/dji_osdk_ros/samples/flight_control_node.cpp b/src/dji_osdk_ros/samples/flight_control_node.cpp index 15f601b0..f3685f21 100644 --- a/src/dji_osdk_ros/samples/flight_control_node.cpp +++ b/src/dji_osdk_ros/samples/flight_control_node.cpp @@ -32,16 +32,28 @@ #include #include -#include -#include +#include +#include +#include +#include +#include +#include + +#include +#include //CODE using namespace dji_osdk_ros; ros::ServiceClient task_control_client; +ros::ServiceClient set_joystick_mode_client; +ros::ServiceClient joystick_action_client; -bool moveByPosOffset(FlightTaskControl& task, MoveOffset&& move_offset); +bool moveByPosOffset(FlightTaskControl& task,const JoystickCommand &offsetDesired, + float posThresholdInM = 0.8, + float yawThresholdInDeg = 1.0); +void velocityAndYawRateCtrl(const JoystickCommand &offsetDesired, uint32_t timeMs); int main(int argc, char** argv) { @@ -49,9 +61,16 @@ int main(int argc, char** argv) ros::NodeHandle nh; task_control_client = nh.serviceClient("/flight_task_control"); auto set_go_home_altitude_client = nh.serviceClient("/set_go_home_altitude"); - auto set_current_point_as_home_client = nh.serviceClient("/set_current_point_as_home"); - auto enable_avoid_client = nh.serviceClient("/enable_avoid"); - auto enable_upward_avoid_client = nh.serviceClient("/enable_upwards_avoid"); + auto get_go_home_altitude_client = nh.serviceClient("get_go_home_altitude"); + auto set_current_point_as_home_client = nh.serviceClient("/set_current_aircraft_point_as_home"); + auto enable_horizon_avoid_client = nh.serviceClient("/set_horizon_avoid_enable"); + auto enable_upward_avoid_client = nh.serviceClient("/set_upwards_avoid_enable"); + auto get_avoid_enable_client = nh.serviceClient("get_avoid_enable_status"); + auto obtain_ctrl_authority_client = nh.serviceClient("obtain_release_control_authority"); + auto emergency_brake_client = nh.serviceClient("emergency_brake"); + + set_joystick_mode_client = nh.serviceClient("set_joystick_mode"); + joystick_action_client = nh.serviceClient("joystick_action"); std::cout << "| Available commands: |" << std::endl; @@ -64,12 +83,18 @@ int main(int argc, char** argv) std::cout << "| [c] Monitored Takeoff + Position Control + Force Landing " "Avoid Ground |" << std::endl; + std::cout << "| [d] Monitored Takeoff + Velocity Control + Landing |" + << std::endl; std::cout << "Please select command: "; char inputChar; std::cin >> inputChar; - + EmergencyBrake emergency_brake; FlightTaskControl control_task; + ObtainControlAuthority obtainCtrlAuthority; + + obtainCtrlAuthority.request.enable_obtain = true; + obtain_ctrl_authority_client.call(obtainCtrlAuthority); switch (inputChar) { @@ -112,13 +137,13 @@ int main(int argc, char** argv) { ROS_INFO_STREAM("Takeoff task successful"); ros::Duration(2.0).sleep(); - + ROS_INFO_STREAM("Move by position offset request sending ..."); - moveByPosOffset(control_task, MoveOffset(0.0, 6.0, 6.0, 30.0)); + moveByPosOffset(control_task, {0.0, 6.0, 6.0, 30.0}, 0.8, 1); ROS_INFO_STREAM("Step 1 over!"); - moveByPosOffset(control_task, MoveOffset(6.0, 0.0, -3.0, -30.0)); + moveByPosOffset(control_task, {6.0, 0.0, -3, -30.0}, 0.8, 1); ROS_INFO_STREAM("Step 2 over!"); - moveByPosOffset(control_task, MoveOffset(-6.0, -6.0, 0.0, 0.0)); + moveByPosOffset(control_task, {-6.0, -6.0, 0.0, 0.0}, 0.8, 1); ROS_INFO_STREAM("Step 3 over!"); control_task.request.task = FlightTaskControl::Request::TASK_LAND; @@ -150,17 +175,17 @@ int main(int argc, char** argv) ROS_INFO_STREAM("Takeoff task successful"); ros::Duration(2.0).sleep(); - ROS_INFO_STREAM("turn on Collision-Avoidance-Enabled"); - AvoidEnable avoid_req; - avoid_req.request.enable = true; - enable_avoid_client.call(avoid_req); - if(avoid_req.response.result == false) + ROS_INFO_STREAM("turn on Horizon_Collision-Avoidance-Enabled"); + SetAvoidEnable horizon_avoid_req; + horizon_avoid_req.request.enable = true; + enable_horizon_avoid_client.call(horizon_avoid_req); + if(horizon_avoid_req.response.result == false) { - ROS_ERROR_STREAM("Enable Avoid FAILED"); + ROS_ERROR_STREAM("Enable Horizon Avoid FAILED"); } ROS_INFO_STREAM("turn on Upwards-Collision-Avoidance-Enabled"); - AvoidEnable upward_avoid_req; + SetAvoidEnable upward_avoid_req; upward_avoid_req.request.enable = true; enable_upward_avoid_client.call(upward_avoid_req); if(upward_avoid_req.response.result == false) @@ -168,14 +193,23 @@ int main(int argc, char** argv) ROS_ERROR_STREAM("Enable Upward Avoid FAILED"); } + GetAvoidEnable getAvoidEnable; + get_avoid_enable_client.call(getAvoidEnable); + if (getAvoidEnable.response.result) + { + ROS_INFO("get horizon avoid enable status:%d, get upwards avoid enable status:%d", + getAvoidEnable.response.horizon_avoid_enable_status, + getAvoidEnable.response.upwards_avoid_enable_status); + } + ROS_INFO_STREAM("Move by position offset request sending ..."); ROS_INFO_STREAM("Move to higher altitude"); - moveByPosOffset(control_task, MoveOffset(0.0, 0.0, 30.0, 0.0)); + moveByPosOffset(control_task, {0.0, 0.0, 30.0, 0.0}, 0.8, 1); ROS_INFO_STREAM("Move a short distance"); - moveByPosOffset(control_task, MoveOffset(10.0, 0.0, 0.0, 0.0)); + moveByPosOffset(control_task, {10.0, 0.0, 0.0, 0.0}, 0.8, 1); ROS_INFO_STREAM("Set aircraft current position as new home location"); - SetNewHomePoint home_set_req; + SetCurrentAircraftLocAsHomePoint home_set_req; set_current_point_as_home_client.call(home_set_req); if(home_set_req.response.result == false) { @@ -183,6 +217,17 @@ int main(int argc, char** argv) break; } + + ROS_INFO_STREAM("Get current go home altitude"); + GetGoHomeAltitude current_go_home_altitude; + get_go_home_altitude_client.call(current_go_home_altitude); + if(current_go_home_altitude.response.result == false) + { + ROS_ERROR_STREAM("Get altitude for go home FAILED"); + break; + } + ROS_INFO("Current go home altitude is :%d m", current_go_home_altitude.response.altitude); + ROS_INFO_STREAM("Set new go home altitude"); SetGoHomeAltitude altitude_go_home; altitude_go_home.request.altitude = 50; @@ -193,15 +238,39 @@ int main(int argc, char** argv) break; } + get_go_home_altitude_client.call(current_go_home_altitude); + if(current_go_home_altitude.response.result == false) + { + ROS_ERROR_STREAM("Get altitude for go home FAILED"); + break; + } + ROS_INFO("Current go home altitude is :%d m", current_go_home_altitude.response.altitude); + ROS_INFO_STREAM("Move to another position"); - moveByPosOffset(control_task, MoveOffset(50.0, 0.0, 0.0, 0.0)); + moveByPosOffset(control_task, {50.0, 0.0, 0.0, 0.0} , 0.8, 1); + + ROS_INFO_STREAM("Shut down Horizon_Collision-Avoidance-Enabled"); + horizon_avoid_req.request.enable = false; + enable_horizon_avoid_client.call(horizon_avoid_req); + if(horizon_avoid_req.response.result == false) + { + ROS_ERROR_STREAM("Disable Horizon Avoid FAILED"); + } + + ROS_INFO_STREAM("Shut down Upwards-Collision-Avoidance-Enabled"); + upward_avoid_req.request.enable = false; + enable_upward_avoid_client.call(upward_avoid_req); + if(upward_avoid_req.response.result == false) + { + ROS_ERROR_STREAM("Enable Upward Avoid FAILED"); + } - ROS_INFO_STREAM("Shut down Collision-Avoidance-Enabled"); - avoid_req.request.enable = false; - enable_avoid_client.call(avoid_req); - if(avoid_req.response.result == false) + get_avoid_enable_client.call(getAvoidEnable); + if (getAvoidEnable.response.result) { - ROS_ERROR_STREAM("Disable Avoid FAILED"); + ROS_INFO("get horizon avoid enable status:%d, get upwards avoid enable status:%d", + getAvoidEnable.response.horizon_avoid_enable_status, + getAvoidEnable.response.upwards_avoid_enable_status); } ROS_INFO_STREAM("Go home..."); @@ -215,6 +284,52 @@ int main(int argc, char** argv) break; } } + case 'd': + { + control_task.request.task = FlightTaskControl::Request::TASK_TAKEOFF; + ROS_INFO_STREAM("Takeoff request sending ..."); + task_control_client.call(control_task); + if(control_task.response.result == false) + { + ROS_ERROR_STREAM("Takeoff task failed"); + break; + } + + if(control_task.response.result == true) + { + ROS_INFO_STREAM("Takeoff task successful"); + ros::Duration(2).sleep(); + + velocityAndYawRateCtrl( {0, 0, 5.0, 0}, 2000); + ROS_INFO_STREAM("Step 1 over!EmergencyBrake for 2s\n"); + emergency_brake_client.call(emergency_brake); + ros::Duration(2).sleep(); + velocityAndYawRateCtrl({-1.5, 2, 0, 0}, 2000); + ROS_INFO_STREAM("Step 2 over!EmergencyBrake for 2s\n"); + emergency_brake_client.call(emergency_brake); + ros::Duration(2).sleep(); + velocityAndYawRateCtrl({3, 0, 0, 0}, 2500); + ROS_INFO_STREAM("Step 3 over!EmergencyBrake for 2s\n"); + emergency_brake_client.call(emergency_brake); + ros::Duration(2).sleep(); + velocityAndYawRateCtrl({-1.6, -2, 0, 0}, 2200); + ROS_INFO_STREAM("Step 4 over!EmergencyBrake for 2s\n"); + emergency_brake_client.call(emergency_brake); + ros::Duration(2).sleep(); + + control_task.request.task = FlightTaskControl::Request::TASK_LAND; + ROS_INFO_STREAM("Landing request sending ..."); + task_control_client.call(control_task); + if(control_task.response.result == true) + { + ROS_INFO_STREAM("Land task successful"); + break; + } + ROS_INFO_STREAM("Land task failed."); + break; + } + break; + } default: break; } @@ -226,19 +341,51 @@ int main(int argc, char** argv) } -bool moveByPosOffset(FlightTaskControl& task, MoveOffset&& move_offset) +bool moveByPosOffset(FlightTaskControl& task,const JoystickCommand &offsetDesired, + float posThresholdInM, + float yawThresholdInDeg) { - task.request.task = FlightTaskControl::Request::TASK_GO_LOCAL_POS; - // pos_offset: A vector contains that position_x_offset, position_y_offset, position_z_offset in order - task.request.pos_offset.clear(); - task.request.pos_offset.push_back(move_offset.x); - task.request.pos_offset.push_back(move_offset.y); - task.request.pos_offset.push_back(move_offset.z); - // yaw_params: A vector contains that yaw_desired, position_threshold(Meter), yaw_threshold(Degree) - task.request.yaw_params.clear(); - task.request.yaw_params.push_back(move_offset.yaw); - task.request.yaw_params.push_back(move_offset.pos_threshold); - task.request.yaw_params.push_back(move_offset.yaw_threshold); + task.request.task = FlightTaskControl::Request::TASK_POSITION_AND_YAW_CONTROL; + task.request.joystickCommand.x = offsetDesired.x; + task.request.joystickCommand.y = offsetDesired.y; + task.request.joystickCommand.z = offsetDesired.z; + task.request.joystickCommand.yaw = offsetDesired.yaw; + task.request.posThresholdInM = posThresholdInM; + task.request.yawThresholdInDeg = yawThresholdInDeg; + task_control_client.call(task); return task.response.result; } + +void velocityAndYawRateCtrl(const JoystickCommand &offsetDesired, uint32_t timeMs) +{ + double originTime = 0; + double currentTime = 0; + uint64_t elapsedTimeInMs = 0; + + SetJoystickMode joystickMode; + JoystickAction joystickAction; + + joystickMode.request.horizontal_mode = joystickMode.request.HORIZONTAL_VELOCITY; + joystickMode.request.vertical_mode = joystickMode.request.VERTICAL_VELOCITY; + joystickMode.request.yaw_mode = joystickMode.request.YAW_RATE; + joystickMode.request.horizontal_coordinate = joystickMode.request.HORIZONTAL_GROUND; + joystickMode.request.stable_mode = joystickMode.request.STABLE_ENABLE; + set_joystick_mode_client.call(joystickMode); + + joystickAction.request.joystickCommand.x = offsetDesired.x; + joystickAction.request.joystickCommand.y = offsetDesired.y; + joystickAction.request.joystickCommand.z = offsetDesired.z; + joystickAction.request.joystickCommand.yaw = offsetDesired.yaw; + + originTime = ros::Time::now().toSec(); + currentTime = originTime; + elapsedTimeInMs = (currentTime - originTime)*1000; + + while(elapsedTimeInMs <= timeMs) + { + currentTime = ros::Time::now().toSec(); + elapsedTimeInMs = (currentTime - originTime) * 1000; + joystick_action_client.call(joystickAction); + } +} diff --git a/src/dji_osdk_ros/samples/gimbal_camera_control_node.cpp b/src/dji_osdk_ros/samples/gimbal_camera_control_node.cpp index 36cbcda6..e1cbed31 100644 --- a/src/dji_osdk_ros/samples/gimbal_camera_control_node.cpp +++ b/src/dji_osdk_ros/samples/gimbal_camera_control_node.cpp @@ -36,6 +36,7 @@ #include #include #include +#include #include #include #include @@ -58,6 +59,7 @@ int main(int argc, char** argv) { auto camera_set_iso_client = nh.serviceClient("camera_task_set_ISO"); auto camera_set_focus_point_client = nh.serviceClient("camera_task_set_focus_point"); auto camera_set_tap_zoom_point_client = nh.serviceClient("camera_task_tap_zoom_point"); + auto camera_set_zoom_para_client = nh.serviceClient("camera_task_set_zoom_para"); auto camera_task_zoom_ctrl_client = nh.serviceClient("camera_task_zoom_ctrl"); auto camera_start_shoot_single_photo_client = nh.serviceClient( "camera_start_shoot_single_photo"); @@ -74,46 +76,44 @@ int main(int argc, char** argv) { while (true) { std::cout << std::endl; std::cout - << "| Available commands: |" - << std::endl - << "| [a] Set camera shutter speed |" - << std::endl - << "| [b] Set camera aperture |" - << std::endl - << "| [c] Set camera EV value |" - << std::endl - << "| [d] Set camera ISO value |" - << std::endl - << "| [e] Set camera focus point |" - << std::endl - << "| [f] Set camera tap zoom point |" - << std::endl - << "| [g] Set camera zoom parameter |" - << std::endl - << "| [h] Shoot Single photo Sample |" - << std::endl - << "| [i] Shoot AEB photo Sample |" - << std::endl - << "| [j] Shoot Burst photo Sample |" - << std::endl - << "| [k] Shoot Interval photo Sample |" - << std::endl - << "| [l] Record video Sample |" - << std::endl - << "| [m] Rotate gimbal sample |" - << std::endl - << "| [n] Reset gimbal sample |" - << std::endl - << "| [q] Quit |" - << std::endl; + << "| Available commands: |\n" + << "| [a] Set camera shutter speed |\n" + << "| Main camera : X5S, X7, Z30, H20/H20T(zoom mode) etc. |\n" + << "| [b] Set camera aperture |\n" + << "| Main camera : X5S, X7, Z30, H20/H20T(zoom mode) etc. |\n" + << "| [c] Set camera EV value |\n" + << "| Main camera : X5S, X7, Z30, H20/H20T(zoom mode) etc. |\n" + << "| [d] Set camera ISO value |\n" + << "| Main camera : X5S, X7, Z30, H20/H20T(zoom mode) etc. |\n" + << "| [e] Set camera focus point |\n" + << "| Main camera : X5S, X7, H20/H20T(zoom mode) etc. |\n" + << "| [f] Set camera tap zoom point |\n" + << "| Vice camera : Z30, H20/H20T(zoom mode) etc. |\n" + << "| [g] Set camera zoom parameter |\n" + << "| Vice camera : Z30, H20/H20T(zoom mode) etc. |\n" + << "| [h] Shoot Single photo Sample |\n" + << "| Main camera : X5S, X7, XTS, Z30, H20/H20T etc. |\n" + << "| [i] Shoot AEB photo Sample |\n" + << "| Main camera : X5S, X7 etc. |\n" + << "| [j] Shoot Burst photo Sample |\n" + << "| Main camera : X5S, X7 etc. |\n" + << "| [k] Shoot Interval photo Sample |\n" + << "| Main camera : X5S, X7, XTS, Z30, H20/H20T etc. |\n" + << "| [l] Record video Sample |\n" + << "| Main camera : X5S, X7, XTS, Z30, H20/H20T etc. |\n" + << "| [m] Rotate gimbal sample |\n" + << "| Main camera : X5S, X7, XTS, Z30, H20/H20T etc. |\n" + << "| [n] Reset gimbal sample |\n" + << "| Main camera : X5S, X7, XTS, Z30, H20/H20T etc. |\n" + << "| [q] Quit |\n"; std::cin >> inputChar; switch (inputChar) { case 'a': { CameraShutterSpeed cameraShutterSpeed; cameraShutterSpeed.request.payload_index = static_cast(dji_osdk_ros::PayloadIndex::PAYLOAD_INDEX_0); - cameraShutterSpeed.request.exposure_mode = static_cast(ExposureMode::SHUTTER_PRIORITY); - cameraShutterSpeed.request.shutter_speed = static_cast(ShutterSpeed::SHUTTER_SPEED_1_50); + cameraShutterSpeed.request.exposure_mode = static_cast(ExposureMode::EXPOSURE_MANUAL); + cameraShutterSpeed.request.shutter_speed = static_cast(ShutterSpeed::SHUTTER_SPEED_1_100); camera_set_shutter_speed_client.call(cameraShutterSpeed); sleep(2); break; @@ -122,8 +122,8 @@ int main(int argc, char** argv) { case 'b': { CameraAperture cameraAperture; cameraAperture.request.payload_index = static_cast(dji_osdk_ros::PayloadIndex::PAYLOAD_INDEX_0); - cameraAperture.request.exposure_mode = static_cast(ExposureMode::APERTURE_PRIORITY); - cameraAperture.request.aperture = static_cast(Aperture::F_3_DOT_5); + cameraAperture.request.exposure_mode = static_cast(ExposureMode::EXPOSURE_MANUAL); + cameraAperture.request.aperture = static_cast(Aperture::F_4); camera_set_aperture_client.call(cameraAperture); sleep(2); break; @@ -172,21 +172,23 @@ int main(int argc, char** argv) { break; } case 'g': { + CameraSetZoomPara cameraSetZoomPara; + cameraSetZoomPara.request.payload_index = static_cast(dji_osdk_ros::PayloadIndex::PAYLOAD_INDEX_1); + cameraSetZoomPara.request.factor = 5; + camera_set_zoom_para_client.call(cameraSetZoomPara); + sleep(4); + cameraSetZoomPara.request.factor = 10; + camera_set_zoom_para_client.call(cameraSetZoomPara); + sleep(4); CameraZoomCtrl cameraZoomCtrl; cameraZoomCtrl.request.start_stop = 1; cameraZoomCtrl.request.payload_index = static_cast(dji_osdk_ros::PayloadIndex::PAYLOAD_INDEX_1); - cameraZoomCtrl.request.direction = static_cast(dji_osdk_ros::ZoomDirection::ZOOM_IN); - cameraZoomCtrl.request.speed = static_cast(dji_osdk_ros::ZoomSpeed::NORMAL); - camera_task_zoom_ctrl_client.call(cameraZoomCtrl); - sleep(4); - cameraZoomCtrl.request.start_stop = 0; - camera_task_zoom_ctrl_client.call(cameraZoomCtrl); - sleep(2); - cameraZoomCtrl.request.start_stop = 1; cameraZoomCtrl.request.direction = static_cast(dji_osdk_ros::ZoomDirection::ZOOM_OUT); cameraZoomCtrl.request.speed = static_cast(dji_osdk_ros::ZoomSpeed::FASTEST); + camera_task_zoom_ctrl_client.call(cameraZoomCtrl); sleep(8); cameraZoomCtrl.request.start_stop = 0; + cameraZoomCtrl.request.payload_index = static_cast(dji_osdk_ros::PayloadIndex::PAYLOAD_INDEX_1); camera_task_zoom_ctrl_client.call(cameraZoomCtrl); break; } @@ -218,7 +220,7 @@ int main(int argc, char** argv) { CameraStartShootIntervalPhoto cameraStartShootIntervalPhoto; cameraStartShootIntervalPhoto.request.payload_index = static_cast(dji_osdk_ros::PayloadIndex::PAYLOAD_INDEX_0); cameraStartShootIntervalPhoto.request.photo_num_conticap = 255; - cameraStartShootIntervalPhoto.request.time_interval = 4; + cameraStartShootIntervalPhoto.request.time_interval = 3; camera_start_shoot_interval_photo_client.call(cameraStartShootIntervalPhoto); std::cout << "Sleep 15 seconds" << std::endl; sleep(15); diff --git a/src/dji_osdk_ros/samples/hms_node.cpp b/src/dji_osdk_ros/samples/hms_node.cpp new file mode 100644 index 00000000..6d56bbdf --- /dev/null +++ b/src/dji_osdk_ros/samples/hms_node.cpp @@ -0,0 +1,73 @@ +/** @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 +#include + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "hms_node"); + ros::NodeHandle nh; + + ros::Duration(1).sleep(); + ros::AsyncSpinner spinner(1); + spinner.start(); + + auto get_hms_data_client = nh.serviceClient("get_hms_data"); + dji_osdk_ros::GetHMSData get_hms_data; + get_hms_data.request.enable = true; + ros::Rate rate(5); + + while(ros::ok()) + { + get_hms_data_client.call(get_hms_data); + // ROS_INFO("%d", get_hms_data.response.result); + + for (int i = 0; i < get_hms_data.response.errList.size(); i++) + { + ROS_INFO("hmsErrListNum: %d, timeStamp:%ld, alarm_id: 0x%08x, sensorIndex: %d, level: %d", + i+1, get_hms_data.response.timeStamp, + get_hms_data.response.errList[i].alarmID, + get_hms_data.response.errList[i].sensorIndex, + get_hms_data.response.errList[i].reportLevel); + if (get_hms_data.response.deviceIndex != 0xff) + { + ROS_INFO("%d Camera/Gimbal has trouble!", get_hms_data.response.deviceIndex); + } + } + + rate.sleep(); + } + + ROS_INFO_STREAM("Finished. Press CTRL-C to terminate the node"); + + ros::waitForShutdown(); + return 0; +} + + diff --git a/src/dji_osdk_ros/samples/mission_node.cpp b/src/dji_osdk_ros/samples/mission_node.cpp index 61fe531d..99fce535 100644 --- a/src/dji_osdk_ros/samples/mission_node.cpp +++ b/src/dji_osdk_ros/samples/mission_node.cpp @@ -28,10 +28,12 @@ #include #include +#include using namespace DJI::OSDK; // global variables +ros::ServiceClient obtain_ctrl_authority_client; ros::ServiceClient waypoint_upload_client; ros::ServiceClient waypoint_action_client; ros::ServiceClient hotpoint_upload_client; @@ -450,6 +452,8 @@ int main(int argc, char** argv) ros::NodeHandle nh; // ROS stuff + obtain_ctrl_authority_client = nh.serviceClient( + "obtain_release_control_authority"); waypoint_upload_client = nh.serviceClient( "dji_osdk_ros/mission_waypoint_upload"); waypoint_action_client = nh.serviceClient( @@ -470,10 +474,14 @@ int main(int argc, char** argv) "dji_osdk_ros/gps_position", 10, &gpsPosCallback); // Setup variables for use + dji_osdk_ros::ObtainControlAuthority obtainCtrlAuthority; + obtainCtrlAuthority.request.enable_obtain = true; uint8_t wayptPolygonSides; int hotptInitRadius; int responseTimeout = 1; + obtain_ctrl_authority_client.call(obtainCtrlAuthority); + // Display interactive prompt std::cout << "| Available commands: |" diff --git a/src/dji_osdk_ros/samples/mobile_device_node.cpp b/src/dji_osdk_ros/samples/mobile_device_node.cpp index 5c611cc6..6e115e67 100644 --- a/src/dji_osdk_ros/samples/mobile_device_node.cpp +++ b/src/dji_osdk_ros/samples/mobile_device_node.cpp @@ -105,6 +105,11 @@ void fromMobileDataSubCallback(const dji_osdk_ros::MobileData::ConstPtr& fromMob ackReturnToMobile.ackResult = recordVideo(); sendToMobile(ackReturnToMobile); } + + case 256: + { + ackReturnToMobile.ackResult = obtainJoystickControlAuthority(); + } break; } ros::Duration(1.0).sleep(); @@ -146,33 +151,32 @@ bool gohomeAndConfirmLanding() return flightTaskControl.response.result; } -bool moveByPosOffset(MoveOffset&& move_offset) + +bool moveByPosOffset(const JoystickCommand &offsetDesired, + float posThresholdInM, + float yawThresholdInDeg) { FlightTaskControl flightTaskControl; - flightTaskControl.request.task = FlightTaskControl::Request::TASK_GO_LOCAL_POS; - // pos_offset: A vector contains that position_x_offset, position_y_offset, position_z_offset in order - flightTaskControl.request.pos_offset.clear(); - flightTaskControl.request.pos_offset.push_back(move_offset.x); - flightTaskControl.request.pos_offset.push_back(move_offset.y); - flightTaskControl.request.pos_offset.push_back(move_offset.z); - // yaw_params: A vector contains that yaw_desired, position_threshold(Meter), yaw_threshold(Degree) - flightTaskControl.request.yaw_params.clear(); - flightTaskControl.request.yaw_params.push_back(move_offset.yaw); - flightTaskControl.request.yaw_params.push_back(move_offset.pos_threshold); - flightTaskControl.request.yaw_params.push_back(move_offset.yaw_threshold); - flight_control_client.call(flightTaskControl); + flightTaskControl.request.task = FlightTaskControl::Request::TASK_POSITION_AND_YAW_CONTROL; + flightTaskControl.request.joystickCommand.x = offsetDesired.x; + flightTaskControl.request.joystickCommand.y = offsetDesired.y; + flightTaskControl.request.joystickCommand.z = offsetDesired.z; + flightTaskControl.request.joystickCommand.yaw = offsetDesired.yaw; + flightTaskControl.request.posThresholdInM = posThresholdInM; + flightTaskControl.request.yawThresholdInDeg = yawThresholdInDeg; + flight_control_client.call(flightTaskControl); return flightTaskControl.response.result; } bool localPositionCtrl() { ROS_INFO_STREAM("Move by position offset request sending ..."); - moveByPosOffset(MoveOffset(0.0, 6.0, 6.0, 30.0)); + moveByPosOffset({0.0, 6.0, 6.0, 30.0}, 0.8, 1); ROS_INFO_STREAM("Step 1 over!"); - moveByPosOffset(MoveOffset(6.0, 0.0, -3.0, -30.0)); + moveByPosOffset({6.0, 0.0, -3, -30.0}, 0.8, 1); ROS_INFO_STREAM("Step 2 over!"); - moveByPosOffset(MoveOffset(-6.0, -6.0, 0.0, 0.0)); + moveByPosOffset({-6.0, -6.0, 0.0, 0.0}, 0.8, 1); ROS_INFO_STREAM("Step 3 over!"); return true; @@ -221,6 +225,15 @@ bool recordVideo() camera_record_video_action_client.call(cameraRecordVideoAction); } +bool obtainJoystickControlAuthority() +{ + ObtainControlAuthority obtainJoystickControlAuthority; + obtainJoystickControlAuthority.request.enable_obtain = true; + obtain_ctrl_authority_client.call(obtainJoystickControlAuthority); + + return obtainJoystickControlAuthority.response.result; +} + static void DisplayMainMenu(void) { printf("\r\n"); @@ -238,6 +251,7 @@ int main(int argc, char *argv[]) { ROS_INFO("mobile device node is running!"); ros::NodeHandle nh; + obtain_ctrl_authority_client = nh.serviceClient("obtain_release_control_authority"); send_to_mobile_data_client = nh.serviceClient("send_data_to_mobile_device"); flight_control_client = nh.serviceClient("flight_task_control"); gimbal_control_client = nh.serviceClient("gimbal_task_control"); diff --git a/src/dji_osdk_ros/samples/stereo_vision_depth_perception_node.cpp b/src/dji_osdk_ros/samples/stereo_vision_depth_perception_node.cpp index ae66c905..6b80e548 100644 --- a/src/dji_osdk_ros/samples/stereo_vision_depth_perception_node.cpp +++ b/src/dji_osdk_ros/samples/stereo_vision_depth_perception_node.cpp @@ -84,7 +84,7 @@ main(int argc, char** argv) } else { - M210_STEREO::Config::setParamFile(M300_FRONT_STEREO_PARAM_YAML_NAME); + M210_STEREO::Config::setParamFile("/home/alireza/catkin_ws/src/Onboard-SDK-4.0/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/m300_front_stereo_param.yaml"); } } @@ -107,9 +107,9 @@ main(int argc, char** argv) //! Setup ros related stuff rect_img_left_publisher = - nh.advertise("/stereo_depth_perception/rectified_vga_front_left_image", 10); + nh.advertise("/dji_osdk_ros/stereo_vga_front/left/image_raw", 10); rect_img_right_publisher = - nh.advertise("/stereo_depth_perception/rectified_vga_front_right_image", 10); + nh.advertise("/dji_osdk_ros/stereo_vga_front/right/image_raw", 10); left_disparity_publisher = nh.advertise("/stereo_depth_perception/disparity_front_left_image", 10); point_cloud_publisher = @@ -532,4 +532,4 @@ shutDownHandler(int s) } exit(1); -} \ No newline at end of file +} diff --git a/src/dji_osdk_ros/samples/telemetry_node.cpp b/src/dji_osdk_ros/samples/telemetry_node.cpp index 18ab6a09..91bed36a 100644 --- a/src/dji_osdk_ros/samples/telemetry_node.cpp +++ b/src/dji_osdk_ros/samples/telemetry_node.cpp @@ -222,6 +222,42 @@ void flightAnomalySubCallback(const dji_osdk_ros::FlightAnomaly::ConstPtr& fligh flight_anomaly_ = *flightAnomaly; } +ros::Subscriber batteryStateSub; +ros::Subscriber imuSub; +ros::Subscriber flightStatusSub; +ros::Subscriber gpsHealthSub; +ros::Subscriber gpsPositionSub; +ros::Subscriber heightSub; +ros::Subscriber localPositionSub; +ros::Subscriber velocitySub; +ros::Subscriber gimbalAngleSub; +ros::Subscriber rcDataSub; +ros::Subscriber attitudeSub; + +ros::Subscriber fromMobileDataSub; +ros::Subscriber fromPayloadDataSub; + +ros::Subscriber localFrameRefSub; +ros::Subscriber timeSyncNmeaSub; +ros::Subscriber timeSyncGpsUtcSub; +ros::Subscriber timeSyncFcUtcSub; +ros::Subscriber timeSyncPpsSourceSub; + +ros::Subscriber voPositionSub; +ros::Subscriber angularRateSub; +ros::Subscriber accelerationSub; +ros::Subscriber displayModeSub; +ros::Subscriber triggerSub; + +ros::Subscriber rcConnectionStatusSub; +ros::Subscriber rtkPositionSub; +ros::Subscriber rtkVelocitySub; +ros::Subscriber rtkYawSub; +ros::Subscriber rtkPositionInfoSub; +ros::Subscriber rtkYawInfoSub; +ros::Subscriber rtkConnectionStatusSub; +ros::Subscriber flightAnomalySub; + int main(int argc ,char** argv) { ros::init(argc, argv, "telemetry_node"); @@ -229,45 +265,44 @@ int main(int argc ,char** argv) bool userSelectBroadcast = false; nh.getParam("/vehicle_node/use_broadcast", userSelectBroadcast); - ros::Subscriber batteryStateSub = nh.subscribe("dji_osdk_ros/battery_state", 10, &batteryStateSubCallback); - ros::Subscriber imuSub = nh.subscribe("dji_osdk_ros/imu", 10, &imuSubCallback); - ros::Subscriber flightStatusSub = nh.subscribe("dji_osdk_ros/flight_status", 10, &flightStatusSubCallback); - ros::Subscriber gpsHealthSub = nh.subscribe("dji_osdk_ros/gps_health", 10, &gpsHealthSubCallback); - ros::Subscriber gpsPositionSub = nh.subscribe("dji_osdk_ros/gps_position", 10, &gpsPositionSubCallback); - ros::Subscriber heightSub = nh.subscribe("dji_osdk_ros/height_above_takeoff", 10, &heightSubCallback); - ros::Subscriber localPositionSub = nh.subscribe("dji_osdk_ros/local_position", 10, &localPositionSubCallback); - ros::Subscriber velocitySub = nh.subscribe("dji_osdk_ros/velocity", 10, &velocitySubCallback); - ros::Subscriber gimbalAngleSub = nh.subscribe("dji_osdk_ros/gimbal_angle", 10, &gimbalAngleSubCallback); - ros::Subscriber rcDataSub = nh.subscribe("dji_osdk_ros/rc", 10, &rcDataCallback); - ros::Subscriber attitudeSub = nh.subscribe("dji_osdk_ros/attitude", 10, &attitudeSubCallback); - - ros::Subscriber fromMobileDataSub = nh.subscribe("dji_osdk_ros/from_mobile_data", 10, &fromMobileDataSubCallback); - ros::Subscriber fromPayloadDataSub = nh.subscribe("dji_osdk_ros/from_payload_data", 10, &fromPayloadDataSubCallback); + batteryStateSub = nh.subscribe("dji_osdk_ros/battery_state", 10, &batteryStateSubCallback); + imuSub = nh.subscribe("dji_osdk_ros/imu", 10, &imuSubCallback); + flightStatusSub = nh.subscribe("dji_osdk_ros/flight_status", 10, &flightStatusSubCallback); + gpsHealthSub = nh.subscribe("dji_osdk_ros/gps_health", 10, &gpsHealthSubCallback); + gpsPositionSub = nh.subscribe("dji_osdk_ros/gps_position", 10, &gpsPositionSubCallback); + heightSub = nh.subscribe("dji_osdk_ros/height_above_takeoff", 10, &heightSubCallback); + localPositionSub = nh.subscribe("dji_osdk_ros/local_position", 10, &localPositionSubCallback); + velocitySub = nh.subscribe("dji_osdk_ros/velocity", 10, &velocitySubCallback); + gimbalAngleSub = nh.subscribe("dji_osdk_ros/gimbal_angle", 10, &gimbalAngleSubCallback); + rcDataSub = nh.subscribe("dji_osdk_ros/rc", 10, &rcDataCallback); + + fromMobileDataSub = nh.subscribe("dji_osdk_ros/from_mobile_data", 10, &fromMobileDataSubCallback); + fromPayloadDataSub = nh.subscribe("dji_osdk_ros/from_payload_data", 10, &fromPayloadDataSubCallback); /* only if you call the service of set_local_pos_reference ,the topic can be valid" */ - ros::Subscriber localFrameRefSub = nh.subscribe("dji_osdk_ros/local_frame_ref", 10, &localFrameRefSubCallback); - ros::Subscriber timeSyncNmeaSub = nh.subscribe("dji_osdk_ros/time_sync_nmea_msg", 10, &timeSyncNmeaSubSCallback); - ros::Subscriber timeSyncGpsUtcSub = nh.subscribe("dji_osdk_ros/time_sync_gps_utc", 10, &timeSyncGpsUtcSubCallback); - ros::Subscriber timeSyncFcUtcSub = nh.subscribe("dji_osdk_ros/time_sync_fc_time_utc", 10, &timeSyncFcUtcSubCallback); - ros::Subscriber timeSyncPpsSourceSub = nh.subscribe("dji_osdk_ros/time_sync_pps_source", 10, &timeSyncPpsSourceSubCallback); + localFrameRefSub = nh.subscribe("dji_osdk_ros/local_frame_ref", 10, &localFrameRefSubCallback); + timeSyncNmeaSub = nh.subscribe("dji_osdk_ros/time_sync_nmea_msg", 10, &timeSyncNmeaSubSCallback); + timeSyncGpsUtcSub = nh.subscribe("dji_osdk_ros/time_sync_gps_utc", 10, &timeSyncGpsUtcSubCallback); + timeSyncFcUtcSub = nh.subscribe("dji_osdk_ros/time_sync_fc_time_utc", 10, &timeSyncFcUtcSubCallback); + timeSyncPpsSourceSub = nh.subscribe("dji_osdk_ros/time_sync_pps_source", 10, &timeSyncPpsSourceSubCallback); if (!userSelectBroadcast) { - ros::Subscriber attitudeSub = nh.subscribe("dji_osdk_ros/attitude", 10, &attitudeSubCallback); - ros::Subscriber voPositionSub = nh.subscribe("dji_osdk_ros/vo_position", 10, &voPositionSubCallback); - ros::Subscriber angularRateSub = nh.subscribe("dji_osdk_ros/angular_velocity_fused", 10, &angularRateSubSCallback); - ros::Subscriber accelerationSub = nh.subscribe("dji_osdk_ros/acceleration_ground_fused", 10, &accelerationSubCallback); - ros::Subscriber displayModeSub = nh.subscribe("dji_osdk_ros/display_mode", 10, &displayModeSubCallback); - ros::Subscriber triggerSub = nh.subscribe("dji_osdk_ros/trigger_time", 10, &triggerSubCallback); - - ros::Subscriber rcConnectionStatusSub = nh.subscribe("dji_osdk_ros/rc_connection_status", 10, &rcConnectionStatusSubCallback); - ros::Subscriber rtkPositionSub = nh.subscribe("dji_osdk_ros/rtk_position", 10, &rtkPositionSubCallback); - ros::Subscriber rtkVelocitySub = nh.subscribe("dji_osdk_ros/rtk_velocity", 10, &rtkVelocitySubCallback); - ros::Subscriber rtkYawSub = nh.subscribe("dji_osdk_ros/rtk_yaw", 10, &rtkYawSubCallback); - ros::Subscriber rtkPositionInfoSub = nh.subscribe("dji_osdk_ros/rtk_info_position", 10, &rtkPositionInfoSubCallback); - ros::Subscriber rtkYawInfoSub = nh.subscribe("dji_osdk_ros/rtk_info_yaw", 10, &rtkYawInfoSubCallback); - ros::Subscriber rtkConnectionStatusSub = nh.subscribe("dji_osdk_ros/rtk_connection_status", 10, &rtkConnectionStatusSubCallback); - ros::Subscriber flightAnomalySub = nh.subscribe("dji_osdk_ros/flight_anomaly", 10, &flightAnomalySubCallback); + voPositionSub = nh.subscribe("dji_osdk_ros/vo_position", 10, &voPositionSubCallback); + angularRateSub = nh.subscribe("dji_osdk_ros/angular_velocity_fused", 10, &angularRateSubSCallback); + accelerationSub = nh.subscribe("dji_osdk_ros/acceleration_ground_fused", 10, &accelerationSubCallback); + displayModeSub = nh.subscribe("dji_osdk_ros/display_mode", 10, &displayModeSubCallback); + triggerSub = nh.subscribe("dji_osdk_ros/trigger_time", 10, &triggerSubCallback); + + rcConnectionStatusSub = nh.subscribe("dji_osdk_ros/rc_connection_status", 10, &rcConnectionStatusSubCallback); + rtkPositionSub = nh.subscribe("dji_osdk_ros/rtk_position", 10, &rtkPositionSubCallback); + rtkVelocitySub = nh.subscribe("dji_osdk_ros/rtk_velocity", 10, &rtkVelocitySubCallback); + rtkYawSub = nh.subscribe("dji_osdk_ros/rtk_yaw", 10, &rtkYawSubCallback); + rtkPositionInfoSub = nh.subscribe("dji_osdk_ros/rtk_info_position", 10, &rtkPositionInfoSubCallback); + rtkYawInfoSub = nh.subscribe("dji_osdk_ros/rtk_info_yaw", 10, &rtkYawInfoSubCallback); + rtkConnectionStatusSub = nh.subscribe("dji_osdk_ros/rtk_connection_status", 10, &rtkConnectionStatusSubCallback); + flightAnomalySub = nh.subscribe("dji_osdk_ros/flight_anomaly", 10, &flightAnomalySubCallback); + attitudeSub = nh.subscribe("dji_osdk_ros/attitude", 10, &attitudeSubCallback); } ros::Duration(1).sleep(); diff --git a/src/dji_osdk_ros/samples/waypointV2_node.cpp b/src/dji_osdk_ros/samples/waypointV2_node.cpp index 8925465d..bdb07e29 100644 --- a/src/dji_osdk_ros/samples/waypointV2_node.cpp +++ b/src/dji_osdk_ros/samples/waypointV2_node.cpp @@ -430,6 +430,13 @@ int main(int argc, char** argv) ros::NodeHandle nh; ros::Subscriber gpsPositionSub = nh.subscribe("dji_osdk_ros/gps_position", 10, &gpsPositionSubCallback); + auto obtain_ctrl_authority_client = nh.serviceClient( + "obtain_release_control_authority"); + + //if you want to fly without rc ,you need to obtain ctrl authority.Or it will enter rc lost. + dji_osdk_ros::ObtainControlAuthority obtainCtrlAuthority; + obtainCtrlAuthority.request.enable_obtain = true; + obtain_ctrl_authority_client.call(obtainCtrlAuthority); ros::Duration(1).sleep(); ros::AsyncSpinner spinner(1); diff --git a/src/dji_osdk_ros_obsoleted/CMakeLists.txt b/src/dji_osdk_ros_obsoleted/CMakeLists.txt index 3a2d9cd5..34178f3b 100644 --- a/src/dji_osdk_ros_obsoleted/CMakeLists.txt +++ b/src/dji_osdk_ros_obsoleted/CMakeLists.txt @@ -17,6 +17,9 @@ set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html #catkin_python_setup() +find_package(OpenCV REQUIRED) +find_package(catkin REQUIRED COMPONENTS cv_bridge roscpp rospy std_msgs) + ################################################ ## Declare ROS messages, services and actions ## ################################################ @@ -63,12 +66,14 @@ set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") ## Specify additional locations of header files ## Your package locations should be listed before other locations -# include_directories(include) +# include_directories(include + include_directories( ../../include/dji_osdk_ros_obsoleted ../../include # ${DJIOSDK_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} ) ## Declare a cpp library @@ -98,14 +103,21 @@ add_executable(dji_sdk_node add_dependencies(dji_sdk_node dji_osdk_ros_generate_messages_cpp) -## Specify libraries to link a library or executable target against -target_link_libraries(dji_sdk_node +set(link_Libraries ${catkin_LIBRARIES} djiosdk-core.a dji-linker.a advanced-sensing.a ${LIBUSB_1_LIBRARIES} ${FFMPEG_LIBRARIES} + ${OpenCV_LIBRARIES} +) + + +## Specify libraries to link a library or executable target against +target_link_libraries(dji_sdk_node + ${link_Libraries} + # waypointv2-interface.a # waypointv2-core.a # DJIProtobuf.a diff --git a/src/dji_osdk_ros_obsoleted/dji_linux_environment.cpp b/src/dji_osdk_ros_obsoleted/dji_linux_environment.cpp index a3a0275e..ec62beda 100644 --- a/src/dji_osdk_ros_obsoleted/dji_linux_environment.cpp +++ b/src/dji_osdk_ros_obsoleted/dji_linux_environment.cpp @@ -52,8 +52,17 @@ DJI_Environment::findFile(std::string file) throw std::runtime_error("Error getting current directory"); std::string strCWD(cwd); - // configFile = strCWD + "/osdk-core/" + file; - configFile = strCWD + "/" + file; // just in the current working directory + return findFileInDir(file,strCWD); +} + + +/** + * @return a string of the path to a file, if found. Empty otherwise. + */ +std::string +DJI_Environment::findFileInDir(std::string file, std::string dir) +{ + std::string configFile = dir + "/" + file; std::ifstream fStream(configFile.c_str()); diff --git a/src/dji_osdk_ros_obsoleted/dji_linux_helpers.cpp b/src/dji_osdk_ros_obsoleted/dji_linux_helpers.cpp index ed573828..3105a14c 100644 --- a/src/dji_osdk_ros_obsoleted/dji_linux_helpers.cpp +++ b/src/dji_osdk_ros_obsoleted/dji_linux_helpers.cpp @@ -170,8 +170,15 @@ LinuxSetup::setupEnvironment(int argc, char** argv) } else { - config_file_path = DJI_Environment::findFile("UserConfig.txt"); - + std::string config_filename = "UserConfig.txt"; + config_file_path = DJI_Environment::findFile(config_filename); + + //look in installation dir if not found in expected places + if(config_file_path.empty()){ + //FIXME: find a better place to declare this without ros distro (melodic) + std::string installationDir = "/opt/ros/melodic/share/dji_osdk_ros/launch"; + config_file_path = DJI_Environment::findFileInDir(config_filename,installationDir); + } if (config_file_path.empty()) throw std::runtime_error("User configuration file not found"); } diff --git a/src/dji_osdk_ros_obsoleted/modules/dji_sdk_node.cpp b/src/dji_osdk_ros_obsoleted/modules/dji_sdk_node.cpp index 0ef42676..9e851936 100644 --- a/src/dji_osdk_ros_obsoleted/modules/dji_sdk_node.cpp +++ b/src/dji_osdk_ros_obsoleted/modules/dji_sdk_node.cpp @@ -29,6 +29,7 @@ DJISDKNode::DJISDKNode(ros::NodeHandle& nh, ros::NodeHandle& nh_private, int arg nh_private.param("gravity_const", gravity_const, 9.801); nh_private.param("align_time", align_time_with_FC, false); nh_private.param("use_broadcast", user_select_broadcast, false); + nh_private.param("enable_advanced_sensing", enable_advanced_sensing, true); //! Default values for local Position local_pos_ref_latitude = 0; @@ -76,6 +77,15 @@ DJISDKNode::~DJISDKNode() { cleanUpSubscribeFromFC(); } + +#ifdef ADVANCED_SENSING + if(latest_camera_ > 0) + { + vehicle->advancedSensing->unsubscribeVGAImages(latest_camera_); + ROS_INFO("Unsubscribed from the VGA Stream"); + } +#endif + if (vehicle) { delete vehicle; @@ -86,11 +96,11 @@ bool DJISDKNode::initVehicle(ros::NodeHandle& nh_private, int argc, char** argv) { bool threadSupport = true; - bool enable_advanced_sensing = false; + bool advanced_sensing = false; #ifdef ADVANCED_SENSING - enable_advanced_sensing = true; - ROS_INFO("Advanced Sensing is Enabled on M210."); + advanced_sensing = enable_advanced_sensing; + ROS_INFO("Advanced Sensing is %s on M210.", advanced_sensing ? "Enabled" : "Disabled"); #endif //! @note currently does not work without thread support @@ -214,6 +224,11 @@ DJISDKNode::initPublisher(ros::NodeHandle& nh) { rc_publisher = nh.advertise("dji_osdk_ros/rc", 10); + relative_position_publisher = nh.advertise("dji_osdk_ros/relative_position", 10); + + // device control status, 0=RC, 1=MSDK, 2=OSDK + device_status_publisher = nh.advertise("dji_osdk_ros/control_status", 10); + attitude_publisher = nh.advertise("dji_osdk_ros/attitude", 10); @@ -224,7 +239,7 @@ DJISDKNode::initPublisher(ros::NodeHandle& nh) * - Fused attitude (duplicated from attitude topic) * - Raw linear acceleration (body frame: FLU, m/s^2) * Z value is +9.8 when placed on level ground statically - * - Raw angular velocity (body frame: FLU, rad/s^2) + * - Raw angular velocity (body frame: FLU, rad/s) */ imu_publisher = nh.advertise("dji_osdk_ros/imu", 10); @@ -312,16 +327,26 @@ DJISDKNode::initPublisher(ros::NodeHandle& nh) nh.advertise("dji_osdk_ros/stereo_240p_front_depth_images", 10); stereo_vga_front_left_publisher = - nh.advertise("dji_osdk_ros/stereo_vga_front_left_images", 10); + nh.advertise("dji_osdk_ros/stereo_vga/left/image_raw", 10); + + left_camera_info_pub_ = + nh.advertise("/dji_osdk_ros/stereo_vga/left/camera_info",10); stereo_vga_front_right_publisher = - nh.advertise("dji_osdk_ros/stereo_vga_front_right_images", 10); + nh.advertise("dji_osdk_ros/stereo_vga/right/image_raw", 10); + + right_camera_info_pub_ = + nh.advertise("/dji_osdk_ros/stereo_vga/right/camera_info",10); main_camera_stream_publisher = nh.advertise("dji_osdk_ros/main_camera_images", 10); fpv_camera_stream_publisher = nh.advertise("dji_osdk_ros/fpv_camera_images", 10); + + + + #endif @@ -451,6 +476,8 @@ DJISDKNode::initDataSubscribeFromFC(ros::NodeHandle& nh) if(vehicle->getFwVersion() > versionBase33) { topicList50Hz.push_back(Telemetry::TOPIC_POSITION_VO); + topicList50Hz.push_back(Telemetry::TOPIC_AVOID_DATA); + topicList50Hz.push_back(Telemetry::TOPIC_CONTROL_DEVICE); topicList50Hz.push_back(Telemetry::TOPIC_RC_WITH_FLAG_DATA); topicList50Hz.push_back(Telemetry::TOPIC_FLIGHT_ANOMALY); diff --git a/src/dji_osdk_ros_obsoleted/modules/dji_sdk_node_publisher.cpp b/src/dji_osdk_ros_obsoleted/modules/dji_sdk_node_publisher.cpp index 66ebc063..23ea6a70 100644 --- a/src/dji_osdk_ros_obsoleted/modules/dji_sdk_node_publisher.cpp +++ b/src/dji_osdk_ros_obsoleted/modules/dji_sdk_node_publisher.cpp @@ -14,6 +14,10 @@ #include #include +#include +#include + + #define _TICK2ROSTIME(tick) (ros::Duration((double)(tick) / 1000.0)) @@ -369,12 +373,33 @@ DJISDKNode::publish50HzData(Vehicle* vehicle, RecvContainer recvFrame, p->local_position_publisher.publish(local_pos); } + // Telemetry::RelativePosition relative_position; + // relative_position = vehicle->broadcast->getRelativePosition(); + Telemetry::TypeMap::type relative_position = + vehicle->subscribe->getValue(); + + dji_osdk_ros::RelPosition rel_pos_msg; + rel_pos_msg.header.frame_id = "/rel_pos"; + rel_pos_msg.header.stamp = msg_time; + rel_pos_msg.down = relative_position.down; + rel_pos_msg.front = relative_position.front; + rel_pos_msg.right = relative_position.right; + rel_pos_msg.back = relative_position.back; + rel_pos_msg.left = relative_position.left; + rel_pos_msg.up = relative_position.up; + rel_pos_msg.downHealth = relative_position.downHealth; + rel_pos_msg.frontHealth = relative_position.frontHealth; + rel_pos_msg.rightHealth = relative_position.rightHealth; + rel_pos_msg.backHealth = relative_position.backHealth; + rel_pos_msg.leftHealth = relative_position.leftHealth; + rel_pos_msg.upHealth = relative_position.upHealth; + p->relative_position_publisher.publish(rel_pos_msg); + Telemetry::TypeMap::type fused_height = vehicle->subscribe->getValue(); std_msgs::Float32 height; height.data = fused_height; p->height_publisher.publish(height); - Telemetry::TypeMap::type fs = vehicle->subscribe->getValue(); @@ -538,8 +563,30 @@ DJISDKNode::publish50HzData(Vehicle* vehicle, RecvContainer recvFrame, rc_joy.axes.push_back(static_cast(rc.gear*1.0)); p->rc_publisher.publish(rc_joy); } + + uint16_t data_enable_flag = vehicle->broadcast->getPassFlag(); + + + //update device control info + // if (data_enable_flag & DataBroadcast::DATA_ENABLE_FLAG::A3_HAS_DEVICE) + // { + Telemetry::TypeMap::type sdk_info = + vehicle->subscribe->getValue(); + std_msgs::UInt8 status_device; + // status_device.data = vehicle->broadcast->getSDKInfo().deviceStatus; + status_device.data = sdk_info.deviceStatus; + // TODO The underlying status changed, but we look for a value of 2. Fix this. + //https://github.com/dji-sdk/Onboard-SDK/issues/528 + //Jeff's farewell Mary Poppins + if (status_device.data == 4){ + status_device.data = 2; + ROS_INFO_THROTTLE(1,"**UNDER AM CONTROL**"); + } + p->device_status_publisher.publish(status_device); + // } } + void DJISDKNode::publish100HzData(Vehicle *vehicle, RecvContainer recvFrame, DJI::OSDK::UserData userData) @@ -749,6 +796,120 @@ void DJISDKNode::alignRosTimeWithFlightController(ros::Time now_time, uint32_t t } #ifdef ADVANCED_SENSING + +sensor_msgs::CameraInfo DJISDKNode::getCameraInfo(int camera_select, bool isLeftRequired) +{ + sensor_msgs::CameraInfo cam_info; + + cam_info.width = 320; + cam_info.height = 240; + + cam_info.distortion_model = "plumb_bob"; + + cam_info.D = std::vector(5,0.0); + + switch(camera_select) + { + //front camera + case 1: + { + cam_info.K = {407.1327819824219, 0, 321.8077392578125, 0.0, 407.1327819824219, 236.9904937744141, 0.0, 0.0, 1.0}; + cam_info.R = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}; + cam_info.P = {407.1327819824219, 0, 321.8077392578125, -116.0410842895508, 0.0, 407.1327819824219, 236.9904937744141, 0.0, 0.0, 0.0, 1.0, 0.0}; + break; + } + //left camera + case 2: + { + if(isLeftRequired) + { + cam_info.K = {405.863209, 0.000000, 321.034642,0.000000, 406.405344, 234.984922,0.000000, 0.000000, 1.000000}; + cam_info.D = {-0.004661, 0.003370, 0.000506, 0.000643, 0.000000}; + cam_info.R = {0.999999, 0.000188, 0.001306,-0.000189, 1.000000, 0.000714,-0.001306, -0.000715, 0.999999}; + cam_info.P = {406.427434, 0.000000, 320.900703, 0.000000,0.000000, 406.427434, 234.718628, 0.000000,0.000000, 0.000000, 1.000000, 0.000000}; + break; + } + + cam_info.K = {405.958187, 0.000000, 322.083430,0.000000, 406.341539, 234.588475, 0.000000, 0.000000, 1.000000}; + cam_info.D = {-0.005493, 0.003726, -0.000700, 0.000489, 0.000000}; + cam_info.R = {0.999997, 0.000116, 0.002235,-0.000115, 1.000000, -0.000715,-0.002235, 0.000714, 0.999997}; + cam_info.P = {406.427434, 0.000000, 320.900703, -48.321484,0.000000, 406.427434, 234.718628, 0.000000,0.000000, 0.000000, 1.000000, 0.000000}; + break; + } + //right camera + case 3: + { + cam_info.K = {488.5599365234375, 0.0, 319.80328369140625, 0.0, 488.5599365234375, 239.97528076171875, 0.0, 0.0, 1.0}; + cam_info.R = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}; + cam_info.P = {488.5599365234375, 0.0, 319.80328369140625, -99.43592071533203, 0.0, 488.5599365234375, 239.97528076171875, 0.0, 0.0, 0.0, 1.0, 0.0}; + break; + } + //rear camera + case 4: + { + cam_info.K = {486.2147216796875, 0.0, 318.2126159667969, 0.0, 486.2147216796875, 238.1256408691406, 0.0, 0.0, 1.0}; + cam_info.R = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}; + cam_info.P = {486.2147216796875, 0.0, 318.2126159667969, -99.34449005126953, 0.0, 486.2147216796875, 238.1256408691406, 0.0, 0.0, 0.0, 1.0, 0.0}; + break; + } + //down camera + case 5: + { + cam_info.K = {480.2206726074219, 0.0, 318.1107177734375, 0.0, 480.2206726074219, 242.7613830566406, 0.0, 0.0, 1.0}; + cam_info.R = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}; + cam_info.P = {480.2206726074219, 0.0, 318.1107177734375, 0.0, 0.0, 480.2206726074219, 242.7613830566406, 79.36713409423828, 0.0, 0.0, 1.0, 0.0}; + break; + } + + //up camera + case 6: + { + cam_info.width = 240; + cam_info.height = 320; + if(isLeftRequired) + { + cam_info.K = {201.990533, 0.000000, 118.106840,0.000000, 202.493255, 158.157213, 0.000000, 0.000000, 1.000000}; + cam_info.D = {0.001765, -0.002758, -0.000692, -0.001906, 0.000000}; + cam_info.R = {0.999994, -0.001909, -0.002898,0.001904, 0.999997, -0.001712,0.002901, 0.001707, 0.999994}; + cam_info.P = {204.589978, 0.000000, 118.121675, 0.000000,0.000000, 204.589978, 158.783770, 0.000000,0.000000, 0.000000, 1.000000, 0.000000}; + break; + } + + cam_info.K = {202.192738, 0.000000, 118.737948,0.000000, 202.403765, 159.274456, 0.000000, 0.000000, 1.000000}; + cam_info.D = {0.003813, 0.001792, 0.001038, -0.000216, 0.000000}; + cam_info.R = {0.999995, -0.001825, 0.002471,0.001820, 0.999997, 0.001712,-0.002474, -0.001707, 0.999995}; + cam_info.P = {204.589978, 0.000000, 118.121675, 20.188123,0.000000, 204.589978, 158.783770, 0.000000,0.000000, 0.000000, 1.000000, 0.000000}; + break; + } + default: + break; + } + + + return cam_info; +} + +void DJISDKNode::publishCameraInfo(const std_msgs::Header &header) +{ + if(latest_camera_ < 1) + { + ROS_WARN_THROTTLE(5.0,"selected camera id is not correct"); + return; + } + + sensor_msgs::CameraInfo left_camera_info = getCameraInfo(latest_camera_, true); + sensor_msgs::CameraInfo right_camera_info = getCameraInfo(latest_camera_, false); + + left_camera_info.distortion_model = right_camera_info.distortion_model = "plumb_bob"; + + left_camera_info.header = right_camera_info.header = header; + left_camera_info.header.frame_id = "left_camera"; + right_camera_info.header.frame_id = "right_camera"; + left_camera_info_pub_.publish(left_camera_info); + right_camera_info_pub_.publish(right_camera_info); + +} + void DJISDKNode::publish240pStereoImage(Vehicle* vehicle, RecvContainer recvFrame, DJI::OSDK::UserData userData) @@ -799,23 +960,86 @@ void DJISDKNode::publishVGAStereoImage(Vehicle* vehicle, DJISDKNode *node_ptr = (DJISDKNode *)userData; node_ptr->stereo_vga_subscription_success = true; - sensor_msgs::Image img; img.height = 480; img.width = 640; img.step = 640; img.encoding = "mono8"; img.data.resize(img.height*img.width); - + memcpy((char*)(&img.data[0]), recvFrame.recvData.stereoVGAImgData->img_vec[0], 480*640); + //processRosImage(img, node_ptr->latest_camera_); img.header.seq = recvFrame.recvData.stereoVGAImgData->frame_index; img.header.stamp = ros::Time::now(); // @todo img.header.frame_id = "vga_left"; - memcpy((char*)(&img.data[0]), recvFrame.recvData.stereoVGAImgData->img_vec[0], 480*640); - node_ptr->stereo_vga_front_left_publisher.publish(img); - + node_ptr->stereo_vga_front_left_publisher.publish(img); + + + + img.height = 480; + img.width = 640; + img.step = 640; + img.encoding = "mono8"; + img.data.resize(img.height*img.width); + memcpy((char*)(&img.data[0]), recvFrame.recvData.stereoVGAImgData->img_vec[1], 480*640); + //processRosImage(img, node_ptr->latest_camera_); img.header.frame_id = "vga_right"; - memcpy((char*)(&img.data[0]), recvFrame.recvData.stereoVGAImgData->img_vec[1], 480*640); - node_ptr->stereo_vga_front_right_publisher.publish(img); + node_ptr->stereo_vga_front_right_publisher.publish(img); + + + //img.header.frame_id = "vga_right"; + //unsigned char * img_data_ptr2 = (unsigned char*) &recvFrame.recvData.stereoVGAImgData->img_vec[1]; + //cv::Mat mat2(img.height, img.width, CV_8U, img_data_ptr, img.step); + //perform operations on the cv image + // cv::resize(mat2, mat2, cv::Size(240, 320),0,0,cv::INTER_LINEAR); + //memcpy((char*)(&img.data[0]), recvFrame.recvData.stereoVGAImgData->img_vec[1], 480*640); + //img_bridge = cv_bridge::CvImage(img.header, sensor_msgs::image_encodings::MONO8, mat2); + //img_bridge.toImageMsg(img); + //node_ptr->stereo_vga_front_right_publisher.publish(img); + + node_ptr->publishCameraInfo(img.header); +} + +void DJISDKNode::processRosImage(sensor_msgs::Image &img, int camera_select) +{ + + //get cv image + cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(img, img.encoding); + cv::Mat mat = cv_ptr->image; + //perform operations on the cv image + cv::resize(mat, mat, cv::Size(img.width/2, img.height/2),0,0,cv::INTER_LINEAR); + + + //convert to ros image + img.height = 240; + img.width = 320; + img.step = 320; + img.encoding = "mono8"; + img.data.resize(img.height*img.width); + + if(camera_select == 6) + { + double angle = 90.0; + // get rotation matrix for rotating the image around its center in pixel coordinates + cv::Point2f center((mat.cols-1)/2.0, (mat.rows-1)/2.0); + cv::Mat rot = cv::getRotationMatrix2D(center, angle, 1.0); + // determine bounding rectangle, center not relevant + cv::Rect2f bbox = cv::RotatedRect(cv::Point2f(), mat.size(), angle).boundingRect2f(); + // adjust transformation matrix + rot.at(0,2) += bbox.width/2.0 - mat.cols/2.0; + rot.at(1,2) += bbox.height/2.0 - mat.rows/2.0; + + cv::warpAffine(mat, mat, rot, bbox.size()); + + img.height = 320; + img.width = 240; + img.step = 240; + } + + + + + cv_bridge::CvImage img_bridge = cv_bridge::CvImage(img.header, sensor_msgs::image_encodings::MONO8, mat); + img_bridge.toImageMsg(img); } void DJISDKNode::publishFPVCameraImage(CameraRGBImage rgbImg, void* userData) diff --git a/src/dji_osdk_ros_obsoleted/modules/dji_sdk_node_services.cpp b/src/dji_osdk_ros_obsoleted/modules/dji_sdk_node_services.cpp index 1c9bbe8f..0d348914 100644 --- a/src/dji_osdk_ros_obsoleted/modules/dji_sdk_node_services.cpp +++ b/src/dji_osdk_ros_obsoleted/modules/dji_sdk_node_services.cpp @@ -394,6 +394,8 @@ DJISDKNode::stereoDepthSubscriptionCallback(dji_osdk_ros::StereoDepthSubscriptio return true; } + + bool DJISDKNode::stereoVGASubscriptionCallback(dji_osdk_ros::StereoVGASubscription::Request& request, dji_osdk_ros::StereoVGASubscription::Response& response) @@ -402,9 +404,10 @@ DJISDKNode::stereoVGASubscriptionCallback(dji_osdk_ros::StereoVGASubscription::R if (request.unsubscribe_vga == 1) { - vehicle->advancedSensing->unsubscribeVGAImages(); + vehicle->advancedSensing->unsubscribeVGAImages(request.front_vga); response.result = true; ROS_INFO("unsubscribe stereo vga images"); + latest_camera_ = -1; return true; } @@ -415,17 +418,37 @@ DJISDKNode::stereoVGASubscriptionCallback(dji_osdk_ros::StereoVGASubscription::R response.result = false; return true; } - - if (request.front_vga == 1) - { + + + /* In this version of the function, request.front_vga represent the side of the camera according to the followings + * 1 ---------------------> front + * 2 ---------------------> left + * 3 ---------------------> right + * 4 ---------------------> rear + * 5 ---------------------> down + * 6 ---------------------> top + */ + if (request.front_vga > 0 && request.front_vga <= 6) + { + //un-subscribe from the previous stream first + if(request.front_vga != latest_camera_ && latest_camera_ > 0) + { + vehicle->advancedSensing->unsubscribeVGAImages(latest_camera_); + ROS_INFO("unsubscribe stereo vga images"); + latest_camera_ = -1; + ros::Duration(1).sleep(); + } + this->stereo_vga_subscription_success = false; - vehicle->advancedSensing->subscribeFrontStereoVGA(request.vga_freq, &publishVGAStereoImage, this); + vehicle->advancedSensing->subscribeFrontStereoVGA(request.vga_freq, request.front_vga, &publishVGAStereoImage, this); ros::Duration(1).sleep(); } if (this->stereo_vga_subscription_success == true) { response.result = true; + //if the service call returns success, only then update the buffer + latest_camera_ = request.front_vga; } else { diff --git a/srv/CameraAperture.srv b/srv/CameraAperture.srv index 0b516b91..dc25dec6 100644 --- a/srv/CameraAperture.srv +++ b/srv/CameraAperture.srv @@ -1,7 +1,7 @@ #request uint8 payload_index #see enum class PayloadIndex in common_type.h uint8 exposure_mode #see enum class ExposureMode in common_type.h -uint8 aperture #see enum class Aperture in common_type.h +uint16 aperture #see enum class Aperture in common_type.h --- #response bool result \ No newline at end of file diff --git a/srv/CameraSetZoomPara.srv b/srv/CameraSetZoomPara.srv new file mode 100644 index 00000000..da888491 --- /dev/null +++ b/srv/CameraSetZoomPara.srv @@ -0,0 +1,6 @@ +#request +uint8 payload_index #see enum class PayloadIndex in common_type.h +float32 factor +--- +#response +bool result \ No newline at end of file diff --git a/srv/EmergencyBrake.srv b/srv/EmergencyBrake.srv new file mode 100644 index 00000000..1dadaa42 --- /dev/null +++ b/srv/EmergencyBrake.srv @@ -0,0 +1,4 @@ +#request +--- +#reponse +bool result \ No newline at end of file diff --git a/srv/FlightTaskControl.srv b/srv/FlightTaskControl.srv index aacffa28..f95e866b 100644 --- a/srv/FlightTaskControl.srv +++ b/srv/FlightTaskControl.srv @@ -1,18 +1,24 @@ #constant for tasks uint8 TASK_GOHOME = 1 -uint8 TASK_GO_LOCAL_POS = 2 +uint8 TASK_POSITION_AND_YAW_CONTROL = 2 uint8 TASK_GOHOME_AND_CONFIRM_LANDING = 3 uint8 TASK_TAKEOFF = 4 -uint8 TASK_LAND = 6 +uint8 TASK_VELOCITY_AND_YAWRATE_CONTROL = 5 +uint8 TASK_LAND = 6 +uint8 START_MOTOR = 7 +uint8 STOP_MOTOR = 8 +uint8 TASK_EXIT_GO_HOME = 12 +uint8 TASK_EXIT_LANDING = 14 +uint8 TASK_FORCE_LANDING_AVOID_GROUND = 30 #/*!< confirm landing */ +uint8 TASK_FORCE_LANDING = 31 #/*!< force landing */ #request uint8 task # see constants above for possible tasks -float64[] pos_offset #A vector contains that position_x_offset, position_y_offset, position_z_offset in order -float64[] yaw_params #A vector contains that yaw_desired, position_threshold(Meter), yaw_threshold(Degree) +JoystickParams joystickCommand #Provide Position and Velocity control +uint32 velocityControlTimeMs #Velocity control time +float32 posThresholdInM #(Meter) +float32 yawThresholdInDeg #(Degree) --- +#response bool result -# for debugging usage -uint8 cmd_set -uint8 cmd_id -uint32 ack_data diff --git a/srv/GetAvoidEnable.srv b/srv/GetAvoidEnable.srv new file mode 100644 index 00000000..9c3df29d --- /dev/null +++ b/srv/GetAvoidEnable.srv @@ -0,0 +1,6 @@ +#request +--- +#response +bool result +uint8 horizon_avoid_enable_status #0:disable 1:enable 0xF:invalid +uint8 upwards_avoid_enable_status \ No newline at end of file diff --git a/srv/GetGoHomeAltitude.srv b/srv/GetGoHomeAltitude.srv new file mode 100644 index 00000000..3570fd9a --- /dev/null +++ b/srv/GetGoHomeAltitude.srv @@ -0,0 +1,5 @@ +#request +--- +#response +bool result +uint16 altitude \ No newline at end of file diff --git a/srv/GetHMSData.srv b/srv/GetHMSData.srv new file mode 100644 index 00000000..676a6988 --- /dev/null +++ b/srv/GetHMSData.srv @@ -0,0 +1,9 @@ +#request +bool enable +--- +#reponse +bool result +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 +uint32 timeStamp \ No newline at end of file diff --git a/srv/GetSingleBatteryDynamicInfo.srv b/srv/GetSingleBatteryDynamicInfo.srv new file mode 100644 index 00000000..9e8ddad3 --- /dev/null +++ b/srv/GetSingleBatteryDynamicInfo.srv @@ -0,0 +1,8 @@ +#request +uint8 first_smart_battery = 1 +uint8 second_smart_battery = 2 + +uint8 batteryIndex +--- +#response +SmartBatteryDynamicInfo smartBatteryDynamicInfo \ No newline at end of file diff --git a/srv/GetWholeBatteryInfo.srv b/srv/GetWholeBatteryInfo.srv new file mode 100644 index 00000000..616f855e --- /dev/null +++ b/srv/GetWholeBatteryInfo.srv @@ -0,0 +1,4 @@ +#request +--- +#response +BatteryWholeInfo battery_whole_info \ No newline at end of file diff --git a/srv/JoystickAction.srv b/srv/JoystickAction.srv new file mode 100644 index 00000000..33f08f3b --- /dev/null +++ b/srv/JoystickAction.srv @@ -0,0 +1,5 @@ +#request +JoystickParams joystickCommand +--- +#response +bool result \ No newline at end of file diff --git a/srv/AvoidEnable.srv b/srv/KillSwitch.srv similarity index 58% rename from srv/AvoidEnable.srv rename to srv/KillSwitch.srv index 6a01a571..bcec9893 100644 --- a/srv/AvoidEnable.srv +++ b/srv/KillSwitch.srv @@ -1,3 +1,5 @@ +#request bool enable --- +#response bool result \ No newline at end of file diff --git a/srv/ObtainControlAuthority.srv b/srv/ObtainControlAuthority.srv new file mode 100644 index 00000000..c1f570ee --- /dev/null +++ b/srv/ObtainControlAuthority.srv @@ -0,0 +1,5 @@ +#request +bool enable_obtain +--- +#reponse +bool result \ No newline at end of file diff --git a/srv/SetAvoidEnable.srv b/srv/SetAvoidEnable.srv new file mode 100644 index 00000000..bcec9893 --- /dev/null +++ b/srv/SetAvoidEnable.srv @@ -0,0 +1,5 @@ +#request +bool enable +--- +#response +bool result \ No newline at end of file diff --git a/srv/SetNewHomePoint.srv b/srv/SetCurrentAircraftLocAsHomePoint.srv similarity index 100% rename from srv/SetNewHomePoint.srv rename to srv/SetCurrentAircraftLocAsHomePoint.srv diff --git a/srv/SetHomePoint.srv b/srv/SetHomePoint.srv new file mode 100644 index 00000000..493c4c92 --- /dev/null +++ b/srv/SetHomePoint.srv @@ -0,0 +1,6 @@ +#request +float64 latitude #/*!< unit: rad */ +float64 longitude #/*!< unit: rad */ +--- +#response +bool result \ No newline at end of file diff --git a/srv/SetJoystickMode.srv b/srv/SetJoystickMode.srv new file mode 100644 index 00000000..6684fc40 --- /dev/null +++ b/srv/SetJoystickMode.srv @@ -0,0 +1,79 @@ +#request +#contant for horizontal_mode +# Set the control-mode to control pitch & roll angle of the vehicle. +# Need to be referenced to either the ground or body frame +# by HorizontalCoordinate setting. +# Limit: 35 degree +uint8 HORIZONTAL_ANGLE = 0 +# Set the control-mode to control horizontal vehicle velocities. +# Need to be referenced to either the ground or body frame by +# HorizontalCoordinate setting. +# Limit: 30 m/s +uint8 HORIZONTAL_VELOCITY = 1 +# Set the control-mode to control position offsets of pitch & roll directions +# Need to be referenced to either the ground r body frame by HorizontalCoordinate setting. +# Limit: N/A +uint8 HORIZONTAL_POSITION = 2 +# Set the control-mode to control rate of change of the vehicle's attitude +# Need to be referenced to either the ground or body frame by HorizontalCoordinate setting. +# Limit: 150.0 deg/s +uint8 HORIZONTAL_ANGULAR_RATE = 3 + +#contant for vertical_mode +# Set the control-mode to control the vertical +# speed of UAV, upward is positive +# Limit: -5 to 5 m/s +uint8 VERTICAL_VELOCITY = 0 +# Set the control-mode to control the height of UAV +# Limit: 0 to 120 m +uint8 VERTICAL_POSITION = 1 +# Set the control-mode to directly control the thrust +# Range: 0% to 100% +uint8 VERTICAL_THRUST = 2 + +#contant for yaw_mode +# Set the control-mode to control yaw angle. +# Yaw angle is referenced to the ground frame. +# In this control mode, Ground frame is enforeced in Autopilot. +uint8 YAW_ANGLE = 0 +# Set the control-mode to control yaw angular velocity. +# Same reference frame as YAW_ANGLE. +# Limite: 150 deg/s +uint8 YAW_RATE = 1 +#contant for horizontal_coordinate +# Set the x-y of ground frame as the horizontal frame (NEU) */ +uint8 HORIZONTAL_GROUND = 0 +# Set the x-y of body frame as the horizontal frame (FRU) */ +uint8 HORIZONTAL_BODY = 1 +#contant for stable_mode +# Disable the stable mode +uint8 STABLE_DISABLE = 0 +# Enable the stable mode +uint8 STABLE_ENABLE = 1 + +# Only when the GPS signal is good (health_flag >=3)，horizontal +# position control (HORIZONTAL_POSITION) related control modes can be used. +# Only when GPS signal is good (health_flag >=3)，or when AdvancedSensing +# system is working properly with Autopilot，horizontal velocity control +# （HORIZONTAL_VELOCITY）related control modes can be used. +uint8 horizontal_mode + +# We suggest developers do not use VERTICAL_POSITION control mode indoor +# when your UAV flight height is larger than 3 meters. +# This is because in indoor environments, barometer can be inaccurate, and +# the vertical controller may fail to keep the height of the UAV. +uint8 vertical_mode +uint8 yaw_mode +uint8 horizontal_coordinate + +# Only works in Horizontal velocity control mode +# In velocity stable mode, drone will brake and hover at one position once +# the input command is zero. +# Drone will try to stay in position once in hover state. +# In velocity non-stable mode, drone will follow the velocity command and +# doesn’t hover when the command is zero. +# That’s to say drone will drift with the wind. +uint8 stable_mode +--- +#response +bool result \ No newline at end of file