Haoxiang Li | 830834b | 2020-03-05 15:54:34 -0800 | [diff] [blame^] | 1 | /* |
| 2 | * Copyright 2020 The Android Open Source Project |
| 3 | * |
| 4 | * Licensed under the Apache License, Version 2.0 (the "License"); |
| 5 | * you may not use this file except in compliance with the License. |
| 6 | * You may obtain a copy of the License at |
| 7 | * |
| 8 | * http://www.apache.org/licenses/LICENSE-2.0 |
| 9 | * |
| 10 | * Unless required by applicable law or agreed to in writing, software |
| 11 | * distributed under the License is distributed on an "AS IS" BASIS, |
| 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 13 | * See the License for the specific language governing permissions and |
| 14 | * limitations under the License. |
| 15 | */ |
| 16 | #define LOG_TAG "SurroundViewService" |
| 17 | |
| 18 | #include <android-base/logging.h> |
| 19 | #include <android/hardware_buffer.h> |
| 20 | #include <android/hidl/memory/1.0/IMemory.h> |
| 21 | #include <hidlmemory/mapping.h> |
| 22 | #include <set> |
| 23 | #include <utils/SystemClock.h> |
| 24 | |
| 25 | #include "SurroundView3dSession.h" |
| 26 | #include "sv_3d_params.h" |
| 27 | |
| 28 | using ::android::hidl::memory::V1_0::IMemory; |
| 29 | using ::android::hardware::hidl_memory; |
| 30 | |
| 31 | namespace android { |
| 32 | namespace hardware { |
| 33 | namespace automotive { |
| 34 | namespace sv { |
| 35 | namespace V1_0 { |
| 36 | namespace implementation { |
| 37 | |
| 38 | static const char kGrayColor = 128; |
| 39 | static const int kNumChannels = 4; |
| 40 | |
| 41 | SurroundView3dSession::SurroundView3dSession() : |
| 42 | mStreamState(STOPPED){ |
| 43 | mEvsCameraIds = {"0" , "1", "2", "3"}; |
| 44 | } |
| 45 | |
| 46 | // Methods from ::android::hardware::automotive::sv::V1_0::ISurroundViewSession. |
| 47 | Return<SvResult> SurroundView3dSession::startStream( |
| 48 | const sp<ISurroundViewStream>& stream) { |
| 49 | LOG(DEBUG) << __FUNCTION__; |
| 50 | scoped_lock<mutex> lock(mAccessLock); |
| 51 | |
| 52 | if (!mIsInitialized && !initialize()) { |
| 53 | LOG(ERROR) << "There is an error while initializing the use case. " |
| 54 | << "Exiting"; |
| 55 | return SvResult::INTERNAL_ERROR; |
| 56 | } |
| 57 | |
| 58 | if (mStreamState != STOPPED) { |
| 59 | LOG(ERROR) << "Ignoring startVideoStream call when a stream is " |
| 60 | << "already running."; |
| 61 | return SvResult::INTERNAL_ERROR; |
| 62 | } |
| 63 | |
| 64 | if (mViews.empty()) { |
| 65 | LOG(ERROR) << "No views have been set for current Surround View" |
| 66 | << "3d Session. Please call setViews before starting" |
| 67 | << "the stream."; |
| 68 | return SvResult::VIEW_NOT_SET; |
| 69 | } |
| 70 | |
| 71 | if (stream == nullptr) { |
| 72 | LOG(ERROR) << "The input stream is invalid"; |
| 73 | return SvResult::INTERNAL_ERROR; |
| 74 | } |
| 75 | mStream = stream; |
| 76 | |
| 77 | LOG(DEBUG) << "Notify SvEvent::STREAM_STARTED"; |
| 78 | mStream->notify(SvEvent::STREAM_STARTED); |
| 79 | |
| 80 | // Start the frame generation thread |
| 81 | mStreamState = RUNNING; |
| 82 | mCaptureThread = thread([this](){ |
| 83 | generateFrames(); |
| 84 | }); |
| 85 | |
| 86 | return SvResult::OK; |
| 87 | } |
| 88 | |
| 89 | Return<void> SurroundView3dSession::stopStream() { |
| 90 | LOG(DEBUG) << __FUNCTION__; |
| 91 | unique_lock <mutex> lock(mAccessLock); |
| 92 | |
| 93 | if (mStreamState == RUNNING) { |
| 94 | // Tell the GenerateFrames loop we want it to stop |
| 95 | mStreamState = STOPPING; |
| 96 | |
| 97 | // Block outside the mutex until the "stop" flag has been acknowledged |
| 98 | // We won't send any more frames, but the client might still get some |
| 99 | // already in flight |
| 100 | LOG(DEBUG) << __FUNCTION__ << ": Waiting for stream thread to end..."; |
| 101 | lock.unlock(); |
| 102 | mCaptureThread.join(); |
| 103 | lock.lock(); |
| 104 | |
| 105 | mStreamState = STOPPED; |
| 106 | mStream = nullptr; |
| 107 | LOG(DEBUG) << "Stream marked STOPPED."; |
| 108 | } |
| 109 | |
| 110 | return {}; |
| 111 | } |
| 112 | |
| 113 | Return<void> SurroundView3dSession::doneWithFrames( |
| 114 | const SvFramesDesc& svFramesDesc){ |
| 115 | LOG(DEBUG) << __FUNCTION__; |
| 116 | scoped_lock <mutex> lock(mAccessLock); |
| 117 | |
| 118 | framesRecord.inUse = false; |
| 119 | |
| 120 | (void)svFramesDesc; |
| 121 | return {}; |
| 122 | } |
| 123 | |
| 124 | // Methods from ISurroundView3dSession follow. |
| 125 | Return<SvResult> SurroundView3dSession::setViews( |
| 126 | const hidl_vec<View3d>& views) { |
| 127 | LOG(DEBUG) << __FUNCTION__; |
| 128 | scoped_lock <mutex> lock(mAccessLock); |
| 129 | |
| 130 | mViews.resize(views.size()); |
| 131 | for (int i=0; i<views.size(); i++) { |
| 132 | mViews[i] = views[i]; |
| 133 | } |
| 134 | |
| 135 | return SvResult::OK; |
| 136 | } |
| 137 | |
| 138 | Return<SvResult> SurroundView3dSession::set3dConfig(const Sv3dConfig& sv3dConfig) { |
| 139 | LOG(DEBUG) << __FUNCTION__; |
| 140 | scoped_lock <mutex> lock(mAccessLock); |
| 141 | |
| 142 | if (sv3dConfig.width <=0 || sv3dConfig.width > 4096) { |
| 143 | LOG(WARNING) << "The width of 3d config is out of the range (0, 4096]" |
| 144 | << "Ignored!"; |
| 145 | return SvResult::INVALID_ARG; |
| 146 | } |
| 147 | |
| 148 | if (sv3dConfig.height <=0 || sv3dConfig.height > 4096) { |
| 149 | LOG(WARNING) << "The height of 3d config is out of the range (0, 4096]" |
| 150 | << "Ignored!"; |
| 151 | return SvResult::INVALID_ARG; |
| 152 | } |
| 153 | |
| 154 | mConfig.width = sv3dConfig.width; |
| 155 | mConfig.height = sv3dConfig.height; |
| 156 | mConfig.carDetails = sv3dConfig.carDetails; |
| 157 | |
| 158 | if (mStream != nullptr) { |
| 159 | LOG(DEBUG) << "Notify SvEvent::CONFIG_UPDATED"; |
| 160 | mStream->notify(SvEvent::CONFIG_UPDATED); |
| 161 | } |
| 162 | |
| 163 | return SvResult::OK; |
| 164 | } |
| 165 | |
| 166 | Return<void> SurroundView3dSession::get3dConfig(get3dConfig_cb _hidl_cb) { |
| 167 | LOG(DEBUG) << __FUNCTION__; |
| 168 | |
| 169 | _hidl_cb(mConfig); |
| 170 | return {}; |
| 171 | } |
| 172 | |
| 173 | bool VerifyOverlayData(const OverlaysData& overlaysData) { |
| 174 | // Check size of shared memory matches overlaysMemoryDesc. |
| 175 | const int kVertexSize = 16; |
| 176 | const int kIdSize = 2; |
| 177 | int memDescSize = 0; |
| 178 | for (auto& overlayMemDesc : overlaysData.overlaysMemoryDesc) { |
| 179 | memDescSize += kIdSize + kVertexSize * overlayMemDesc.verticesCount; |
| 180 | } |
| 181 | if (memDescSize != overlaysData.overlaysMemory.size()) { |
| 182 | LOG(ERROR) << "shared memory and overlaysMemoryDesc size mismatch."; |
| 183 | return false; |
| 184 | } |
| 185 | |
| 186 | // Map memory. |
| 187 | sp<IMemory> pSharedMemory = mapMemory(overlaysData.overlaysMemory); |
| 188 | if(pSharedMemory == nullptr) { |
| 189 | LOG(ERROR) << "mapMemory failed."; |
| 190 | return false; |
| 191 | } |
| 192 | |
| 193 | // Get Data pointer. |
| 194 | uint8_t* pData = static_cast<uint8_t*>( |
| 195 | static_cast<void*>(pSharedMemory->getPointer())); |
| 196 | if (pData == nullptr) { |
| 197 | LOG(ERROR) << "Shared memory getPointer() failed."; |
| 198 | return false; |
| 199 | } |
| 200 | |
| 201 | int idOffset = 0; |
| 202 | set<uint16_t> overlayIdSet; |
| 203 | for (auto& overlayMemDesc : overlaysData.overlaysMemoryDesc) { |
| 204 | |
| 205 | if (overlayIdSet.find(overlayMemDesc.id) != overlayIdSet.end()) { |
| 206 | LOG(ERROR) << "Duplicate id within memory descriptor."; |
| 207 | return false; |
| 208 | } |
| 209 | overlayIdSet.insert(overlayMemDesc.id); |
| 210 | |
| 211 | if(overlayMemDesc.verticesCount < 3) { |
| 212 | LOG(ERROR) << "Less than 3 vertices."; |
| 213 | return false; |
| 214 | } |
| 215 | |
| 216 | if (overlayMemDesc.overlayPrimitive == OverlayPrimitive::TRIANGLES && |
| 217 | overlayMemDesc.verticesCount % 3 != 0) { |
| 218 | LOG(ERROR) << "Triangles primitive does not have vertices " |
| 219 | << "multiple of 3."; |
| 220 | return false; |
| 221 | } |
| 222 | |
| 223 | const uint16_t overlayId = *((uint16_t*)(pData + idOffset)); |
| 224 | |
| 225 | if (overlayId != overlayMemDesc.id) { |
| 226 | LOG(ERROR) << "Overlay id mismatch " |
| 227 | << overlayId |
| 228 | << ", " |
| 229 | << overlayMemDesc.id; |
| 230 | return false; |
| 231 | } |
| 232 | |
| 233 | idOffset += kIdSize + (kVertexSize * overlayMemDesc.verticesCount); |
| 234 | } |
| 235 | |
| 236 | return true; |
| 237 | } |
| 238 | |
| 239 | // TODO(b/150412555): the overlay related methods are incomplete. |
| 240 | Return<SvResult> SurroundView3dSession::updateOverlays( |
| 241 | const OverlaysData& overlaysData) { |
| 242 | |
| 243 | if(!VerifyOverlayData(overlaysData)) { |
| 244 | LOG(ERROR) << "VerifyOverlayData failed."; |
| 245 | return SvResult::INVALID_ARG; |
| 246 | } |
| 247 | |
| 248 | return SvResult::OK; |
| 249 | } |
| 250 | |
| 251 | Return<void> SurroundView3dSession::projectCameraPointsTo3dSurface( |
| 252 | const hidl_vec<Point2dInt>& cameraPoints, |
| 253 | const hidl_string& cameraId, |
| 254 | projectCameraPointsTo3dSurface_cb _hidl_cb) { |
| 255 | |
| 256 | vector<Point3dFloat> points3d; |
| 257 | bool cameraIdFound = false; |
| 258 | for (auto& evsCameraId : mEvsCameraIds) { |
| 259 | if (cameraId == evsCameraId) { |
| 260 | cameraIdFound = true; |
| 261 | LOG(INFO) << "Camera id found."; |
| 262 | break; |
| 263 | } |
| 264 | } |
| 265 | |
| 266 | if (!cameraIdFound) { |
| 267 | LOG(ERROR) << "Camera id not found."; |
| 268 | _hidl_cb(points3d); |
| 269 | return {}; |
| 270 | } |
| 271 | |
| 272 | for (const auto& cameraPoint : cameraPoints) { |
| 273 | Point3dFloat point3d; |
| 274 | point3d.isValid = (cameraPoint.x >= 0 |
| 275 | && cameraPoint.x < mConfig.width |
| 276 | && cameraPoint.y >= 0 |
| 277 | && cameraPoint.y < mConfig.height); |
| 278 | if (!point3d.isValid) { |
| 279 | LOG(WARNING) << "Camera point out of bounds."; |
| 280 | } |
| 281 | points3d.push_back(point3d); |
| 282 | } |
| 283 | _hidl_cb(points3d); |
| 284 | return {}; |
| 285 | } |
| 286 | |
| 287 | void SurroundView3dSession::generateFrames() { |
| 288 | int sequenceId = 0; |
| 289 | |
| 290 | // TODO(b/150412555): do not use the setViews for frames generation |
| 291 | // since there is a discrepancy between the HIDL APIs and core lib APIs. |
| 292 | vector<vector<float>> matrix; |
| 293 | matrix.resize(4); |
| 294 | for (int i=0; i<4; i++) { |
| 295 | matrix[i].resize(4); |
| 296 | } |
| 297 | |
| 298 | while(true) { |
| 299 | { |
| 300 | scoped_lock<mutex> lock(mAccessLock); |
| 301 | |
| 302 | if (mStreamState != RUNNING) { |
| 303 | // Break out of our main thread loop |
| 304 | LOG(INFO) << "StreamState does not equal to RUNNING. " |
| 305 | << "Exiting the loop"; |
| 306 | break; |
| 307 | } |
| 308 | |
| 309 | if (mOutputWidth != mConfig.width |
| 310 | || mOutputHeight != mConfig.height) { |
| 311 | LOG(DEBUG) << "Config changed. Re-allocate memory. " |
| 312 | << "Old width: " |
| 313 | << mOutputWidth |
| 314 | << ", old height: " |
| 315 | << mOutputHeight |
| 316 | << "; New width: " |
| 317 | << mConfig.width |
| 318 | << ", new height: " |
| 319 | << mConfig.height; |
| 320 | delete[] static_cast<char*>(mOutputPointer.data_pointer); |
| 321 | mOutputWidth = mConfig.width; |
| 322 | mOutputHeight = mConfig.height; |
| 323 | mOutputPointer.height = mOutputHeight; |
| 324 | mOutputPointer.width = mOutputWidth; |
| 325 | mOutputPointer.format = Format::RGBA; |
| 326 | mOutputPointer.data_pointer = |
| 327 | new char[mOutputHeight * mOutputWidth * kNumChannels]; |
| 328 | |
| 329 | if (!mOutputPointer.data_pointer) { |
| 330 | LOG(ERROR) << "Memory allocation failed. Exiting."; |
| 331 | break; |
| 332 | } |
| 333 | |
| 334 | Size2dInteger size = Size2dInteger(mOutputWidth, mOutputHeight); |
| 335 | mSurroundView->Update3dOutputResolution(size); |
| 336 | |
| 337 | mSvTexture = new GraphicBuffer(mOutputWidth, |
| 338 | mOutputHeight, |
| 339 | HAL_PIXEL_FORMAT_RGBA_8888, |
| 340 | 1, |
| 341 | GRALLOC_USAGE_HW_TEXTURE, |
| 342 | "SvTexture"); |
| 343 | if (mSvTexture->initCheck() == OK) { |
| 344 | LOG(INFO) << "Successfully allocated Graphic Buffer"; |
| 345 | } else { |
| 346 | LOG(ERROR) << "Failed to allocate Graphic Buffer"; |
| 347 | break; |
| 348 | } |
| 349 | } |
| 350 | } |
| 351 | |
| 352 | // TODO(b/150412555): use hard-coded views for now. Change view every 10 |
| 353 | // frames. |
| 354 | int recViewId = sequenceId / 10 % 16; |
| 355 | for (int i=0; i<4; i++) |
| 356 | for (int j=0; j<4; j++) { |
| 357 | matrix[i][j] = kRecViews[recViewId][i*4+j]; |
| 358 | } |
| 359 | |
| 360 | if (mSurroundView->Get3dSurroundView( |
| 361 | mInputPointers, matrix, &mOutputPointer)) { |
| 362 | LOG(INFO) << "Get3dSurroundView succeeded"; |
| 363 | } else { |
| 364 | LOG(ERROR) << "Get3dSurroundView failed. " |
| 365 | << "Using memset to initialize to gray."; |
| 366 | memset(mOutputPointer.data_pointer, kGrayColor, |
| 367 | mOutputHeight * mOutputWidth * kNumChannels); |
| 368 | } |
| 369 | |
| 370 | void* textureDataPtr = nullptr; |
| 371 | mSvTexture->lock(GRALLOC_USAGE_SW_WRITE_OFTEN |
| 372 | | GRALLOC_USAGE_SW_READ_NEVER, |
| 373 | &textureDataPtr); |
| 374 | if (!textureDataPtr) { |
| 375 | LOG(ERROR) << "Failed to gain write access to GraphicBuffer!"; |
| 376 | break; |
| 377 | } |
| 378 | |
| 379 | // Note: there is a chance that the stride of the texture is not the |
| 380 | // same as the width. For example, when the input frame is 1920 * 1080, |
| 381 | // the width is 1080, but the stride is 2048. So we'd better copy the |
| 382 | // data line by line, instead of single memcpy. |
| 383 | uint8_t* writePtr = static_cast<uint8_t*>(textureDataPtr); |
| 384 | uint8_t* readPtr = static_cast<uint8_t*>(mOutputPointer.data_pointer); |
| 385 | const int readStride = mOutputWidth * kNumChannels; |
| 386 | const int writeStride = mSvTexture->getStride() * kNumChannels; |
| 387 | if (readStride == writeStride) { |
| 388 | memcpy(writePtr, readPtr, readStride * mSvTexture->getHeight()); |
| 389 | } else { |
| 390 | for (int i=0; i<mSvTexture->getHeight(); i++) { |
| 391 | memcpy(writePtr, readPtr, readStride); |
| 392 | writePtr = writePtr + writeStride; |
| 393 | readPtr = readPtr + readStride; |
| 394 | } |
| 395 | } |
| 396 | LOG(INFO) << "memcpy finished!"; |
| 397 | mSvTexture->unlock(); |
| 398 | |
| 399 | ANativeWindowBuffer* buffer = mSvTexture->getNativeBuffer(); |
| 400 | LOG(DEBUG) << "ANativeWindowBuffer->handle: " << buffer->handle; |
| 401 | |
| 402 | framesRecord.frames.svBuffers.resize(1); |
| 403 | SvBuffer& svBuffer = framesRecord.frames.svBuffers[0]; |
| 404 | svBuffer.viewId = 0; |
| 405 | svBuffer.hardwareBuffer.nativeHandle = buffer->handle; |
| 406 | AHardwareBuffer_Desc* pDesc = |
| 407 | reinterpret_cast<AHardwareBuffer_Desc *>( |
| 408 | &svBuffer.hardwareBuffer.description); |
| 409 | pDesc->width = mOutputWidth; |
| 410 | pDesc->height = mOutputHeight; |
| 411 | pDesc->layers = 1; |
| 412 | pDesc->usage = GRALLOC_USAGE_HW_TEXTURE; |
| 413 | pDesc->stride = mSvTexture->getStride(); |
| 414 | pDesc->format = HAL_PIXEL_FORMAT_RGBA_8888; |
| 415 | framesRecord.frames.timestampNs = elapsedRealtimeNano(); |
| 416 | framesRecord.frames.sequenceId = sequenceId++; |
| 417 | |
| 418 | { |
| 419 | scoped_lock<mutex> lock(mAccessLock); |
| 420 | |
| 421 | if (framesRecord.inUse) { |
| 422 | LOG(DEBUG) << "Notify SvEvent::FRAME_DROPPED"; |
| 423 | mStream->notify(SvEvent::FRAME_DROPPED); |
| 424 | } else { |
| 425 | framesRecord.inUse = true; |
| 426 | mStream->receiveFrames(framesRecord.frames); |
| 427 | } |
| 428 | } |
| 429 | } |
| 430 | |
| 431 | // If we've been asked to stop, send an event to signal the actual end of stream |
| 432 | LOG(DEBUG) << "Notify SvEvent::STREAM_STOPPED"; |
| 433 | mStream->notify(SvEvent::STREAM_STOPPED); |
| 434 | } |
| 435 | |
| 436 | bool SurroundView3dSession::initialize() { |
| 437 | lock_guard<mutex> lock(mAccessLock, adopt_lock); |
| 438 | |
| 439 | // TODO(b/150412555): ask core-lib team to add API description for "create" |
| 440 | // method in the .h file. |
| 441 | // The create method will never return a null pointer based the API |
| 442 | // description. |
| 443 | mSurroundView = unique_ptr<SurroundView>(Create()); |
| 444 | |
| 445 | mSurroundView->SetStaticData(GetCameras(), Get2dParams(), Get3dParams(), |
| 446 | GetUndistortionScales(), GetBoundingBox()); |
| 447 | |
| 448 | // TODO(b/150412555): remove after EVS camera is used |
| 449 | mInputPointers = mSurroundView->ReadImages( |
| 450 | "/etc/automotive/sv/cam0.png", |
| 451 | "/etc/automotive/sv/cam1.png", |
| 452 | "/etc/automotive/sv/cam2.png", |
| 453 | "/etc/automotive/sv/cam3.png"); |
| 454 | if (mInputPointers.size() == 4 |
| 455 | && mInputPointers[0].cpu_data_pointer != nullptr) { |
| 456 | LOG(INFO) << "ReadImages succeeded"; |
| 457 | } else { |
| 458 | LOG(ERROR) << "Failed to read images"; |
| 459 | return false; |
| 460 | } |
| 461 | |
| 462 | mOutputWidth = Get3dParams().resolution.width; |
| 463 | mOutputHeight = Get3dParams().resolution.height; |
| 464 | |
| 465 | mConfig.width = mOutputWidth; |
| 466 | mConfig.height = mOutputHeight; |
| 467 | mConfig.carDetails = SvQuality::HIGH; |
| 468 | |
| 469 | mOutputPointer.height = mOutputHeight; |
| 470 | mOutputPointer.width = mOutputWidth; |
| 471 | mOutputPointer.format = Format::RGBA; |
| 472 | mOutputPointer.data_pointer = new char[ |
| 473 | mOutputHeight * mOutputWidth * kNumChannels]; |
| 474 | |
| 475 | if (!mOutputPointer.data_pointer) { |
| 476 | LOG(ERROR) << "Memory allocation failed. Exiting."; |
| 477 | return false; |
| 478 | } |
| 479 | |
| 480 | mSvTexture = new GraphicBuffer(mOutputWidth, |
| 481 | mOutputHeight, |
| 482 | HAL_PIXEL_FORMAT_RGBA_8888, |
| 483 | 1, |
| 484 | GRALLOC_USAGE_HW_TEXTURE, |
| 485 | "SvTexture"); |
| 486 | |
| 487 | if (mSvTexture->initCheck() == OK) { |
| 488 | LOG(INFO) << "Successfully allocated Graphic Buffer"; |
| 489 | } else { |
| 490 | LOG(ERROR) << "Failed to allocate Graphic Buffer"; |
| 491 | return false; |
| 492 | } |
| 493 | |
| 494 | if (mSurroundView->Start3dPipeline()) { |
| 495 | LOG(INFO) << "Start3dPipeline succeeded"; |
| 496 | } else { |
| 497 | LOG(ERROR) << "Start3dPipeline failed"; |
| 498 | return false; |
| 499 | } |
| 500 | |
| 501 | mIsInitialized = true; |
| 502 | return true; |
| 503 | } |
| 504 | |
| 505 | } // namespace implementation |
| 506 | } // namespace V1_0 |
| 507 | } // namespace sv |
| 508 | } // namespace automotive |
| 509 | } // namespace hardware |
| 510 | } // namespace android |
| 511 | |