31#ifndef DW_FRAMEWORK_CHANNEL_DW_PACKET_IMPL_HPP_
32#define DW_FRAMEWORK_CHANNEL_DW_PACKET_IMPL_HPP_
35#include <dw/roadcast/base_types/RoadCastBaseTypes.h>
51 auto* props = specimen.
getData<dwImageProperties>();
60 throw Exception(DW_INVALID_ARGUMENT,
"Invalid reference data provided.");
88 if (m_prop.type != DW_IMAGE_CPU)
92 FRWK_CHECK_DW_ERROR(dwImageStreamer_initialize(&m_streamerToCPU, &m_prop, DW_IMAGE_CPU, ctx));
96 dwImageProperties cpuProp = m_prop;
97 cpuProp.type = DW_IMAGE_CPU;
99 if (m_prop.type != DW_IMAGE_CPU)
101 FRWK_CHECK_DW_ERROR(dwImageStreamer_initialize(&m_streamerFromCPU, &cpuProp, m_prop.type, ctx));
105 FRWK_CHECK_DW_ERROR(dwImage_getDataLayout(&m_elementSize, &m_planeCount, m_planeChannelCount, m_planeSize, &cpuProp));
108 size_t bufferSize =
sizeof(dwImageCPU);
109 for (
size_t i = 0; i < m_planeCount; i++)
111 size_t planeRowBytes = m_elementSize * m_planeSize[i].x * m_planeChannelCount[i];
112 bufferSize += planeRowBytes * m_planeSize[i].y;
115 initBuffer(bufferSize);
120 if (m_prop.type != DW_IMAGE_CPU)
122 dwImage_destroy(m_imageHandleCPU);
123 dwImageStreamer_release(m_streamerToCPU);
124 dwImageStreamer_release(m_streamerFromCPU);
129 static size_t serializeImage(dwImageCPU* cpuImage,
unsigned char* buffer_start,
size_t bufferSize,
130 size_t planeCount,
size_t elementSize, uint32_t planeChannelCount[],
131 dwVector2ui planeSize[])
134 memcpy(buffer_start, cpuImage,
sizeof(dwImageCPU));
136 size_t bufferOffset =
sizeof(dwImageCPU);
137 for (
size_t i = 0; i < planeCount; i++)
139 size_t planeOffset = 0U;
140 size_t pitch = cpuImage->pitch[i];
141 size_t planeRowBytes = elementSize * planeSize[i].x * planeChannelCount[i];
142 for (
size_t j = 0; j < planeSize[i].y; j++)
144 if (bufferOffset + planeRowBytes > bufferSize)
146 throw Exception(DW_OUT_OF_BOUNDS,
"ChannelPacket<dwImageHandle_t>: serialize: send packet size, buffer size mismatch.");
148 memcpy(buffer_start + bufferOffset, cpuImage->data[i] + planeOffset, planeRowBytes);
149 planeOffset += pitch;
150 bufferOffset += planeRowBytes;
156 static void deserializeImage(dwImageHandle_t copyToImage,
unsigned char* buffer_start,
size_t bufferSize,
157 size_t planeCount,
size_t elementSize, uint32_t planeChannelCount[],
158 dwVector2ui planeSize[])
162 dwImageCPU* cpuImage =
nullptr;
165 size_t bufferOffset =
sizeof(dwImageCPU);
166 for (
size_t i = 0; i < planeCount; i++)
168 size_t planeOffset = 0U;
169 size_t pitch = cpuImage->pitch[i];
170 size_t planeRowBytes = elementSize * planeSize[i].x * planeChannelCount[i];
171 for (
size_t j = 0; j < planeSize[i].y; j++)
173 if (bufferOffset + planeRowBytes > bufferSize)
175 throw Exception(DW_OUT_OF_BOUNDS,
"ChannelPacket<dwImageHandle_t>: deserialize: recieve packet size, buffer size mismatch.");
177 memcpy(cpuImage->data[i] + planeOffset, buffer_start + bufferOffset, planeRowBytes);
178 planeOffset += pitch;
179 bufferOffset += planeRowBytes;
187 dwImageCPU* cpuImage =
nullptr;
188 dwImageHandle_t recvImage =
nullptr;
189 dwImageHandle_t* returnedImage =
nullptr;
191 if (m_prop.type != DW_IMAGE_CPU)
203 serializeImage(cpuImage, m_buffer.get(), m_bufferSize, m_planeCount, m_elementSize, m_planeChannelCount, m_planeSize);
205 if (m_prop.type != DW_IMAGE_CPU)
215 dwImageHandle_t copyToImage = m_dispatchImage;
216 if (m_prop.type != DW_IMAGE_CPU)
218 copyToImage = m_imageHandleCPU;
222 dwImageCPU recvImage{};
223 memcpy(&recvImage, m_buffer.get(),
sizeof(dwImageCPU));
225 deserializeImage(copyToImage, m_buffer.get(), m_bufferSize, m_planeCount, m_elementSize, m_planeChannelCount, m_planeSize);
227 if (m_prop.type != DW_IMAGE_CPU)
229 dwImageHandle_t recvImageHandle =
nullptr;
230 dwImageHandle_t* returnedImage =
nullptr;
233 FRWK_CHECK_DW_ERROR(dwImageStreamer_consumerReceive(&recvImageHandle, 0, m_streamerFromCPU));
235 dwImage_copyConvert(m_dispatchImage, recvImageHandle, m_ctx);
236 dwImage_setMetaData(&recvImage.prop.meta, m_dispatchImage);
237 dwImage_setTimestamp(recvImage.timestamp_us, m_dispatchImage);
245 dwImageHandle_t m_imageHandleCPU = DW_NULL_HANDLE;
247 size_t m_elementSize = 0;
248 size_t m_planeCount = 0;
249 uint32_t m_planeChannelCount[DW_MAX_IMAGE_PLANES]{};
250 dwVector2ui m_planeSize[DW_MAX_IMAGE_PLANES]{};
252 dwImageStreamerHandle_t m_streamerToCPU = DW_NULL_HANDLE;
253 dwImageStreamerHandle_t m_streamerFromCPU = DW_NULL_HANDLE;
255 dwContextHandle_t m_ctx = DW_NULL_HANDLE;
269 m_allocation = std::make_unique<uint8_t[]>(props->size);
275 throw Exception(DW_INVALID_ARGUMENT,
"Invalid reference data provided.");
298 m_objectSize =
sizeof(uint8_t);
299 m_maxCount = m_data.size;
305 initBuffer(m_headerSize + m_maxCount * m_objectSize);
312 memcpy(m_buffer.get(), &m_data, m_headerSize);
315 memcpy(m_buffer.get() + m_headerSize, m_data.data, m_data.size * m_objectSize);
322 memcpy(&m_data, m_buffer.get(), m_headerSize);
326 m_data.data = m_buffer.get() + m_headerSize;
341 auto* props = specimen.
getData<dwPyramidImageProperties>();
350 throw Exception(DW_INVALID_ARGUMENT,
"Invalid reference data provided.");
378 size_t bufferSize =
sizeof(dwImageCPU) * m_props.levelCount;
380 for (uint32_t level = 0; level < m_props.levelCount; ++level)
382 dwImageProperties& prop = m_props.levelProps[level];
384 dwImageProperties cpuProp = prop;
385 cpuProp.type = DW_IMAGE_CPU;
388 FRWK_CHECK_DW_ERROR(dwImageStreamer_initialize(&m_streamerToCPU[level], &prop, DW_IMAGE_CPU, ctx));
389 FRWK_CHECK_DW_ERROR(dwImageStreamer_initialize(&m_streamerFromCPU[level], &cpuProp, prop.type, ctx));
391 size_t elementSize = 0;
392 uint32_t planeChannelCount[DW_MAX_IMAGE_PLANES]{};
393 dwVector2ui planeSize[DW_MAX_IMAGE_PLANES]{};
395 FRWK_CHECK_DW_ERROR(dwImage_getDataLayout(&elementSize, &m_planeCount[level], planeChannelCount, planeSize, &cpuProp));
398 for (
size_t i = 0; i < m_planeCount[level]; i++)
400 bufferSize += (elementSize * planeSize[i].x * planeChannelCount[i]) * planeSize[i].y;
404 m_pyramidImage.levelCount = m_props.levelCount;
405 initBuffer(bufferSize);
410 for (uint32_t level = 0; level < m_pyramidImage.levelCount; ++level)
412 dwImage_destroy(m_imageHandleCPU[level]);
413 dwImageStreamer_release(m_streamerToCPU[level]);
414 dwImageStreamer_release(m_streamerFromCPU[level]);
423 for (uint32_t i = 0; i < m_pyramidImage.levelCount; i++)
425 dwImageHandle_t recvImage =
nullptr;
426 dwImageHandle_t* returnedImage =
nullptr;
428 FRWK_CHECK_DW_ERROR(dwImageStreamer_producerSend(m_pyramidImage.levelImages[i], m_streamerToCPU[i]));
431 dwImageCPU* cpuImage =
nullptr;
434 memcpy(m_buffer.get() + written, cpuImage,
sizeof(dwImageCPU));
435 written +=
sizeof(dwImageCPU);
437 for (
size_t j = 0; j < m_planeCount[i]; j++)
439 size_t planeSize = cpuImage->pitch[j] * cpuImage->prop.height;
440 memcpy(m_buffer.get() + written, cpuImage->data[j], planeSize);
441 written += planeSize;
454 for (uint32_t i = 0; i < m_pyramidImage.levelCount; i++)
456 dwImageCPU recvImage{};
457 memcpy(&recvImage, m_buffer.get() + read,
sizeof(dwImageCPU));
458 read +=
sizeof(dwImageCPU);
460 dwImageCPU* cpuImage =
nullptr;
463 for (
size_t j = 0; j < m_planeCount[i]; j++)
465 size_t planeSize = cpuImage->pitch[j] * cpuImage->prop.height;
466 memcpy(cpuImage->data[j], m_buffer.get() + read, planeSize);
470 dwImageHandle_t recvImageHandle =
nullptr;
471 dwImageHandle_t* returnedImage =
nullptr;
473 FRWK_CHECK_DW_ERROR(dwImageStreamer_producerSend(m_imageHandleCPU[i], m_streamerFromCPU[i]));
474 FRWK_CHECK_DW_ERROR(dwImageStreamer_consumerReceive(&recvImageHandle, 0, m_streamerFromCPU[i]));
476 dwImage_copyConvert(m_pyramidImage.levelImages[i], recvImageHandle, m_ctx);
477 dwImage_setMetaData(&recvImage.prop.meta, m_pyramidImage.levelImages[i]);
478 dwImage_setTimestamp(recvImage.timestamp_us, m_pyramidImage.levelImages[i]);
480 FRWK_CHECK_DW_ERROR(dwImageStreamer_consumerReturn(&recvImageHandle, m_streamerFromCPU[i]));
481 FRWK_CHECK_DW_ERROR(dwImageStreamer_producerReturn(returnedImage, 0, m_streamerFromCPU[i]));
486 size_t m_planeCount[DW_PYRAMID_LEVEL_MAX_COUNT]{0};
487 dwImageStreamerHandle_t m_streamerToCPU[DW_PYRAMID_LEVEL_MAX_COUNT]{};
488 dwImageStreamerHandle_t m_streamerFromCPU[DW_PYRAMID_LEVEL_MAX_COUNT]{};
489 dwImageHandle_t m_imageHandleCPU[DW_PYRAMID_LEVEL_MAX_COUNT]{};
490 dwContextHandle_t m_ctx = DW_NULL_HANDLE;
508 throw Exception(DW_INVALID_ARGUMENT,
"Invalid reference data provided.");
534 initBuffer(m_ncc.size);
541 FRWK_CHECK_CUDA_ERROR(cudaMemcpy(m_buffer.get(), m_ncc.d_nccScores, m_ncc.size, cudaMemcpyDeviceToHost));
549 FRWK_CHECK_CUDA_ERROR(cudaMemcpy(m_ncc.d_nccScores, m_buffer.get(), m_bufferSize, cudaMemcpyHostToDevice));
559 auto* props = specimen.
getData<dwFeatureArray>();
566 if (ret != DW_SUCCESS)
568 throw Exception(DW_BAD_ALLOC,
"FeatureArrayPacket: cannot allocate packet");
574 throw Exception(DW_INVALID_ARGUMENT,
"Invalid reference data provided.");
601 m_propIndex[0] =
sizeof(dwFeatureArray);
602 m_propIndex[1] = m_propIndex[0] +
sizeof(dwFeature2DStatus) * m_featureArray.maxFeatures;
603 m_propIndex[2] = m_propIndex[1] +
sizeof(uint32_t) * m_featureArray.maxFeatures;
604 m_propIndex[3] = m_propIndex[2] +
sizeof(float32_t) * m_featureArray.maxFeatures;
605 m_propIndex[4] = m_propIndex[3] +
sizeof(uint32_t) * m_featureArray.maxFeatures;
606 m_propIndex[5] = m_propIndex[4] +
sizeof(uint32_t) * m_featureArray.maxFeatures;
607 m_propIndex[6] = m_propIndex[5] +
sizeof(dwVector2f) * m_featureArray.maxFeatures;
608 m_propIndex[7] = m_propIndex[6] +
sizeof(uint32_t);
609 m_propIndex[8] = m_propIndex[7] +
sizeof(uint32_t);
611 initBuffer(m_propIndex[8]);
617 static_assert(
sizeof(dwFeatureArray) == 80,
"dwFeatureArray size has changed, update serialization");
619 memcpy(m_buffer.get(), &m_featureArray, m_propIndex[0]);
621 if (m_featureArray.memoryType == DW_MEMORY_TYPE_CUDA)
624 FRWK_CHECK_CUDA_ERROR(cudaMemcpy(m_buffer.get() + m_propIndex[0], m_featureArray.statuses, m_propIndex[1] - m_propIndex[0], cudaMemcpyDeviceToHost));
625 FRWK_CHECK_CUDA_ERROR(cudaMemcpy(m_buffer.get() + m_propIndex[1], m_featureArray.ages, m_propIndex[2] - m_propIndex[1], cudaMemcpyDeviceToHost));
626 FRWK_CHECK_CUDA_ERROR(cudaMemcpy(m_buffer.get() + m_propIndex[2], m_featureArray.scales, m_propIndex[3] - m_propIndex[2], cudaMemcpyDeviceToHost));
627 FRWK_CHECK_CUDA_ERROR(cudaMemcpy(m_buffer.get() + m_propIndex[3], m_featureArray.ids, m_propIndex[4] - m_propIndex[3], cudaMemcpyDeviceToHost));
628 FRWK_CHECK_CUDA_ERROR(cudaMemcpy(m_buffer.get() + m_propIndex[4], m_featureArray.newToOldMap, m_propIndex[5] - m_propIndex[4], cudaMemcpyDeviceToHost));
629 FRWK_CHECK_CUDA_ERROR(cudaMemcpy(m_buffer.get() + m_propIndex[5], m_featureArray.locations, m_propIndex[6] - m_propIndex[5], cudaMemcpyDeviceToHost));
630 FRWK_CHECK_CUDA_ERROR(cudaMemcpy(m_buffer.get() + m_propIndex[6], m_featureArray.featureCount, m_propIndex[7] - m_propIndex[6], cudaMemcpyDeviceToHost));
631 FRWK_CHECK_CUDA_ERROR(cudaMemcpy(m_buffer.get() + m_propIndex[7], m_featureArray.validTrackedCount, m_propIndex[8] - m_propIndex[7], cudaMemcpyDeviceToHost));
634 else if (m_featureArray.memoryType == DW_MEMORY_TYPE_CPU ||
635 m_featureArray.memoryType == DW_MEMORY_TYPE_PINNED)
637 memcpy(m_buffer.get() + m_propIndex[0], m_featureArray.statuses, m_propIndex[1] - m_propIndex[0]);
638 memcpy(m_buffer.get() + m_propIndex[1], m_featureArray.ages, m_propIndex[2] - m_propIndex[1]);
639 memcpy(m_buffer.get() + m_propIndex[2], m_featureArray.scales, m_propIndex[3] - m_propIndex[2]);
640 memcpy(m_buffer.get() + m_propIndex[3], m_featureArray.ids, m_propIndex[4] - m_propIndex[3]);
641 memcpy(m_buffer.get() + m_propIndex[4], m_featureArray.newToOldMap, m_propIndex[5] - m_propIndex[4]);
642 memcpy(m_buffer.get() + m_propIndex[5], m_featureArray.locations, m_propIndex[6] - m_propIndex[5]);
643 memcpy(m_buffer.get() + m_propIndex[6], m_featureArray.featureCount, m_propIndex[7] - m_propIndex[6]);
644 memcpy(m_buffer.get() + m_propIndex[7], m_featureArray.validTrackedCount, m_propIndex[8] - m_propIndex[7]);
651 static_assert(
sizeof(dwFeatureArray) == 80,
"dwFeatureArray size has changed, update deserialization");
653 auto array =
reinterpret_cast<dwFeatureArray*
>(m_buffer.get());
654 if (m_featureArray.maxFeatures != array->maxFeatures)
656 throw Exception(DW_INVALID_ARGUMENT,
"ChannelPacket<dwFeatureArray>: deserialize: maxFeatures does not match");
659 if (m_featureArray.memoryType == DW_MEMORY_TYPE_CUDA)
662 FRWK_CHECK_CUDA_ERROR(cudaMemcpy(m_featureArray.statuses, m_buffer.get() + m_propIndex[0], m_propIndex[1] - m_propIndex[0], cudaMemcpyHostToDevice));
663 FRWK_CHECK_CUDA_ERROR(cudaMemcpy(m_featureArray.ages, m_buffer.get() + m_propIndex[1], m_propIndex[2] - m_propIndex[1], cudaMemcpyHostToDevice));
664 FRWK_CHECK_CUDA_ERROR(cudaMemcpy(m_featureArray.scales, m_buffer.get() + m_propIndex[2], m_propIndex[3] - m_propIndex[2], cudaMemcpyHostToDevice));
665 FRWK_CHECK_CUDA_ERROR(cudaMemcpy(m_featureArray.ids, m_buffer.get() + m_propIndex[3], m_propIndex[4] - m_propIndex[3], cudaMemcpyHostToDevice));
666 FRWK_CHECK_CUDA_ERROR(cudaMemcpy(m_featureArray.newToOldMap, m_buffer.get() + m_propIndex[4], m_propIndex[5] - m_propIndex[4], cudaMemcpyHostToDevice));
667 FRWK_CHECK_CUDA_ERROR(cudaMemcpy(m_featureArray.locations, m_buffer.get() + m_propIndex[5], m_propIndex[6] - m_propIndex[5], cudaMemcpyHostToDevice));
668 FRWK_CHECK_CUDA_ERROR(cudaMemcpy(m_featureArray.featureCount, m_buffer.get() + m_propIndex[6], m_propIndex[7] - m_propIndex[6], cudaMemcpyHostToDevice));
669 FRWK_CHECK_CUDA_ERROR(cudaMemcpy(m_featureArray.validTrackedCount, m_buffer.get() + m_propIndex[7], m_propIndex[8] - m_propIndex[7], cudaMemcpyHostToDevice));
672 else if (m_featureArray.memoryType == DW_MEMORY_TYPE_CPU ||
673 m_featureArray.memoryType == DW_MEMORY_TYPE_PINNED)
675 memcpy(m_featureArray.statuses, m_buffer.get() + m_propIndex[0], m_propIndex[1] - m_propIndex[0]);
676 memcpy(m_featureArray.ages, m_buffer.get() + m_propIndex[1], m_propIndex[2] - m_propIndex[1]);
677 memcpy(m_featureArray.scales, m_buffer.get() + m_propIndex[2], m_propIndex[3] - m_propIndex[2]);
678 memcpy(m_featureArray.ids, m_buffer.get() + m_propIndex[3], m_propIndex[4] - m_propIndex[3]);
679 memcpy(m_featureArray.newToOldMap, m_buffer.get() + m_propIndex[4], m_propIndex[5] - m_propIndex[4]);
680 memcpy(m_featureArray.locations, m_buffer.get() + m_propIndex[5], m_propIndex[6] - m_propIndex[5]);
681 memcpy(m_featureArray.featureCount, m_buffer.get() + m_propIndex[6], m_propIndex[7] - m_propIndex[6]);
682 memcpy(m_featureArray.validTrackedCount, m_buffer.get() + m_propIndex[7], m_propIndex[8] - m_propIndex[7]);
685 m_featureArray.timeIdx = array->timeIdx;
689 size_t m_propIndex[9] = {};
698 auto* props = specimen.
getData<dwFeatureHistoryArray>();
706 if (ret != DW_SUCCESS)
708 throw Exception(DW_BAD_ALLOC,
"FeatureHistoryArrayPacket: cannot allocate packet");
714 throw Exception(DW_INVALID_ARGUMENT,
"Invalid reference data provided.");
740 initBuffer(
sizeof(dwFeatureHistoryArray) + m_featureHistoryArray.bytes);
746 static_assert(
sizeof(dwFeatureHistoryArray) == 104,
"dwFeatureHistoryArray size has changed, update serialization");
748 uint8_t* ptr = m_buffer.get() +
sizeof(dwFeatureHistoryArray);
749 memcpy(m_buffer.get(), &m_featureHistoryArray,
sizeof(dwFeatureHistoryArray));
751 if (m_featureHistoryArray.memoryType == DW_MEMORY_TYPE_CUDA)
754 FRWK_CHECK_CUDA_ERROR(cudaMemcpy(ptr, m_featureHistoryArray.data, m_featureHistoryArray.bytes, cudaMemcpyDeviceToHost));
757 else if (m_featureHistoryArray.memoryType == DW_MEMORY_TYPE_CPU ||
758 m_featureHistoryArray.memoryType == DW_MEMORY_TYPE_PINNED)
760 memcpy(ptr, m_featureHistoryArray.data, m_featureHistoryArray.bytes);
767 static_assert(
sizeof(dwFeatureHistoryArray) == 104,
"dwFeatureHistoryArray size has changed, update deserialization");
769 auto array =
reinterpret_cast<dwFeatureHistoryArray*
>(m_buffer.get());
770 if (m_featureHistoryArray.maxFeatures != array->maxFeatures || m_featureHistoryArray.maxHistory != array->maxHistory)
772 throw Exception(DW_INVALID_ARGUMENT,
"ChannelPacket<dwFeatureHistoryArray>: deserialize: maxFeatures or maxHistory does not match");
775 uint8_t* ptr = m_buffer.get() +
sizeof(dwFeatureHistoryArray);
776 if (m_featureHistoryArray.memoryType == DW_MEMORY_TYPE_CUDA)
779 FRWK_CHECK_CUDA_ERROR(cudaMemcpy(m_featureHistoryArray.data, ptr, m_featureHistoryArray.bytes, cudaMemcpyHostToDevice));
782 else if (m_featureHistoryArray.memoryType == DW_MEMORY_TYPE_CPU ||
783 m_featureHistoryArray.memoryType == DW_MEMORY_TYPE_PINNED)
785 memcpy(m_featureHistoryArray.data, ptr, m_featureHistoryArray.bytes);
788 m_featureHistoryArray.currentTimeIdx = array->currentTimeIdx;
801 auto* frame = getFrame();
818 m_bufferSize = bufferSize;
825 memcpy(m_buffer.get(), &m_data, m_bufferSize);
832 memcpy(&m_data, m_buffer.get(), m_bufferSize);
848 auto* frame = getFrame();
855 size_t bufferSize = std::max(
static_cast<size_t>(DW_RADARSCAN_MINIMUM_PAYLOAD_SIZE),
856 (std::max(
sizeof(dwRadarStatus), std::max(
sizeof(dwRadarTrack),
sizeof(dwRadarDetection))))) *
858 m_scanData.reset(
new uint8_t[bufferSize]);
859 m_data.data = m_scanData.get();
861 bufferSize += (
sizeof(dwRadarDetectionMisc) +
sizeof(dwRadarDetectionStdDev) +
sizeof(dwRadarDetectionQuality) +
862 sizeof(dwRadarDetectionProbability) +
sizeof(dwRadarDetectionFFTPatch)) *
864 bufferSize +=
sizeof(dwRadarScan);
866 m_scanDetectionMisc = std::make_unique<uint8_t[]>(
sizeof(dwRadarDetectionMisc) * ref.numReturns);
867 m_scanDetectionStdDev = std::make_unique<uint8_t[]>(
sizeof(dwRadarDetectionStdDev) * ref.numReturns);
868 m_scanDetectionQuality = std::make_unique<uint8_t[]>(
sizeof(dwRadarDetectionQuality) * ref.numReturns);
869 m_scanDetectionProbability = std::make_unique<uint8_t[]>(
sizeof(dwRadarDetectionProbability) * ref.numReturns);
870 m_scanDetectionFFTPatch = std::make_unique<uint8_t[]>(
sizeof(dwRadarDetectionFFTPatch) * ref.numReturns);
872 m_data.detectionMisc =
reinterpret_cast<const dwRadarDetectionMisc*
>(m_scanDetectionMisc.get());
873 m_data.detectionStdDev =
reinterpret_cast<const dwRadarDetectionStdDev*
>(m_scanDetectionStdDev.get());
874 m_data.detectionQuality =
reinterpret_cast<const dwRadarDetectionQuality*
>(m_scanDetectionQuality.get());
875 m_data.detectionProbability =
reinterpret_cast<const dwRadarDetectionProbability*
>(m_scanDetectionProbability.get());
876 m_data.detectionFFTPatch =
reinterpret_cast<const dwRadarDetectionFFTPatch*
>(m_scanDetectionFFTPatch.get());
878 initBuffer(bufferSize);
888 m_bufferSize = bufferSize;
894 size_t bufferSize = getRadarDataSize(m_data);
897 memcpy(m_buffer.get(), &m_data,
sizeof(dwRadarScan));
898 offset +=
sizeof(dwRadarScan);
899 memcpy(m_buffer.get() + offset, m_data.data, bufferSize);
900 offset += bufferSize;
901 memcpy(m_buffer.get() + offset, m_data.detectionMisc,
sizeof(dwRadarDetectionMisc) * m_data.numReturns);
902 offset +=
sizeof(dwRadarDetectionMisc) * m_data.numReturns;
903 memcpy(m_buffer.get() + offset, m_data.detectionStdDev,
sizeof(dwRadarDetectionStdDev) * m_data.numReturns);
904 offset +=
sizeof(dwRadarDetectionStdDev) * m_data.numReturns;
905 memcpy(m_buffer.get() + offset, m_data.detectionQuality,
sizeof(dwRadarDetectionQuality) * m_data.numReturns);
906 offset +=
sizeof(dwRadarDetectionQuality) * m_data.numReturns;
907 memcpy(m_buffer.get() + offset, m_data.detectionProbability,
sizeof(dwRadarDetectionProbability) * m_data.numReturns);
908 offset +=
sizeof(dwRadarDetectionProbability) * m_data.numReturns;
909 memcpy(m_buffer.get() + offset, m_data.detectionFFTPatch,
sizeof(dwRadarDetectionFFTPatch) * m_data.numReturns);
910 offset +=
sizeof(dwRadarDetectionFFTPatch) * m_data.numReturns;
920 memcpy(&m_data, m_buffer.get(),
sizeof(dwRadarScan));
921 offset +=
sizeof(dwRadarScan);
922 m_data.data = m_buffer.get() + offset;
923 offset += getRadarDataSize(m_data);
924 m_data.detectionMisc =
reinterpret_cast<const dwRadarDetectionMisc*
>(m_buffer.get() + offset);
925 offset +=
sizeof(dwRadarDetectionMisc) * m_data.numReturns;
926 m_data.detectionStdDev =
reinterpret_cast<const dwRadarDetectionStdDev*
>(m_buffer.get() + offset);
927 offset +=
sizeof(dwRadarDetectionStdDev) * m_data.numReturns;
928 m_data.detectionQuality =
reinterpret_cast<const dwRadarDetectionQuality*
>(m_buffer.get() + offset);
929 offset +=
sizeof(dwRadarDetectionQuality) * m_data.numReturns;
930 m_data.detectionProbability =
reinterpret_cast<const dwRadarDetectionProbability*
>(m_buffer.get() + offset);
931 offset +=
sizeof(dwRadarDetectionProbability) * m_data.numReturns;
932 m_data.detectionFFTPatch =
reinterpret_cast<const dwRadarDetectionFFTPatch*
>(m_buffer.get() + offset);
933 offset +=
sizeof(dwRadarDetectionFFTPatch) * m_data.numReturns;
937 size_t getRadarDataSize(
const dwRadarScan& radarScan)
939 size_t bufferSize = 0;
940 switch (radarScan.scanType.returnType)
942 case DW_RADAR_RETURN_TYPE_DETECTION:
943 bufferSize =
sizeof(dwRadarDetection) * radarScan.numReturns;
945 case DW_RADAR_RETURN_TYPE_TRACK:
946 bufferSize = std::max(
sizeof(dwRadarTrack),
static_cast<size_t>(DW_RADARSCAN_MINIMUM_PAYLOAD_SIZE)) * radarScan.numReturns;
948 case DW_RADAR_RETURN_TYPE_STATUS:
949 bufferSize =
sizeof(dwRadarStatus) * radarScan.numReturns;
951 case DW_RADAR_RETURN_TYPE_COUNT:
953 throw Exception(DW_INVALID_ARGUMENT,
"[ChannelPacketImpl]: Invalid radarScan return type");
958 dwRadarScan m_data{};
959 std::unique_ptr<uint8_t[]> m_scanData;
960 std::unique_ptr<uint8_t[]> m_scanDetectionMisc;
961 std::unique_ptr<uint8_t[]> m_scanDetectionStdDev;
962 std::unique_ptr<uint8_t[]> m_scanDetectionQuality;
963 std::unique_ptr<uint8_t[]> m_scanDetectionProbability;
964 std::unique_ptr<uint8_t[]> m_scanDetectionFFTPatch;
974 :
ChannelPacket(*specimen.getData<dwLidarDecodedPacket>(), ctx)
976 auto* frame = getFrame();
983 size_t bufferSize =
sizeof(dwLidarDecodedPacket) +
sizeof(dwLidarPointRTHI) * ref.maxPoints +
sizeof(dwLidarPointXYZI) * ref.maxPoints;
985 m_pointsRTHI = std::make_unique<uint8_t[]>(
sizeof(dwLidarPointRTHI) * ref.maxPoints);
986 m_pointsXYZI = std::make_unique<uint8_t[]>(
sizeof(dwLidarPointXYZI) * ref.maxPoints);
988 m_data.pointsRTHI =
reinterpret_cast<const dwLidarPointRTHI*
>(m_pointsRTHI.get());
989 m_data.pointsXYZI =
reinterpret_cast<const dwLidarPointXYZI*
>(m_pointsXYZI.get());
991 initBuffer(bufferSize);
1001 m_bufferSize = bufferSize;
1008 size_t header =
sizeof(dwLidarDecodedPacket);
1009 memcpy(m_buffer.get(), &m_data, header);
1010 memcpy(m_buffer.get() + header, m_data.pointsRTHI,
sizeof(dwLidarPointRTHI) * m_data.nPoints);
1011 header +=
sizeof(dwLidarPointRTHI) * m_data.nPoints;
1012 memcpy(m_buffer.get() + header, m_data.pointsXYZI,
sizeof(dwLidarPointXYZI) * m_data.nPoints);
1019 memcpy(&m_data, m_buffer.get(),
sizeof(dwLidarDecodedPacket));
1020 m_data.pointsRTHI =
reinterpret_cast<const dwLidarPointRTHI*
>(m_buffer.get() +
1021 sizeof(dwLidarDecodedPacket));
1022 m_data.pointsXYZI =
reinterpret_cast<const dwLidarPointXYZI*
>(m_buffer.get() +
sizeof(dwLidarDecodedPacket) +
1023 sizeof(dwLidarPointRTHI) * m_data.nPoints);
1027 dwLidarDecodedPacket m_data{};
1028 std::unique_ptr<uint8_t[]> m_pointsRTHI;
1029 std::unique_ptr<uint8_t[]> m_pointsXYZI;
1038 auto* props = specimen.
getData<dwEgomotionStateParams>();
1046 throw Exception(DW_INVALID_ARGUMENT,
"Invalid reference data provided.");
1072 dwEgomotionState_getMaxNumBytes(&m_numBytes, m_egomotionState);
1073 m_bufferSize = m_numBytes +
sizeof(m_numBytes);
1074 initBuffer(m_bufferSize);
1084 return &m_dispatchEgomotionState;
1089 size_t numBytes = 0;
1090 memcpy(&numBytes, m_buffer.get(),
sizeof(numBytes));
1097 dwEgomotionState_serialize(&m_numBytes,
1098 m_buffer.get() +
sizeof(m_numBytes),
1099 m_bufferSize -
sizeof(m_numBytes),
1100 m_dispatchEgomotionState);
1101 memcpy(m_buffer.get(), &m_numBytes,
sizeof(m_numBytes));
1106 memcpy(&m_numBytes, m_buffer.get(),
sizeof(m_numBytes));
1107 if (m_numBytes > m_bufferSize -
sizeof(m_numBytes))
1109 throw Exception(DW_OUT_OF_BOUNDS,
"ChannelPacket<dwEgomotionStateHandle_t>: deserialize: recv packet size mismatch available buffer size.");
1112 dwEgomotionState_deserialize(m_buffer.get() +
sizeof(m_numBytes),
1114 m_dispatchEgomotionState);
1118 size_t m_numBytes = 0;
1127 auto* props = specimen.
getData<dwPointCloud>();
1128 if (props !=
nullptr)
1132 dwStatus ret = dwPointCloud_createBuffer(&
m_data);
1133 if (ret != DW_SUCCESS)
1135 throw Exception(DW_BAD_ALLOC,
"PointCloudPacket: cannot allocate packet");
1142 throw Exception(DW_INVALID_ARGUMENT,
"Invalid reference data provided.");
1169 if (m_data.numAuxChannels > 0)
1171 throw Exception(DW_NOT_IMPLEMENTED,
"ChannelPacket<dwPointCloud>: time to add support for aux data: DRIV-8273");
1174 m_objectSize = getFormatSize(m_data.format);
1175 m_maxCount = m_data.capacity;
1176 m_headerSize =
sizeof(dwPointCloud);
1178 initBuffer(m_headerSize + m_maxCount * m_objectSize);
1184 uint8_t* ptr = m_buffer.get() + m_headerSize;
1185 memcpy(m_buffer.get(), &m_data, m_headerSize);
1186 if (m_data.type == DW_MEMORY_TYPE_CUDA)
1189 FRWK_CHECK_CUDA_ERROR(cudaMemcpy(ptr, m_data.points, m_data.size * m_objectSize, cudaMemcpyDeviceToHost));
1192 else if (m_data.type == DW_MEMORY_TYPE_CPU ||
1193 m_data.type == DW_MEMORY_TYPE_PINNED)
1195 memcpy(ptr, m_data.points, m_data.size * m_objectSize);
1201 uint8_t* ptr = m_buffer.get() + m_headerSize;
1202 memcpy(&m_data, m_buffer.get(), m_headerSize);
1204 if (m_data.type == DW_MEMORY_TYPE_CUDA)
1206 m_data.points = m_dataOri.points;
1208 FRWK_CHECK_CUDA_ERROR(cudaMemcpy(m_data.points, ptr, m_data.size * m_objectSize, cudaMemcpyHostToDevice));
1211 else if (m_data.type == DW_MEMORY_TYPE_CPU ||
1212 m_data.type == DW_MEMORY_TYPE_PINNED)
1214 m_data.points =
static_cast<void*
>(m_buffer.get() + m_headerSize);
1220 uint32_t formatSize = 0;
1223 case DW_POINTCLOUD_FORMAT_XYZI:
1224 formatSize =
sizeof(dwLidarPointXYZI);
1226 case DW_POINTCLOUD_FORMAT_RTHI:
1227 formatSize =
sizeof(dwLidarPointRTHI);
1230 throw std::runtime_error(
"ChannelPacket<dwPointCloud>: unknown dwPointCloudFormat");
1236 size_t m_headerSize{};
1237 size_t m_objectSize{};
1238 size_t m_maxCount{};
1250 auto* frame = getFrame();
1258 size_t lidarDecodedPacketsSize = m_data.maxPacketsPerSpin *
sizeof(dwLidarDecodedPacket);
1259 size_t pointsXYZISize = m_data.maxPacketsPerSpin * m_data.maxPointsPerPacket *
sizeof(dwLidarPointXYZI);
1260 size_t pointsRTHISize = m_data.maxPacketsPerSpin * m_data.maxPointsPerPacket *
sizeof(dwLidarPointRTHI);
1262 initBuffer(m_headerSize + lidarDecodedPacketsSize + pointsXYZISize + pointsRTHISize);
1264 m_lidarPacketsPtr =
reinterpret_cast<dwLidarDecodedPacket*
>(m_buffer.get() + m_headerSize);
1265 m_pointsXYZIPtr =
reinterpret_cast<dwLidarPointXYZI*
>(m_buffer.get() + m_headerSize + lidarDecodedPacketsSize);
1266 m_pointsRTHIPtr =
reinterpret_cast<dwLidarPointRTHI*
>(m_buffer.get() + m_headerSize + lidarDecodedPacketsSize + pointsXYZISize);
1268 m_data.packets = m_lidarPacketsPtr;
1269 m_data.pointsXYZIArray = m_pointsXYZIPtr;
1270 m_data.pointsRTHIArray = m_pointsRTHIPtr;
1281 size_t lidarDecodedPacketsSize = m_data.packetSize *
sizeof(dwLidarDecodedPacket);
1282 size_t pointsXYZISize = m_data.packetSize * m_data.maxPointsPerPacket *
sizeof(dwLidarPointXYZI);
1283 size_t pointsRTHISize = m_data.packetSize * m_data.maxPointsPerPacket *
sizeof(dwLidarPointRTHI);
1285 memcpy(m_buffer.get(), &m_data, m_headerSize);
1287 if (m_data.packets != m_lidarPacketsPtr)
1289 memcpy(m_lidarPacketsPtr, m_data.packets, lidarDecodedPacketsSize);
1291 if (m_data.pointsXYZIArray != m_pointsXYZIPtr)
1293 memcpy(m_pointsXYZIPtr, m_data.pointsXYZIArray, pointsXYZISize);
1295 if (m_data.pointsRTHIArray != m_pointsRTHIPtr)
1297 memcpy(m_pointsRTHIPtr, m_data.pointsRTHIArray, pointsRTHISize);
1303 memcpy(&m_data, m_buffer.get(), m_headerSize);
1304 m_data.packets = m_lidarPacketsPtr;
1305 m_data.pointsXYZIArray = m_pointsXYZIPtr;
1306 m_data.pointsRTHIArray = m_pointsRTHIPtr;
1307 for (
size_t packetsIndex = 0; packetsIndex < m_data.packetSize; ++packetsIndex)
1309 m_data.packets[packetsIndex].pointsXYZI = &(m_data.pointsXYZIArray[packetsIndex * m_data.maxPointsPerPacket]);
1310 m_data.packets[packetsIndex].pointsRTHI = &(m_data.pointsRTHIArray[packetsIndex * m_data.maxPointsPerPacket]);
1317 size_t m_headerSize{};
1319 dwLidarDecodedPacket* m_lidarPacketsPtr{};
1320 dwLidarPointXYZI* m_pointsXYZIPtr{};
1321 dwLidarPointRTHI* m_pointsRTHIPtr{};
1332 auto* frame = getFrame();
1337 : m_data(ref), m_headerSize(sizeof(
dwTraceNodeData)), m_maxDataSize(ref.maxDataSize)
1339 initBuffer(m_headerSize + m_maxDataSize);
1340 m_TraceData = m_buffer.get() + m_headerSize;
1341 m_data.data = m_TraceData;
1355 if (m_data.dataSize > m_maxDataSize)
1357 throw Exception(DW_OUT_OF_BOUNDS,
"ChannelPacket<dwTraceNodeData>: serializeImpl: send packet size mismatch buffer size.");
1359 memcpy(m_buffer.get(), &m_data, m_headerSize);
1360 if (m_data.data != m_TraceData && m_data.dataSize <= m_maxDataSize)
1362 memcpy(m_TraceData, m_data.data, m_data.dataSize *
sizeof(uint8_t));
1369 memcpy(&m_data, m_buffer.get(), m_headerSize);
1371 if (m_data.dataSize > m_maxDataSize)
1373 throw Exception(DW_OUT_OF_BOUNDS,
"ChannelPacket<dwTraceNodeData>: deserialize: recv packet size mismatch buffer size.");
1375 m_data.data = m_buffer.get() + m_headerSize;
1380 size_t m_headerSize{};
1381 size_t m_maxDataSize{};
1382 uint8_t* m_TraceData;
1392 auto* size = specimen.
getData<
size_t>();
1393 if (size !=
nullptr)
1400 throw Exception(DW_INVALID_ARGUMENT,
"Invalid codec packet buffer size.");
1423 m_headerSize =
sizeof(dwCodecPacket);
1424 initBuffer(m_headerSize + m_maxDataSize);
1431 if (m_packet.dataSizeBytes > m_maxDataSize)
1433 throw Exception(DW_OUT_OF_BOUNDS,
"ChannelPacket<dwCodecPacket>: serializeImpl: send packet size mismatch buffer size.");
1435 memcpy(m_buffer.get(), &m_packet, m_headerSize);
1437 memcpy(m_buffer.get() + m_headerSize, m_packet.data, m_packet.dataSizeBytes);
1443 memcpy(&m_packet, m_buffer.get(), m_headerSize);
1445 if (m_packet.dataSizeBytes > m_maxDataSize)
1447 throw Exception(DW_OUT_OF_BOUNDS,
"ChannelPacket<dwCodecPacket>: deserialize: recv packet size mismatch buffer size.");
1450 memcpy(m_dataBuffer.get(), m_buffer.get() + m_headerSize, m_packet.dataSizeBytes);
1451 m_packet.data =
static_cast<uint8_t*
>(m_dataBuffer.get());
1455 size_t m_headerSize;
1465 auto* size = specimen.
getData<
size_t>();
1466 if (size !=
nullptr)
1473 throw Exception(DW_INVALID_ARGUMENT,
"Invalid maximum buffer size.");
1497 initBuffer(m_headerSize + m_maxDataSize);
1504 if (m_packet.size > m_maxDataSize)
1506 throw Exception(DW_OUT_OF_BOUNDS,
"ChannelPacket<SensorServiceNodeRawData>: serializeImpl: send packet size mismatch buffer size.");
1508 memcpy(m_buffer.get(), &m_packet, m_headerSize);
1510 memcpy(m_buffer.get() + m_headerSize, m_packet.data, m_packet.size);
1516 memcpy(&m_packet, m_buffer.get(), m_headerSize);
1518 if (m_packet.size > m_maxDataSize)
1520 throw Exception(DW_OUT_OF_BOUNDS,
"ChannelPacket<SensorServiceNodeRawData>: deserialize: recv packet size mismatch buffer size.");
1523 memcpy(m_dataBuffer.get(), m_buffer.get() + m_headerSize, m_packet.size);
1524 m_packet.data =
static_cast<uint8_t*
>(m_dataBuffer.get());
1528 size_t m_headerSize;
#define FRWK_CHECK_CUDA_ERROR(x)
#define FRWK_CHECK_DW_ERROR(x)
void deserialize(size_t) final
ChannelPacket(const GenericData &specimen, dwContextHandle_t)
void serializeImpl() final
ChannelPacket(const GenericData &specimen, dwContextHandle_t)
void serializeImpl() final
void deserialize(size_t) final
dwEgomotionStateHandle_t * getFrame()
void serializeImpl() final
void deserialize(size_t) final
ChannelPacket(dwEgomotionStateParams ¶ms, dwContextHandle_t ctx)
ChannelPacket(const GenericData &specimen, dwContextHandle_t ctx)
ChannelPacket(const GenericData &specimen, dwContextHandle_t ctx)
void serializeImpl() override
void deserialize(size_t) override
ChannelPacket(const GenericData &specimen, dwContextHandle_t ctx)
void deserialize(size_t) override
void serializeImpl() override
void deserialize(size_t) override
void serializeImpl() override
ChannelPacket(const GenericData &specimen, dwContextHandle_t)
~ChannelPacket() override
void deserialize(size_t) final
static size_t serializeImage(dwImageCPU *cpuImage, unsigned char *buffer_start, size_t bufferSize, size_t planeCount, size_t elementSize, uint32_t planeChannelCount[], dwVector2ui planeSize[])
static void deserializeImage(dwImageHandle_t copyToImage, unsigned char *buffer_start, size_t bufferSize, size_t planeCount, size_t elementSize, uint32_t planeChannelCount[], dwVector2ui planeSize[])
void serializeImpl() final
ChannelPacket(const GenericData &specimen, dwContextHandle_t ctx)
void deserialize(size_t) final
ChannelPacket(const GenericData &specimen, dwContextHandle_t)
void serializeImpl() final
void serializeImpl() final
void deserialize(size_t) final
ChannelPacket(const GenericData &specimen, dwContextHandle_t ctx)
void setBufferSize(size_t bufferSize)
ChannelPacket(dwLidarDecodedPacket &ref, dwContextHandle_t)
dwLidarDecodedPacket * getFrame()
ChannelPacket(dwLidarPacketsArray &ref, dwContextHandle_t)
ChannelPacket(const GenericData &specimen, dwContextHandle_t ctx)
void serializeImpl() final
dwLidarPacketsArray * getFrame()
void deserialize(size_t) final
void deserialize(size_t) final
ChannelPacket(const GenericData &specimen, dwContextHandle_t)
void serializeImpl() final
static size_t getFormatSize(dwPointCloudFormat format)
ChannelPacket(const GenericData &specimen, dwContextHandle_t ctx)
void deserialize(size_t) override
~ChannelPacket() override
void serializeImpl() override
void deserialize(size_t) final
void setBufferSize(size_t bufferSize)
ChannelPacket(dwRadarScan &ref, dwContextHandle_t)
ChannelPacket(const GenericData &specimen, dwContextHandle_t ctx)
dwSensorNodeRawData * getFrame()
ChannelPacket(const GenericData &specimen, dwContextHandle_t ctx)
void setBufferSize(size_t bufferSize)
void serializeImpl() final
void deserialize(size_t) final
ChannelPacket(dwSensorNodeRawData &ref, dwContextHandle_t)
ChannelPacket(const GenericData &specimen, dwContextHandle_t ctx)
void deserialize(size_t) final
ChannelPacket(dwTraceNodeData &ref, dwContextHandle_t)
dwTraceNodeData * getFrame()
~ChannelPacket() override=default
void serializeImpl() final
CodecPacket(const GenericData &specimen)
GenericData getGenericData() final
std::unique_ptr< uint8_t[]> m_dataBuffer
dwEgomotionStateHandle_t m_egomotionState
dwEgomotionStateHandle_t m_dispatchEgomotionState
~EgomotionStateHandlePacket()
GenericData getGenericData() final
EgomotionStateHandlePacket(const GenericData &specimen, dwContextHandle_t ctx)
FeatureArrayPacket(const GenericData &specimen, dwContextHandle_t ctx)
dwFeatureArray m_featureArrayOrig
GenericData getGenericData() override
~FeatureArrayPacket() override
dwFeatureArray m_featureArray
dwFeatureHistoryArray m_featureHistoryArrayOrig
~FeatureHistoryArrayPacket() override
dwFeatureHistoryArray m_featureHistoryArray
GenericData getGenericData() final
FeatureHistoryArrayPacket(const GenericData &specimen, dwContextHandle_t ctx)
~FeatureNccScoresPacket() override
GenericData getGenericData() override
dwFeatureNccScores m_dispatchNcc
FeatureNccScoresPacket(const GenericData &specimen)
dwImageHandle_t m_imageHandle
dwImageHandle_t m_dispatchImage
ImageHandlePacket(const GenericData &specimen, dwContextHandle_t ctx)
GenericData getGenericData() override
std::unique_ptr< uint8_t[]> m_allocation
LatencyPacket(const GenericData &specimen)
GenericData getGenericData() override
~PointCloudPacket() override
PointCloudPacket(const GenericData &specimen)
GenericData getGenericData() final
dwPyramidImageProperties m_props
GenericData getGenericData() override
dwPyramidImage m_dispatchPyramid
dwPyramidImage m_pyramidImage
PyramidImagePacket(const GenericData &specimen, dwContextHandle_t ctx)
std::unique_ptr< uint8_t[]> m_dataBuffer
SensorServiceNodeRawData m_packet
GenericData getGenericData() final
SensorServiceNodeRawDataPacket(const GenericData &specimen)
dwTraceNodeData { size_t maxDataSize dwTraceNodeData
dwLidarPacketsArray { dwLidarDecodedPacket *packets dwLidarPacketsArray