NEW: release DJI Payload-SDK version 3.0

Signed-off-by: DJI-Martin <DJI-Martin@dji.com>
This commit is contained in:
DJI-Martin
2021-11-12 16:27:03 +08:00
parent 2cce11a80b
commit e478e9a7a5
541 changed files with 2192778 additions and 2 deletions

File diff suppressed because it is too large Load Diff

View File

@ -0,0 +1,95 @@
/**
********************************************************************
* @file dji_camera_image_handler.cpp
* @brief
*
* @copyright (c) 2021 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 DJIs 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.
*
*********************************************************************
*/
/* Includes ------------------------------------------------------------------*/
#include "dji_camera_image_handler.hpp"
/* Private constants ---------------------------------------------------------*/
/* Private types -------------------------------------------------------------*/
/* Private values -------------------------------------------------------------*/
/* Private functions declaration ---------------------------------------------*/
/* Exported functions definition ---------------------------------------------*/
DJICameraImageHandler::DJICameraImageHandler() : m_newImageFlag(false)
{
pthread_mutex_init(&m_mutex, NULL);
pthread_cond_init(&m_condv, NULL);
}
DJICameraImageHandler::~DJICameraImageHandler()
{
pthread_mutex_destroy(&m_mutex);
pthread_cond_destroy(&m_condv);
}
bool DJICameraImageHandler::getNewImageWithLock(CameraRGBImage &copyOfImage, int timeoutMilliSec)
{
int result;
/*! @note
* Here result == 0 means successful.
* Because this is the behavior of pthread_cond_timedwait.
*/
pthread_mutex_lock(&m_mutex);
if (m_newImageFlag) {
/* At this point, a copy of m_img is made, so it is safe to
* do any modifications to copyOfImage in user code.
*/
copyOfImage = m_img;
m_newImageFlag = false;
result = 0;
} else {
struct timespec absTimeout;
clock_gettime(CLOCK_REALTIME, &absTimeout);
absTimeout.tv_nsec += timeoutMilliSec * 1e6;
result = pthread_cond_timedwait(&m_condv, &m_mutex, &absTimeout);
if (result == 0) {
copyOfImage = m_img;
m_newImageFlag = false;
}
}
pthread_mutex_unlock(&m_mutex);
return (result == 0) ? true : false;
}
void DJICameraImageHandler::writeNewImageWithLock(uint8_t *buf, int bufSize, int width, int height)
{
pthread_mutex_lock(&m_mutex);
m_img.rawData.assign(buf, buf + bufSize);
m_img.height = height;
m_img.width = width;
m_newImageFlag = true;
pthread_cond_signal(&m_condv);
pthread_mutex_unlock(&m_mutex);
}
/* Private functions definition-----------------------------------------------*/
/****************** (C) COPYRIGHT DJI Innovations *****END OF FILE****/

View File

@ -0,0 +1,74 @@
/**
********************************************************************
* @file dji_camera_image_handler.hpp
* @brief This is the header file for "dji_camera_image_handler.cpp", defining the structure and
* (exported) function prototypes.
*
* @copyright (c) 2021 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 DJIs 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 DJI_CAMERA_IMAGE_HANDLER_H
#define DJI_CAMERA_IMAGE_HANDLER_H
/* Includes ------------------------------------------------------------------*/
#include "pthread.h"
#include <cstdint>
#include <vector>
#ifdef __cplusplus
extern "C" {
#endif
/* Exported constants --------------------------------------------------------*/
/* Exported types ------------------------------------------------------------*/
struct CameraRGBImage {
std::vector<uint8_t> rawData;
int height;
int width;
};
typedef void (*CameraImageCallback)(CameraRGBImage pImg, void *userData);
typedef void (*H264Callback)(const uint8_t *buf, int bufLen, void *userData);
class DJICameraImageHandler {
public:
DJICameraImageHandler();
~DJICameraImageHandler();
void writeNewImageWithLock(uint8_t *buf, int bufSize, int width, int height);
bool getNewImageWithLock(CameraRGBImage &copyOfImage, int timeoutMilliSec);
private:
pthread_mutex_t m_mutex;
pthread_cond_t m_condv;
CameraRGBImage m_img;
bool m_newImageFlag;
};
/* Exported functions --------------------------------------------------------*/
#ifdef __cplusplus
}
#endif
#endif // DJI_CAMERA_IMAGE_HANDLER_H
/************************ (C) COPYRIGHT DJI Innovations *******END OF FILE******/

View File

@ -0,0 +1,277 @@
/**
********************************************************************
* @file dji_camera_stream_decoder.cpp
* @brief
*
* @copyright (c) 2021 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 DJIs 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.
*
*********************************************************************
*/
/* Includes ------------------------------------------------------------------*/
#include "dji_camera_stream_decoder.hpp"
#include "unistd.h"
#include "pthread.h"
#include "dji_logger.h"
/* Private constants ---------------------------------------------------------*/
/* Private types -------------------------------------------------------------*/
/* Private values -------------------------------------------------------------*/
/* Private functions declaration ---------------------------------------------*/
/* Exported functions definition ---------------------------------------------*/
DJICameraStreamDecoder::DJICameraStreamDecoder()
: initSuccess(false),
cbThreadIsRunning(false),
cbThreadStatus(-1),
cb(nullptr),
cbUserParam(nullptr),
pCodecCtx(nullptr),
pCodec(nullptr),
pCodecParserCtx(nullptr),
pSwsCtx(nullptr),
pFrameYUV(nullptr),
pFrameRGB(nullptr),
rgbBuf(nullptr),
bufSize(0)
{
pthread_mutex_init(&decodemutex, nullptr);
}
DJICameraStreamDecoder::~DJICameraStreamDecoder()
{
pthread_mutex_destroy(&decodemutex);
if(cb)
{
registerCallback(nullptr, nullptr);
}
cleanup();
}
bool DJICameraStreamDecoder::init()
{
pthread_mutex_lock(&decodemutex);
if (true == initSuccess) {
USER_LOG_INFO("Decoder already initialized.\n");
return true;
}
avcodec_register_all();
pCodecCtx = avcodec_alloc_context3(nullptr);
if (!pCodecCtx) {
return false;
}
pCodecCtx->thread_count = 4;
pCodec = avcodec_find_decoder(AV_CODEC_ID_H264);
if (!pCodec || avcodec_open2(pCodecCtx, pCodec, nullptr) < 0) {
return false;
}
pCodecParserCtx = av_parser_init(AV_CODEC_ID_H264);
if (!pCodecParserCtx) {
return false;
}
pFrameYUV = av_frame_alloc();
if (!pFrameYUV) {
return false;
}
pFrameRGB = av_frame_alloc();
if (!pFrameRGB) {
return false;
}
pSwsCtx = nullptr;
pCodecCtx->flags2 |= AV_CODEC_FLAG2_SHOW_ALL;
initSuccess = true;
pthread_mutex_unlock(&decodemutex);
return true;
}
void DJICameraStreamDecoder::cleanup()
{
pthread_mutex_lock(&decodemutex);
initSuccess = false;
if (nullptr != pSwsCtx) {
sws_freeContext(pSwsCtx);
pSwsCtx = nullptr;
}
if (nullptr != pFrameYUV) {
av_free(pFrameYUV);
pFrameYUV = nullptr;
}
if (nullptr != pCodecParserCtx) {
av_parser_close(pCodecParserCtx);
pCodecParserCtx = nullptr;
}
if (nullptr != pCodec) {
avcodec_close(pCodecCtx);
pCodec = nullptr;
}
if (nullptr != pCodecCtx) {
av_free(pCodecCtx);
pCodecCtx = nullptr;
}
if (nullptr != rgbBuf) {
av_free(rgbBuf);
rgbBuf = nullptr;
}
if (nullptr != pFrameRGB) {
av_free(pFrameRGB);
pFrameRGB = nullptr;
}
pthread_mutex_unlock(&decodemutex);
}
void *DJICameraStreamDecoder::callbackThreadEntry(void *p)
{
//DSTATUS_PRIVATE("****** Decoder Callback Thread Start ******\n");
usleep(50 * 1000);
static_cast<DJICameraStreamDecoder *>(p)->callbackThreadFunc();
return nullptr;
}
void DJICameraStreamDecoder::callbackThreadFunc()
{
while (cbThreadIsRunning) {
CameraRGBImage copyOfImage;
if (!decodedImageHandler.getNewImageWithLock(copyOfImage, 1000)) {
//DDEBUG_PRIVATE("Decoder Callback Thread: Get image time out\n");
continue;
}
if (cb) {
(*cb)(copyOfImage, cbUserParam);
}
}
}
void DJICameraStreamDecoder::decodeBuffer(const uint8_t *buf, int bufLen)
{
const uint8_t *pData = buf;
int remainingLen = bufLen;
int processedLen = 0;
AVPacket pkt;
av_init_packet(&pkt);
pthread_mutex_lock(&decodemutex);
while (remainingLen > 0) {
if (!pCodecParserCtx || !pCodecCtx) {
//DSTATUS("Invalid decoder ctx.");
break;
}
processedLen = av_parser_parse2(pCodecParserCtx, pCodecCtx,
&pkt.data, &pkt.size,
pData, remainingLen,
AV_NOPTS_VALUE, AV_NOPTS_VALUE, AV_NOPTS_VALUE);
remainingLen -= processedLen;
pData += processedLen;
if (pkt.size > 0) {
int gotPicture = 0;
avcodec_decode_video2(pCodecCtx, pFrameYUV, &gotPicture, &pkt);
if (!gotPicture) {
////DSTATUS_PRIVATE("Got Frame, but no picture\n");
continue;
} else {
int w = pFrameYUV->width;
int h = pFrameYUV->height;
////DSTATUS_PRIVATE("Got picture! size=%dx%d\n", w, h);
if (nullptr == pSwsCtx) {
pSwsCtx = sws_getContext(w, h, pCodecCtx->pix_fmt,
w, h, AV_PIX_FMT_RGB24,
4, nullptr, nullptr, nullptr);
}
if (nullptr == rgbBuf) {
bufSize = avpicture_get_size(AV_PIX_FMT_RGB24, w, h);
rgbBuf = (uint8_t *) av_malloc(bufSize);
avpicture_fill((AVPicture *) pFrameRGB, rgbBuf, AV_PIX_FMT_RGB24, w, h);
}
if (nullptr != pSwsCtx && nullptr != rgbBuf) {
sws_scale(pSwsCtx,
(uint8_t const *const *) pFrameYUV->data, pFrameYUV->linesize, 0, pFrameYUV->height,
pFrameRGB->data, pFrameRGB->linesize);
pFrameRGB->height = h;
pFrameRGB->width = w;
decodedImageHandler.writeNewImageWithLock(pFrameRGB->data[0], bufSize, w, h);
}
}
}
}
pthread_mutex_unlock(&decodemutex);
av_free_packet(&pkt);
}
bool DJICameraStreamDecoder::registerCallback(CameraImageCallback f, void *param)
{
cb = f;
cbUserParam = param;
/* When users register a non-nullptr callback, we will start the callback thread. */
if (nullptr != cb) {
if (!cbThreadIsRunning) {
cbThreadStatus = pthread_create(&callbackThread, nullptr, callbackThreadEntry, this);
if (0 == cbThreadStatus) {
//DSTATUS_PRIVATE("User callback thread created successfully!\n");
cbThreadIsRunning = true;
return true;
} else {
//DERROR_PRIVATE("User called thread creation failed!\n");
cbThreadIsRunning = false;
return false;
}
} else {
//DERROR_PRIVATE("Callback thread already running!\n");
return true;
}
} else {
if (cbThreadStatus == 0) {
cbThreadIsRunning = false;
pthread_join(callbackThread, nullptr);
cbThreadStatus = -1;
}
return true;
}
}
/* Private functions definition-----------------------------------------------*/
/****************** (C) COPYRIGHT DJI Innovations *****END OF FILE****/

View File

@ -0,0 +1,96 @@
/**
********************************************************************
* @file dji_camera_stream_decoder.hpp
* @brief This is the header file for "dji_camera_stream_decoder.cpp", defining the structure and
* (exported) function prototypes.
*
* @copyright (c) 2021 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 DJIs 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 DJI_CAMERA_STREAM_DECCODER_H
#define DJI_CAMERA_STREAM_DECCODER_H
/* Includes ------------------------------------------------------------------*/
extern "C" {
#include <libavcodec/avcodec.h>
#include <libavformat/avformat.h>
#include <libswscale/swscale.h>
}
#include "pthread.h"
#include "dji_camera_image_handler.hpp"
#ifdef __cplusplus
extern "C" {
#endif
/* Exported constants --------------------------------------------------------*/
/* Exported types ------------------------------------------------------------*/
class DJICameraStreamDecoder {
public:
DJICameraStreamDecoder();
~DJICameraStreamDecoder();
bool init();
void cleanup();
void callbackThreadFunc();
void decodeBuffer(const uint8_t *pBuf, int len);
static void *callbackThreadEntry(void *p);
bool registerCallback(CameraImageCallback f, void *param);
DJICameraImageHandler decodedImageHandler;
private:
pthread_t callbackThread;
bool initSuccess;
bool cbThreadIsRunning;
int cbThreadStatus;
CameraImageCallback cb;
void *cbUserParam;
pthread_mutex_t decodemutex;
AVCodecContext *pCodecCtx;
AVCodec *pCodec;
AVCodecParserContext *pCodecParserCtx;
SwsContext *pSwsCtx;
AVFrame *pFrameYUV;
AVFrame *pFrameRGB;
uint8_t *rgbBuf;
size_t bufSize;
};
/* Exported functions --------------------------------------------------------*/
#ifdef __cplusplus
}
#endif
#endif // DJI_CAMERA_STREAM_DECCODER_H
/************************ (C) COPYRIGHT DJI Innovations *******END OF FILE******/

