dwSensorDrainerTemplate.hpp

Go to the documentation of this file.

 
// 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) 2020-2022 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.
//
#ifndef DW_FRAMEWORK_SENSOR_DRAINER_TEMPLATE_HPP_
#define DW_FRAMEWORK_SENSOR_DRAINER_TEMPLATE_HPP_
 
#include <dw/sensors/Sensors.h>
#include <dw/sensors/radar/Radar.h>
#include <dw/sensors/canbus/CAN.h>
#include <dw/core/base/Types.h>
 
#if defined(DW_SDK_BUILD_EXPERIMENTAL) && defined(LINUX)
#include <dwexperimental/sensors/lockstep/Lockstep.h>
#include <dwexperimental/sensors/Sensors.h>
#endif
 
#include <dwcgf/Exception.hpp>
#include <dwcgf/node/Node.hpp>
#include <dwframework/dwnodes/common/ChannelPacketTypes.hpp>
 
#include <unistd.h>
#include <memory>
 
namespace dw
{
namespace framework
{
 
struct dwSensorDrainerParams
{
    const char* nodeName;
    dwTime_t blockingTimeout;    // Blocking timeout must be > 1/f_sensor
    dwTime_t nonBlockingTimeout; // Nonblocking timeout must be < 1/f_sensor
    bool isVirtual;              // true if this sensor reads from dataset
    bool drainStaleData;         // true if drainer should read from sensor until DW SAL is empty
    bool waitForNewData;         // true if drainer should block until new data is available
    bool isLockstep;
};
 
template <typename ProcessedDataType, typename ReadProcessedDataFunc, typename DataSourceType>
class dwBaseDrainerTemplate
{
public:
    static constexpr char LOG_TAG[] = "dwBaseDrainerTemplate";
 
    // WAR: This is a guard for drain out sensor data, avoid huge sensor data make too long loop time, eg: drain out can msg > 100 ms, current find 50 is good enough
    static constexpr int32_t DRAIN_SENSOR_DATA_COUNT_MAX = 50;
    using OnDataDropped                                  = dw::core::Function<void(dwTime_t const)>;
 
    explicit dwBaseDrainerTemplate(dwSensorDrainerParams params, std::unique_ptr<ReadProcessedDataFunc> readProcessedDataFunc, DataSourceType dataSource)
        : m_dataSource(dataSource)
        , m_name(params.nodeName)
        , m_blockingTimeout(params.blockingTimeout)
        , m_nonBlockingTimeout(params.nonBlockingTimeout)
        , m_isVirtual(params.isVirtual)
        , m_dropStaleData(params.drainStaleData)
        , m_waitForNewData(params.waitForNewData)
        , m_readProcessedDataFunc(std::move(readProcessedDataFunc))
    {
    }
 
    void setOnDataDropped(OnDataDropped onDataDropped)
    {
        m_onDataDropped = onDataDropped;
    }
 
    virtual dwStatus getNextTimestamp(dwTime_t& timestamp, dwTime_t timeout)
    {
        // If dwSensor_getNextTimestamp API not available, get timestamp by looking ahead in dataset
        if (!m_nextDataReady)
        {
            dwStatus status = m_readProcessedDataFunc->readNextData(timeout, m_dataSource);
            if (status != DW_SUCCESS)
            {
                return status;
            }
            m_nextDataReady = true;
        }
 
        return m_readProcessedDataFunc->getNextDataTimestamp(timestamp);
    }
 
    virtual dwStatus getNextData(ProcessedDataType* outFrame, dwTime_t timeout)
    {
        if (!m_nextDataReady)
        {
            // Do the read here if look ahead did not happen
            dwStatus status = m_readProcessedDataFunc->readNextData(timeout, m_dataSource);
            if (status != DW_SUCCESS)
            {
                FRWK_LOGE << "getNextData: readNextData error!" << Logger::State::endl;
                return status;
            }
            m_nextDataReady = true;
        }
        return m_readProcessedDataFunc->getNextData(outFrame, m_dataSource);
    }
 
