///////////////////////////////////////////////////////////////////////////////////////// // This code contains NVIDIA Confidential Information and is disclosed // under the Mutual Non-Disclosure Agreement. // // Notice // ALL NVIDIA DESIGN SPECIFICATIONS AND CODE ("MATERIALS") ARE PROVIDED "AS IS" NVIDIA MAKES // NO REPRESENTATIONS, WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO // THE MATERIALS, AND EXPRESSLY DISCLAIMS ANY IMPLIED WARRANTIES OF NONINFRINGEMENT, // MERCHANTABILITY, OR FITNESS FOR A PARTICULAR PURPOSE. // // NVIDIA Corporation assumes no responsibility for the consequences of use of such // information or for any infringement of patents or other rights of third parties that may // result from its use. No license is granted by implication or otherwise under any patent // or patent rights of NVIDIA Corporation. No third party distribution is allowed unless // expressly authorized by NVIDIA. Details are subject to change without notice. // This code supersedes and replaces all information previously supplied. // NVIDIA Corporation products are not authorized for use as critical // components in life support devices or systems without express written approval of // NVIDIA Corporation. // // Copyright (c) 2017-2019 NVIDIA Corporation. All rights reserved. // // NVIDIA Corporation and its licensors retain all intellectual property and proprietary // rights in and to this software and related documentation and any modifications thereto. // Any use, reproduction, disclosure or distribution of this software and related // documentation without an express license agreement from NVIDIA Corporation is // strictly prohibited. // /////////////////////////////////////////////////////////////////////////////////////// #include #include // Include all relevant DriveWorks modules #include #include using namespace dw_samples::common; //------------------------------------------------------------------------------ // Template of a sample. Put some description what the sample does here //------------------------------------------------------------------------------ class LidarReplaySample : public DriveWorksSample { private: dwContextHandle_t m_context = DW_NULL_HANDLE; dwSALHandle_t m_sal = DW_NULL_HANDLE; dwSensorHandle_t m_lidarSensor = DW_NULL_HANDLE; dwLidarProperties m_lidarProperties{}; bool m_recordedLidar = false; std::unique_ptr m_pointCloud; // Rendering dwVisualizationContextHandle_t m_visualizationContext = DW_NULL_HANDLE; dwRenderEngineHandle_t m_renderEngine = DW_NULL_HANDLE; uint32_t m_gridBuffer = 0; uint32_t m_gridBufferPrimitiveCount = 0; uint32_t m_pointCloudBuffer = 0; uint32_t m_pointCloudBufferCapacity = 0; // max storage uint32_t m_pointCloudBufferSize = 0; // actual size std::string m_message1; std::string m_message2; std::string m_message3; std::string m_message4; std::string m_message5; std::string m_message6; std::string m_message7; // Lidar // pixel 1D array for one picture (frame) Timestamp* pY16Pixels1D = new Timestamp[ImgWidth * ImgHeight]; public: LidarReplaySample(const ProgramArguments& args) : DriveWorksSample(args) {} void initializeDriveWorks(dwContextHandle_t& context) { // initialize logger to print verbose message on console in color CHECK_DW_ERROR(dwLogger_initialize(getConsoleLoggerCallback(true))); CHECK_DW_ERROR(dwLogger_setLogLevel(DW_LOG_VERBOSE)); // initialize SDK context, using data folder dwContextParameters sdkParams = {}; sdkParams.dataPath = DataPath::get_cstr(); #ifdef VIBRANTE sdkParams.eglDisplay = getEGLDisplay(); #endif CHECK_DW_ERROR(dwInitialize(&context, DW_VERSION, &sdkParams)); CHECK_DW_ERROR(dwSAL_initialize(&m_sal, context)); std::string parameterString; std::string protocolString; dwSensorParams params{}; if (strcmp(getArgument("protocol").c_str(), "") != 0) { protocolString = getArgument("protocol"); m_recordedLidar = (protocolString == "lidar.virtual") ? true : false; } if (strcmp(getArgument("params").c_str(), "") != 0) parameterString = getArgument("params"); if (protocolString.empty() || parameterString.empty()) { logError("INVALID PARAMETERS\n"); exit(-1); } params.protocol = protocolString.c_str(); params.parameters = parameterString.c_str(); std::cout << "params:" << std::endl; std::cout << "- protocol: " << params.protocol << std::endl; std::cout << "- parameters: " << params.parameters << std::endl; std::cout << "- auxiliarydata: " << params.auxiliarydata << std::endl; CHECK_DW_ERROR(dwSAL_createSensor(&m_lidarSensor, params, m_sal)); // Get lidar properties // todo CHECK_DW_ERROR(dwSensorLidar_getProperties(&m_lidarProperties, m_lidarSensor)); // set Falcon properties manually strncpy(m_lidarProperties.deviceString, "Fastree3D_Falcon", 18); m_lidarProperties.spinFrequency = 30; // 0.0333; // 1/30; m_lidarProperties.packetsPerSecond = 30; m_lidarProperties.packetsPerSpin = 1; m_lidarProperties.pointsPerSecond = 30*32*60; m_lidarProperties.pointsPerPacket = 32*60; m_lidarProperties.pointsPerSpin = 32*60; m_lidarProperties.pointStride = 4; std::cout << "m_lidarProperties.deviceString = " << m_lidarProperties.deviceString << std::endl; std::cout << "m_lidarProperties.spinFrequency = " << m_lidarProperties.spinFrequency << std::endl; std::cout << "m_lidarProperties.packetsPerSecond = " << m_lidarProperties.packetsPerSecond << std::endl; std::cout << "m_lidarProperties.packetsPerSpin = " << m_lidarProperties.packetsPerSpin << std::endl; std::cout << "m_lidarProperties.pointsPerSecond = " << m_lidarProperties.pointsPerSecond << std::endl; std::cout << "m_lidarProperties.pointsPerPacket = " << m_lidarProperties.pointsPerPacket << std::endl; std::cout << "m_lidarProperties.pointsPerSpin = " << m_lidarProperties.pointsPerSpin << std::endl; std::cout << "m_lidarProperties.pointStride = " << m_lidarProperties.pointStride << std::endl; // Allocate bigger buffer in case certain spin exceeds the pointsPerSpin in lidar property m_pointCloudBufferCapacity = m_lidarProperties.pointsPerSecond; m_pointCloud.reset(new float32_t[m_pointCloudBufferCapacity * m_lidarProperties.pointStride]); } /// ----------------------------- /// Initialize everything of a sample here incl. SDK components /// ----------------------------- bool onInitialize() override { log("Starting my sample application...\n"); std::cout << "onInitialize" << std::endl; initializeDriveWorks(m_context); dwVisualizationInitialize(&m_visualizationContext, m_context); std::cout << "initialize LUT" << std::endl; // read LUT csv files and generate double pointer to LUT // LUT_Theta and LUT_Phi are set readLUT(); // ----------------------------- // Initialize RenderEngine // ----------------------------- dwRenderEngineParams renderEngineParams{}; CHECK_DW_ERROR(dwRenderEngine_initDefaultParams(&renderEngineParams, getWindowWidth(), getWindowHeight())); renderEngineParams.defaultTile.backgroundColor = {0.0f, 0.0f, 1.0f, 1.0f}; CHECK_DW_ERROR_MSG(dwRenderEngine_initialize(&m_renderEngine, &renderEngineParams, m_visualizationContext), "Cannot initialize Render Engine, maybe no GL context available?"); CHECK_DW_ERROR(dwRenderEngine_createBuffer(&m_pointCloudBuffer, DW_RENDER_ENGINE_PRIMITIVE_TYPE_POINTS_3D, sizeof(dwVector3f), 0, m_pointCloudBufferCapacity * sizeof(dwVector4f), m_renderEngine)); CHECK_DW_ERROR(dwRenderEngine_createBuffer(&m_gridBuffer, DW_RENDER_ENGINE_PRIMITIVE_TYPE_LINES_3D, sizeof(dwVector3f), 0, 10000, m_renderEngine)); dwMatrix4f identity = DW_IDENTITY_MATRIX4F; CHECK_DW_ERROR(dwRenderEngine_setBufferPlanarGrid3D(m_gridBuffer, {0.f, 0.f, 100.f, 100.f}, 5.0f, 5.0f, &identity, m_renderEngine)); dwRenderEngine_getBufferMaxPrimitiveCount(&m_gridBufferPrimitiveCount, m_gridBuffer, m_renderEngine); dwSensor_start(m_lidarSensor); return true; } ///------------------------------------------------------------------------------ /// This method is executed when user presses `R`, it indicates that sample has to reset ///------------------------------------------------------------------------------ void onReset() override { dwSensor_reset(m_lidarSensor); dwRenderEngine_reset(m_renderEngine); } ///------------------------------------------------------------------------------ /// This method is executed on release, free up used memory here ///------------------------------------------------------------------------------ void onRelease() override { if (m_pointCloudBuffer != 0) { CHECK_DW_ERROR(dwRenderEngine_destroyBuffer(m_pointCloudBuffer, m_renderEngine)); } if (m_gridBuffer != 0) { CHECK_DW_ERROR(dwRenderEngine_destroyBuffer(m_gridBuffer, m_renderEngine)); } if (m_renderEngine != DW_NULL_HANDLE) { CHECK_DW_ERROR(dwRenderEngine_release(m_renderEngine)); } if (m_visualizationContext != DW_NULL_HANDLE) { CHECK_DW_ERROR(dwVisualizationRelease(m_visualizationContext)); } // ----------------------------------------- // Release DriveWorks context and SAL // ----------------------------------------- dwSAL_release(m_sal); if (m_context != DW_NULL_HANDLE) { CHECK_DW_ERROR(dwRelease(m_context)); } CHECK_DW_ERROR(dwLogger_release()); } ///------------------------------------------------------------------------------ /// Change renderer properties when main rendering window is resized ///------------------------------------------------------------------------------ void onResizeWindow(int width, int height) override { dwRectf bounds{.x = 0.0f, .y = 0.0f}; bounds.width = width; bounds.height = height; dwRenderEngine_setBounds(bounds, m_renderEngine); } void updateFrame(uint32_t accumulatedPoints, uint32_t packetCount, dwTime_t hostTimestamp, dwTime_t sensorTimestamp) { m_pointCloudBufferSize = accumulatedPoints; // Grab properties, in case they were changed while running // todo dwSensorLidar_getProperties(&m_lidarProperties, m_lidarSensor); // set Falcon properties manually strncpy(m_lidarProperties.deviceString, "Fastree3D_Falcon", 18); m_lidarProperties.spinFrequency = 30; // 0.0333; // 1/30; m_lidarProperties.packetsPerSecond = 30; m_lidarProperties.packetsPerSpin = 1; m_lidarProperties.pointsPerSecond = 30*32*60; m_lidarProperties.pointsPerPacket = 32*60; m_lidarProperties.pointsPerSpin = 32*60; m_lidarProperties.pointStride = 4; dwRenderEngine_setBuffer(m_pointCloudBuffer, DW_RENDER_ENGINE_PRIMITIVE_TYPE_POINTS_3D, m_pointCloud.get(), sizeof(dwLidarPointRTHI), 0, m_pointCloudBufferSize, m_renderEngine); m_message1 = "Host timestamp (us) " + std::to_string(hostTimestamp); m_message2 = "Sensor timestamp (us) " + std::to_string(sensorTimestamp); m_message3 = "Packets per scan " + std::to_string(packetCount); m_message4 = "Points per scan " + std::to_string(accumulatedPoints); m_message5 = "Frequency (Hz) " + std::to_string(m_lidarProperties.spinFrequency); m_message6 = "Lidar Device " + std::string{m_lidarProperties.deviceString}; m_message7 = "Press ESC to exit"; } void computeSpin() { const dwLidarDecodedPacket* nextPacket; static uint32_t packetCount = 0; static uint32_t accumulatedPoints = 0; static bool endOfSpin = false; static auto tStart = std::chrono::high_resolution_clock::now(); static auto tEnd = tStart; // For recorded data throttling check how long to a full spin and match to the lidar frequency if (m_recordedLidar && endOfSpin) { tEnd = std::chrono::high_resolution_clock::now(); float64_t duration = std::chrono::duration(tEnd - tStart).count(); float64_t sleepTime = 1000.0 / m_lidarProperties.spinFrequency - duration; //std::cout << "sleepTime = " << sleepTime << std::endl; if (sleepTime > 0.0) return; else endOfSpin = false; } // Empty the queue and append all points within the same spin. // Update render structures only when a full spin is done. dwStatus status = DW_NOT_AVAILABLE; dwTime_t hostTimestamp = 0; dwTime_t sensorTimestamp = 0; // read 1st Y16 dump and copy into pY16Pixels 1D pY16Pixels1D = getPointerY16Pixels1D("FalconDump.y16",packetCount*ImgWidth*ImgHeight*2); /* std::cout << "Pixel 1D printout: quality, timestamp" << std::endl; for (int i=0; i<10; i++) { std::cout << "pixel " << i << ": " << (pY16Pixels1D + i)->quality << ", " << (pY16Pixels1D + i)->timestamp << std::endl; } */ tStart = std::chrono::high_resolution_clock::now(); while (1) { // converts ptrY16Pixels1D of Timestamp struct to decodedPacket of dwLidarDecodedPacket struct nextPacket = convertNextPacket(pY16Pixels1D); std::cout << std::endl; std::cout << "DECODED PACKET " << packetCount << ": " << std::endl; printLidarDecodedPacket(nextPacket,5); if (packetCount > 94) { // 96 (0...95) frames in y16 dump status = DW_END_OF_STREAM; } else { status = DW_SUCCESS; } std::cout << "status = " << status << std::endl; // todo status = dwSensorLidar_readPacket(&nextPacket, 100000, m_lidarSensor); if (status == DW_SUCCESS) { packetCount++; hostTimestamp = nextPacket->hostTimestamp; sensorTimestamp = nextPacket->sensorTimestamp; // Append the packet to the buffer float32_t* map = &m_pointCloud[accumulatedPoints * m_lidarProperties.pointStride]; memcpy(map, nextPacket->pointsRTHI, nextPacket->nPoints * sizeof(dwLidarPointRTHI)); accumulatedPoints += nextPacket->nPoints; // If we go beyond a full spin, update the render data then return if (nextPacket->scanComplete) { updateFrame(accumulatedPoints, packetCount, hostTimestamp, sensorTimestamp); accumulatedPoints = 0; // todo packetCount = 0; endOfSpin = true; // todo dwSensorLidar_returnPacket(nextPacket, m_lidarSensor); // TODO needed? return; } // todo dwSensorLidar_returnPacket(nextPacket, m_lidarSensor); } else if (status == DW_END_OF_STREAM) { updateFrame(accumulatedPoints, packetCount, hostTimestamp, sensorTimestamp); // For recorded data, start over at the end of the file dwSensor_reset(m_lidarSensor); accumulatedPoints = 0; packetCount = 0; endOfSpin = true; return; } else if (status == DW_TIME_OUT) { std::cout << "Read lidar packet: timeout" << std::endl; } else { stop(); return; } } } void onProcess() override { computeSpin(); } void onRender() override { // render text in the middle of the window dwRenderEngine_reset(m_renderEngine); dwRenderEngine_setTile(0, m_renderEngine); dwRenderEngine_setModelView(getMouseView().getModelView(), m_renderEngine); dwRenderEngine_setProjection(getMouseView().getProjection(), m_renderEngine); dwRenderEngine_setColor(DW_RENDER_ENGINE_COLOR_DARKGREY, m_renderEngine); dwRenderEngine_setBackgroundColor({0.0f, 0.0f, 0.0f, 1.0f}, m_renderEngine); dwRenderEngine_renderBuffer(m_gridBuffer, m_gridBufferPrimitiveCount, m_renderEngine); dwRenderEngine_setColorByValue(DW_RENDER_ENGINE_COLOR_BY_VALUE_MODE_XY, 130.0f, m_renderEngine); dwRenderEngine_renderBuffer(m_pointCloudBuffer, m_pointCloudBufferSize, m_renderEngine); dwRenderEngine_setProjection(&DW_IDENTITY_MATRIX4F, m_renderEngine); dwRenderEngine_setModelView(&DW_IDENTITY_MATRIX4F, m_renderEngine); dwVector2f range{static_cast(getWindowWidth()), static_cast(getWindowHeight())}; dwRenderEngine_setCoordinateRange2D(range, m_renderEngine); dwRenderEngine_setFont(DW_RENDER_ENGINE_FONT_VERDANA_16, m_renderEngine); dwRenderEngine_setColor(DW_RENDER_ENGINE_COLOR_GREEN, m_renderEngine); dwRenderEngine_renderText2D(m_message1.c_str(), {20.f, getWindowHeight() - 30.f}, m_renderEngine); dwRenderEngine_renderText2D(m_message2.c_str(), {20.f, getWindowHeight() - 50.f}, m_renderEngine); dwRenderEngine_renderText2D(m_message3.c_str(), {20.f, getWindowHeight() - 70.f}, m_renderEngine); dwRenderEngine_renderText2D(m_message4.c_str(), {20.f, getWindowHeight() - 90.f}, m_renderEngine); dwRenderEngine_renderText2D(m_message5.c_str(), {20.f, getWindowHeight() - 110.f}, m_renderEngine); dwRenderEngine_renderText2D(m_message6.c_str(), {20.f, getWindowHeight() - 130.f}, m_renderEngine); dwRenderEngine_renderText2D(m_message7.c_str(), {20.f, 20.f}, m_renderEngine); renderutils::renderFPS(m_renderEngine, getCurrentFPS()); } }; //------------------------------------------------------------------------------ int main(int argc, const char** argv) { // ------------------- // define all arguments used by the application // parse user given arguments and bail out if there is --help request or proceed ProgramArguments args(argc, argv, { ProgramArguments::Option_t("protocol", "lidar.virtual"), ProgramArguments::Option_t("params", ("file=" + DataPath::get() + "/samples/sensors/lidar/lidar:top:hdl64e.bin").c_str()), // TODO ProgramArguments::Option_t("params", ("file=" + DataPath::get() + "/samples/sensors/lidar/lidar_velodyne64.bin").c_str()), ProgramArguments::Option_t("show-intensity", "false")}); // ------------------- // initialize and start a window application (with offscreen support if required) LidarReplaySample app(args); app.initializeWindow("Lidar Replay Sample", 1024, 800, args.enabled("offscreen")); return app.run(); }