View File

@ -0,0 +1,209 @@
/**
********************************************************************
* @file test_liveview.cpp
* @brief
*
* @copyright (c) 2021 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 DJIs 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.
*
*********************************************************************
*/
/* Includes ------------------------------------------------------------------*/
#include "test_liveview.hpp"
/* Private constants ---------------------------------------------------------*/
/* Private types -------------------------------------------------------------*/
/* Private values -------------------------------------------------------------*/
std::map<::E_DjiLiveViewCameraPosition, DJICameraStreamDecoder *> streamDecoder;
/* Private functions declaration ---------------------------------------------*/
static void LiveviewConvertH264ToRgbCallback(E_DjiLiveViewCameraPosition position, const uint8_t *buf, uint32_t bufLen);
/* Exported functions definition ---------------------------------------------*/
LiveviewSample::LiveviewSample()
{
T_DjiReturnCode returnCode;
returnCode = DjiLiveview_Init();
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
throw std::runtime_error("Liveview init failed");
}
streamDecoder = {
{DJI_LIVEVIEW_CAMERA_POSITION_FPV, (new DJICameraStreamDecoder())},
{DJI_LIVEVIEW_CAMERA_POSITION_NO_1, (new DJICameraStreamDecoder())},
{DJI_LIVEVIEW_CAMERA_POSITION_NO_2, (new DJICameraStreamDecoder())},
{DJI_LIVEVIEW_CAMERA_POSITION_NO_3, (new DJICameraStreamDecoder())},
};
}
LiveviewSample::~LiveviewSample()
{
T_DjiReturnCode returnCode;
returnCode = DjiLiveview_Deinit();
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
throw std::runtime_error("Liveview deinit failed");
}
for (auto pair : streamDecoder) {
if (pair.second) {
delete pair.second;
}
}
}
T_DjiReturnCode LiveviewSample::StartFpvCameraStream(CameraImageCallback callback, void *userData)
{
auto deocder = streamDecoder.find(DJI_LIVEVIEW_CAMERA_POSITION_FPV);
if ((deocder != streamDecoder.end()) && deocder->second) {
deocder->second->init();
deocder->second->registerCallback(callback, userData);
return DjiLiveview_StartH264Stream(DJI_LIVEVIEW_CAMERA_POSITION_FPV, DJI_LIVEVIEW_CAMERA_SOURCE_DEFAULT,
LiveviewConvertH264ToRgbCallback);
} else {
return DJI_ERROR_SYSTEM_MODULE_CODE_NOT_FOUND;
}
}
T_DjiReturnCode LiveviewSample::StartMainCameraStream(CameraImageCallback callback, void *userData)
{
auto deocder = streamDecoder.find(DJI_LIVEVIEW_CAMERA_POSITION_NO_1);
if ((deocder != streamDecoder.end()) && deocder->second) {
deocder->second->init();
deocder->second->registerCallback(callback, userData);
return DjiLiveview_StartH264Stream(DJI_LIVEVIEW_CAMERA_POSITION_NO_1, DJI_LIVEVIEW_CAMERA_SOURCE_DEFAULT,
LiveviewConvertH264ToRgbCallback);
} else {
return DJI_ERROR_SYSTEM_MODULE_CODE_NOT_FOUND;
}
}
T_DjiReturnCode LiveviewSample::StartViceCameraStream(CameraImageCallback callback, void *userData)
{
auto deocder = streamDecoder.find(DJI_LIVEVIEW_CAMERA_POSITION_NO_2);
if ((deocder != streamDecoder.end()) && deocder->second) {
deocder->second->init();
deocder->second->registerCallback(callback, userData);
return DjiLiveview_StartH264Stream(DJI_LIVEVIEW_CAMERA_POSITION_NO_2, DJI_LIVEVIEW_CAMERA_SOURCE_DEFAULT,
LiveviewConvertH264ToRgbCallback);
} else {
return DJI_ERROR_SYSTEM_MODULE_CODE_NOT_FOUND;
}
}
T_DjiReturnCode LiveviewSample::StartTopCameraStream(CameraImageCallback callback, void *userData)
{
auto deocder = streamDecoder.find(DJI_LIVEVIEW_CAMERA_POSITION_NO_3);
if ((deocder != streamDecoder.end()) && deocder->second) {
deocder->second->init();
deocder->second->registerCallback(callback, userData);
return DjiLiveview_StartH264Stream(DJI_LIVEVIEW_CAMERA_POSITION_NO_3, DJI_LIVEVIEW_CAMERA_SOURCE_DEFAULT,
LiveviewConvertH264ToRgbCallback);
} else {
return DJI_ERROR_SYSTEM_MODULE_CODE_NOT_FOUND;
}
}
T_DjiReturnCode LiveviewSample::StopFpvCameraStream()
{
T_DjiReturnCode returnCode;
returnCode = DjiLiveview_StopH264Stream(DJI_LIVEVIEW_CAMERA_POSITION_FPV);
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
return returnCode;
}
auto deocder = streamDecoder.find(DJI_LIVEVIEW_CAMERA_POSITION_FPV);
if ((deocder != streamDecoder.end()) && deocder->second) {
deocder->second->cleanup();
}
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}
T_DjiReturnCode LiveviewSample::StopMainCameraStream()
{
T_DjiReturnCode returnCode;
returnCode = DjiLiveview_StopH264Stream(DJI_LIVEVIEW_CAMERA_POSITION_NO_1);
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
return returnCode;
}
auto deocder = streamDecoder.find(DJI_LIVEVIEW_CAMERA_POSITION_NO_1);
if ((deocder != streamDecoder.end()) && deocder->second) {
deocder->second->cleanup();
}
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}
T_DjiReturnCode LiveviewSample::StopViceCameraStream()
{
T_DjiReturnCode returnCode;
returnCode = DjiLiveview_StopH264Stream(DJI_LIVEVIEW_CAMERA_POSITION_NO_2);
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
return returnCode;
}
auto deocder = streamDecoder.find(DJI_LIVEVIEW_CAMERA_POSITION_NO_2);
if ((deocder != streamDecoder.end()) && deocder->second) {
deocder->second->cleanup();
}
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}
T_DjiReturnCode LiveviewSample::StopTopCameraStream()
{
T_DjiReturnCode returnCode;
returnCode = DjiLiveview_StopH264Stream(DJI_LIVEVIEW_CAMERA_POSITION_NO_3);
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
return returnCode;
}
auto deocder = streamDecoder.find(DJI_LIVEVIEW_CAMERA_POSITION_NO_3);
if ((deocder != streamDecoder.end()) && deocder->second) {
deocder->second->cleanup();
}
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}
/* Private functions definition-----------------------------------------------*/
static void LiveviewConvertH264ToRgbCallback(E_DjiLiveViewCameraPosition position, const uint8_t *buf, uint32_t bufLen)
{
auto deocder = streamDecoder.find(position);
if ((deocder != streamDecoder.end()) && deocder->second) {
deocder->second->decodeBuffer(buf, bufLen);
}
}
/****************** (C) COPYRIGHT DJI Innovations *****END OF FILE****/