    virtual dwStatus tryRead(ProcessedDataType* outFrame,
                             dwTime_t& latestTimestamp,
                             dwTime_t timeout,
                             bool isDroppingData = false)
    {
        dwTime_t nextTime{};
        dwStatus status = getNextTimestamp(nextTime, timeout);
        if (status != DW_SUCCESS)
        {
            return status;
        }
 
        // Check if next data is ready. If not, mimic Live Sensor DW SAL buffer empty with a DW_TIME_OUT
        if (m_isVirtual && !isVirtualDataReady(nextTime, timeout))
        {
            return DW_TIME_OUT;
        }
 
        status = getNextData(outFrame, timeout);
        if (status != DW_SUCCESS)
        {
            FRWK_LOGE << "tryRead: getNextData error!" << Logger::State::endl;
            return status;
        }
 
        if (isDroppingData)
        {
            FRWK_LOGW << m_name << " Dropping data" << Logger::State::endl;
            if (m_onDataDropped)
            {
                m_onDataDropped(latestTimestamp);
            }
        }
 
        latestTimestamp   = nextTime;
        m_nextDataReady   = false;
        m_outputAvailable = true;
 
        return status;
    }
 
    virtual dwStatus reset()
    {
        m_virtualSyncTime = 0;
        m_nextDataReady   = false;
        m_readProcessedDataFunc->reset();
        return DW_SUCCESS;
    }
 
    virtual dwTime_t getReadTimeout() const
    {
        return m_waitForNewData ? m_blockingTimeout : m_nonBlockingTimeout;
    }
 
    virtual void setVirtualSyncTime(dwTime_t virtualSyncTime)
    {
        m_virtualSyncTime = virtualSyncTime;
    }
 
    // This function is original designed for mimicing real time live sensors for non-determinisitc mode.
    // But now removed the sleep before return as deterministic mode is the only supported dataset mode.
    // Returns true if the next sensor data should be available
    virtual bool isVirtualDataReady(dwTime_t dataTime, dwTime_t timeout)
    {
        dwTime_t timeToNextData = dataTime - m_virtualSyncTime;
 
        // For dataset, the first frame's m_virtualSyncTime is 0. Return true directly and set m_virtualSyncTime = -1
        // so that the second round tryRead will return DW_TIME_OUT and use the first frame.
        if (m_virtualSyncTime == 0)
        {
            // WAR, fix this
            m_virtualSyncTime = -1;
            return true;
        }
        if (timeToNextData >= timeout)
        {
            // Next sensor data is far in the future. Timeout specified not sufficient to reach next sensor data
            // Technically, we should still usleep(timeout) to mimic live sensor, but can skip this for perf
            // TODO(Oven): WAR to guarantee some data must be read when timeout is m_blockingTimeout,
            // otherwise we may send out uninitialized sensor data to downstreams
            if (timeout == m_blockingTimeout)
            {
                FRWK_LOGD << "isVirtualDataReady: timeToNextData is larger than m_blockingTimeout, must read some data. "
                          << "timeToNextData: " << timeToNextData << ", dataTime: " << dataTime << Logger::State::endl;
            }
            else
            {
                return false;
            }
        }
        if (timeToNextData <= 0)
        {
            // Next sensor data is in the past. Should be available.
            return true;
        }
        return true;
    }
 
