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

Go to the documentation of this file.

#include "jaus/extras/rangesensor/rangesubscriber.h"
#include <cxutils/math/cxmath.h>
using namespace JAUS;

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


RangeSubscriber::RangeSubscriber() : Management::Child(Service::ID(RangeSubscriber::Name),
                                                       Service::ID(Management::Name))
{
    
}


RangeSubscriber::~RangeSubscriber()
{
}


void RangeSubscriber::Shutdown()
{
}


bool RangeSubscriber::CreateRangeSubscription(const Address& id, 
                                              const Byte deviceID,
                                              const unsigned int waitTimeMs)
{
    QueryRangeSensorConfiguration queryConfig(id, GetComponentID());
    Send(&queryConfig);

    QueryLocalRangeScan queryEvent;
    queryEvent.SetSensorID(deviceID);
    return EventsService()->RequestEveryChangeEvent(id, &queryEvent, waitTimeMs);
}


bool RangeSubscriber::GetRangeSensorInfo(const Address& id, 
                                         RangeSensorConfig::List& list,
                                         const unsigned int waitTimeMs) const
{
    QueryRangeSensorConfiguration query(id, GetComponentID());
    ReportRangeSensorConfiguration report;
    if(Send(&query, &report, waitTimeMs))
    {
        list = *report.GetConfiguration();
        return list.size() > 0 ? true : false;
    }
    return false;
}


bool RangeSubscriber::HaveRangeSubscription(const Address& id,
                                            const int deviceID) const
{
    bool result = false;
    Events::Subscription::List list = GetComponent()->EventsService()->GetSubscriptions(id, REPORT_LOCAL_RANGE_SCAN);
    Events::Subscription::List::iterator s;
    for(s = list.begin();
        s != list.end();
        s++)
    {
        if(deviceID < 0)
        {
            result = true;
            break;
        }
        else
        {
            QueryLocalRangeScan* query = dynamic_cast<QueryLocalRangeScan *>(s->mpQueryMessage);
            if(query && query->GetSensorID() == (Byte)deviceID)
            {
                result = true;
                break;
            }
        }
    }
 
    return result;
}


bool RangeSubscriber::CancelRangeSubscription(const Address& id,
                                              const int deviceID)
{
    bool result = false;

    Events::Subscription::List list = GetComponent()->EventsService()->GetSubscriptions(id, REPORT_LOCAL_RANGE_SCAN);
    Events::Subscription::List::iterator s;
    for(s = list.begin();
        s != list.end();
        s++)
    {
        if(deviceID < 0)
        {
            if(GetComponent()->EventsService()->CancelSubscription(s->mProducer,
                                                                   REPORT_LOCAL_RANGE_SCAN,
                                                                   s->mID,
                                                                   Service::DefaultWaitMs))
            {
                result = true;
            }
        }
        else
        {
            QueryLocalRangeScan* query = dynamic_cast<QueryLocalRangeScan *>(s->mpQueryMessage);
            if(query && query->GetSensorID() == (Byte)deviceID)
            {
                if(GetComponent()->EventsService()->CancelSubscription(s->mProducer,
                                                                       REPORT_LOCAL_RANGE_SCAN,
                                                                       s->mID,
                                                                       Service::DefaultWaitMs))
                {
                    result = true;
                }
                break;
            }
        }
    }

    return result;
}


void RangeSubscriber::RegisterCallback(Callback* callback, const bool add)
{
    Mutex::ScopedLock lock(&mRangeCallbacksMutex);
    if(add)
    {
        mRangeCallbacks.insert(callback);
    }
    else
    {
        Callback::Set::iterator cb;

        cb = mRangeCallbacks.find(callback);
        if(cb != mRangeCallbacks.end())
        {
            mRangeCallbacks.erase(cb);
        }
    }
}