View File

@ -0,0 +1,70 @@
/**
********************************************************************
* @file test_liveview.hpp
* @brief This is the header file for "test_liveview.cpp", defining the structure and
* (exported) function prototypes.
*
* @copyright (c) 2021 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 DJIs 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 TEST_LIVEVIEW_H
#define TEST_LIVEVIEW_H
/* Includes ------------------------------------------------------------------*/
#include "dji_liveview.h"
#include <map>
#include "dji_camera_stream_decoder.hpp"
#ifdef __cplusplus
extern "C" {
#endif
/* Exported constants --------------------------------------------------------*/
/* Exported types ------------------------------------------------------------*/
using namespace std;
class LiveviewSample {
public:
LiveviewSample();
~LiveviewSample();
T_DjiReturnCode StartFpvCameraStream(CameraImageCallback callback, void *userData);
T_DjiReturnCode StopFpvCameraStream();
T_DjiReturnCode StartMainCameraStream(CameraImageCallback callback, void *userData);
T_DjiReturnCode StopMainCameraStream();
T_DjiReturnCode StartViceCameraStream(CameraImageCallback callback, void *userData);
T_DjiReturnCode StopViceCameraStream();
T_DjiReturnCode StartTopCameraStream(CameraImageCallback callback, void *userData);
T_DjiReturnCode StopTopCameraStream();
};
/* Exported functions --------------------------------------------------------*/
#ifdef __cplusplus
}
#endif
#endif // TEST_LIVEVIEW_H
/************************ (C) COPYRIGHT DJI Innovations *******END OF FILE******/

View File