    // Utility to read Sensor data from DW SAL in a consistent way across live/virtual sensor
    // May block on new sensor data, or drop stale data. See dwSensorDrainerParams
    // Retuns DW_SUCCESS if processedOutput and timestampOutput is valid
    // nextTimestampOutput will always be valid for virtual case
    //
    // [out] processedOutput the latest valid sensor frame
    // [out] timestampOutput timestamp associated with the latest valid sensor frame
    // [out] nextTimestampOutput timestamp associated with the next sensor frame. Only available in virtual case
    // [in] virtualSyncTime current timestamp to decide whether virtual data is ready to be sent
    // [in] sensor handle to sensor
    virtual dwStatus drainProcessedData(ProcessedDataType* processedOutput,
                                        dwTime_t& timestampOutput,
                                        dwTime_t& nextTimestampOutput,
                                        dwTime_t virtualSyncTime)
    {
        m_virtualSyncTime = virtualSyncTime;
 
        dwStatus status   = DW_SUCCESS;
        m_outputAvailable = false;
 
        // Blocking read guarantees some data is read, and slows processing down to data rate
        dwTime_t readTimeout = getReadTimeout();
        int32_t drainCount   = 0;
 
        // For determinisitic mode, camera/radar/imugps will always return available frame at the first loop with longer readTimeout.
        // They will return DW_TIME_OUT with shorter m_nonBlockingTimeout to guarentee deterministic for each frame at second loop.
        // Note: This do-while has different behavior when reach EndOfStream for flag m_dropStaleData. Assume dataset has 0~100 frames.
        //       If true, the first loop will read frame 100 successfully. Then enter the second loop and hit EndOfStream. It will reset
        //       the sensor and tryRead again. Which means it will return frame 0 and drop frame 100. Later, getNextTimestamp will return
        //       frame 1's timestamp. So the final result is: processedOutput = frame 0, nextTimestampOutput = frame 1' timestamp
        //       If false, the first loop will read frame 100 successfully and exit do-while loop directly. Later, nextTimestampOutput is
        //       reset to 0 and failed getNextTimestamp. So the final result is: processedOutput = frame 100, nextTimestampOutput = 0
        do
        {
            status = readProcessedData(processedOutput, timestampOutput, readTimeout, drainCount > 0);
            // Non-blocking read and drop stale data to drain the DW SAL buffer, send newest available data
            readTimeout = m_nonBlockingTimeout;
 
#if defined(DW_SDK_BUILD_EXPERIMENTAL) && defined(LINUX)
            isLockstepDataAvailable(status, readTimeout);
#endif
        } while (status == DW_SUCCESS && m_dropStaleData && (drainCount++ < DRAIN_SENSOR_DATA_COUNT_MAX));
 
        if (!m_outputAvailable && m_waitForNewData)
        {
            FRWK_LOGE << m_name << " blockingTimeout not sufficient to successfully waitForNewData. No data available!" << Logger::State::endl;
        }
        else if (status == DW_TIME_OUT && m_outputAvailable)
        {
            // DW_TIME_OUT indicates that DW SAL has been successfully drained
            status = DW_SUCCESS;
        }
 
        if (m_isVirtual)
        {
            // In virtual case, send out the timestamp of next sensor data
            nextTimestampOutput           = 0;
            const dwStatus nextTimeStatus = getNextTimestamp(nextTimestampOutput, m_blockingTimeout);
            if (nextTimeStatus != DW_SUCCESS)
            {
                FRWK_LOGD << m_name << " Failed to get next timestamp: " << dwGetStatusName(nextTimeStatus);
            }
        }
 
        return status;
    }
 
    virtual dwStatus readProcessedData(ProcessedDataType* outFrame, dwTime_t& latestTimestamp, dwTime_t timeout, bool isDroppingData = false) = 0;
 
    virtual void isLockstepDataAvailable(dwStatus& status, dwTime_t& readTimeout) {}
 
protected:
    DataSourceType m_dataSource;
 
    std::unique_ptr<ReadProcessedDataFunc> m_readProcessedDataFunc;
 
    dwTime_t m_blockingTimeout    = 60000;
    dwTime_t m_nonBlockingTimeout = 100;
    dwTime_t m_virtualSyncTime    = 0;
 
    Node::Name_t m_name{};
    bool m_outputAvailable = false;
    bool m_nextDataReady   = false;
    bool m_isVirtual       = false;
    bool m_dropStaleData   = false;
    bool m_waitForNewData  = false;
 
    OnDataDropped m_onDataDropped{};
 
    static constexpr dwTime_t REPLAY_SENSOR_READ_TIMEOUT = 10000000;
};
 
template <typename ProcessedDataType, typename ReadProcessedDataFunc>
class dwSensorDrainerTemplate : public dwBaseDrainerTemplate<ProcessedDataType, ReadProcessedDataFunc, dwSensorHandle_t>
{
public:
    static constexpr char LOG_TAG[] = "dwSensorDrainerTemplate";
    using Base                      = dwBaseDrainerTemplate<ProcessedDataType, ReadProcessedDataFunc, dwSensorHandle_t>;
 
    explicit dwSensorDrainerTemplate(dwSensorDrainerParams params, std::unique_ptr<ReadProcessedDataFunc> readProcessedDataFunc, dwSensorHandle_t hsensor)
        : dwBaseDrainerTemplate<ProcessedDataType, ReadProcessedDataFunc, dwSensorHandle_t>(params, std::move(readProcessedDataFunc), hsensor)
        , m_isLockstep(params.isLockstep)
    {
    }
 
