jaus++/2.0/src/jaus/extras/rangesensor/rangesensor.cpp

Go to the documentation of this file.

#include "jaus/extras/rangesensor/rangesensor.h"


using namespace JAUS;


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


RangeSensor::RangeSensor() : Events::Child(Service::ID(RangeSensor::Name), Service::ID(Events::Name))
{
}


RangeSensor::~RangeSensor()
{
}


void RangeSensor::AddRangeDevice(const RangeSensorConfig& configuration)
{
    Mutex::ScopedLock lock(&mRangeSensorMutex);
    mRangeSensors[configuration.mID] = configuration;
    mRangeScans[configuration.mID] = ReportLocalRangeScan();
    mRangeScans[configuration.mID].SetSourceID(GetComponentID());
    if(GetComponent())
    {
        SignalEvent(REPORT_RANGE_SENSOR_CONFIGURATION);
    }
}


void RangeSensor::SetLocalRangeScan(const Byte deviceID, 
                                    const Point3D &deviceLocation, 
                                    const Point3D &deviceOrientation, 
                                    const std::vector<UShort>& scan,
                                    const Time& timestamp)
{
    Mutex::ScopedLock lock(&mRangeSensorMutex);

    std::map<Byte, ReportLocalRangeScan>::iterator s;
    s = mRangeScans.find(deviceID);
    if(s != mRangeScans.end())
    {
        s->second.SetSensorID(deviceID);
        s->second.SetSensorLocation(deviceLocation);
        s->second.SetSensorOrientation(deviceOrientation);
        RangeSensorConfig::Map::iterator config;
        config = mRangeSensors.find(deviceID);
        ReportLocalRangeScan::Scan* ptr = s->second.GetScan();
        ptr->Clear();
        ptr->Reserve((unsigned int)scan.size());

        std::vector<UShort>::const_iterator r;
        for(r = scan.begin();
            r != scan.end();
            r++)
        {
            ptr->PushBack((UShort)(*r));
        }

        SignalEvent(REPORT_LOCAL_RANGE_SCAN);
    }
}


void RangeSensor::SetLocalRangeScan(const Byte deviceID, 
                                    const Point3D &deviceLocation, 
                                    const Point3D &deviceOrientation, 
                                    const std::vector<double>& scan,
                                    const Time& timestamp)
{
    Mutex::ScopedLock lock(&mRangeSensorMutex);

    std::map<Byte, ReportLocalRangeScan>::iterator s;
    s = mRangeScans.find(deviceID);
    if(s != mRangeScans.end())
    {
        s->second.SetSensorID(deviceID);
        s->second.SetSensorLocation(deviceLocation);
        s->second.SetSensorOrientation(deviceOrientation);
        RangeSensorConfig::Map::iterator config;
        config = mRangeSensors.find(deviceID);
        ReportLocalRangeScan::Scan* ptr = s->second.GetScan();
        ptr->Clear();
        ptr->Reserve((unsigned int)scan.size());

        std::vector<double>::const_iterator r;
        double multiplier = 1000.0;
        if(config->second.mUnitType == RangeSensorConfig::CM)
        {
            multiplier = 100.0;
        }
        for(r = scan.begin();
            r != scan.end();
            r++)
        {
            ptr->PushBack((UShort)(multiplier*(*r)));
        }

        SignalEvent(REPORT_LOCAL_RANGE_SCAN);
    }
}