@ -0,0 +1,309 @@
/**
********************************************************************
* @file test_liveview_entry.cpp
* @brief
*
* @copyright (c) 2021 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 DJIs 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.
*
*********************************************************************
*/
/* Includes ------------------------------------------------------------------*/
#include <iostream>
#include <dji_logger.h>
#include "test_liveview_entry.hpp"
#include "test_liveview.hpp"
#ifdef OPEN_CV_INSTALLED
#include "opencv2/opencv.hpp"
#include "opencv2/dnn.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "../../../sample_c/module_sample/utils/util_misc.h"
using namespace cv;
#endif
using namespace std;
/* Private constants ---------------------------------------------------------*/
/* Private types -------------------------------------------------------------*/
/* Private values -------------------------------------------------------------*/
const char *classNames[] = {"background", "person", "bicycle", "car", "motorcycle", "airplane", "bus", "train", "truck",
"boat", "traffic light",
"fire hydrant", "background", "stop sign", "parking meter", "bench", "bird", "cat", "dog",
"horse", "sheep", "cow", "elephant", "bear", "zebra", "giraffe", "background", "backpack",
"umbrella", "background", "background", "handbag", "tie", "suitcase", "frisbee", "skis",
"snowboard", "sports ball", "kite", "baseball bat", "baseball glove", "skateboard",
"surfboard", "tennis racket",
"bottle", "background", "wine glass", "cup", "fork", "knife", "spoon", "bowl", "banana",
"apple", "sandwich", "orange", "broccoli", "carrot", "hot dog", "pizza", "donut",
"cake", "chair", "couch", "potted plant", "bed", "background", "dining table", "background",
"background", "toilet", "background", "tv", "laptop", "mouse", "remote", "keyboard",
"cell phone", "microwave", "oven", "toaster", "sink", "refrigerator", "background", "book",
"clock", "vase", "scissors", "teddy bear", "hair drier", "toothbrush"};
const size_t inWidth = 320;
const size_t inHeight = 300;
const float WHRatio = inWidth / (float) inHeight;
static int32_t s_demoIndex = -1;
char curFileDirPath[DJI_FILE_PATH_SIZE_MAX];
char tempFileDirPath[DJI_FILE_PATH_SIZE_MAX];
char prototxtFileDirPath[DJI_FILE_PATH_SIZE_MAX];
char weightsFileDirPath[DJI_FILE_PATH_SIZE_MAX];
/* Private functions declaration ---------------------------------------------*/
static void DjiUser_ShowRgbImageCallback(CameraRGBImage img, void *userData);
static T_DjiReturnCode DjiUser_GetCurrentFileDirPath(const char *filePath, uint32_t pathBufferSize, char *dirPath);
/* Exported functions definition ---------------------------------------------*/
void DjiUser_RunCameraStreamViewSample()
{
char cameraIndexChar = 0;
char demoIndexChar = 0;
char isQuit = 0;
CameraRGBImage camImg;
char fpvName[] = "FPV_CAM";
char mainName[] = "MAIN_CAM";
char viceName[] = "VICE_CAM";
char topName[] = "TOP_CAM";
auto *liveviewSample = new LiveviewSample();
T_DjiReturnCode returnCode;
returnCode = DjiUser_GetCurrentFileDirPath(__FILE__, DJI_FILE_PATH_SIZE_MAX, curFileDirPath);
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("Get file current path error, stat = 0x%08llX", returnCode);
}
cout << "Please enter the type of camera stream you want to view\n\n"
<< "--> [f] Fpv Camera\n"
<< "--> [m] Main Camera\n"
<< "--> [v] Vice Camera\n"
<< "--> [t] Top Camera\n"
<< endl;
cin >> cameraIndexChar;
switch (cameraIndexChar) {
case 'f':
case 'F':
liveviewSample->StartFpvCameraStream(&DjiUser_ShowRgbImageCallback, &fpvName);
break;
case 'm':
case 'M':
liveviewSample->StartMainCameraStream(&DjiUser_ShowRgbImageCallback, &mainName);
break;
case 'v':
case 'V':
liveviewSample->StartViceCameraStream(&DjiUser_ShowRgbImageCallback, &viceName);
break;
case 't':
case 'T':
liveviewSample->StartTopCameraStream(&DjiUser_ShowRgbImageCallback, &topName);
break;
default:
cout << "No camera selected";
delete liveviewSample;
return;
}
cout << "Please choose the stream demo you want to run\n\n"
<< "--> [0] Normal RGB image display\n"
<< "--> [1] Binary image display\n"
<< "--> [2] Faces detection demo\n"
<< "--> [3] Tensorflow Object detection demo\n"
<< endl;
cin >> demoIndexChar;
switch (demoIndexChar) {
case '0':
s_demoIndex = 0;
break;
case '1':
s_demoIndex = 1;
break;
case '2':
s_demoIndex = 2;
break;
case '3':
s_demoIndex = 3;
break;
default:
cout << "No demo selected";
delete liveviewSample;
return;
}
cout << "Please enter the 'q' or 'Q' to quit camera stream view\n"
<< endl;
while (true) {
cin >> isQuit;
if (isQuit == 'q' || isQuit == 'Q') {
break;
}
}
switch (cameraIndexChar) {
case 'f':
case 'F':
liveviewSample->StopFpvCameraStream();
break;
case 'm':
case 'M':
liveviewSample->StopMainCameraStream();
break;
case 'v':
case 'V':
liveviewSample->StopViceCameraStream();
break;
case 't':
case 'T':
liveviewSample->StopTopCameraStream();
break;
default:
cout << "No camera selected";
delete liveviewSample;
return;
}
delete liveviewSample;
}
/* Private functions definition-----------------------------------------------*/
static void DjiUser_ShowRgbImageCallback(CameraRGBImage img, void *userData)
{
string name = string(reinterpret_cast<char *>(userData));
#ifdef OPEN_CV_INSTALLED
Mat mat(img.height, img.width, CV_8UC3, img.rawData.data(), img.width * 3);
if (s_demoIndex == 0) {
cvtColor(mat, mat, COLOR_RGB2BGR);
imshow(name, mat);
} else if (s_demoIndex == 1) {
cvtColor(mat, mat, COLOR_RGB2GRAY);
Mat mask;
cv::threshold(mat, mask, 0, 255, cv::THRESH_BINARY | cv::THRESH_OTSU);
imshow(name, mask);
} else if (s_demoIndex == 2) {
cvtColor(mat, mat, COLOR_RGB2BGR);
snprintf(tempFileDirPath, DJI_FILE_PATH_SIZE_MAX, "%s/data/haarcascade_frontalface_alt.xml", curFileDirPath);
auto faceDetector = cv::CascadeClassifier(tempFileDirPath);
std::vector<Rect> faces;
faceDetector.detectMultiScale(mat, faces, 1.1, 3, 0, Size(50, 50));
for (int i = 0; i < faces.size(); ++i) {
cout << "index: " << i;
cout << " x: " << faces[i].x;
cout << " y: " << faces[i].y << endl;
cv::rectangle(mat, cvPoint(faces[i].x, faces[i].y),
cvPoint(faces[i].x + faces[i].width, faces[i].y + faces[i].height),
Scalar(0, 0, 255), 2, 1, 0);
}
imshow(name, mat);
} else if (s_demoIndex == 3) {
snprintf(prototxtFileDirPath, DJI_FILE_PATH_SIZE_MAX,
"%s/data/tensorflow/ssd_inception_v2_coco_2017_11_17.pbtxt",
curFileDirPath);
//Attention: If you want to run the Tensorflow Object detection demo, Please download the tensorflow model.
//Download Url: http://download.tensorflow.org/models/object_detection/ssd_inception_v2_coco_2017_11_17.tar.gz
snprintf(weightsFileDirPath, DJI_FILE_PATH_SIZE_MAX, "%s/data/tensorflow/frozen_inference_graph.pb",
curFileDirPath);
dnn::Net net = cv::dnn::readNetFromTensorflow(weightsFileDirPath, prototxtFileDirPath);
Size frame_size = mat.size();
Size cropSize;
if (frame_size.width / (float) frame_size.height > WHRatio) {
cropSize = Size(static_cast<int>(frame_size.height * WHRatio),
frame_size.height);
} else {
cropSize = Size(frame_size.width,
static_cast<int>(frame_size.width / WHRatio));
}
Rect crop(Point((frame_size.width - cropSize.width) / 2,
(frame_size.height - cropSize.height) / 2),
cropSize);
cv::Mat blob = cv::dnn::blobFromImage(mat, 1, Size(300, 300));
net.setInput(blob);
Mat output = net.forward();
Mat detectionMat(output.size[2], output.size[3], CV_32F, output.ptr<float>());
mat = mat(crop);
float confidenceThreshold = 0.50;
for (int i = 0; i < detectionMat.rows; i++) {
float confidence = detectionMat.at<float>(i, 2);
if (confidence > confidenceThreshold) {
auto objectClass = (size_t) (detectionMat.at<float>(i, 1));
int xLeftBottom = static_cast<int>(detectionMat.at<float>(i, 3) * mat.cols);
int yLeftBottom = static_cast<int>(detectionMat.at<float>(i, 4) * mat.rows);
int xRightTop = static_cast<int>(detectionMat.at<float>(i, 5) * mat.cols);
int yRightTop = static_cast<int>(detectionMat.at<float>(i, 6) * mat.rows);
ostringstream ss;
ss << confidence;
String conf(ss.str());
Rect object((int) xLeftBottom, (int) yLeftBottom,
(int) (xRightTop - xLeftBottom),
(int) (yRightTop - yLeftBottom));
rectangle(mat, object, Scalar(0, 255, 0), 2);
String label = String(classNames[objectClass]) + ": " + conf;
int baseLine = 0;
Size labelSize = getTextSize(label, FONT_HERSHEY_SIMPLEX, 0.5, 1, &baseLine);
rectangle(mat, Rect(Point(xLeftBottom, yLeftBottom - labelSize.height),
Size(labelSize.width, labelSize.height + baseLine)), Scalar(0, 255, 0), CV_FILLED);
putText(mat, label, Point(xLeftBottom, yLeftBottom), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 0, 0));
}
}
imshow(name, mat);
}
cv::waitKey(1);
#endif
}
static T_DjiReturnCode DjiUser_GetCurrentFileDirPath(const char *filePath, uint32_t pathBufferSize, char *dirPath)
{
uint32_t i = strlen(filePath) - 1;
uint32_t dirPathLen;
while (filePath[i] != '/') {
i--;
}
dirPathLen = i + 1;
if (dirPathLen + 1 > pathBufferSize) {
return DJI_ERROR_SYSTEM_MODULE_CODE_INVALID_PARAMETER;
}
memcpy(dirPath, filePath, dirPathLen);
dirPath[dirPathLen] = 0;
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}
/****************** (C) COPYRIGHT DJI Innovations *****END OF FILE****/

View File

@ -0,0 +1,48 @@
/**
********************************************************************
* @file test_liveview_entry.hpp
* @brief This is the header file for "test_liveview_entry.cpp", defining the structure and
* (exported) function prototypes.
*
* @copyright (c) 2021 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 DJIs 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 TEST_LIVEVIEW_ENTRY_H
#define TEST_LIVEVIEW_ENTRY_H
/* Includes ------------------------------------------------------------------*/
#ifdef __cplusplus
extern "C" {
#endif
/* Exported constants --------------------------------------------------------*/
/* Exported types ------------------------------------------------------------*/
/* Exported functions --------------------------------------------------------*/
void DjiUser_RunCameraStreamViewSample();
#ifdef __cplusplus
}
#endif
#endif // TEST_LIVEVIEW_ENTRY_H
/************************ (C) COPYRIGHT DJI Innovations *******END OF FILE******/

View File

