Commit 024c86bf authored by Larkin Heintzman's avatar Larkin Heintzman

init commit

parents
/opt/ros/noetic/share/catkin/cmake/toplevel.cmake
\ No newline at end of file
./CMakeLists.txt
*.swn
*.swo
*.swp
*sdk.launch
build.sh
.idea/
*.git
!.gitignore
This diff is collapsed.
This diff is collapsed.
cmake_minimum_required(VERSION 2.8.3)
include(${CMAKE_CURRENT_SOURCE_DIR}/External_djiosdkcore.cmake)
# Download,compile and install djiosdk-core
include(ExternalProject)
# TODO VERSION may be changed
set(VERSION "3.8.1")
set(EXTERNAL_SOURCE_NAME Onboard-SDK)
set(EXTERNAL_SOURCE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/../${EXTERNAL_SOURCE_NAME}-${VERSION})
set(EXTERNAL_BUILD_PATH ${EXTERNAL_SOURCE_PATH}/../${EXTERNAL_SOURCE_NAME}-${VERSION}-build)
message(STATUS "${EXTERNAL_SOURCE_NAME} ${VERSION} path is ${EXTERNAL_SOURCE_PATH}")
# Set appropriate branch name
set(BRANCH_NAME ${VERSION})
ExternalProject_Add (
${EXTERNAL_SOURCE_NAME}
GIT_REPOSITORY https://github.com/dji-sdk/Onboard-SDK
GIT_TAG ${BRANCH_NAME}
SOURCE_DIR ${EXTERNAL_SOURCE_PATH}
BINARY_DIR ${EXTERNAL_BUILD_PATH}
CMAKE_COMMAND cd ${EXTERNAL_BUILD_PATH} && cmake ${EXTERNAL_SOURCE_PATH} -DADVANCED_SENSING=ON
BUILD_COMMAND cd ${EXTERNAL_BUILD_PATH} && make djiosdk-core
INSTALL_COMMAND cd ${EXTERNAL_BUILD_PATH} && sudo make install djiosdk-core
)
ExternalProject_Get_Property(${EXTERNAL_SOURCE_NAME} source_dir)
# Once done this will define
#
# FOUND_ADVANCED_SENSING - system has advanced-sensing
if (NOT FOUND_ADVANCED_SENSING)
find_path(ADVANCED_SENSING_LIB
NAMES
libadvanced-sensing.a
PATHS
/usr/lib
/usr/local/lib
/opt/local/lib
/sw/lib
PATH_SUFFIXES
lib
)
if(NOT ADVANCED_SENSING_LIB STREQUAL ADVANCED_SENSING_LIB-NOTFOUND)
message(STATUS "Found Advanced Sensing in the system")
set(FOUND_ADVANCED_SENSING TRUE)
endif()
endif (NOT FOUND_ADVANCED_SENSING)
\ No newline at end of file
#
# Find the native FFMPEG includes and library
#
# This module defines
# FFMPEG_INCLUDE_DIR, where to find avcodec.h, avformat.h ...
# FFMPEG_LIBRARIES, the libraries to link against to use FFMPEG.
# FFMPEG_FOUND, If false, do not try to use FFMPEG.
# also defined, but not for general use are
# FFMPEG_avformat_LIBRARY and FFMPEG_avcodec_LIBRARY, where to find the FFMPEG library.
# This is useful to do it this way so that we can always add more libraries
# if needed to FFMPEG_LIBRARIES if ffmpeg ever changes...
# if ffmpeg headers are all in one directory
FIND_PATH(FFMPEG_INCLUDE_DIR avformat.h
PATHS
$ENV{FFMPEG_DIR}/include
$ENV{OSGDIR}/include
$ENV{OSG_ROOT}/include
~/Library/Frameworks
/Library/Frameworks
/usr/local/include
/usr/include
/sw/include # Fink
/opt/local/include # DarwinPorts
/opt/csw/include # Blastwave
/opt/include
/usr/freeware/include
PATH_SUFFIXES ffmpeg
DOC "Location of FFMPEG Headers"
)
# if ffmpeg headers are seperated to each of libavformat, libavcodec etc..
IF( NOT FFMPEG_INCLUDE_DIR )
FIND_PATH(FFMPEG_INCLUDE_DIR libavformat/avformat.h
PATHS
$ENV{FFMPEG_DIR}/include
$ENV{OSGDIR}/include
$ENV{OSG_ROOT}/include
~/Library/Frameworks
/Library/Frameworks
/usr/local/include
/usr/include
/sw/include # Fink
/opt/local/include # DarwinPorts
/opt/csw/include # Blastwave
/opt/include
/usr/freeware/include
PATH_SUFFIXES ffmpeg
DOC "Location of FFMPEG Headers"
)
ENDIF( NOT FFMPEG_INCLUDE_DIR )
# we want the -I include line to use the parent directory of ffmpeg as
# ffmpeg uses relative includes such as <ffmpeg/avformat.h> or <libavcodec/avformat.h>
get_filename_component(FFMPEG_INCLUDE_DIR ${FFMPEG_INCLUDE_DIR} ABSOLUTE)
FIND_LIBRARY(FFMPEG_avformat_LIBRARY avformat
/usr/local/lib
/usr/lib
)
FIND_LIBRARY(FFMPEG_avcodec_LIBRARY avcodec
/usr/local/lib
/usr/lib
)
FIND_LIBRARY(FFMPEG_avutil_LIBRARY avutil
/usr/local/lib
/usr/lib
)
FIND_LIBRARY(FFMPEG_vorbis_LIBRARY vorbis
/usr/local/lib
/usr/lib
)
FIND_LIBRARY(FFMPEG_dc1394_LIBRARY dc1394_control
/usr/local/lib
/usr/lib
)
FIND_LIBRARY(FFMPEG_vorbisenc_LIBRARY vorbisenc
/usr/local/lib
/usr/lib
)
FIND_LIBRARY(FFMPEG_theora_LIBRARY theora
/usr/local/lib
/usr/lib
)
FIND_LIBRARY(FFMPEG_dts_LIBRARY dts
/usr/local/lib
/usr/lib
)
FIND_LIBRARY(FFMPEG_gsm_LIBRARY gsm
/usr/local/lib
/usr/lib
)
FIND_LIBRARY(FFMPEG_swscale_LIBRARY swscale
/usr/local/lib
/usr/lib
)
FIND_LIBRARY(FFMPEG_z_LIBRARY z
/usr/local/lib
/usr/lib
)
SET(FFMPEG_LIBRARIES)
IF(FFMPEG_INCLUDE_DIR)
IF(FFMPEG_avformat_LIBRARY)
IF(FFMPEG_avcodec_LIBRARY)
IF(FFMPEG_avutil_LIBRARY)
SET( FFMPEG_FOUND "YES" )
SET( FFMPEG_BASIC_LIBRARIES
${FFMPEG_avcodec_LIBRARY}
${FFMPEG_avformat_LIBRARY}
${FFMPEG_avutil_LIBRARY}
)
# swscale is always a part of newer ffmpeg distros
IF(FFMPEG_swscale_LIBRARY)
LIST(APPEND FFMPEG_BASIC_LIBRARIES ${FFMPEG_swscale_LIBRARY})
ENDIF(FFMPEG_swscale_LIBRARY)
SET(FFMPEG_LIBRARIES ${FFMPEG_BASIC_LIBRARIES})
IF(FFMPEG_vorbis_LIBRARY)
LIST(APPEND FFMPEG_LIBRARIES ${FFMPEG_vorbis_LIBRARY})
ENDIF(FFMPEG_vorbis_LIBRARY)
IF(FFMPEG_dc1394_LIBRARY)
LIST(APPEND FFMPEG_LIBRARIES ${FFMPEG_dc1394_LIBRARY})
ENDIF(FFMPEG_dc1394_LIBRARY)
IF(FFMPEG_vorbisenc_LIBRARY)
LIST(APPEND FFMPEG_LIBRARIES ${FFMPEG_vorbisenc_LIBRARY})
ENDIF(FFMPEG_vorbisenc_LIBRARY)
IF(FFMPEG_theora_LIBRARY)
LIST(APPEND FFMPEG_LIBRARIES ${FFMPEG_theora_LIBRARY})
ENDIF(FFMPEG_theora_LIBRARY)
IF(FFMPEG_dts_LIBRARY)
LIST(APPEND FFMPEG_LIBRARIES ${FFMPEG_dts_LIBRARY})
ENDIF(FFMPEG_dts_LIBRARY)
IF(FFMPEG_gsm_LIBRARY)
LIST(APPEND FFMPEG_LIBRARIES ${FFMPEG_gsm_LIBRARY})
ENDIF(FFMPEG_gsm_LIBRARY)
IF(FFMPEG_z_LIBRARY)
LIST(APPEND FFMPEG_LIBRARIES ${FFMPEG_z_LIBRARY})
ENDIF(FFMPEG_z_LIBRARY)
SET(FFMPEG_LIBRARIES ${FFMPEG_LIBRARIES} CACHE INTERNAL "All presently found FFMPEG libraries.")
ENDIF(FFMPEG_avutil_LIBRARY)
ENDIF(FFMPEG_avcodec_LIBRARY)
ENDIF(FFMPEG_avformat_LIBRARY)
ENDIF(FFMPEG_INCLUDE_DIR)
MARK_AS_ADVANCED(
FFMPEG_INCLUDE_DIR
FFMPEG_avformat_LIBRARY
FFMPEG_avcodec_LIBRARY
FFMPEG_avutil_LIBRARY
FFMPEG_vorbis_LIBRARY
FFMPEG_dc1394_LIBRARY
FFMPEG_vorbisenc_LIBRARY
FFMPEG_theora_LIBRARY
FFMPEG_dts_LIBRARY
FFMPEG_gsm_LIBRARY
FFMPEG_swscale_LIBRARY
FFMPEG_z_LIBRARY
)
# source of this file
# https://github.com/OpenKinect/libfreenect/blob/master/cmake_modules/Findlibusb-1.0.cmake
# Once done this will define
#
# LIBUSB_1_FOUND - system has libusb
# LIBUSB_1_INCLUDE_DIRS - the libusb include directory
# LIBUSB_1_LIBRARIES - Link these to use libusb
# LIBUSB_1_DEFINITIONS - Compiler switches required for using libusb
#
if (LIBUSB_1_LIBRARIES AND LIBUSB_1_INCLUDE_DIRS)
# in cache already
set(LIBUSB_FOUND TRUE)
else (LIBUSB_1_LIBRARIES AND LIBUSB_1_INCLUDE_DIRS)
find_path(LIBUSB_1_INCLUDE_DIR
NAMES
libusb.h
PATHS
/usr/include
/usr/local/include
/opt/local/include
/sw/include
PATH_SUFFIXES
libusb-1.0
)
find_library(LIBUSB_1_LIBRARY
NAMES
usb-1.0 usb
PATHS
/usr/lib
/usr/local/lib
/opt/local/lib
/sw/lib
)
set(LIBUSB_1_INCLUDE_DIRS
${LIBUSB_1_INCLUDE_DIR}
)
set(LIBUSB_1_LIBRARIES
${LIBUSB_1_LIBRARY}
)
if (LIBUSB_1_INCLUDE_DIRS AND LIBUSB_1_LIBRARIES)
set(LIBUSB_1_FOUND TRUE)
endif (LIBUSB_1_INCLUDE_DIRS AND LIBUSB_1_LIBRARIES)
if (LIBUSB_1_FOUND)
if (NOT libusb_1_FIND_QUIETLY)
message(STATUS "Found libusb-1.0:")
message(STATUS " - Includes: ${LIBUSB_1_INCLUDE_DIRS}")
message(STATUS " - Libraries: ${LIBUSB_1_LIBRARIES}")
endif (NOT libusb_1_FIND_QUIETLY)
else (LIBUSB_1_FOUND)
if (libusb_1_FIND_REQUIRED)
message(FATAL_ERROR "Could not find libusb")
endif (libusb_1_FIND_REQUIRED)
endif (LIBUSB_1_FOUND)
# show the LIBUSB_1_INCLUDE_DIRS and LIBUSB_1_LIBRARIES variables only in the advanced view
mark_as_advanced(LIBUSB_1_INCLUDE_DIRS LIBUSB_1_LIBRARIES)
endif (LIBUSB_1_LIBRARIES AND LIBUSB_1_INCLUDE_DIRS)
# Once done this will define
#
# FOUND_OPENCV_CONTRIB_IMG_PROC - system has opencv contrib img proc module
# This boolean is currently not used in the system.
# It's nice to have and not required.
if (NOT FOUND_OPENCV_CONTRIB_IMG_PROC)
find_path(OPENCV_CONTRIB_IMG_PROC
NAMES
disparity_filter.hpp
PATHS
/usr/include/opencv2/ximgproc
/usr/local/include/opencv2/ximgproc
/opt/local/include/opencv2/ximgproc
/sw/include/opencv2/ximgproc
PATH_SUFFIXES
ximgproc
)
if(NOT OPENCV_CONTRIB_IMG_PROC STREQUAL OPENCV_CONTRIB_IMG_PROC-NOTFOUND)
message(STATUS "Found ximgproc module in OpenCV, will use it to filter disparity map in depth perception sample")
set(FOUND_OPENCV_CONTRIB_IMG_PROC TRUE)
add_definitions(-DUSE_OPEN_CV_CONTRIB)
endif()
endif (NOT FOUND_OPENCV_CONTRIB_IMG_PROC)
\ No newline at end of file
# Once done this will define
#
# FOUND_OPENCV_VIZ - system has advanced-sensing
if (NOT FOUND_OPENCV_VIZ)
find_path(OPENCV_VIZ
NAMES
viz3d.hpp
PATHS
/usr/include/opencv2/viz
/usr/local/include/opencv2/viz
/opt/local/include/opencv2/viz
/sw/include/opencv2/viz
PATH_SUFFIXES
opencv2
)
if(NOT OPENCV_VIZ STREQUAL OPENCV_VIZ-NOTFOUND)
message(STATUS "Found viz3d in OpenCV, will use it to visualize point cloud")
set(FOUND_OPENCV_VIZ TRUE)
else()
message(STATUS "Did not find viz3d in OpenCV")
endif()
endif (NOT FOUND_OPENCV_VIZ)
\ No newline at end of file
# Once do# Once done this will be defined
#
# djiosdk-core_FOUND - system has djiosdk-core installed relative to the DJI OSDK Core
# djiosdk-core_INCLUDE_DIRS - the djiosdk-core include directory
# djiosdk-core_LIBRARY - djiosdk-core library
#
message(STATUS "Using Finddjiosdk-core.cmake find djiosdk-core")
set(INSTALL_LIB_DIR lib)
set(INSTALL_INCLUDE_DIR include)
set(DEF_INSTALL_CMAKE_DIR lib/cmake)
find_path(djiosdk-core_INCLUDE_DIRS
NAMES
djiosdk
PATHS
${INSTALL_INCLUDE_DIR}
)
find_library(djiosdk-core_LIBRARY
NAMES
djiosdk-core
PATHS
${INSTALL_LIB_DIR}
)
set(djiosdk-core_INCLUDE_DIRS
${djiosdk-core_INCLUDE_DIRS}
CACHE STRING "" FORCE)
set(djiosdk-core_LIBRARY
${djiosdk-core_LIBRARY}
CACHE STRING "" FORCE)
if ( djiosdk-core_INCLUDE_DIRS AND djiosdk-core_LIBRARY)
set(djiosdk-core_FOUND TRUE)
endif ()
if (djiosdk-core_FOUND)
if (NOT libdjiosdk-core_FIND_QUIETLY)
message(STATUS "Found djiosdk-core:")
message(STATUS " - Includes: ${djiosdk-core_INCLUDE_DIRS}")
message(STATUS " - Libraries: ${djiosdk-core_LIBRARY}")
endif ()
else ()
if (djiosdk-core_FIND_REQUIRED)
message(FATAL_ERROR "Could not find djiosdk-core")
endif ()
endif ()
/** @file mission_node.h
* @version 4.0
* @date June 2020
*
* @brief node of hotpoint 1.0/waypoint 1.0.
*
* @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.
*
*/
#ifndef MISSION_NODE_H
#define MISSION_NODE_H
// System includes
#include "unistd.h"
#include <iostream>
#include <string>
// DJI SDK includes
#include <dji_osdk_ros/MissionWpAction.h>
#include <dji_osdk_ros/MissionWpUpload.h>
#include <dji_osdk_ros/vehicle_wrapper.h>
#include <dji_osdk_ros/dji_vehicle_node.h>
// ROS includes
#include <ros/ros.h>
#include <sensor_msgs/NavSatFix.h>
//#include <dji_vehicle.hpp>
#define C_EARTH (double)6378137.0
#define C_PI (double)3.141592653589793
#define DEG2RAD(DEG) ((DEG) * ((C_PI) / (180.0)))
typedef struct ServiceAck
{
bool result;
int cmd_set;
int cmd_id;
unsigned int ack_data;
ServiceAck(bool res, int set, int id, unsigned int ack)
: result(res)
, cmd_set(set)
, cmd_id(id)
, ack_data(ack)
{
}
ServiceAck()
{
}
} ServiceAck;
bool runWaypointMission(int responseTimeout);
void setWaypointDefaults(DJI::OSDK::WayPointSettings* wp);
void setWaypointInitDefaults(dji_osdk_ros::MissionWaypointTask& waypointTask);
std::vector<DJI::OSDK::WayPointSettings> createWaypoints();
std::vector<DJI::OSDK::WayPointSettings> generateWaypointsPolygon(
DJI::OSDK::WayPointSettings* start_data, DJI::OSDK::float64_t increment,
int num_wp);
std::vector<DJI::OSDK::WayPointSettings> loadWaypoints();
void uploadWaypoints(std::vector<DJI::OSDK::WayPointSettings>& wp_list,
int responseTimeout,
dji_osdk_ros::MissionWaypointTask& waypointTask);
ServiceAck initWaypointMission(dji_osdk_ros::MissionWaypointTask& waypointTask);
ServiceAck missionAction(DJI::OSDK::DJI_MISSION_TYPE type,
DJI::OSDK::MISSION_ACTION action);
ServiceAck activate();
ServiceAck obtainCtrlAuthority();
bool takeoff();
bool land();
void gpsPosCallback(const sensor_msgs::NavSatFix::ConstPtr& msg);
//void waypointEventCallback(Vehicle *vehicle, RecvContainer recvFrame, UserData userData);
#endif // MISSION_NODE_H
This diff is collapsed.
This diff is collapsed.
/** @file mission_node.h
* @version 4.0
* @date June 2020
*
* @brief node of hotpoint 1.0/waypoint 1.0.
*
* @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.
*
*/
#ifndef MISSION_NODE_H
#define MISSION_NODE_H
// System includes
#include "unistd.h"
#include <iostream>
// DJI SDK includes
#include <dji_osdk_ros/MissionWpAction.h>
#include <dji_osdk_ros/MissionHpAction.h>
#include <dji_osdk_ros/MissionWpUpload.h>
#include <dji_osdk_ros/MissionHpUpload.h>
#include <dji_osdk_ros/MissionHpUpdateRadius.h>
#include <dji_osdk_ros/MissionHpUpdateYawRate.h>
#include <dji_osdk_ros/dji_vehicle_node.h>
// ROS includes
#include <ros/ros.h>
#include <sensor_msgs/NavSatFix.h>
#define C_EARTH (double)6378137.0
#define C_PI (double)3.141592653589793
#define DEG2RAD(DEG) ((DEG) * ((C_PI) / (180.0)))
typedef struct ServiceAck
{
bool result;
int cmd_set;
int cmd_id;
unsigned int ack_data;
ServiceAck(bool res, int set, int id, unsigned int ack)
: result(res)
, cmd_set(set)
, cmd_id(id)
, ack_data(ack)
{
}
ServiceAck()
{
}
} ServiceAck;
bool runWaypointMission(uint8_t numWaypoints, int responseTimeout);
void setWaypointDefaults(DJI::OSDK::WayPointSettings* wp);
void setWaypointInitDefaults(dji_osdk_ros::MissionWaypointTask& waypointTask);
std::vector<DJI::OSDK::WayPointSettings> createWaypoints(
int numWaypoints, DJI::OSDK::float64_t distanceIncrement,
DJI::OSDK::float32_t start_alt);
std::vector<DJI::OSDK::WayPointSettings> generateWaypointsPolygon(
DJI::OSDK::WayPointSettings* start_data, DJI::OSDK::float64_t increment,
int num_wp);
void uploadWaypoints(std::vector<DJI::OSDK::WayPointSettings>& wp_list,
int responseTimeout,
dji_osdk_ros::MissionWaypointTask& waypointTask);
bool runHotpointMission(int initialRadius, int responseTimeout);
void setHotpointInitDefault(dji_osdk_ros::MissionHotpointTask& hotpointTask);
ServiceAck initWaypointMission(dji_osdk_ros::MissionWaypointTask& waypointTask);
ServiceAck initHotpointMission(dji_osdk_ros::MissionHotpointTask& hotpointTask);
ServiceAck missionAction(DJI::OSDK::DJI_MISSION_TYPE type,
DJI::OSDK::MISSION_ACTION action);
ServiceAck activate();
ServiceAck obtainCtrlAuthority();
bool takeoff();
bool land();
ServiceAck hotpointUpdateRadius(float radius);
ServiceAck hotpointUpdateYawRate(float yawRate, int direction);
void gpsPosCallback(const sensor_msgs::NavSatFix::ConstPtr& msg);
#endif // MISSION_NODE_H
\ No newline at end of file
/** @file mobile_device_node.h
* @version 4.0
* @date May 2020
*
* @brief sample node of mobile device.
*
* @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.
*
*/
#ifndef SRC_MOBILE_DEVICE_NODE_H
#define SRC_MOBILE_DEVICE_NODE_H
#include <ros/ros.h>
#include <dji_osdk_ros/common_type.h>
#include <dji_osdk_ros/dji_vehicle_node.h>
using namespace dji_osdk_ros;
#pragma pack(1)
typedef struct AckReturnToMobile{
uint16_t cmdID;
bool ackResult;
} AckReturnToMobile;
#pragma pack()
/*! service */
ros::ServiceClient send_to_mobile_data_client;
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;
/*! function */
static void DisplayMainMenu(void);
bool sendToMobile(AckReturnToMobile returnAckMobile);
void fromMobileDataSubCallback(const dji_osdk_ros::MobileData::ConstPtr& fromMobileData);
/*! action */
bool takeoff();
bool land();
bool gohome();
bool gohomeAndConfirmLanding();
bool moveByPosOffset(MoveOffset&& move_offset);
bool localPositionCtrl();
bool resetGimbal();
bool Rotategimbal();
bool takePicture();
bool recordVideo();
bool obtainJoystickControlAuthority();
#endif //SRC_MOBILE_DEVICE_NODE_H
/** @file payload_device_node.h
* @version 4.0
* @date May 2020
*
* @brief sample node of payload device.
*
* @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.
*
*/
#ifndef SRC_PAYLOAD_DEVICE_NODE_H
#define SRC_PAYLOAD_DEVICE_NODE_H
//include
#include <ros/ros.h>
#include <dji_osdk_ros/dji_vehicle_node.h>
using namespace dji_osdk_ros;
//const
#define TEST_DATA_0 0x32
#define TEST_DATA_1 0x33
//service
ros::ServiceClient send_to_payload_data_client;
ros::Subscriber fromPayloadData;
//function
bool sendToPayload(dji_osdk_ros::SendPayloadData sendPayloadData);
void fromPayloadDataSubCallback(const dji_osdk_ros::PayloadData::ConstPtr& fromPayloadData);
#endif //SRC_PAYLOAD_DEVICE_NODE_H
#ifndef ONBOARDSDK_CAMERA_H
#define ONBOARDSDK_CAMERA_H
#include <opencv2/opencv.hpp>
#include <memory>
#include "config.hpp"
namespace M210_STEREO
{
class CameraParam
{
public:
enum POSITION
{
FRONT_LEFT = 0,
FRONT_RIGHT = 1,
DOWN_LEFT = 2,
DOWN_RIGHT = 3
};
typedef std::shared_ptr<CameraParam> Ptr;
CameraParam(uint8_t position);
~CameraParam();
public:
static CameraParam::Ptr createCameraParam(uint8_t position);
inline cv::Mat getIntrinsic() { return this->param_intrinsic_; }
inline cv::Mat getDistortion() { return this->param_distortion_; }
inline double getPrincipalX() { return this->param_intrinsic_.at<double>(0, 2); }
inline double getPrincipalY() { return this->param_intrinsic_.at<double>(1, 2); }
inline double getFocalX() { return this->param_intrinsic_.at<double>(0, 0); }
inline double getFocalY() { return this->param_intrinsic_.at<double>(1, 1); }
protected:
bool initIntrinsicParam();
bool initDistortionParam();
protected:
cv::Mat param_intrinsic_;
cv::Mat param_distortion_;
float depth_scale_;
uint8_t location_;
};
} // namespace M210_STEREO
#endif //ONBOARDSDK_CAMERA_H
#ifndef ONBOARDSDK_CONFIG_H
#define ONBOARDSDK_CONFIG_H
#include <opencv2/core/core.hpp>
#include <iostream>
namespace M210_STEREO
{
class Config
{
private:
Config();
public:
~Config();
static Config& instance();
static Config* instancePtr();
static void setParamFile(const std::string& file_name);
template <typename T>
static T get(const std::string& key)
{
T t;
Config::instancePtr()->file_[key] >> t;
return t;
}
private:
static Config* single_instance_;
cv::FileStorage file_;
};
} // namespace M210_STEREO
#endif //ONBOARDSDK_CONFIG_H
#ifndef ONBOARDSDK_FRAME_H
#define ONBOARDSDK_FRAME_H
#include <memory>
#include <opencv2/opencv.hpp>
namespace M210_STEREO
{
static const int VGA_HEIGHT = 480;
static const int VGA_WIDTH = 640;
struct Frame
{
typedef std::shared_ptr<Frame> Ptr;
Frame(uint64_t id, uint32_t time_stamp, cv::Mat img)
: id(id)
, time_stamp(time_stamp)
, raw_image(img) {};
static Frame::Ptr createFrame(uint64_t id, uint32_t time_stamp, cv::Mat img)
{
return std::make_shared<Frame>(id, time_stamp, img);
}
inline cv::Mat getImg() { return this->raw_image; }
uint64_t id;
uint32_t time_stamp;
cv::Mat raw_image;
};
} // namespace M210_STEREO
#endif //ONBOARDSDK_FRAME_H
#ifndef ONBOARDSDK_M300_STEREO_PARAM_TOOL_H
#define ONBOARDSDK_M300_STEREO_PARAM_TOOL_H
#include "dji_vehicle.hpp"
#include "dji_perception.hpp"
#include "dji_osdk_ros/stereo_utility/config.hpp"
using namespace M210_STEREO;
using namespace DJI;
using namespace DJI::OSDK;
class M300StereoParamTool
{
public:
M300StereoParamTool(Vehicle *vehicle);
~M300StereoParamTool();
public:
typedef struct DoubleCamParamType {
Perception::DirectionType direction;
double leftIntrinsics[9];
double rightIntrinsics[9];
double rotaionLeftInRight[9];
double translationLeftInRight[3];
} DoubleCamParamType;
/*!*/
Perception::CamParamType getM300stereoParams(Perception::DirectionType direction);
bool createStereoParamsYamlFile(std::string fileName,
Perception::CamParamType param);
void setParamFileForM300(std::string fileName);
private:
static void PerceptionCamParamCB(Perception::CamParamPacketType pack,
void *userData);
Vehicle *vehicle;
};
#endif //ONBOARDSDK_M300_STEREO_PARAM_TOOL_H
#ifndef ONBOARDSDK_POINT_CLOUD_VIEWER_HPP
#define ONBOARDSDK_POINT_CLOUD_VIEWER_HPP
#include <opencv2/opencv.hpp>
#include <opencv2/viz/viz3d.hpp>
#include <opencv2/viz/vizcore.hpp>
namespace M210_STEREO
{
class PointCloudViewer
{
public:
~PointCloudViewer();
static void showPointCloud(cv::viz::WCloud &cloud);
static inline void spinOnce() { PointCloudViewer::instancePtr()->viewer_.spinOnce(); };
static PointCloudViewer& instance();
static PointCloudViewer* instancePtr();
private:
PointCloudViewer(const std::string &name);
private:
static PointCloudViewer* single_instance_;
cv::viz::Viz3d viewer_;
cv::Point3d view_pos_;
cv::Point3d view_point_;
cv::Point3d view_y_dir_;
cv::viz::WCoordinateSystem world_coordinate_;
};
} // namespace M210_STEREO
#endif //ONBOARDSDK_POINT_CLOUD_VIEWER_HPP
#ifndef ONBOARDSDK_STEREO_FRAME_H
#define ONBOARDSDK_STEREO_FRAME_H
#include <memory>
#include <opencv2/opencv.hpp>
#include "frame.hpp"
#include "camera_param.hpp"
#include "dji_ack.hpp"
#include "dji_log.hpp"
#include "point_cloud_viewer.hpp"
#ifdef USE_GPU
#include <opencv2/cudastereo.hpp>
#include <opencv2/cudawarping.hpp>
#endif
#ifdef USE_OPEN_CV_CONTRIB
#include <opencv2/ximgproc/disparity_filter.hpp>
#endif
#include "sensor_msgs/Image.h"
#include "sensor_msgs/point_cloud2_iterator.h"
#include "visualization_msgs/MarkerArray.h"
#include "ros/ros.h"
#ifdef USE_DARKNET_ROS
#include "darknet_ros_msgs/BoundingBoxes.h"
#endif
namespace M210_STEREO
{
class StereoFrame
{
public:
typedef std::shared_ptr<StereoFrame> Ptr;
StereoFrame(CameraParam::Ptr left_cam, CameraParam::Ptr right_cam,
int num_disp = 64, int block_size = 13);
~StereoFrame();
public:
static StereoFrame::Ptr createStereoFrame(CameraParam::Ptr left_cam,
CameraParam::Ptr right_cam);
void readStereoImgs(const DJI::OSDK::ACK::StereoVGAImgData &imgs);
void rectifyImgs();
void computeDisparityMap();
void filterDisparityMap();
void unprojectPtCloud();
void unprojectROSPtCloud();
#ifdef USE_DARKNET_ROS
void calcObjectInfo(const darknet_ros_msgs::BoundingBoxesConstPtr &b_box, visualization_msgs::MarkerArray &marker_array);
#endif
inline cv::Mat getRectLeftImg() { return this->rectified_img_left_; }
inline cv::Mat getRectRightImg() { return this->rectified_img_right_; }
inline cv::Mat getDisparityMap() { return this->disparity_map_8u_; }
inline cv::viz::WCloud getPtCloud() { return this->pt_cloud_; }
inline sensor_msgs::PointCloud2 getROSPtCloud() { return this->ros_pt_cloud_; }
#ifdef USE_OPEN_CV_CONTRIB
inline cv::Mat getFilteredDispMap() { return this->filtered_disparity_map_8u_; }
#endif
protected:
bool initStereoParam();
protected:
//! Image frames
Frame::Ptr frame_left_ptr_;
Frame::Ptr frame_right_ptr_;
//! Camera related
CameraParam::Ptr camera_left_ptr_;
CameraParam::Ptr camera_right_ptr_;
//! Stereo camera parameters
cv::Mat param_rect_left_;
cv::Mat param_rect_right_;
cv::Mat param_proj_left_;
cv::Mat param_proj_right_;
cv::Mat rectified_mapping_[2][2];
//! Rectified images
cv::Mat rectified_img_left_;
cv::Mat rectified_img_right_;
//! Block matching related
int num_disp_;
int block_size_;
cv::Ptr<cv::StereoBM> block_matcher_;
cv::Mat disparity_map_8u_;
cv::Mat raw_disparity_map_;
//! Point Cloud related
double principal_x_;
double principal_y_;
double fx_;
double fy_;
double baseline_x_fx_;
// due to rectification, the image boarder are blank
// we cut them out
const int border_size_;
const int trunc_img_width_end_;
const int trunc_img_height_end_;
std::vector<uint8_t> color_buffer_;
cv::Mat color_mat_;
cv::Mat_<cv::Vec3f> mat_vec3_pt_;
cv::viz::WCloud pt_cloud_;
#ifdef USE_GPU
cv::cuda::GpuMat cuda_rectified_mapping_[2][2];
cv::cuda::GpuMat cuda_rect_src;
cv::cuda::GpuMat cuda_rectified_img_left_;
cv::cuda::GpuMat cuda_rectified_img_right_;
cv::cuda::GpuMat cuda_disparity_map_8u;
#endif
#ifdef USE_OPEN_CV_CONTRIB
cv::Ptr<cv::ximgproc::DisparityWLSFilter> wls_filter_;
cv::Ptr<cv::StereoMatcher> right_matcher_;
cv::Mat raw_right_disparity_map_;
cv::Mat filtered_disparity_map_;
cv::Mat filtered_disparity_map_8u_;
#endif
//! ROS related
sensor_msgs::PointCloud2 ros_pt_cloud_;
sensor_msgs::PointCloud2Modifier *cloud_modifier_;
sensor_msgs::PointCloud2Iterator<float> *x_it_;
sensor_msgs::PointCloud2Iterator<float> *y_it_;
sensor_msgs::PointCloud2Iterator<float> *z_it_;
sensor_msgs::PointCloud2Iterator<uint8_t> *r_it_;
sensor_msgs::PointCloud2Iterator<uint8_t> *g_it_;
sensor_msgs::PointCloud2Iterator<uint8_t> *b_it_;
visualization_msgs::Marker marker_template_;
};
} // namespace M210_STEREO
#endif //ONBOARDSDK_STEREO_FRAME_H
/** @file stereo_vision_depth_perception.h
* @version 4.0
* @date May 2020
*
* @brief sample node of stereo vision depth perception.
*
* @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.
*
*/
#ifndef DEMO_ADVANCED_SENSING_DEPTH_PERCEPTION_H
#define DEMO_ADVANCED_SENSING_DEPTH_PERCEPTION_H
// System includes
#include "chrono"
#include <signal.h>
// DJI SDK includes
#include <dji_osdk_ros/common_type.h>
#include <dji_osdk_ros/StereoVGASubscription.h>
#include <dji_osdk_ros/GetDroneType.h>
#include <dji_osdk_ros/GetM300StereoParams.h>
// ROS includes
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
// Utility includes
#include "dji_osdk_ros/stereo_utility/stereo_frame.hpp"
typedef std::chrono::time_point<std::chrono::high_resolution_clock> timer;
typedef std::chrono::duration<float> duration;
void displayStereoRectImgCallback(const sensor_msgs::ImageConstPtr &img_left,
const sensor_msgs::ImageConstPtr &img_right,
M210_STEREO::StereoFrame::Ptr stereo_frame_ptr);
void displayStereoDisparityCallback(const sensor_msgs::ImageConstPtr &img_left,
const sensor_msgs::ImageConstPtr &img_right,
M210_STEREO::StereoFrame::Ptr stereo_frame_ptr);
void displayStereoFilteredDisparityCallback(const sensor_msgs::ImageConstPtr &img_left,
const sensor_msgs::ImageConstPtr &img_right,
M210_STEREO::StereoFrame::Ptr stereo_frame_ptr);
void displayStereoPtCloudCallback(const sensor_msgs::ImageConstPtr &img_left,
const sensor_msgs::ImageConstPtr &img_right,
M210_STEREO::StereoFrame::Ptr stereo_frame_ptr);
void visualizeRectImgHelper(M210_STEREO::StereoFrame::Ptr stereo_frame_ptr);
void visualizeDisparityMapHelper(M210_STEREO::StereoFrame::Ptr stereo_frame_ptr);
bool imgSubscriptionHelper(dji_osdk_ros::StereoVGASubscription &service);
void shutDownHandler(int s);
#endif //DEMO_ADVANCED_SENSING_DEPTH_PERCEPTION_H
This diff is collapsed.
/** @file mission_node.cpp
* @version 4.0
* @date July 2020
*
* @brief node of waypoint V2.0.
*
* @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.
*
*/
#ifndef WAYPOINTV2_NODE_H
#define WAYPOINTV2_NODE_H
#include <dji_osdk_ros/dji_vehicle_node.h>
// ROS includes
#include <ros/ros.h>
dji_osdk_ros::GetDroneType drone_type;
dji_osdk_ros::InitWaypointV2Setting initWaypointV2Setting_;
dji_osdk_ros::UploadWaypointV2Mission uploadWaypointV2Mission_;
dji_osdk_ros::UploadWaypointV2Action uploadWaypointV2Action_;
dji_osdk_ros::DownloadWaypointV2Mission downloadWaypointV2Mission_;
dji_osdk_ros::StartWaypointV2Mission startWaypointV2Mission_;
dji_osdk_ros::StopWaypointV2Mission stopWaypointV2Mission_;
dji_osdk_ros::PauseWaypointV2Mission pauseWaypointV2Mission_;
dji_osdk_ros::ResumeWaypointV2Mission resumeWaypointV2Mission_;
dji_osdk_ros::SetGlobalCruisespeed setGlobalCruisespeed_;
dji_osdk_ros::GetGlobalCruisespeed getGlobalCruisespeed_;
dji_osdk_ros::GenerateWaypointV2Action generateWaypointV2Action_;
dji_osdk_ros::SubscribeWaypointV2Event subscribeWaypointV2Event_;
dji_osdk_ros::SubscribeWaypointV2State subscribeWaypointV2State_;
ros::ServiceClient waypointV2_init_setting_client;
ros::ServiceClient waypointV2_upload_mission_client;
ros::ServiceClient waypointV2_upload_action_client;
ros::ServiceClient waypointV2_download_mission_client;
ros::ServiceClient waypointV2_start_mission_client;
ros::ServiceClient waypointV2_stop_mission_client;
ros::ServiceClient waypointV2_pause_mission_client;
ros::ServiceClient waypointV2_resume_mission_client;
ros::ServiceClient waypointV2_set_global_cruisespeed_client;
ros::ServiceClient waypointV2_get_global_cruisespeed_client;
ros::ServiceClient waypointV2_generate_actions_client;
ros::ServiceClient waypointV2_mission_event_push_client;
ros::ServiceClient waypointV2_mission_state_push_client;
ros::Subscriber waypointV2EventSub;
ros::Subscriber waypointV2StateSub;
ros::ServiceClient get_drone_type_client;
sensor_msgs::NavSatFix gps_position_;
dji_osdk_ros::WaypointV2MissionEventPush waypoint_V2_mission_event_push_;
dji_osdk_ros::WaypointV2MissionStatePush waypoint_V2_mission_state_push_;
void gpsPositionSubCallback(const sensor_msgs::NavSatFix::ConstPtr& gpsPosition);
void waypointV2MissionStateSubCallback(const dji_osdk_ros::WaypointV2MissionStatePush::ConstPtr& waypointV2MissionStatePush);
void waypointV2MissionEventSubCallback(const dji_osdk_ros::WaypointV2MissionEventPush::ConstPtr& waypointV2MissionEventPush);
void setWaypointV2Defaults(dji_osdk_ros::WaypointV2& waypointV2);
std::vector<dji_osdk_ros::WaypointV2> generatePolygonWaypoints(const ros::NodeHandle &nh, float32_t radius, uint16_t polygonNum);
bool initWaypointV2Setting(ros::NodeHandle &nh);
bool uploadWaypointV2Mission(ros::NodeHandle &nh);
bool uploadWaypointV2Action(ros::NodeHandle &nh);
bool downloadWaypointV2Mission(ros::NodeHandle &nh, std::vector<dji_osdk_ros::WaypointV2> &mission);
bool startWaypointV2Mission(ros::NodeHandle &nh);
bool stopWaypointV2Mission(ros::NodeHandle &nh);
bool pauseWaypointV2Mission(ros::NodeHandle &nh);
bool resumeWaypointV2Mission(ros::NodeHandle &nh);
bool generateWaypointV2Actions(ros::NodeHandle &nh, uint16_t actionNum);
bool setGlobalCruiseSpeed(ros::NodeHandle &nh, float32_t cruiseSpeed);
float32_t getGlobalCruiseSpeed(ros::NodeHandle &nh);
bool runWaypointV2Mission(ros::NodeHandle &nh);
#endif // WAYPOINTV2_NODE_H
\ No newline at end of file
/*! @file dji_linux_environment.hpp
* @version 3.3
* @date Jun 05 2017
*
* @brief
* Helper functions to handle user configuration parsing
*
* @Copyright (c) 2017 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.
*
*/
#ifndef ONBOARDSDK_DJI_ENVIRONMENT_H
#define ONBOARDSDK_DJI_ENVIRONMENT_H
#include <fstream>
#include <iostream>
#include <ostream>
#include <stdexcept>
#include <unistd.h>
class DJI_Environment
{
public:
DJI_Environment(const std::string& config_file_path);
~DJI_Environment();
static std::string findFile(std::string file);
int getApp_id() const;
const std::string& getEnc_key() const;
const std::string& getDevice() const;
unsigned int getBaudrate() const;
unsigned int getACMDefaultBaudrate() const;
bool getConfigResult() const;
bool parse(std::string config_file_path);
const std::string& getDeviceAcm() const;
void setDeviceAcm(std::string dev_path);
void setSampleCase(std::string sample_case);
std::string getSampleCase();
private:
std::string config_file_path;
int app_id;
std::string enc_key;
std::string device;
unsigned int baudrate;
bool config_read_result;
std::string device_acm;
std::string sample_case;
const static unsigned int default_acm_baudrate = 921600;
};
#endif // ONBOARDSDK_DJI_ENVIRONMENT_H
/*! @file dji_linux_helpers.hpp
* @version 3.3
* @date Jun 05 2017
*
* @brief
* Helper functions to handle user configuration parsing, version query and
* activation.
*
* @Copyright (c) 2017 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.
*
*/
#ifndef ONBOARDSDK_HELPERS_H
#define ONBOARDSDK_HELPERS_H
#include <iostream>
#include <fstream>
#include <dji_linux_environment.hpp>
#include <dji_vehicle.hpp>
#include <dji_platform.hpp>
#include <dji_setup_helpers.hpp>
using namespace std;
class LinuxSetup : private Setup {
public:
LinuxSetup(int argc, char **argv, bool enableAdvancedSensing = false);
~LinuxSetup();
public:
void setupEnvironment(int argc, char** argv);
bool initVehicle();
public:
DJI_Environment *getEnvironment() {
return this->environment;
}
Vehicle::ActivateData *getActivateData() {
return &this->activateData;
}
Vehicle *getVehicle() {
return vehicle;
}
private:
uint32_t functionTimeout;
Vehicle::ActivateData activateData;
DJI_Environment* environment;
};
#endif // ONBOARDSDK_HELPERS_H
/** @file dji_sdk.h
* @version 3.7
* @date July, 2018
*
* @brief
* Definitions and Enums for client code to use dji_sdk ros modules
*
* @copyright 2018 DJI. All rights reserved.
*
*/
#ifndef PROJECT_DJI_SDK_H_H
#define PROJECT_DJI_SDK_H_H
#include <dji_control.hpp>
#include <dji_status.hpp>
#include <dji_version.hpp>
namespace DJISDK {
/*!
* This enum is used with service query_drone_version to
* check if the drone is M100 or not. We only support
* M100 with this particular FW version.
*/
enum DroneFirmwareVersion
{
M100_31 = DJI::OSDK::Version::M100_31,
};
typedef enum AircraftVersion
{
UNKNOWN,
M100,
M600,
A3,
N3,
M210
} AircraftVersion;
enum FlightControlFlag
{
HORIZONTAL_ANGLE = DJI::OSDK::Control::HORIZONTAL_ANGLE,
HORIZONTAL_VELOCITY = DJI::OSDK::Control::HORIZONTAL_VELOCITY,
HORIZONTAL_POSITION = DJI::OSDK::Control::HORIZONTAL_POSITION,
// Horizontal angular rate is supported only by A3/N3 based platform
// and is NOT supported by M100
HORIZONTAL_ANGULAR_RATE = DJI::OSDK::Control::HORIZONTAL_ANGULAR_RATE,
VERTICAL_VELOCITY = DJI::OSDK::Control::VERTICAL_VELOCITY,
VERTICAL_POSITION = DJI::OSDK::Control::VERTICAL_POSITION,
VERTICAL_THRUST = DJI::OSDK::Control::VERTICAL_THRUST,
YAW_ANGLE = DJI::OSDK::Control::YAW_ANGLE,
YAW_RATE = DJI::OSDK::Control::YAW_RATE,
HORIZONTAL_GROUND = DJI::OSDK::Control::HORIZONTAL_GROUND,
HORIZONTAL_BODY = DJI::OSDK::Control::HORIZONTAL_BODY,
STABLE_DISABLE = DJI::OSDK::Control::STABLE_DISABLE,
STABLE_ENABLE = DJI::OSDK::Control::STABLE_ENABLE
};
/*!
* Refer to demo_flight_control.cpp in dji_sdk_demo for how to
* use the display mode.
*/
enum DisplayMode
{
/*! This mode requires the user to manually
* control the aircraft to remain stable in air. */
MODE_MANUAL_CTRL=DJI::OSDK::VehicleStatus::DisplayMode::MODE_MANUAL_CTRL,
/*! In this mode, the aircraft can keep
* attitude stabilization and only use the
* barometer for positioning to control the altitude. <br>
* The aircraft can not autonomously locate and hover stably.*/
MODE_ATTITUDE=DJI::OSDK::VehicleStatus::DisplayMode::MODE_ATTITUDE,
/*! The aircraft is in normal GPS mode. <br>
* In normal GPS mode, the aircraft can
* autonomously locate and hover stably. <br>
* The sensitivity of the aircraft to the
* command response is moderate.
*/
MODE_P_GPS=DJI::OSDK::VehicleStatus::DisplayMode::MODE_P_GPS,
/*! In hotpoint mode */
MODE_HOTPOINT_MODE=DJI::OSDK::VehicleStatus::DisplayMode::MODE_HOTPOINT_MODE,
/*! In this mode, user can push the throttle
* stick to complete stable take-off. */
MODE_ASSISTED_TAKEOFF=DJI::OSDK::VehicleStatus::DisplayMode::MODE_ASSISTED_TAKEOFF,
/*! In this mode, the aircraft will autonomously
* start motor, ascend and finally hover. */
MODE_AUTO_TAKEOFF=DJI::OSDK::VehicleStatus::DisplayMode::MODE_AUTO_TAKEOFF,
/*! In this mode, the aircraft can land autonomously. */
MODE_AUTO_LANDING=DJI::OSDK::VehicleStatus::DisplayMode::MODE_AUTO_LANDING,
/*! In this mode, the aircraft can antonomously return the
* last recorded Home Point. <br>
* There are three types of this mode: Smart RTH(Return-to-Home),
* Low Batterry RTH, and Failsafe RTTH. */
MODE_NAVI_GO_HOME=DJI::OSDK::VehicleStatus::DisplayMode::MODE_NAVI_GO_HOME,
/*! In this mode, the aircraft is controled by SDK API. <br>
* User can directly define the control mode of horizon
* and vertical directions and send control datas to aircraft. */
MODE_NAVI_SDK_CTRL=DJI::OSDK::VehicleStatus::DisplayMode::MODE_NAVI_SDK_CTRL,
/*! drone is forced to land, might due to low battery */
MODE_FORCE_AUTO_LANDING=DJI::OSDK::VehicleStatus::DisplayMode::MODE_FORCE_AUTO_LANDING,
/*! drone will search for the last position where the rc is not lost */
MODE_SEARCH_MODE =DJI::OSDK::VehicleStatus::DisplayMode::MODE_SEARCH_MODE,
/*! Mode for motor starting. <br>
* Every time user unlock the motor, this will be the first mode. */
MODE_ENGINE_START = DJI::OSDK::VehicleStatus::DisplayMode::MODE_ENGINE_START
};
/*!
* Note that the flight status for M100 and A3/N3 are different.
*
* Refer to demo_flight_control.cpp in dji_sdk_demo for how to
* use the flight status.
*
*/
enum FlightStatus
{
STATUS_STOPPED = DJI::OSDK::VehicleStatus::FlightStatus::STOPED,
STATUS_ON_GROUND = DJI::OSDK::VehicleStatus::FlightStatus::ON_GROUND,
STATUS_IN_AIR = DJI::OSDK::VehicleStatus::FlightStatus::IN_AIR
};
enum M100FlightStatus
{
M100_STATUS_ON_GROUND = DJI::OSDK::VehicleStatus::M100FlightStatus::ON_GROUND_STANDBY,
M100_STATUS_TAKINGOFF = DJI::OSDK::VehicleStatus::M100FlightStatus::TAKEOFF,
M100_STATUS_IN_AIR = DJI::OSDK::VehicleStatus::M100FlightStatus::IN_AIR_STANDBY,
M100_STATUS_LANDING = DJI::OSDK::VehicleStatus::M100FlightStatus::LANDING,
M100_STATUS_FINISHED_LANDING = DJI::OSDK::VehicleStatus::M100FlightStatus::FINISHING_LANDING
};
}
#endif //PROJECT_DJI_SDK_H_H
/**
********************************************************************
* @file osdkhal_linux.h
* @version V2.0.0
* @date 2019/07/01
* @brief This is the header file for "osdkhal_linux.c", defining the structure and
* (exported) function prototypes.
*
* @copyright (c) 2018-2019 DJI. All rights reserved.
*
* All information contained herein is, and remains, the property of DJI.
* The intellectual and technical concepts contained herein are proprietary
* to DJI and may be covered by U.S. and foreign patents, patents in process,
* and protected by trade secret or copyright law. Dissemination of this
* information, including but not limited to data and other proprietary
* material(s) incorporated within the information, in any form, is strictly
* prohibited without the express written consent of DJI.
*
* If you receive this source code without DJI’s authorization, you may not
* further disseminate the information, and you must immediately remove the
* source code and notify DJI of its removal. DJI reserves the right to pursue
* legal actions against you for any loss(es) or damage(s) caused by your
* failure to do so.
*
*********************************************************************
*/
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef OSDK_HAL_LINUX_H
#define OSDK_HAL_LINUX_H
/* Includes ------------------------------------------------------------------*/
#include "stdint.h"
#include <stdio.h>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include <string.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include "osdk_typedef.h"
#include "osdk_platform.h"
#ifdef ADVANCED_SENSING
#include <libusb-1.0/libusb.h>
#endif
#ifdef __cplusplus
extern "C" {
#endif
/* Exported constants --------------------------------------------------------*/
/* Exported types ------------------------------------------------------------*/
/* Exported functions --------------------------------------------------------*/
E_OsdkStat OsdkLinux_UartSendData(const T_HalObj *obj, const uint8_t *pBuf, uint32_t bufLen);
E_OsdkStat OsdkLinux_UartReadData(const T_HalObj *obj, uint8_t *pBuf, uint32_t *bufLen);
E_OsdkStat OsdkLinux_UartInit(const char *port, const int baudrate, T_HalObj *obj);
E_OsdkStat OsdkLinux_UartClose(T_HalObj *obj);
#ifdef ADVANCED_SENSING
E_OsdkStat OsdkLinux_USBBulkInit(uint16_t pid, uint16_t vid, uint16_t num, uint16_t epIn,
uint16_t epOut, T_HalObj *obj);
E_OsdkStat OsdkLinux_USBBulkSendData(const T_HalObj *obj, const uint8_t *pBuf,
uint32_t bufLen);
E_OsdkStat OsdkLinux_USBBulkReadData(const T_HalObj *obj, uint8_t *pBuf,
uint32_t *bufLen);
E_OsdkStat OsdkLinux_USBBulkClose(T_HalObj *obj);
#endif
#ifdef __cplusplus
}
#endif
#endif // OSDK_HAL_LINUX_H
/************************ (C) COPYRIGHT DJI Innovations *******END OF FILE******/
/**
********************************************************************
* @file osdkosal_linux.h
* @version V2.0.0
* @date 2019/07/01
* @brief This is the header file for "osdk_osal.c", defining the structure
*and
* (exported) function prototypes.
*
* @copyright (c) 2018-2019 DJI. All rights reserved.
*
* All information contained herein is, and remains, the property of DJI.
* The intellectual and technical concepts contained herein are proprietary
* to DJI and may be covered by U.S. and foreign patents, patents in process,
* and protected by trade secret or copyright law. Dissemination of this
* information, including but not limited to data and other proprietary
* material(s) incorporated within the information, in any form, is strictly
* prohibited without the express written consent of DJI.
*
* If you receive this source code without DJI’s authorization, you may not
* further disseminate the information, and you must immediately remove the
* source code and notify DJI of its removal. DJI reserves the right to pursue
* legal actions against you for any loss(es) or damage(s) caused by your
* failure to do so.
*
*********************************************************************
*/
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef OSDK_OSAL_LINUX_H
#define OSDK_OSAL_LINUX_H
/* Includes ------------------------------------------------------------------*/
#include <pthread.h>
#include <semaphore.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/time.h>
#include <unistd.h>
#include "osdk_typedef.h"
#include "osdk_platform.h"
#ifdef __cplusplus
extern "C" {
#endif
/* Exported constants --------------------------------------------------------*/
/* Exported types ------------------------------------------------------------*/
/* Exported functions --------------------------------------------------------*/
// TODO:adapter for different system task interface void*
E_OsdkStat OsdkLinux_TaskCreate(T_OsdkTaskHandle *task, void *(*taskFunc)(void *),
uint32_t stackSize, void *arg);
E_OsdkStat OsdkLinux_TaskDestroy(T_OsdkTaskHandle task);
E_OsdkStat OsdkLinux_TaskSleepMs(uint32_t time_ms);
E_OsdkStat OsdkLinux_MutexCreate(T_OsdkMutexHandle *mutex);
E_OsdkStat OsdkLinux_MutexDestroy(T_OsdkMutexHandle mutex);
E_OsdkStat OsdkLinux_MutexLock(T_OsdkMutexHandle mutex);
E_OsdkStat OsdkLinux_MutexUnlock(T_OsdkMutexHandle mutex);
E_OsdkStat OsdkLinux_SemaphoreCreate(T_OsdkSemHandle *semaphore,
uint32_t initValue);
E_OsdkStat OsdkLinux_SemaphoreDestroy(T_OsdkSemHandle semaphore);
E_OsdkStat OsdkLinux_SemaphoreWait(T_OsdkSemHandle semaphore);
E_OsdkStat OsdkLinux_SemaphoreTimedWait(T_OsdkSemHandle semaphore,
uint32_t waitTime);
E_OsdkStat OsdkLinux_SemaphorePost(T_OsdkSemHandle semaphore);
E_OsdkStat OsdkLinux_GetTimeMs(uint32_t *ms);
#ifdef OS_DEBUG
E_OsdkStat OsdkLinux_GetTimeUs(uint64_t *us);
#endif
void *OsdkLinux_Malloc(uint32_t size);
void OsdkLinux_Free(void *ptr);
#ifdef __cplusplus
}
#endif
#endif // OSDK_OSAL_LINUX_H
/************************ (C) COPYRIGHT DJI Innovations *******END OF FILE******/
<launch>
<node pkg="dji_osdk_ros" type="dji_sdk_node" name="dji_sdk" output="screen">
<!-- node parameters -->
<param name="acm_name" type="string" value="/dev/ttyACM0"/>
<param name="serial_name" type="string" value="/dev/ttyUSB0"/>
<param name="baud_rate" type="int" value="921600"/>
<param name="app_id" type="int" value= "12345"/>
<param name="app_version" type="int" value="1"/>
<param name="align_time" type="bool" value="false"/>
<param name="enc_key" type="string" value="abc123"/>
<param name="use_broadcast" type="bool" value="false"/>
<param name="dxc" type="bool" value="false"/>
</node>
</launch>
<launch>
<rosparam file="$(find dji_osdk_ros)/launch/waypoints_musical_drones.json"/>
<node pkg="dji_osdk_ros" type="dji_vehicle_node" name="vehicle_node" output="screen">
<!-- node parameters -->
<param name="acm_name" type="string" value="/dev/ttyACM0"/>
<param name="serial_name" type="string" value="/dev/dji_usb"/>
<param name="baud_rate" type="int" value="921600"/>
<param name="app_id" type="int" value="1069806"/>
<param name="app_version" type="int" value="1"/>
<param name="align_time" type="bool" value="false"/>
<param name="enc_key" type="string" value="e180b993ca8365653437fbafe7211ba040386d77c3c87627882857a11bd8efbd"/>
<param name="use_broadcast" type="bool" value="false"/>
</node>
<param name="base_speed" type="double" value="0.5"/>
<param name="drone_id" type="int" value="0"/>
<node pkg="dji_osdk_ros" type="bus_driver" name="bus_driver" output="screen"/>
</launch>
<launch>
<rosparam file="$(find dji_osdk_ros)/launch/waypoints_musical_drones.json"/>
<node pkg="dji_osdk_ros" type="dji_vehicle_node" name="vehicle_node" output="screen">
<!-- node parameters -->
<param name="acm_name" type="string" value="/dev/ttyACM0"/>
<param name="serial_name" type="string" value="/dev/dji_usb"/>
<param name="baud_rate" type="int" value="921600"/>
<param name="app_id" type="int" value="1069806"/>
<param name="app_version" type="int" value="1"/>
<param name="align_time" type="bool" value="false"/>
<param name="enc_key" type="string" value="e180b993ca8365653437fbafe7211ba040386d77c3c87627882857a11bd8efbd"/>
<param name="use_broadcast" type="bool" value="false"/>
</no
de>
<node pkg="dji_osdk_ros" type="bus_driver" name="bus_driver" output="screen">
<param name="base_speed" type="double" value="0.5"/>
</node>
</launch>
<launch>
<rosparam file="$(find dji_osdk_ros)/launch/waypoints_musical_drones.json"/>
<node pkg="dji_osdk_ros" type="dji_vehicle_node" name="vehicle_node" output="screen">
<!-- node parameters -->
<param name="acm_name" type="string" value="/dev/ttyACM0"/>
<param name="serial_name" type="string" value="/dev/dji_usb"/>
<param name="baud_rate" type="int" value="921600"/>
<param name="app_id" type="int" value="1069806"/>
<param name="app_version" type="int" value="1"/>
<param name="align_time" type="bool" value="false"/>
<param name="enc_key" type="string" value="e180b993ca8365653437fbafe7211ba040386d77c3c87627882857a11bd8efbd"/>
<param name="use_broadcast" type="bool" value="false"/>
</node>
<param name="base_speed" type="double" value="0.5"/>
<param name="drone_id" type="int" value="0"/>
<param name="camera_type" type="int" value="0"/>
<!-- <node pkg="dji_osdk_ros" type="bus_driver" name="bus_driver" output="screen"/> -->
<!-- <node pkg="dji_osdk_ros" type="image_saver.py" name="image_saver" output="screen"/> -->
</launch>
{"robot_list": {"robot_0": {"waypoint_0": [0, -1, 37.22302383901093, -80.43273099999999, 2], "waypoint_1": [1, -1, 37.22312499999998, -80.4326039590342, 2], "waypoint_2": [2, -1, 37.223226160853365, -80.43273099999999, 2], "waypoint_3": [3, -1, 37.22317558044364, -80.43273099999999, 2], "waypoint_4": [4, -1, 37.22312499999998, -80.4327945204829, 2], "waypoint_5": [5, -1, 37.22307441952242, -80.43273099999999, 2], "waypoint_6": [6, -1, 37.22309465171751, -80.43273099999999, 2], "waypoint_7": [7, -1, 37.22312499999998, -80.43269288771026, 2], "waypoint_8": [8, -1, 37.22315534827025, -80.43273099999999, 2], "waypoint_9": [9, -1, 37.22312499999998, -80.4326039590342, 2]}, "robot_1": {"waypoint_0": [0, -1, 37.223226160853365, -80.43273099999999, 2], "waypoint_1": [1, -1, 37.22312499999998, -80.43285804096581, 2], "waypoint_2": [2, -1, 37.22302383901093, -80.43273099999999, 2], "waypoint_3": [3, -1, 37.22307441952242, -80.43273099999999, 2], "waypoint_4": [4, -1, 37.22312499999998, -80.4326674795171, 2], "waypoint_5": [5, -1, 37.22317558044364, -80.43273099999999, 2], "waypoint_6": [6, -1, 37.22315534827025, -80.43273099999999, 2], "waypoint_7": [7, -1, 37.22312499999998, -80.43276911228975, 2], "waypoint_8": [8, -1, 37.22309465171751, -80.43273099999999, 2], "waypoint_9": [9, -1, 37.22312499999998, -80.43285804096581, 2]}}}
\ No newline at end of file
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
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
# the time stamp of getting this data in the ROS system
time stamp
# the time stamp in FC
uint32 fc_timestamp_us
uint32 fc_utc_yymmdd
uint32 fc_utc_hhmmss
# Here is an example how to use FlightAnomaly msg:
#
# void flightAnomalyCallback(const dji_osdk_ros::FlightAnomaly::ConstPtr & msg)
# {
# uint32_t flightAnomalyData = msg->data;
# if(flightAnomalyData)
# {
# ROS_INFO("Flight Anomaly Reported by Flight Controller. Here are details:");
# if(flightAnomalyData && dji_osdk_ros::FlightAnomaly::COMPASS_INSTALLATION_ERROR)
# {
# ROS_INFO("COMPASS_INSTALLATION_ERROR");
# }
#
# if(flightAnomalyData && dji_osdk_ros::FlightAnomaly::IMU_INSTALLATION_ERROR)
# {
# ROS_INFO("IMU_INSTALLATION_ERROR");
# }
#
# // etc...
# }
# }
#
#
# constants for anomaly details
uint32 IMPACT_IN_AIR = 1 #
uint32 RANDOM_FLY = 2 #
uint32 VERTICAL_CONTROL_FAIL = 4 #
uint32 HORIZONTAL_CONTROL_FAIL = 8 #
uint32 YAW_CONTROL_FAIL = 16 #
uint32 AIRCRAFT_IS_FALLING = 32 #
uint32 STRONG_WIND_LEVEL1 = 64 #
uint32 STRONG_WIND_LEVEL2 = 128 #
uint32 COMPASS_INSTALLATION_ERROR = 256 #
uint32 IMU_INSTALLATION_ERROR = 512 #
uint32 ESC_TEMPERATURE_HIGH = 1024 #
uint32 ESC_DISCONNECTED = 2048 #
uint32 GPS_YAW_ERROR = 4096 #
#
uint32 data
# the time stamp of getting this data in the ROS system
time stamp
# the time stamp from GPS/RTK
string UTCTimeData
Header header
# ts is the time it takes to achieve the desired angle,
# so the shorter the time, the faster the gimbal rotates.
int32 ts # sec
# Mode is 1 byte size:
# Bit #: | Set to 0: | Set to 1:
# ----------- | ------------------------------------- | -------------------------------------
# bit 0 | Incremental control, | Absolute control,
# | the angle reference is the | the angle reference is
# | current Gimbal location | related to configuration in DJI Go App
# bit 1 | Gimbal will follow the command in Yaw | Gimbal will maintain position in Yaw
# bit 2 | Roll invalid bit, the same as bit[1] | Roll invalid bit, the same as bit[1]
# bit 3 |Pitch invalid bit, the same as bit[1] | Pitch invalid bit, the same as bit[1]
# bit [4:7] | bit [4:7]: reserved, set to be 0 |
uint8 mode
float32 yaw # rads
float32 pitch # rads
float32 roll # rads
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
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
float64 latitude # degree
float64 longitude # degree
float64 altitude # meter
float64 radius # meter
float32 angular_speed #deg/s
uint8 is_clockwise
uint8 start_point
uint8 yaw_mode
float64 latitude # degree
float64 longitude # degree
float32 altitude # relative altitude from takeoff point
float32 damping_distance # Bend length (effective coordinated turn mode only)
int16 target_yaw # Yaw (degree)
int16 target_gimbal_pitch # Gimbal pitch
uint8 turn_mode # 0: clockwise, 1: counter-clockwise
uint8 has_action # 0: no, 1: yes
uint16 action_time_limit
MissionWaypointAction waypoint_action
# action_repeat
# lower 4 bit: Total number of actions
# hight 4 bit: Total running times
uint8 action_repeat
uint8[16] command_list
uint16[16] command_parameter
# constant for action_on_finish
uint8 FINISH_NO_ACTION = 0 # no action
uint8 FINISH_RETURN_TO_HOME = 1 # return to home
uint8 FINISH_AUTO_LANDING = 2 # auto landing
uint8 FINISH_RETURN_TO_POINT = 3 # return to point 0
uint8 FINISH_NO_EXIT = 4 # infinite mode, no exit
# constant for yaw_mode
uint8 YAW_MODE_AUTO = 0 # auto mode (point to next waypoint)
uint8 YAW_MODE_LOCK = 1 # lock as an initial value
uint8 YAW_MODE_RC = 2 # controlled by RC
uint8 YAW_MODE_WAYPOINT = 3 # use waypoint's yaw(tgt_yaw)
# constant for trace_mode
uint8 TRACE_POINT = 0 # point to point, after reaching the target waypoint hover, complete waypt action (if any), then fly to the next waypt
uint8 TRACE_COORDINATED = 1 # 1: Coordinated turn mode, smooth transition between waypts, no waypts task
# constants for action_on_rc_lost
uint8 ACTION_FREE = 0 # exit waypoint and failsafe
uint8 ACTION_AUTO = 1 # continue the waypoint
# constants for gimbal_pitch_mode
uint8 GIMBAL_PITCH_FREE = 0 # free mode, no control on gimbal
uint8 GIMBAL_PITCH_AUTO = 1 # auto mode, Smooth transition between waypoints on gimbal
float32 velocity_range # Maximum speed joystick input(2~15m)
float32 idle_velocity # Cruising Speed (without joystick input, no more than vel_cmd_range)
uint8 action_on_finish # See constants above for possible actions
uint8 mission_exec_times # 1: once ; 2: twice
uint8 yaw_mode # see constants above for possible yaw modes
uint8 trace_mode # see constants above for possible trace modes
uint8 action_on_rc_lost # see constants above for possible actions
uint8 gimbal_pitch_mode # see constants above for pissible gimbal modes
MissionWaypoint[] mission_waypoint # a vector of waypoints
uint8[] data
\ No newline at end of file
uint8[] data
\ No newline at end of file
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
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
Header header
float32 x
float32 y
float32 z
uint8 xHealth
uint8 yHealth
uint8 zHealth
\ No newline at end of file
float64 latitude # in degree
float64 longitude # in degree
float32 altitude
int16 heading #heading is in [-180,180]
uint16 staytime # in second
# The struct represents a target point in the waypoint mission. For a waypoint
#constant for flightpathMode
uint8 DJIWaypointV2FlightPathModeGoToPointAlongACurve = 0 #In the mission, the aircraft will go to the waypoint along a curve and fly past the waypoint.
uint8 DJIWaypointV2FlightPathModeGoToPointAlongACurveAndStop = 1 #In the mission, the aircraft will go to the waypoint along a curve and stop at the waypoint.
uint8 DJIWaypointV2FlightPathModeGoToPointInAStraightLineAndStop = 2 #In the mission, the aircraft will go to the waypoint along a straight line and stop at the waypoint.
uint8 DJIWaypointV2FlightPathModeCoordinateTurn = 3 #In the mission, the aircraft will fly from the previous waypoint to the next waypoint along a smooth curve without stopping at this waypoint.
#the next in a curved motion, adhering to the ``DJIWaypointV2_dampingDistance``, which is
#set in ``DJIWaypointV2``.
uint8 DJIWaypointV2FlightPathModeGoToFirstPointAlongAStraightLine = 4 # In the mission, the aircraft will go to the first waypoint along a straight line.
# This is only valid for the first waypoint.
uint8 DJIWaypointV2FlightPathModeStraightOut = 5 # Straight exit the Last waypoint, Only valid for last waypoint.
uint8 DJIWaypointV2FlightPathModeUnknown = 255 # Unknown
#constant for headMode
uint8 DJIWaypointV2HeadingModeAuto = 0 # Aircraft's heading will always be in the direction of flight.
uint8 DJIWaypointV2HeadingFixed = 1 # Aircraft's heading will be set to the heading when reaching the first waypoint.
# Before reaching the first waypoint, the aircraft's heading can be controlled by
# the remote controller. When the aircraft reaches the first waypoint, its
# heading will be fixed.
uint8 DJIWaypointV2HeadingManual = 2 # The aircraft's heading in the mission can be controlled by the remote controller.
uint8 DJIWaypointV2HeadingWaypointCustom = 3 # In the mission, the aircraft's heading will change dynamically and adapt to the heading set at the next waypoint.
# See ``DJIWaypointV2_heading`` to preset the heading.
uint8 DJIWaypointV2HeadingTowardPointOfInterest = 4 # Aircraft's heading will always toward point of interest.
# using ``DJIWaypointV2_pointOfInterest`` setting point of interest coordiate and ``DJIWaypointV2_pointOfInterestAltitude``
# setting point of interset altitute.
uint8 DJIWaypointV2HeadingGimbalYawFollow = 5 # The aircraft's heading rotate simultaneously with its gimbal's yaw.
uint8 DJIWaypointV2HeadingUnknown = 255 # Unknown.
#constant for turnMode
uint8 DJIWaypointV2TurnModeClockwise = 0 # The aircraft's heading rotates clockwise.
uint8 DJIWaypointV2TurnModeCounterClockwise = 1 # The aircraft's heading rotates counterclockwise.
uint8 DJIWaypointV2TurnModeUnknown = 255 # Changes the heading of the aircraft by rotating the aircraft anti-clockwise.
# mission, a flight route consists of multiple `WaypointV2` objects.
float64 longitude # waypoint position relative to WayPointV2InitSettings's reference point.unit: m
float64 latitude
float32 relativeHeight # relative to takeoff height
uint8 waypointType # Waypoint flight path mode
uint8 headingMode # Represents current aircraft's heading mode on current waypoint.
WaypointV2Config config # Represents current waypoint's speed config.
uint16 dampingDistance
float32 heading # The heading to which the aircraft will rotate by the time it reaches the
# waypoint. The aircraft heading will gradually change between two waypoints with
# different headings if the waypoint mission's `headingMode` is set to
# 'DJIWaypointV2_DJIWaypointV2HeadingMode_WaypointCustom`. A heading has a range of
# [-180, 180] degrees, where 0 represents True North.
uint8 turnMode # Determines whether the aircraft will turn clockwise or anticlockwise when
# changing its heading.
# Property is used when ``DJIWaypointV2_headingMode`` is
# ``DJIWaypointV2_DJIWaypointV2HeadingMode_TowardPointOfInterest``.
# Aircraft will always be heading to point while executing mission. Default is
# "kCLLocationCoordinate2DInvalid".
float32 positionX # X distance to reference point, North is positive
float32 positionY # Y distance to reference point, East is positive
float32 positionZ # Z distance to reference point, UP is positive
# While the aircraft is travelling between waypoints, you can offset its speed by
# using the throttle joystick on the remote controller. "maxFlightSpeed" is this
# offset when the joystick is pushed to maximum deflection. For example, If
# maxFlightSpeed is 10 m/s, then pushing the throttle joystick all the way up will
# add 10 m/s to the aircraft speed, while pushing down will subtract 10 m/s from
# the aircraft speed. If the remote controller stick is not at maximum deflection,
# then the offset speed will be interpolated between "[0, maxFlightSpeed]"" with a
# resolution of 1000 steps. If the offset speed is negative, then the aircraft
# will fly backwards to previous waypoints. When it reaches the first waypoint, it
# will then hover in place until a positive speed is applied. "maxFlightSpeed" has
# a range of [2,15] m/s.
float32 maxFlightSpeed
# The base automatic speed of the aircraft as it moves between waypoints with
# range [-15, 15] m/s. The aircraft's actual speed is a combination of the base
# automatic speed, and the speed control given by the throttle joystick on the
# remote controller. If "autoFlightSpeed >0": Actual speed is "autoFlightSpeed" +
# Joystick Speed (with combined max of "maxFlightSpeed") If "autoFlightSpeed =0":
# Actual speed is controlled only by the remote controller joystick. If
# autoFlightSpeed <0" and the aircraft is at the first waypoint, the aircraft
# will hover in place until the speed is made positive by the remote controller
# joystick. In flight controller firmware 3.2.10.0 or above, different speeds
# between individual waypoints can also be set in waypoint objects which will
# overwrite "autoFlightSpeed".
float32 autoFlightSpeed
\ No newline at end of file
# This class represents an action for ``DJIWaypointV2Mission``. It
# determines how action is performed when a waypoint mission is executed.
# The action will be triggered when action associated executes.
# The parameters should be defined by ``DJIWaypointV2Action_DJIWaypointV2AssociateTriggerParam``.
uint8 DJIWaypointV2ActionTriggerTypeActionAssociated = 2
# The action will be triggered when the aircraft flies from one waypoint to the next.
# The parameters should be defined by ``DJIWaypointV2Action_DJIWaypointV2TrajectoryTriggerParam``.
uint8 DJIWaypointV2ActionTriggerTypeTrajectory = 3
# The action will be triggered when the aircraft flies between two waypoints
# The parameters should be defined by ``DJIWaypointV2Action_DJIWaypointV2IntervalTriggerParam``.
uint8 DJIWaypointV2ActionTriggerTypeInterval = 4
uint8 DJIWaypointV2ActionTriggerTypeSampleReachPoint = 5
# Unknown
uint8 DJIWaypointV2ActionTriggerTypeUnknown = 255
#contant for waypointV2ActuatorTriggerType
# The action will be executed by the camera.
# The parameters should be defined by ``DJIWaypointV2Action_DJIWaypointV2CameraActuatorParam``.
uint8 DJIWaypointV2ActionActuatorTypeCamera = 1
# The action will be executed by the gimbal.
# The parameters should be defined by ``DJIWaypointV2Action_DJIWaypointV2GimbalActuatorParam``.
uint8 DJIWaypointV2ActionActuatorTypeGimbal = 2
# The action will executes by control aircraft.
# The parameters should be setting by ``DJIWaypointV2Action_DJIWaypointV2CameraActuatorParam``.
uint8 DJIWaypointV2ActionActuatorTypeAircraftControl = 4
# Unknown actuator type.
uint8 DJIWaypointV2ActionActuatorTypeUnknown = 255
uint16 actionId # The ID of Action.
uint8 waypointV2ActionTriggerType
uint8 waypointV2ACtionActuatorType
# The trigger of action.You can only choose one to config
WaypointV2AssociateTrigger waypointV2AssociateTrigger
WaypointV2IntervalTrigger waypointV2IntervalTrigger
WaypointV2TrajectoryTrigger waypointV2TrajectoryTrigger
WaypointV2SampleReachPointTrigger waypointV2SampleReachPointTrigger
# The actuator of action.You can only choose one to config
WaypointV2CameraActuator waypointV2CameraActuator
WaypointV2GimbalActuator waypointV2GimbalActuator
WaypointV2AircraftControlActuator waypointV2AircraftControlActuator
\ No newline at end of file
# Parameters for aircraft control actuator. It is valid only when the
# ``DJIWaypointV2Action_DJIWaypointV2Actuator_type`` is
# ``DJIWaypointV2MissionV2_DJIWaypointV2ActionActuatorType_AircraftControl``.
#contant for DJIWaypointV2ActionActuatorAircraftControlOperationType
# Rotates the aircraft's yaw.
uint8 DJIWaypointV2ActionActuatorAircraftControlOperationTypeRotateYaw = 1
# Keeps the aircraft stop flying or start flying.
uint8 DJIWaypointV2ActionActuatorAircraftControlOperationTypeFlyingControl = 2
# Unknown
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.
uint16 DJIWaypointV2ActionActuatorAircraftControlOperationType
WaypointV2AircraftControlActuatorFlying waypointV2AircraftControlActuatorFlying
WaypointV2AircraftControlActuatorRotateHeading waypointV2AircraftControlActuatorRotateHeading
\ No newline at end of file
# This class defines if the aircraft starts or stops the flight.s.
uint8 isStartFlying # Determines the aircraft start flying or stop flying.
# ``TRUE`` for the aircraft to start flying.
\ No newline at end of file
# This class defines how the aircraft rotates on the yaw axis.
uint8 isRelative # Determines the aircraft rotate heading relative.
# if ``TRUE``, when the aircraft is rotating, relative to the current angle.
float32 yaw # Determines the direction how aircraft changes its heading.
\ No newline at end of file
#contant for actionAssociatedType
uint8 DJIWaypointV2TriggerAssociatedTimingTypeSimultaneously = 1 # The trigger starts simultaneously with the trigger that is associated.
uint8 DJIWaypointV2TriggerAssociatedTimingTypeAfterFinised = 2 # The trigger starts after the trigger associated has finished.
uint8 DJIWaypointV2TriggerAssociatedTimingTypeUnknown = 255 # Unkown timing type.
uint8 actionAssociatedType # The type of assciate trigger.
uint8 waitingTime # Waiting time in seconds after ActionTrigger starts.
uint16 actionIdAssociated # Associated action ID.
\ No newline at end of file
# This determines how the camera will be performed when a waypoint mission is executing.
#constant for DJIWaypointV2ActionActuatorCameraOperationType
# Starts to shoot a photo.
uint16 DJIWaypointV2ActionActuatorCameraOperationTypeTakePhoto = 1
# Starts to record a video.
uint16 DJIWaypointV2ActionActuatorCameraOperationTypeStartRecordVideo = 2
# Stops to record a video.
uint16 DJIWaypointV2ActionActuatorCameraOperationTypeStopRecordVideo = 3
# Starts focus.
uint16 DJIWaypointV2ActionActuatorCameraOperationTypeFocus = 4
# Starts focal lenth. Only support those support flocal lenth cameras.
uint16 DJIWaypointV2ActionActuatorCameraOperationTypeFocalLength = 5
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.
uint16 DJIWaypointV2ActionActuatorCameraOperationType
# you can only choose one to config.
WaypointV2CameraActuatorFocusParam focusParam # The parameters for camera focus operation. It is valid only when
# ``DJIWaypointV2Action_DJIWaypointV2CameraActuatorParam_operationType`` is
# ``DJIWaypointV2MissionV2_DJIWaypointV2ActionActuatorCameraOperationType_Focus``
WaypointV2CameraActuatorFocalLengthParam zoomParam # The parameters for camera focus length operation. It is valid only when
# ``DJIWaypointV2Action_DJIWaypointV2CameraActuatorParam_operationType`` is
# ``DJIWaypointV2MissionV2_DJIWaypointV2ActionActuatorCameraOperationType_FocalLength``
\ No newline at end of file
# This class defines a camera focal length operation for ``DJIWaypointV2Action_DJIWaypointV2CameraActuatorParam``.
# Focal length of zoom lens. Valid range is [``DJICamera_DJICameraOpticalZoomSpec_minFocalLength``,
# ``DJICamera_DJICameraOpticalZoomSpec_minFocalLength``] and must be a multiple of
# ``DJICamera_DJICameraOpticalZoomSpec_focalLengthStep``.
# Only support by those camera ``DJICamera_CameraSettings_isOpticalZoomSupported`` return ``TRUE``.
uint16 focalLength
uint8 retryTimes = 1
\ No newline at end of file
# This class defines a camera focus operation for ``DJIWaypointV2Action_DJIWaypointV2CameraActuatorParam``.
# The lens focus target point. When the focus mode is auto, the target point
# is the focal point. When the focus mode is manual, the target point is the zoom
# 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.
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
# Represents current waypoint's speed config.
# 0: set local waypoint's cruise speed,
# 1: unset global waypoint's cruise speed*/
uint8 useLocalCruiseVel
# 0: set local waypoint's max speed,
# 1: unset global waypoint's max speed*/
uint8 useLocalMaxVel
\ No newline at end of file
# gimbal actuator will be performed when a waypoint mission is executed.
#constant for DJIWaypointV2ActionActuatorGimbalOperationType
# Rotates the gimbal. Only valid when the trigger type is
# ``DJIWaypointV2MissionV2_DJIWaypointV2TriggerAssociatedTimingType_ReachPoint``.
uint8 DJIWaypointV2ActionActuatorGimbalOperationTypeRotateGimbal = 1
# Unknown
uint8 DJIWaypointV2ActionActuatorGimbalOperationTypeUnknown = 255
uint8 DJIWaypointV2ActionActuatorGimbalOperationType
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
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*/
uint8 pitchCmdIgnore# 0: pitch command normal, 1: pitch command ignore*/
uint8 yawCmdIgnore # 0: yaw command normal, 1: yaw command ignore*/
uint8 absYawModeRef # 0: absoluate rotate yaw relative to aircraft,
# 1: absoluate rotate yaw relative to North*/
uint8 duationTime # 0: rotate time,unit:0.1s, range[1,255]*/
\ No newline at end of file
# @brief Waypoint V2 Mission Initialization settings
# @details This is one of the few structs in the OSDK codebase that
# is used in both a sending and a receiving API.
#constant for finishedAction
uint8 DJIWaypointV2MissionFinishedNoAction = 0 # No further action will be taken.
# The aircraft can be controlled by the remote controller.
uint8 DJIWaypointV2MissionFinishedGoHome = 1 # Goes home when the mission is finished. The aircraft will
# land directly if it is within 20 meters away from the home point.
uint8 DJIWaypointV2MissionFinishedAutoLanding = 2 # The aircraft will land automatically at the last waypoint.
uint8 DJIWaypointV2MissionFinishedGoToFirstWaypoint = 3 # The aircraft will go back to the first waypoint and hover.
uint8 DJIWaypointV2MissionFinishedContinueUntilStop = 4 # When the aircraft reaches its final waypoint, it will hover without ending the
# mission. The joystick can still be used to pull the aircraft back along its
# previous waypoints. The only way this mission can end is if stopMission is
# called
#constant for gotoFirstWaypintMode
uint8 DJIWaypointV2MissionGotoFirstWaypointModeSafely = 0 # Go to the waypoint safely. The aircraft will rise to the same altitude of the
# waypoint if the current altitude is lower then the waypoint altitude. It then
# goes to the waypoint coordinate from the current altitude, and proceeds to the
# altitude of the waypoint.
uint8 DJIWaypointV2MissionGotoFirstWaypointModePointToPoint = 1 # Go to the waypoint from the current aircraft point to the waypoint directly.
uint32 missionID # The Mission ID. Use to distinguish different mission
uint16 missTotalLen # The Mission waypoint total length, could not exceed 65535
uint8 repeatTimes # Mission execution can be repeated more than once. A value of 0 means the mission
# only executes once, and does not repeat. A value of 1 means the mission will
# execute a total of two times.
uint8 finishedAction # Action the aircraft will take when the waypoint mission is complete.
# While the aircraft is travelling between waypoints, you can offset its speed by
# using the throttle joystick on the remote controller. `maxFlightSpeed` is this
# offset when the joystick is pushed to maximum deflection. For example, If
# maxFlightSpeed is 10 m/s, then pushing the throttle joystick all the way up
# will add 10 m/s to the aircraft speed, while pushing down will subtract 10 m/s
# from the aircraft speed. If the remote controller stick is not at maximum
# deflection, then the offset speed will be interpolated between [0,
# `maxFlightSpeed`] with a resolution of 1000 steps. If the offset speed is
# negative, then the aircraft will fly backwards to previous waypoints. When it
# reaches the first waypoint, it will then hover in place until a positive speed
# is applied. `maxFlightSpeed` has a range of [2,15] m/s.
# unit: m/s
float32 maxFlightSpeed
# The base automatic speed of the aircraft as it moves between waypoints with
# range [-15, 15] m/s. The aircraft's actual speed is a combination of the base
# automatic speed, and the speed control given by the throttle joystick on the
# remote controller. If `autoFlightSpeed` >0: Actual speed is `autoFlightSpeed` +
# Joystick Speed (with combined max of `maxFlightSpeed`) If `autoFlightSpeed` =0:
# Actual speed is controlled only by the remote controller joystick. If
# `autoFlightSpeed` <0 and the aircraft is at the first waypoint, the aircraft
# will hover in place until the speed is made positive by the remote controller
# joystick.
# unit: m/s
float32 autoFlightSpeed
uint8 exitMissionOnRCSignalLost # Determines whether the mission should stop when connection between the aircraft
# and remote controller is lost. Default is `NO`.
uint8 gotoFirstWaypointMode # Defines how the aircraft will go to the first waypoint from its current
# position. Default is ``DJIWaypointV2MissionV2_DJIWaypointV2MissionGotoWaypointMode_Safely``.
WaypointV2[] mission
\ No newline at end of file
#constant for actionIntervalType
uint8 DJIWaypointV2ActionIntervalTypeTime = 1 # The action will be repeated after a particular period of time.
uint8 DJIWaypointV2ActionIntervalTypeDistance = 2 # The action will be repeated after a particular distance.
uint8 DJIWaypointV2ActionIntervalTypeUnknown = 255 # Unknown action trigger type.
uint16 startIndex # It determines the index of the waypoint at which the trigger starts.
# If the ``DJIWaypointV2Action_DJIWaypointV2IntervalTriggerParam_actionIntervalType``
# is ``DJIWaypointV2MissionV2_DJIWaypointV2TriggerAssociatedTimingType_Time``
# The time interval in seconds when two action are executed as the aircraft moves
# from the current waypoint to the next waypoint.
# If the ``DJIWaypointV2Action_DJIWaypointV2IntervalTriggerParam_actionIntervalType`` is
# ``DJIWaypointV2MissionV2_DJIWaypointV2TriggerAssociatedTimingType_Distance``
# The distance interval in meters when two action are executed as the aircraft moves
# from the current waypoint to the next waypoint.
uint16 interval
uint8 actionIntervalType # The type of interval trigger.
# See ``DJIWaypointV2MissionV2_DJIWaypointV2ActionIntervalType``.
\ No newline at end of file
uint8 event
uint32 FCTimestamp
#ID:0x01
uint8 interruptReason #0x00:rc triggered interrupt
#ID:0x02
uint8 recoverProcess #0x00:finished pause recover
#ID:0x03
uint8 finishReason #0x00:finished normally; 0x10:External user trigger ended successfully
#ID:0x10
uint16 waypointIndex
#ID:0x11
uint8 currentMissionExecNum
uint8 finishedAllExecNum #0:not finished; 1:finished all exec num
\ No newline at end of file
int8 commonDataVersion
uint16 commonDataLen
uint16 curWaypointIndex
uint8 state #0x0:ground station not start. 0x1:mission prepared. 0x2:enter mission.
#0x3:execute flying route mission.
#0x4:pause state. 0x5:enter mission after ending pause.
#0x6:exit mission.
uint16 velocity #uint:0.01m/s
\ No newline at end of file
# It describes an action will be triggered when the aircraft reach the certain waypoint.
uint16 startIndex # It determines the index of the waypoint at which the action will be triggered.
uint16 endIndex
uint16 intervalWPNum
uint16 waypointCountToTerminate # It determines the waypoint count till the action triggered stops.
\ No newline at end of file
uint16 waypointIndex # It determines the index of the waypoint at which the action will be triggered.
uint16 terminateNum
# This class represents a trajectory trigger action when should be trigger.
uint16 startIndex # It determines the index of the waypoint at which the trigger starts.
uint16 endIndex # It determines the waypoint when the trigger stops.
This diff is collapsed.
#! /usr/bin/python3
# Copyright (c) 2015, Rethink Robotics, Inc.
# Using this CvBridge Tutorial for converting
# ROS images to OpenCV2 images
# http://wiki.ros.org/cv_bridge/Tutorials/ConvertingBetweenROSImagesAndOpenCVImagesPython
# Using this OpenCV2 tutorial for saving Images:
# http://opencv-python-tutroals.readthedocs.org/en/latest/py_tutorials/py_gui/py_image_display/py_image_display.html
# rospy for the subscriber
import rospy
from dji_osdk_ros.srv import StereoVGASubscription
from dji_osdk_ros.srv import SetupCameraStream
def main():
rospy.init_node('image_activation')
# request drone to open stereo images
rospy.wait_for_service('setup_camera_stream')
try:
camera_service_proxy = rospy.ServiceProxy('setup_camera_stream', SetupCameraStream)
response = camera_service_proxy(rospy.get_param('camera_type'), 1)
print(response)
except:
print("image service call failed")
if __name__ == '__main__':
main()
add_subdirectory(dji_osdk_ros)
add_subdirectory(dji_osdk_ros_obsoleted)
add_subdirectory(modules)
add_subdirectory(samples)
FILE(GLOB EXAMPLES *.cc *.c *.cpp)
if(OPEN_CV_3_3_0_INSTALLED AND FOUND_OPENCV_VIZ)
add_definitions(-DOPEN_CV_INSTALLED)
endif()
FOREACH(EXAMPLE ${EXAMPLES})
GET_FILENAME_COMPONENT(MY_TARGET "${EXAMPLE}" NAME_WE)
ADD_EXECUTABLE(${MY_TARGET} ${EXAMPLE})
## Add cmake target dependencies of the executable/library
## as an example, message headers may need to be generated before nodes
add_dependencies(${MY_TARGET}
dji_osdk_ros_generate_messages_cpp)
TARGET_LINK_LIBRARIES(${MY_TARGET}
${PROJECT_NAME}
)
INSTALL(TARGETS ${MY_TARGET}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
ENDFOREACH()
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment