jaus++/2.0/src/jaus/extras/video/visualsensor.cpp

Go to the documentation of this file.

#include "jaus/extras/video/visualsensor.h"
#include "jaus/extras/video/querycameracount.h"
#include "jaus/extras/video/reportcameracount.h"
#include "jaus/extras/video/sharedimage.h"

using namespace JAUS;

const std::string VisualSensor::Name = "urn:jaus:jss:jpp:extras:VisualSensor";


VisualSensor::VisualSensor(const bool sharedImage) : Events::Child(Service::ID(VisualSensor::Name),
                                                                   Service::ID(Events::Name))
{
    mSharedMemoryImageFlag = false;
    mSharedMemoryImageFlag = sharedImage;
    mCameraCount = 1;
    mQualityJPEG = -1;
}


VisualSensor::~VisualSensor()
{
    std::map<Byte, SharedImage*>::iterator simg;
    mVisualSensorMutex.Lock();
    for(simg = mSharedImages.begin();
        simg != mSharedImages.end();
        simg++)
    {
        delete simg->second;
    }
    mSharedImages.clear();
    mVisualSensorMutex.Unlock();
}


void VisualSensor::SetCameraCount(const Byte cameraCount)
{
    mCameraCount = cameraCount;
}


bool VisualSensor::SetCurrentFrame(const unsigned char* rawImage,
                                   const unsigned int width,
                                   const unsigned int height,
                                   const unsigned char channels,
                                   const Byte cameraID,
                                   const double frameRateHz)
{
    Mutex::ScopedLock lock(&mVisualSensorMutex);

    std::map<UShort, ReportImage>* report = (std::map<UShort, ReportImage>*)(&mCompressedData);
    if(mCameraCount < (Byte)mCompressedData.size())
    {
        mCameraCount = (Byte)mCompressedData.size();
    }

    return SetCurrentFrame(rawImage,
                           width,
                           height,
                           channels,
                           cameraID,
                           (*report)[cameraID].GetFrameNumber() + 1,
                           frameRateHz);
}


bool VisualSensor::SetCurrentFrame(const unsigned char* rawImage,
                                   const unsigned int width,
                                   const unsigned int height,
                                   const unsigned char channels,
                                   const Byte cameraID,
                                   const unsigned int frameNumber,
                                   const double frameRateHz)
{
    Mutex::ScopedLock lock(&mVisualSensorMutex);
    // Initialize frame rates.
    mFrameRates[cameraID] = frameRateHz;

    std::map<UShort, ReportImage>* report = (std::map<UShort, ReportImage>*)(&mCompressedData);
    (*report)[cameraID].SetCameraID(cameraID);
    (*report)[cameraID].SetFrameNumber( frameNumber );
    
    if(mSharedMemoryImageFlag)
    {
        std::map<Byte, SharedImage*>::iterator simg;
        simg = mSharedImages.find(cameraID);
        if(simg == mSharedImages.end())
        {
            SharedImage* sm = new SharedImage();
            if(sm->CreateSharedImage(GetComponentID(), cameraID, width*height*channels*2))
            {
                mSharedImages[cameraID] = sm;
                simg = mSharedImages.find(cameraID);
            }
        }
        if(simg != mSharedImages.end())
        {
            simg->second->SetFrame(rawImage,
                                   width,
                                   height,
                                   channels,
                                   frameNumber);
        }
    }

    if(mPendingQueryMap.size() > 0 || EventsService()->HaveSubscribers(REPORT_IMAGE))
    {
        // Compress image.
        (*report)[cameraID].SetImageFormat(Image::JPEG);
        Packet* payload = (Packet *)(*report)[cameraID].GetImage();
        payload->Clear();

        // Reserve enough memory to store image + payload header.
        payload->Reserve(width*height*channels);
        
        unsigned int jpegSize = 0;
        CxUtils::JPEG::Compressor* jpeg = (CxUtils::JPEG::Compressor*)&mJPEG;
        jpeg->CompressImageNoResize(width,
                                    height,
                                    channels,
                                    rawImage,
                                    payload->Ptr(),
                                    payload->Reserved(),
                                    &jpegSize,
                                    mQualityJPEG);
        payload->SetLength(jpegSize);
        payload->SetWritePos(payload->Length());

        std::map<UShort, QueryImage>::iterator query;
        for(query = mPendingQueryMap.begin();
            query != mPendingQueryMap.end();
            query++)
        {
            (*report)[cameraID].SetSourceID(GetComponentID());
            (*report)[cameraID].SetDestinationID(query->second.GetSourceID());
            Send(&(*report)[cameraID]);
        }
        mPendingQueryMap.clear();

        // Normally we could use the generic SignalEvent method, however
        // we only want to signal events for a change to this specific
        // camera.  That way we don't generate an event for all cameras when
        // only 1 has updated.
        Events::Subscription::List myEvents = GetComponent()->EventsService()->GetProducedEvents(REPORT_IMAGE);
        Events::Subscription::List::iterator e;
        for(e = myEvents.begin();
            e != myEvents.end();
            e++)
        {
            const QueryImage* query = dynamic_cast<const QueryImage*>(e->mpQueryMessage);
            if(query && query->GetCameraID() == cameraID)
            {
                SignalEvent((*e));
            }
        }
    }

    return true;
}