    virtual ~dwSensorDrainerTemplate() = default;
 
    dwStatus getNextTimestamp(dwTime_t& timestamp,
                              dwTime_t timeout) override
    {
#if defined(DW_SDK_BUILD_EXPERIMENTAL) && defined(LINUX)
        if (Base::m_isVirtual)
        {
            dwStatus nextStatus = dwSensor_getNextTimestamp(&timestamp, Base::m_dataSource);
            if (nextStatus == DW_SUCCESS || nextStatus == DW_END_OF_STREAM)
            {
                return nextStatus;
            }
        }
#endif
 
        return Base::getNextTimestamp(timestamp, timeout);
    }
 
    virtual dwStatus reset() override
    {
        dwStatus res = Base::reset();
        if (res != DW_SUCCESS)
        {
            return res;
        }
        return dwSensor_reset(Base::m_dataSource);
    }
 
    // Reads a piece of sensor data from sensor
    // In virtual sensor case, uses m_virtualSyncTime and timeout to determine
    // whether next piece of sensor data should be available
    //
    // [out] outFrame output sensor frame
    // [in/out] latestTimestamp timestamp associated with the sensor frame
    // [in] timeout sensor reading timeout in microseconds
    // [in] isDroppingData indicates if data is being dropped
    dwStatus readProcessedData(ProcessedDataType* outFrame,
                               dwTime_t& latestTimestamp,
                               dwTime_t timeout,
                               bool isDroppingData = false) override
    {
        dwStatus status = Base::tryRead(outFrame, latestTimestamp, timeout, isDroppingData);
        if (status == DW_END_OF_STREAM)
        {
            // Reset the sensor and try to read again, but propagate the DW_END_OF_STREAM
            // Use blocking read after reset since camera prefetch may not be ready
            reset();
            timeout = Base::m_blockingTimeout;
            Base::tryRead(outFrame, latestTimestamp, timeout);
        }
        return status;
    }
 
    // Utility to drop sensor frames according to the events to be replayed
    // until the next non-drop event is read or there are no more replay events
    // @param [output] processedOutput the latest frame read from the sensor
    // @param [output] dataEvent the next non-drop event to be replayed.
    // @param [input] sensor the sensor to replay frames from
    // @param [input] readCb the callback to read replay events
    dwStatus replayDroppedFrames(ProcessedDataType* processedOutput,
                                 SensorNode::DataEvent& dataEvent,
                                 SensorNode::DataEventReadCallback readCb)
    {
        // Process any number of drop events.
        bool isDropEvent;
        do
        {
            // Read next data event
            if (!readCb(dataEvent)) // no more events to be replayed
            {
                FRWK_LOGW << "replayDroppedFrames: Trace cannot be read." << Logger::State::endl;
                return DW_END_OF_STREAM;
            }
 
            // Drop the sensor frame (if applicable)
            isDropEvent = dataEvent.dataEventType == SensorNode::DataEventType::DROP;
            if (isDropEvent)
            {
                dwTime_t timestampOutput{};
                dwStatus status = readProcessedData(processedOutput, timestampOutput, Base::REPLAY_SENSOR_READ_TIMEOUT);
                if (status == DW_END_OF_STREAM)
                {
                    return status;
                }
                else if (status == DW_SUCCESS)
                {
                    // For now we assume that the timestamps read from the sensor need to match the trace exactly
                    // so if there isn't a match then there is no valid way to continue, throw an exception.
                    if (timestampOutput != dataEvent.timestamp)
                    {
                        FRWK_LOGE << "replayDroppedFrames: Data/trace mismatch: current: " << timestampOutput << " recorded: " << dataEvent.timestamp << Logger::State::endl;
                        throw Exception(DW_FAILURE, "dwSensorDrainerTemplate: replayDroppedFrames: data/trace mismatch.");
                    }
                }
                else
                {
                    FRWK_LOGE << "replayDroppedFrames: Cannot read next data." << Logger::State::endl;
                    throw Exception(DW_FAILURE, "dwSensorDrainerTemplate: replayDroppedFrames: cannot read next data.");
                }
            }
        } while (isDropEvent);
 
        return DW_SUCCESS;
    }
 
