test_perception.c
10.2 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
/**
********************************************************************
* @file test_perception.c
* @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 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.
*
*********************************************************************
*/
/* Includes ------------------------------------------------------------------*/
#include <utils/util_misc.h>
#include <widget_interaction_test/test_widget_interaction.h>
#include "test_perception.h"
#include "dji_logger.h"
#include "dji_platform.h"
/* Private constants ---------------------------------------------------------*/
#define TEST_PERCEPTION_SAVE_IMAGE_MAX_NUM 10
/* Private types -------------------------------------------------------------*/
typedef struct {
E_DjiPerceptionDirection direction;
char *name;
} T_DjiTestPerceptionDirectionName;
/* Private values -------------------------------------------------------------*/
static uint16_t s_perceptionImageCount = 0;
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"},
};
/* Private functions declaration ---------------------------------------------*/
static void DjiTest_PerceptionImageCallback(T_DjiPerceptionImageInfo imageInfo, uint8_t *imageRawBuffer,
uint32_t bufferLen);
static int32_t DjiTest_SaveImageData(char *filePath, const uint8_t *data, uint32_t len);
/* Exported functions definition ---------------------------------------------*/
T_DjiReturnCode DjiTest_PerceptionRunSample(E_DjiPerceptionDirection direction)
{
T_DjiReturnCode returnCode;
T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler();
T_DjiPerceptionCameraParametersPacket cameraParametersetersPacket = {0};
USER_LOG_INFO("Perception sample start");
DjiTest_WidgetLogAppend("Perception sample start");
USER_LOG_INFO("--> Step 1: Init Perception module");
DjiTest_WidgetLogAppend("--> Step 1: Init Perception module");
returnCode = DjiPerception_Init();
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("Perception init failed, error code: 0x%08X", returnCode);
goto out;
}
s_perceptionImageCount = 0;
USER_LOG_INFO("--> Step 2: Get stereo camera parameters\r\n");
DjiTest_WidgetLogAppend("--> Step 2: Get stereo camera parameters\r\n");
returnCode = DjiPerception_GetStereoCameraParameters(&cameraParametersetersPacket);
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("Get stereo camera parameters failed, error code: 0x%08X", returnCode);
goto out;
}
if (cameraParametersetersPacket.directionNum <= IMAGE_MAX_DIRECTION_NUM)
for (int i = 0; i < cameraParametersetersPacket.directionNum; i++) {
USER_LOG_INFO(" [%-05s] leftIntrinsics = {%f, %f, %f, %f, %f, %f, %f, %f, %f }",
directionName[cameraParametersetersPacket.cameraParameters[i].direction].name,
cameraParametersetersPacket.cameraParameters[i].leftIntrinsics[0],
cameraParametersetersPacket.cameraParameters[i].leftIntrinsics[1],
cameraParametersetersPacket.cameraParameters[i].leftIntrinsics[2],
cameraParametersetersPacket.cameraParameters[i].leftIntrinsics[3],
cameraParametersetersPacket.cameraParameters[i].leftIntrinsics[4],
cameraParametersetersPacket.cameraParameters[i].leftIntrinsics[5],
cameraParametersetersPacket.cameraParameters[i].leftIntrinsics[6],
cameraParametersetersPacket.cameraParameters[i].leftIntrinsics[7],
cameraParametersetersPacket.cameraParameters[i].leftIntrinsics[8]);
USER_LOG_INFO("[%-05s] rightIntrinsics = {%f, %f, %f, %f, %f, %f, %f, %f, %f }",
directionName[cameraParametersetersPacket.cameraParameters[i].direction].name,
cameraParametersetersPacket.cameraParameters[i].rightIntrinsics[0],
cameraParametersetersPacket.cameraParameters[i].rightIntrinsics[1],
cameraParametersetersPacket.cameraParameters[i].rightIntrinsics[2],
cameraParametersetersPacket.cameraParameters[i].rightIntrinsics[3],
cameraParametersetersPacket.cameraParameters[i].rightIntrinsics[4],
cameraParametersetersPacket.cameraParameters[i].rightIntrinsics[5],
cameraParametersetersPacket.cameraParameters[i].rightIntrinsics[6],
cameraParametersetersPacket.cameraParameters[i].rightIntrinsics[7],
cameraParametersetersPacket.cameraParameters[i].rightIntrinsics[8]);
USER_LOG_INFO("[%-05s] rotationLeftInRight = {%f, %f, %f, %f, %f, %f, %f, %f, %f }",
directionName[cameraParametersetersPacket.cameraParameters[i].direction].name,
cameraParametersetersPacket.cameraParameters[i].rotationLeftInRight[0],
cameraParametersetersPacket.cameraParameters[i].rotationLeftInRight[1],
cameraParametersetersPacket.cameraParameters[i].rotationLeftInRight[2],
cameraParametersetersPacket.cameraParameters[i].rotationLeftInRight[3],
cameraParametersetersPacket.cameraParameters[i].rotationLeftInRight[4],
cameraParametersetersPacket.cameraParameters[i].rotationLeftInRight[5],
cameraParametersetersPacket.cameraParameters[i].rotationLeftInRight[6],
cameraParametersetersPacket.cameraParameters[i].rotationLeftInRight[7],
cameraParametersetersPacket.cameraParameters[i].rotationLeftInRight[8]);
USER_LOG_INFO("[%-05s] translationLeftInRight = {%f, %f, %f }\r\n",
directionName[cameraParametersetersPacket.cameraParameters[i].direction].name,
cameraParametersetersPacket.cameraParameters[i].translationLeftInRight[0],
cameraParametersetersPacket.cameraParameters[i].translationLeftInRight[1],
cameraParametersetersPacket.cameraParameters[i].translationLeftInRight[2]);
osalHandler->TaskSleepMs(100);
}
USER_LOG_INFO("--> Step 3: Subscribe perception image\r\n");
DjiTest_WidgetLogAppend("--> Step 3: Subscribe perception image\r\n");
returnCode = DjiPerception_SubscribePerceptionImage(direction, DjiTest_PerceptionImageCallback);
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("Subscribe perception image failed, error code: 0x%08X", returnCode);
goto out;
}
osalHandler->TaskSleepMs(5000);
USER_LOG_INFO("--> Step 4: Unsubscribe perception image");
DjiTest_WidgetLogAppend("--> Step 4: Unsubscribe perception image");
returnCode = DjiPerception_UnsubscribePerceptionImage(direction);
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("Unsubscribe perception image failed, error code: 0x%08X", returnCode);
goto out;
}
USER_LOG_INFO("--> Step 5: Deinit Perception module");
DjiTest_WidgetLogAppend("--> Step 5: Deinit Perception module");
returnCode = DjiPerception_Deinit();
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("Perception deinit failed, error code: 0x%08X", returnCode);
goto out;
}
out:
USER_LOG_INFO("Perception sample end");
return returnCode;
}
/* Private functions definition-----------------------------------------------*/
static int32_t DjiTest_SaveImageData(char *filePath, const uint8_t *data, uint32_t len)
{
FILE *fp = NULL;
size_t size;
fp = fopen(filePath, "w+");
if (fp == NULL) {
return -1;
}
size = fwrite(data, 1, len, fp);
if (size != len) {
if (fp) {
fclose(fp);
}
return -1;
}
if (fp) {
fclose(fp);
}
return 0;
}
static void DjiTest_PerceptionImageCallback(T_DjiPerceptionImageInfo imageInfo, uint8_t *imageRawBuffer,
uint32_t bufferLen)
{
char fileName[256] = {0};
snprintf(fileName, sizeof(fileName), "./image_%s_%d.raw",
directionName[imageInfo.rawInfo.direction].name,
s_perceptionImageCount);
if (s_perceptionImageCount < TEST_PERCEPTION_SAVE_IMAGE_MAX_NUM) {
DjiTest_SaveImageData(fileName, imageRawBuffer, bufferLen);
USER_LOG_INFO(
"Save perception image to path: ${binary_execute_path}/image_%s_%d.raw, direction:%s, position:%d, size:%dx%d",
directionName[imageInfo.rawInfo.direction].name,
s_perceptionImageCount,
directionName[imageInfo.rawInfo.direction].name,
imageInfo.dataType,
imageInfo.rawInfo.width,
imageInfo.rawInfo.height);
s_perceptionImageCount++;
}
}
/****************** (C) COPYRIGHT DJI Innovations *****END OF FILE****/