bool VisualSensor::SetCurrentFrameCompressed(const unsigned char* compImage,
                                             const unsigned int compImageSize,
                                             const unsigned int width,
                                             const unsigned int height,
                                             const unsigned char channels,
                                             const Byte cameraID,
                                             const unsigned int frameNumber,
                                             const double frameRateHz,
                                             const Image::Format format)
{
    if(format != Image::MJPEG && format != Image::JPEG)
    {
        std::cout << "JAUS::VisualSensor::SetCurrentFrameCompressed: Only MJPEG and JPEG supported.\n";
        return false;
    }
    Mutex::ScopedLock lock(&mVisualSensorMutex);
    // Initialize frame rates.
    mFrameRates[cameraID] = frameRateHz;

    std::map<UShort, ReportImage>* report = (std::map<UShort, ReportImage>*)(&mCompressedData);
    (*report)[cameraID].SetCameraID(cameraID);
    (*report)[cameraID].SetFrameNumber( frameNumber );
    
    if(mSharedMemoryImageFlag)
    {
        std::map<Byte, SharedImage*>::iterator simg;
        simg = mSharedImages.find(cameraID);
        if(simg != mSharedImages.end())
        {
            // Shared image only works with RAW data,
            // and we may not know how to decompress.
            simg->second->CloseSharedImage();
            delete simg->second;
        }
    }

    if(mPendingQueryMap.size() > 0 || EventsService()->HaveSubscribers(REPORT_IMAGE))
    {
        // Compress image.
        (*report)[cameraID].SetImageFormat(Image::JPEG);
        Packet* payload = (Packet *)(*report)[cameraID].GetImage();
        payload->Clear();

        // Reserve enough memory to store image + payload header.
        unsigned int payloadSize = width*height*channels + 256;
        payload->Reserve(payloadSize);
        unsigned char* ptr = payload->Ptr();
        if(format == Image::MJPEG)
        {
            CxUtils::JPEG::ConvertMJPEG2JPEG(compImage, compImageSize, &ptr, &payloadSize);
            payload->SetLength(payloadSize);
        }
        else
        {
            payload->Write(compImage, compImageSize);
        }

        payload->SetWritePos(payload->Length());

        std::map<UShort, QueryImage>::iterator query;
        for(query = mPendingQueryMap.begin();
            query != mPendingQueryMap.end();
            query++)
        {
            (*report)[cameraID].SetSourceID(GetComponentID());
            (*report)[cameraID].SetDestinationID(query->second.GetSourceID());
            Send(&(*report)[cameraID]);
        }
        mPendingQueryMap.clear();

        // Normally we could use the generic SignalEvent method, however
        // we only want to signal events for a change to this specific
        // camera.  That way we don't generate an event for all cameras when
        // only 1 has updated.
        Events::Subscription::List myEvents = GetComponent()->EventsService()->GetProducedEvents(REPORT_IMAGE);
        Events::Subscription::List::iterator e;
        for(e = myEvents.begin();
            e != myEvents.end();
            e++)
        {
            const QueryImage* query = dynamic_cast<const QueryImage*>(e->mpQueryMessage);
            if(query && query->GetCameraID() == cameraID)
            {
                SignalEvent((*e));
            }
        }
    }

    return true;
}


bool VisualSensor::GenerateEvent(const Events::Subscription& info) const
{
    if(info.mpQueryMessage->GetMessageCode() == QUERY_IMAGE)
    {
        const QueryImage* query = dynamic_cast<const QueryImage*>(info.mpQueryMessage);
        Mutex::ScopedLock lock(&mVisualSensorMutex);

        std::map<UShort, ReportImage>* report = (std::map<UShort, ReportImage>*)(&mCompressedData);
        (*report)[query->GetCameraID()].SetCameraID(query->GetCameraID());
        (*report)[query->GetCameraID()].SetImageFormat(Image::JPEG);

        Packet* payload = (Packet *)(*report)[query->GetCameraID()].GetImage();

        // If we have a compressed version of the image data...
        if(payload->Length() > 0)
        {
            SendEvent(info, &(*report)[query->GetCameraID()]);
            if(mCameraCount > 1)
            {
                CxUtils::SleepMs(1);
            }
            return true;
        }
    }

    return false;
}


bool VisualSensor::IsEventSupported(const Events::Type type,
                                    const double requestedPeriodicRate,
                                    const Message* queryMessage,
                                    double& confirmedPeriodicRate,
                                    std::string& errorMessage) const
{
    if(queryMessage->GetMessageCode() == QUERY_IMAGE)
    {
        const QueryImage* query = dynamic_cast<const QueryImage*>(queryMessage);

        // Currently only JPEG is supported.
        if(query->GetFormat() != Image::JPEG)
        {
            return false;
        }
        std::map<Byte, double>::const_iterator rate = mFrameRates.find(query->GetCameraID());
        double hz = 30.0;
        if(rate != mFrameRates.end())
        {
            hz = rate->second;
        }
        else
        {
            // Unknown camera ID.
            return false;
        }
        
        if(hz < requestedPeriodicRate)
        {
            confirmedPeriodicRate = hz;
        }
        else
        {
            confirmedPeriodicRate = requestedPeriodicRate;
        }

        return true;
    }
    return false;
}


void VisualSensor::Receive(const Message* message)
{
    switch(message->GetMessageCode())
    {
    case QUERY_IMAGE:
        {
            const QueryImage* query = dynamic_cast<const QueryImage*>(message);
            if(query)
            {
                Mutex::ScopedLock lock(&mVisualSensorMutex);
                mPendingQueryMap[query->GetSourceID().mSubsystem] = *query;
            }
        }
        break;
    case QUERY_CAMERA_COUNT:
        {
            Mutex::ScopedLock lock(&mVisualSensorMutex);
            ReportCameraCount report(message->GetSourceID(), GetComponentID());
            std::map<Byte, double>::const_iterator f;
            for(f = mFrameRates.begin();
                f != mFrameRates.end();
                f++)
            {
                report.GetCameraList()->push_back(f->first);
            }
            Send(&report);
        }
        break;
    case REPORT_IMAGE:
        {
            
        }
        break;
    default:
        break;
    }
}



Message* VisualSensor::CreateMessage(const UShort messageCode) const
{
    Message* message;
    switch(messageCode)
    {
    case QUERY_IMAGE:
        message = new QueryImage();
        break;
    case QUERY_CAMERA_COUNT:
        message = new QueryCameraCount();
        break;
    case REPORT_IMAGE:
        message = new ReportImage();
        break;
    case REPORT_CAMERA_COUNT:
        message = new ReportCameraCount();
        break;
    default:
        message = NULL;
        break;
    }
    return message;
}


bool VisualSensor::SetCompressionQualityJPEG(const int quality)
{
    if(quality >= -1 && quality <= 100)
    {
        Mutex::ScopedLock lock(&mVisualSensorMutex);
        mQualityJPEG = quality;
        return true;
    }
    return false;
}

/*  End of File */