void RangeSensor::SetLocalRangeScan(const Byte deviceID, 
                                    const Point3D &deviceLocation, 
                                    const Point3D &deviceOrientation, 
                                    const Point3D::List& scan,
                                    const Time& timestamp)
{
    Mutex::ScopedLock lock(&mRangeSensorMutex);

    std::map<Byte, ReportLocalRangeScan>::iterator s;
    s = mRangeScans.find(deviceID);
    if(s != mRangeScans.end())
    {
        s->second.SetSensorID(deviceID);
        s->second.SetSensorLocation(deviceLocation);
        s->second.SetSensorOrientation(deviceOrientation);
        RangeSensorConfig::Map::iterator config;
        config = mRangeSensors.find(deviceID);
        ReportLocalRangeScan::Scan* ptr = s->second.GetScan();
        ptr->Clear();
        ptr->Reserve((unsigned int)scan.size());

        Point3D::List::const_iterator r;
        double multiplier = 1000.0;
        if(config->second.mUnitType == RangeSensorConfig::CM)
        {
            multiplier = 100.0;
        }
        for(r = scan.begin();
            r != scan.end();
            r++)
        {
            ptr->PushBack((UShort)(multiplier*(r->mX)));
        }

        SignalEvent(REPORT_LOCAL_RANGE_SCAN);
    }
}


bool RangeSensor::GenerateEvent(const Events::Subscription& info) const
{
    if(info.mpQueryMessage->GetMessageCode() == QUERY_RANGE_SENSOR_CONFIGURATION)
    {
        ReportRangeSensorConfiguration report;
        RangeSensorConfig::Map::const_iterator config;

        {
            Mutex::ScopedLock lock(&mRangeSensorMutex);
            for(config = mRangeSensors.begin();
                config != mRangeSensors.end();
                config++)
            {
                report.GetConfiguration()->push_back(config->second);
            }
        }
        SendEvent(info, &report);
        return true;
    }
    else if(info.mpQueryMessage->GetMessageCode() == QUERY_LOCAL_RANGE_SCAN)
    {
        std::map<Byte, ReportLocalRangeScan>::const_iterator scan;
        ReportLocalRangeScan report;
        bool haveScan = false;

        {
            Mutex::ScopedLock lock(&mRangeSensorMutex);
            scan = mRangeScans.find( ((QueryLocalRangeScan *)info.mpQueryMessage)->GetSensorID() );
            if(scan != mRangeScans.end())
            {
                report = scan->second;
                haveScan = true;
            }
        }

        if(haveScan)
        {
            SendEvent(info, &report);
            return true;
        }
    }
    return false;
}


bool RangeSensor::IsEventSupported(const Events::Type type,
                                   const double requestedPeriodicRate,
                                   const Message* queryMessage,
                                   double& confirmedPeriodicRate,
                                   std::string& errorMessage) const
{
    if(queryMessage->GetMessageCode() == QUERY_RANGE_SENSOR_CONFIGURATION ||
       queryMessage->GetMessageCode() == QUERY_LOCAL_RANGE_SCAN )
    {
        confirmedPeriodicRate = requestedPeriodicRate;
        return true;
    }

    return false;
}


void RangeSensor::Receive(const JAUS::Message *message)
{
    switch(message->GetMessageCode())
    {
    case QUERY_LOCAL_RANGE_SCAN:

        break;
    case QUERY_RANGE_SENSOR_CONFIGURATION:
        {
            ReportRangeSensorConfiguration report(message->GetSourceID(), GetComponentID());
            Mutex::ScopedLock lock(&mRangeSensorMutex);
            RangeSensorConfig::Map::iterator config;
            for(config = mRangeSensors.begin();
                config != mRangeSensors.end();
                config++)
            {
                report.GetConfiguration()->push_back(config->second);
            }
            Send(&report);
        }
        break;
    default:
        break;
    }
}


Message* RangeSensor::CreateMessage(const UShort messageCode) const
{
    Message* message = NULL;
    switch(messageCode)
    {
    case QUERY_LOCAL_RANGE_SCAN:
        message = new QueryLocalRangeScan();
        break;
    case QUERY_RANGE_SENSOR_CONFIGURATION:
        message = new QueryRangeSensorConfiguration();
        break;
    case REPORT_RANGE_SENSOR_CONFIGURATION:
        message = new ReportRangeSensorConfiguration();
        break;
    case REPORT_LOCAL_RANGE_SCAN:
        message = new ReportLocalRangeScan();
        break;
    default:
        message = NULL;
        break;
    };
    return message;
}

/*  End of File */