void RangeSubscriber::Receive(const Message* message)
{
    switch(message->GetMessageCode())
    {
    case REPORT_LOCAL_RANGE_SCAN:
        {
            const ReportLocalRangeScan* report = 
                    dynamic_cast<const ReportLocalRangeScan*>(message);
            if(report)
            {
                Point3D::List scan;
                Point3D::List scanPolar;
                mRangeSensorMutex.Lock();
                
                std::map<Address, RangeSensorConfig::Map>::iterator comp;
                comp = mRangeSensors.find(report->GetSourceID());
                if(comp != mRangeSensors.end())
                {
                    RangeSensorConfig::Map::iterator config = comp->second.find(report->GetSensorID());
                    if(config != comp->second.end())
                    {
                        // Convert the data using the configuration info.
                        ReportLocalRangeScan::Scan::const_iterator v;
                        const ReportLocalRangeScan::Scan* ptr = report->GetScan();
                        double angle = config->second.mScanAngle/-2.0;
                        double incr = config->second.mAngleIncrement;
                        double divider = 1000.0;
                        if(config->second.mUnitType == RangeSensorConfig::CM)
                        {
                            divider = 100.0;
                        }

                        CxUtils::Quaternion rotX, rotY, rotZ, final;
                        rotX.SetRotationX(report->GetSensorOrientation().mX);
                        rotY.SetRotationY(report->GetSensorOrientation().mY);
                        rotZ.SetRotationZ(report->GetSensorOrientation().mZ);

                        final = rotX*rotY*rotZ;

                        bool first = true;
                        double pitch = 0.0;

                        for(v = ptr->begin();
                            v != ptr->end();
                            v++)
                        {
                            Point3D point((*v)/divider);  // Convert to meters.
                            Point3D polar(point.mX);
                            
                            // Convert to cartesian coordinates, relative to
                            // the sensor.
                            CxUtils::Quaternion rotation;
                            rotation.SetRotationZ(angle);
                            point = rotation.Rotate(point);
                            
                            // Now translate and rotate relative to the
                            // platform frame.                                                     
                            point = final.Rotate(point);
                            point += report->GetSensorLocation();   
                            
                            // Now add to final scan.
                            scan.push_back(point);
                            // If first
                            if(first)
                            {
                                pitch = atan2(point.mZ, 1.0);
                                first = false;
                            }
                            polar.mY = pitch;
                            polar.mZ = angle;
                            scanPolar.push_back(polar);

                            angle += incr;
                        }                        
                    }
                }

                mRangeSensorMutex.Unlock();

                ProcessLocalRangeScan(scan,
                                      report->GetSourceID(),
                                      report->GetSensorID(),
                                      report->GetTimeStamp());

                mRangeCallbacksMutex.Lock();

                Callback::Set::iterator cb;
                for(cb = mRangeCallbacks.begin();
                    cb != mRangeCallbacks.end();
                    cb++)
                {
                    (*cb)->ProcessLocalRangeScan(scan,
                                                 report->GetSourceID(),
                                                 report->GetSensorID(),
                                                 report->GetTimeStamp());
                    (*cb)->ProcessLocalRangeScanPolar(scanPolar,
                                                      report->GetSourceID(),
                                                      report->GetSensorID(),
                                                      report->GetTimeStamp());
                }

                mRangeCallbacksMutex.Unlock();
                
            }
        }
        break;
    case REPORT_RANGE_SENSOR_CONFIGURATION:
        {
            const ReportRangeSensorConfiguration* report = 
                dynamic_cast<const ReportRangeSensorConfiguration*>(message);
            if(report)
            {
                Mutex::ScopedLock lock(&mRangeSensorMutex);
                RangeSensorConfig::List::const_iterator c;
                for(c = report->GetConfiguration()->begin();
                    c != report->GetConfiguration()->end();
                    c++)
                {
                    mRangeSensors[report->GetSourceID()][c->mID] = *c;
                }
            }
        }
        break;
    default:
        break;
    }
}


Message* RangeSubscriber::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 */