@ -0,0 +1,120 @@
/**
********************************************************************
* @file test_perception.cpp
* @brief
*
* @copyright (c) 2021 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 DJIs 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.
*
*********************************************************************
*/
/* Includes ------------------------------------------------------------------*/
#include <stdexcept>
#include "test_perception.hpp"
/* Private constants ---------------------------------------------------------*/
/* Private types -------------------------------------------------------------*/
/* Private values -------------------------------------------------------------*/
/* Private functions declaration ---------------------------------------------*/
/* Exported functions definition ---------------------------------------------*/
PerceptionSample::PerceptionSample()
{
T_DjiReturnCode returnCode;
returnCode = DjiPerception_Init();
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
throw std::runtime_error("Perception init failed");
}
}
PerceptionSample::~PerceptionSample()
{
T_DjiReturnCode returnCode;
returnCode = DjiPerception_Deinit();
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
throw std::runtime_error("Perception deinit failed");
}
}
T_DjiReturnCode PerceptionSample::SubscribeFrontImage(DjiPerceptionImageCallback callback)
{
return DjiPerception_SubscribePerceptionImage(DJI_PERCEPTION_RECTIFY_FRONT, callback);
}
T_DjiReturnCode PerceptionSample::SubscribeRearImage(DjiPerceptionImageCallback callback)
{
return DjiPerception_SubscribePerceptionImage(DJI_PERCEPTION_RECTIFY_REAR, callback);
}
T_DjiReturnCode PerceptionSample::SubscribeLeftImage(DjiPerceptionImageCallback callback)
{
return DjiPerception_SubscribePerceptionImage(DJI_PERCEPTION_RECTIFY_LEFT, callback);
}
T_DjiReturnCode PerceptionSample::SubscribeRightImage(DjiPerceptionImageCallback callback)
{
return DjiPerception_SubscribePerceptionImage(DJI_PERCEPTION_RECTIFY_RIGHT, callback);
}
T_DjiReturnCode PerceptionSample::SubscribeUpImage(DjiPerceptionImageCallback callback)
{
return DjiPerception_SubscribePerceptionImage(DJI_PERCEPTION_RECTIFY_UP, callback);
}
T_DjiReturnCode PerceptionSample::SubscribeDownImage(DjiPerceptionImageCallback callback)
{
return DjiPerception_SubscribePerceptionImage(DJI_PERCEPTION_RECTIFY_DOWN, callback);
}
T_DjiReturnCode PerceptionSample::UnSubscribeFrontImage()
{
return DjiPerception_UnsubscribePerceptionImage(DJI_PERCEPTION_RECTIFY_FRONT);
}
T_DjiReturnCode PerceptionSample::UnSubscribeRearImage()
{
return DjiPerception_UnsubscribePerceptionImage(DJI_PERCEPTION_RECTIFY_REAR);
}
T_DjiReturnCode PerceptionSample::UnSubscribeLeftImage()
{
return DjiPerception_UnsubscribePerceptionImage(DJI_PERCEPTION_RECTIFY_LEFT);
}
T_DjiReturnCode PerceptionSample::UnSubscribeRightImage()
{
return DjiPerception_UnsubscribePerceptionImage(DJI_PERCEPTION_RECTIFY_RIGHT);
}
T_DjiReturnCode PerceptionSample::UnSubscribeUpImage()
{
return DjiPerception_UnsubscribePerceptionImage(DJI_PERCEPTION_RECTIFY_UP);
}
T_DjiReturnCode PerceptionSample::UnSubscribeDownImage()
{
return DjiPerception_UnsubscribePerceptionImage(DJI_PERCEPTION_RECTIFY_DOWN);
}
/* Private functions definition-----------------------------------------------*/
/****************** (C) COPYRIGHT DJI Innovations *****END OF FILE****/

View File

@ -0,0 +1,70 @@
/**
********************************************************************
* @file test_perception.hpp
* @brief This is the header file for "test_perception.cpp", defining the structure and
* (exported) function prototypes.
*
* @copyright (c) 2021 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 DJIs 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 TEST_PERCEPTION_H
#define TEST_PERCEPTION_H
/* Includes ------------------------------------------------------------------*/
#include "dji_perception.h"
#ifdef __cplusplus
extern "C" {
#endif
/* Exported constants --------------------------------------------------------*/
/* Exported types ------------------------------------------------------------*/
using namespace std;
class PerceptionSample {
public:
PerceptionSample();
~PerceptionSample();
T_DjiReturnCode SubscribeFrontImage(DjiPerceptionImageCallback callback);
T_DjiReturnCode SubscribeRearImage(DjiPerceptionImageCallback callback);
T_DjiReturnCode SubscribeLeftImage(DjiPerceptionImageCallback callback);
T_DjiReturnCode SubscribeRightImage(DjiPerceptionImageCallback callback);
T_DjiReturnCode SubscribeUpImage(DjiPerceptionImageCallback callback);
T_DjiReturnCode SubscribeDownImage(DjiPerceptionImageCallback callback);
T_DjiReturnCode UnSubscribeFrontImage();
T_DjiReturnCode UnSubscribeRearImage();
T_DjiReturnCode UnSubscribeLeftImage();
T_DjiReturnCode UnSubscribeRightImage();
T_DjiReturnCode UnSubscribeUpImage();
T_DjiReturnCode UnSubscribeDownImage();
private:
};
/* Exported functions --------------------------------------------------------*/
#ifdef __cplusplus
}
#endif
#endif // TEST_PERCEPTION_H
/************************ (C) COPYRIGHT DJI Innovations *******END OF FILE******/

View File