    // Replay the next event for the sensor node-run.
    // @param [output] processedOutput the frame replayed from the sensor
    // @param [output] timestampOutput the timestamp of the frame replayed from the sensor
    // @param [input] sensor the sensor to replay frames from
    // @param [input] readCb the callback to read replay events
    virtual dwStatus replayProcessedData(ProcessedDataType* processedOutput,
                                         dwTime_t& timestampOutput,
                                         SensorNode::DataEventReadCallback readCb)
    {
        // First drop frames according to the replay event
        // also, retrieve the first non-drop event.
        SensorNode::DataEvent de{};
        dwStatus status = replayDroppedFrames(processedOutput, de, readCb);
 
        // When end of stream was reached, just read the next frame and ignore the replay event.
        if (status == DW_END_OF_STREAM)
        {
            readProcessedData(processedOutput, timestampOutput, Base::REPLAY_SENSOR_READ_TIMEOUT);
            return status;
        }
 
        // If some unexpected status was returned from the sensor just return the status as this is an error.
        if (status != DW_SUCCESS)
        {
            return status;
        }
 
        // otherwise, if no data was produced for this event, then just return the status that was recorded
        // (usually this is DW_NOT_AVAILABLE or DW_TIME_OUT)
        if (de.dataEventType == SensorNode::DataEventType::NONE)
        {
            status = de.status;
        }
        // if data was produced, read the next data from the sensor and make sure it has the timetamp we expect.
        else if (de.dataEventType == SensorNode::DataEventType::PRODUCE)
        {
            status = readProcessedData(processedOutput, timestampOutput, Base::REPLAY_SENSOR_READ_TIMEOUT);
            if (status != DW_SUCCESS && status != DW_END_OF_STREAM)
            {
                FRWK_LOGE << "replayProcessedData: Cannot read next data." << Logger::State::endl;
                throw Exception(DW_FAILURE, "dwSensorDrainerTemplate: replayProcessedData: cannot read next data.");
            }
            // For now we assume that the timestamps read from the sensor need to match the trace exactly
            // so if there isn't a match then there is no valid way to continue, throw an exception.
            if (timestampOutput != de.timestamp)
            {
                FRWK_LOGE << "replayProcessedData: Data/trace mismatch." << Logger::State::endl;
                throw Exception(DW_FAILURE, "dwSensorDrainerTemplate: replayProcessedData: data/trace mismatch.");
            }
        }
        else
        {
            FRWK_LOGE << "replayProcessedData: UnhandledEventType." << Logger::State::endl;
            throw Exception(DW_FAILURE, "dwSensorDrainerTemplate: unhandled event type");
        }
 
        return status;
    }
 
#if defined(DW_SDK_BUILD_EXPERIMENTAL) && defined(LINUX)
    void isLockstepDataAvailable(dwStatus& status, dwTime_t& readTimeout) override
    {
        if (m_isLockstep)
        {
            bool isDataAvailable = false;
            if (dwLockstep_isDataAvailable(&isDataAvailable, Base::m_dataSource) != DW_SUCCESS)
            {
                char8_t const* protocol;
                dwSensor_getProtocol(&protocol, Base::m_dataSource);
                throw Exception(DW_FAILURE, "dwSensorDrainerTemplate: can't get the data available flag from:", protocol, " sensor");
            }
 
            if (!isDataAvailable && status != DW_SUCCESS)
            {
                status = DW_TIME_OUT;
            }
            else
            {
                status      = DW_SUCCESS;
                readTimeout = Base::m_blockingTimeout;
            }
        }
    }
#endif
 
protected:
    bool m_isLockstep = false;
};
 
template <typename ProcessedDataType, typename ReadProcessedDataFunc, typename DataSourceType>
constexpr char dwBaseDrainerTemplate<ProcessedDataType, ReadProcessedDataFunc, DataSourceType>::LOG_TAG[];
 
template <typename ProcessedDataType, typename ReadProcessedDataFunc>
constexpr char dwSensorDrainerTemplate<ProcessedDataType, ReadProcessedDataFunc>::LOG_TAG[];
} // namespace framework
} // namespace dw
 
#endif // DW_FRAMEWORK_SENSOR_DRAINER_TEMPLATE_HPP_