@ -0,0 +1,358 @@
/**
********************************************************************
* @file test_perception_entry.cpp
* @brief
*
* @copyright (c) 2021 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 DJIs 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.
*
*********************************************************************
*/
/* Includes ------------------------------------------------------------------*/
#include <dirent.h>
#include "test_perception_entry.hpp"
#include "dji_logger.h"
#include "dji_perception.h"
#include "test_perception.hpp"
#ifdef OPEN_CV_INSTALLED
#include "opencv2/opencv.hpp"
#include "opencv2/highgui/highgui.hpp"
#endif
/* Private constants ---------------------------------------------------------*/
#define USER_PERCEPTION_TASK_STACK_SIZE (1024)
/* Private types -------------------------------------------------------------*/
typedef struct {
T_DjiPerceptionImageInfo info;
uint8_t *imageRawBuffer;
T_DjiMutexHandle mutex;
bool gotData;
} T_DjiTestStereoImagePacket;
typedef struct {
E_DjiPerceptionCameraPosition cameraPosition;
char const *name;
} T_DjiTestPerceptionCameraPositionName;
typedef struct {
E_DjiPerceptionDirection direction;
char const *name;
} T_DjiTestPerceptionDirectionName;
/* Private values -------------------------------------------------------------*/
static T_DjiTaskHandle s_stereoImageThread;
static T_DjiTestStereoImagePacket s_stereoImagePacket = {
.info = {0},
.imageRawBuffer = nullptr,
.mutex = nullptr,
.gotData = false};
static const T_DjiTestPerceptionDirectionName directionName[] = {
{.direction = DJI_PERCEPTION_RECTIFY_DOWN, .name = "down"},
{.direction = DJI_PERCEPTION_RECTIFY_FRONT, .name = "front"},
{.direction = DJI_PERCEPTION_RECTIFY_REAR, .name = "rear"},
{.direction = DJI_PERCEPTION_RECTIFY_UP, .name = "up"},
{.direction = DJI_PERCEPTION_RECTIFY_LEFT, .name = "left"},
{.direction = DJI_PERCEPTION_RECTIFY_RIGHT, .name = "right"},
};
static const T_DjiTestPerceptionCameraPositionName positionName[] = {
{.cameraPosition = RECTIFY_DOWN_LEFT, .name = "down_l"},
{.cameraPosition = RECTIFY_DOWN_RIGHT, .name = "down_r"},
{.cameraPosition = RECTIFY_FRONT_LEFT, .name = "front_l"},
{.cameraPosition = RECTIFY_FRONT_RIGHT, .name = "front_r"},
{.cameraPosition = RECTIFY_REAR_LEFT, .name = "rear_l"},
{.cameraPosition = RECTIFY_REAR_RIGHT, .name = "rear_r"},
{.cameraPosition = RECTIFY_UP_LEFT, .name = "up_l"},
{.cameraPosition = RECTIFY_UP_RIGHT, .name = "up_r"},
{.cameraPosition = RECTIFY_LEFT_LEFT, .name = "left_l"},
{.cameraPosition = RECTIFY_LEFT_RIGHT, .name = "left_r"},
{.cameraPosition = RECTIFY_RIGHT_LEFT, .name = "right_l"},
{.cameraPosition = RECTIFY_RIGHT_RIGHT, .name = "right_r"},
};
/* Private functions declaration ---------------------------------------------*/
static void DjiTest_PerceptionImageCallback(T_DjiPerceptionImageInfo imageInfo, uint8_t *imageRawBuffer,
uint32_t bufferLen);
static void *DjiTest_StereoImagesDisplayTask(void *arg);
/* Exported functions definition ---------------------------------------------*/
void DjiUser_RunStereoVisionViewSample(void)
{
T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler();
char inputChar;
char isQuit;
auto *perceptionSample = new PerceptionSample;
T_DjiReturnCode returnCode;
T_DjiPerceptionCameraParametersPacket cameraParametersPacket = {0};
returnCode = osalHandler->MutexCreate(&s_stereoImagePacket.mutex);
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("Crete mutex failed, return code:0x%08X", returnCode);
goto DeletePerception;
}
returnCode = osalHandler->TaskCreate("user_perception_task", DjiTest_StereoImagesDisplayTask,
USER_PERCEPTION_TASK_STACK_SIZE, &s_stereoImagePacket, &s_stereoImageThread);
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("Crete task failed, return code:0x%08X", returnCode);
goto DestroyMutex;
}
returnCode = DjiPerception_GetStereoCameraParameters(&cameraParametersPacket);
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("Get camera parameters failed, return code:0x%08X", returnCode);
goto DestroyTask;
}
for (int i = 0; i < cameraParametersPacket.directionNum; i++) {
USER_LOG_INFO(" [%-05s] leftIntrinsics = {%f, %f, %f, %f, %f, %f, %f, %f, %f }",
directionName[cameraParametersPacket.cameraParameters[i].direction].name,
cameraParametersPacket.cameraParameters[i].leftIntrinsics[0],
cameraParametersPacket.cameraParameters[i].leftIntrinsics[1],
cameraParametersPacket.cameraParameters[i].leftIntrinsics[2],
cameraParametersPacket.cameraParameters[i].leftIntrinsics[3],
cameraParametersPacket.cameraParameters[i].leftIntrinsics[4],
cameraParametersPacket.cameraParameters[i].leftIntrinsics[5],
cameraParametersPacket.cameraParameters[i].leftIntrinsics[6],
cameraParametersPacket.cameraParameters[i].leftIntrinsics[7],
cameraParametersPacket.cameraParameters[i].leftIntrinsics[8]);
USER_LOG_INFO("[%-05s] rightIntrinsics = {%f, %f, %f, %f, %f, %f, %f, %f, %f }",
directionName[cameraParametersPacket.cameraParameters[i].direction].name,
cameraParametersPacket.cameraParameters[i].rightIntrinsics[0],
cameraParametersPacket.cameraParameters[i].rightIntrinsics[1],
cameraParametersPacket.cameraParameters[i].rightIntrinsics[2],
cameraParametersPacket.cameraParameters[i].rightIntrinsics[3],
cameraParametersPacket.cameraParameters[i].rightIntrinsics[4],
cameraParametersPacket.cameraParameters[i].rightIntrinsics[5],
cameraParametersPacket.cameraParameters[i].rightIntrinsics[6],
cameraParametersPacket.cameraParameters[i].rightIntrinsics[7],
cameraParametersPacket.cameraParameters[i].rightIntrinsics[8]);
USER_LOG_INFO("[%-05s] rotationLeftInRight = {%f, %f, %f, %f, %f, %f, %f, %f, %f }",
directionName[cameraParametersPacket.cameraParameters[i].direction].name,
cameraParametersPacket.cameraParameters[i].rotationLeftInRight[0],
cameraParametersPacket.cameraParameters[i].rotationLeftInRight[1],
cameraParametersPacket.cameraParameters[i].rotationLeftInRight[2],
cameraParametersPacket.cameraParameters[i].rotationLeftInRight[3],
cameraParametersPacket.cameraParameters[i].rotationLeftInRight[4],
cameraParametersPacket.cameraParameters[i].rotationLeftInRight[5],
cameraParametersPacket.cameraParameters[i].rotationLeftInRight[6],
cameraParametersPacket.cameraParameters[i].rotationLeftInRight[7],
cameraParametersPacket.cameraParameters[i].rotationLeftInRight[8]);
USER_LOG_INFO("[%-05s] translationLeftInRight = {%f, %f, %f }\r\n",
directionName[cameraParametersPacket.cameraParameters[i].direction].name,
cameraParametersPacket.cameraParameters[i].translationLeftInRight[0],
cameraParametersPacket.cameraParameters[i].translationLeftInRight[1],
cameraParametersPacket.cameraParameters[i].translationLeftInRight[2]);
osalHandler->TaskSleepMs(100);
}
while (true) {
std::cout
<< "| Available commands: |"
<<
std::endl;
std::cout
<< "| [d] Subscribe down stereo camera pair images |"
<<
std::endl;
std::cout
<< "| [f] Subscribe front stereo camera pair images |"
<<
std::endl;
std::cout
<< "| [r] Subscribe rear stereo camera pair images |"
<<
std::endl;
std::cout
<< "| [u] Subscribe up stereo camera pair images |"
<<
std::endl;
std::cout
<< "| [l] Subscribe left stereo camera pair images |"
<<
std::endl;
std::cout
<< "| [t] Subscribe right stereo camera pair images |"
<<
std::endl;
std::cout
<< "| [q] quit |"
<<
std::endl;
std::cin >> inputChar;
switch (inputChar) {
case 'd':
USER_LOG_INFO("Subscribe down stereo camera pair images.");
perceptionSample->SubscribeDownImage(DjiTest_PerceptionImageCallback);
break;
case 'f':
USER_LOG_INFO("Subscribe front stereo camera pair images.");
perceptionSample->SubscribeFrontImage(DjiTest_PerceptionImageCallback);
break;
case 'r':
USER_LOG_INFO("Subscribe rear stereo camera pair images.");
perceptionSample->SubscribeRearImage(DjiTest_PerceptionImageCallback);
break;
case 'u':
USER_LOG_INFO("Subscribe up stereo camera pair images.");
perceptionSample->SubscribeUpImage(DjiTest_PerceptionImageCallback);
break;
case 'l':
USER_LOG_INFO("Subscribe left stereo camera pair images.");
perceptionSample->SubscribeLeftImage(DjiTest_PerceptionImageCallback);
break;
case 't':
USER_LOG_INFO("Subscribe right stereo camera pair images.");
perceptionSample->SubscribeRightImage(DjiTest_PerceptionImageCallback);
break;
case 'g':
USER_LOG_INFO("Do stereo camera parameters subscription");
break;
case 'q':
goto DestroyTask;
default:
break;
}
while (true) {
cin >> isQuit;
if (isQuit == 'q' || isQuit == 'Q') {
break;
}
}
switch (inputChar) {
case 'd':
USER_LOG_INFO("Unsubscribe down stereo camera pair images.");
perceptionSample->UnSubscribeDownImage();
break;
case 'f':
USER_LOG_INFO("Unsubscribe front stereo camera pair images.");
perceptionSample->UnSubscribeFrontImage();
break;
case 'r':
USER_LOG_INFO("Unsubscribe rear stereo camera pair images.");
perceptionSample->UnSubscribeRearImage();
break;
case 'u':
USER_LOG_INFO("Unsubscribe up stereo camera pair images.");
perceptionSample->UnSubscribeUpImage();
break;
case 'l':
USER_LOG_INFO("Unsubscribe left stereo camera pair images.");
perceptionSample->UnSubscribeLeftImage();
break;
case 't':
USER_LOG_INFO("Unsubscribe right stereo camera pair images.");
perceptionSample->UnSubscribeRightImage();
break;
default:
break;
}
cv::destroyAllWindows();
}
DestroyTask:
returnCode = osalHandler->TaskDestroy(s_stereoImageThread);
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("Destroy task failed, return code:0x%08X", returnCode);
}
DestroyMutex:
returnCode = osalHandler->MutexDestroy(s_stereoImagePacket.mutex);
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("Destroy mutex failed, return code:0x%08X", returnCode);
}
DeletePerception:
delete perceptionSample;
}
/* Private functions definition-----------------------------------------------*/
static void DjiTest_PerceptionImageCallback(T_DjiPerceptionImageInfo imageInfo, uint8_t *imageRawBuffer,
uint32_t bufferLen)
{
T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler();
USER_LOG_INFO("image info : dataId(%d) seq(%d) timestamp(%llu) datatype(%d) index(%d) h(%d) w(%d) dir(%d) "
"bpp(%d) bufferlen(%d)", imageInfo.dataId, imageInfo.sequence, imageInfo.timeStamp,
imageInfo.dataType,
imageInfo.rawInfo.index, imageInfo.rawInfo.height, imageInfo.rawInfo.width,
imageInfo.rawInfo.direction,
imageInfo.rawInfo.bpp, bufferLen);
if (imageRawBuffer) {
osalHandler->MutexLock(s_stereoImagePacket.mutex);
s_stereoImagePacket.info = imageInfo;
if (s_stereoImagePacket.imageRawBuffer) osalHandler->Free(s_stereoImagePacket.imageRawBuffer);
s_stereoImagePacket.imageRawBuffer = (uint8_t *) osalHandler->Malloc(bufferLen);
memcpy(s_stereoImagePacket.imageRawBuffer, imageRawBuffer, bufferLen);
s_stereoImagePacket.gotData = true;
osalHandler->MutexUnlock(s_stereoImagePacket.mutex);
}
}
static void *DjiTest_StereoImagesDisplayTask(void *arg)
{
T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler();
auto *pack = (T_DjiTestStereoImagePacket *) arg;
char nameStr[32] = {0};
while (true) {
osalHandler->TaskSleepMs(1);
#ifdef OPEN_CV_INSTALLED
/*! Get data here */
osalHandler->MutexLock(pack->mutex);
if (!pack->gotData) {
osalHandler->MutexUnlock(pack->mutex);
continue;
}
cv::Mat cv_img_stereo = cv::Mat(pack->info.rawInfo.height, pack->info.rawInfo.width, CV_8U);
int copySize = pack->info.rawInfo.height * pack->info.rawInfo.width;
if (pack->imageRawBuffer) {
memcpy(cv_img_stereo.data, pack->imageRawBuffer, copySize);
osalHandler->Free(pack->imageRawBuffer);
pack->imageRawBuffer = NULL;
}
for (int i = 0; i < sizeof(positionName) / sizeof(T_DjiTestPerceptionCameraPositionName); ++i) {
if (positionName[i].cameraPosition == pack->info.dataType) {
sprintf(nameStr, "Image position: %s", positionName[i].name);
break;
}
}
pack->gotData = false;
osalHandler->MutexUnlock(pack->mutex);
/*! Using Opencv display here */
cv::imshow(nameStr, cv_img_stereo);
cv::waitKey(1);
#else
osalHandler->TaskSleepMs(1000);
USER_LOG_WARN("Please install opencv to run this stereo image display sample.");
#endif
}
}
/****************** (C) COPYRIGHT DJI Innovations *****END OF FILE****/

View File

@ -0,0 +1,49 @@
/**
********************************************************************
* @file test_perception_entry.hpp
* @brief This is the header file for "test_perception_entry.cpp", defining the structure and
* (exported) function prototypes.
*
* @copyright (c) 2021 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 DJIs 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 TEST_PERCEPTION_ENTRY_H
#define TEST_PERCEPTION_ENTRY_H
/* Includes ------------------------------------------------------------------*/
#include "dji_perception.h"
#ifdef __cplusplus
extern "C" {
#endif
/* Exported constants --------------------------------------------------------*/
/* Exported types ------------------------------------------------------------*/
/* Exported functions --------------------------------------------------------*/
void DjiUser_RunStereoVisionViewSample(void);
#ifdef __cplusplus
}
#endif
#endif // TEST_PERCEPTION_ENTRY_H
/************************ (C) COPYRIGHT DJI Innovations *******END OF FILE******/