JAUS++: jaus++/2.0/src/jaus/mobility/sensors/accelerationstatesensor.cpp Source File

Go to the documentation of this file.
00001 
00002 
00003 
00004 
00005 
00006 
00007 
00008 
00009 
00010 
00011 
00012 
00013 
00014 
00015 
00016 
00017 
00018 
00019 
00020 
00021 
00022 
00023 
00024 
00025 
00026 
00027 
00028 
00029 
00030 
00031 
00032 
00033 
00034 
00035 
00036 
00037 
00038 
00039 
00040 
00041 #include "jaus/mobility/sensors/accelerationstatesensor.h"
00042 #include "jaus/core/events/createevent.h"
00043 
00044 using namespace JAUS;
00045 
00046 const std::string AccelerationStateSensor::Name = "urn:jaus:jss:mobility:AccelerationStateSensor";
00047 
00048 
00054 AccelerationStateSensor::AccelerationStateSensor(const double updateRate)
00055         : Events::Child(Service::ID(AccelerationStateSensor::Name), Service::ID(Events::Name))
00056 {
00057     mMaxUpdateRate = 10;
00058     SetSensorUpdateRate(updateRate);
00059 }
00060 
00061 
00067 AccelerationStateSensor::~AccelerationStateSensor()
00068 {
00069 }
00070 
00071 
00072 
00078 void AccelerationStateSensor::SetAccelerationState(const ReportAccelerationState& state)
00079 {
00080     Mutex::ScopedLock lock(&mAccelerationStateMutex);
00081     mAccelerationState = state;
00082     SignalEvent(REPORT_ACCELERATION_STATE);
00083 }
00084 
00085 
00093 bool AccelerationStateSensor::SetSensorUpdateRate(const double rate)
00094 {
00095     if(rate >= CreateEvent::Limits::MinUpdateRate &&
00096        rate <= CreateEvent::Limits::MaxUpdateRate)
00097     {
00098         mMaxUpdateRate = rate;
00099         return true;
00100     }
00101     return false;      
00102 }
00103 
00104 
00112 ReportAccelerationState AccelerationStateSensor::GetAccelerationState() const
00113 {
00114     Mutex::ScopedLock lock(&mAccelerationStateMutex);
00115     return mAccelerationState;
00116 }
00117 
00118 
00128 bool AccelerationStateSensor::GenerateEvent(const Events::Subscription& info) const
00129 {
00130     if(info.mpQueryMessage->GetMessageCode() == QUERY_ACCELERATION_STATE)
00131     {
00132         const QueryAccelerationState* query = dynamic_cast<const QueryAccelerationState*>(info.mpQueryMessage);
00133 
00134         if(query == NULL)
00135         {
00136             return false;
00137         }
00138 
00139         ReportAccelerationState report;
00140         CreateReportFromQuery(query, report);
00141         SendEvent(info, &report);
00142 
00143         return true;
00144     }
00145     return false;
00146 }
00147 
00148 
00164 bool AccelerationStateSensor::IsEventSupported(const Events::Type type,
00165                                        const double requestedPeriodicRate,
00166                                        const Message* queryMessage,
00167                                        double& confirmedPeriodicRate,
00168                                        std::string& errorMessage) const
00169 {
00170     // Support any type of event for QUERY_ACCELERATION_STATE data.
00171     if(queryMessage->GetMessageCode() == QUERY_ACCELERATION_STATE)
00172     {
00173         if(requestedPeriodicRate >= CreateEvent::Limits::MinUpdateRate &&
00174            requestedPeriodicRate <= mMaxUpdateRate)
00175         {
00176             confirmedPeriodicRate = requestedPeriodicRate;
00177         }
00178         else
00179         {
00180             confirmedPeriodicRate = mMaxUpdateRate;
00181         }
00182         return true;
00183     }
00184     return false;
00185 }
00186 
00187 
00199 void AccelerationStateSensor::Receive(const JAUS::Message *message)
00200 {
00201     switch(message->GetMessageCode())
00202     {
00203     case QUERY_ACCELERATION_STATE:
00204         {
00205             const QueryAccelerationState* query = dynamic_cast<const JAUS::QueryAccelerationState*>(message);
00206             if(query)
00207             {
00208                 ReportAccelerationState report;
00209                 //CreateReportFromQuery already locks mAccelerationStateMutex
00210                 CreateReportFromQuery(query, report);
00211                 Send(&report);
00212             }
00213         }
00214         break;
00215     case REPORT_ACCELERATION_STATE:
00216         {
00217             const JAUS::ReportAccelerationState* report = dynamic_cast<const JAUS::ReportAccelerationState*>(message);
00218             if(report && report->GetSourceID() == GetSynchronizeID())
00219             {
00220                 SetAccelerationState(*report);
00221             }
00222         }
00223         break;
00224     default:
00225         break;
00226     };
00227 }
00228 
00229 
00241 Message* AccelerationStateSensor::CreateMessage(const UShort messageCode) const
00242 {
00243     Message* message;
00244     switch(messageCode)
00245     {
00246     case QUERY_ACCELERATION_STATE:
00247         message = new QueryAccelerationState();
00248         break;
00249     case REPORT_ACCELERATION_STATE:
00250         message = new ReportAccelerationState();
00251         break;
00252     default:
00253         message = NULL;
00254         break;
00255     };
00256     return message;
00257 }
00258 
00259 
00268 void AccelerationStateSensor::CreateReportFromQuery(const QueryAccelerationState* query, ReportAccelerationState& report) const
00269 {
00270     Mutex::ScopedLock lock(&mAccelerationStateMutex);
00271     report.SetDestinationID(query->GetSourceID());
00272     report.SetSourceID(GetComponentID());
00273 
00274    if(query->IsFieldPresent(ReportAccelerationState::PresenceVector::AccelerationX) &&
00275         mAccelerationState.IsFieldPresent(ReportAccelerationState::PresenceVector::AccelerationX))
00276     {
00277         report.SetAccelerationX(mAccelerationState.GetAccelerationX());
00278     }
00279    if(query->IsFieldPresent(ReportAccelerationState::PresenceVector::AccelerationY) &&
00280         mAccelerationState.IsFieldPresent(ReportAccelerationState::PresenceVector::AccelerationY))
00281     {
00282         report.SetAccelerationY(mAccelerationState.GetAccelerationY());
00283     }
00284    if(query->IsFieldPresent(ReportAccelerationState::PresenceVector::AccelerationZ) &&
00285         mAccelerationState.IsFieldPresent(ReportAccelerationState::PresenceVector::AccelerationZ))
00286     {
00287         report.SetAccelerationZ(mAccelerationState.GetAccelerationZ());
00288     }
00289    if(query->IsFieldPresent(ReportAccelerationState::PresenceVector::AccelerationRMS) &&
00290         mAccelerationState.IsFieldPresent(ReportAccelerationState::PresenceVector::AccelerationRMS))
00291     {
00292         report.SetAccelerationRMS(mAccelerationState.GetAccelerationRMS());
00293     }
00294    if(query->IsFieldPresent(ReportAccelerationState::PresenceVector::RollAcceleration) &&
00295         mAccelerationState.IsFieldPresent(ReportAccelerationState::PresenceVector::RollAcceleration))
00296     {
00297         report.SetRollAcceleration(mAccelerationState.GetRollAcceleration());
00298     }
00299    if(query->IsFieldPresent(ReportAccelerationState::PresenceVector::PitchAcceleration) &&
00300         mAccelerationState.IsFieldPresent(ReportAccelerationState::PresenceVector::PitchAcceleration))
00301     {
00302         report.SetPitchAcceleration(mAccelerationState.GetPitchAcceleration());
00303     }
00304    if(query->IsFieldPresent(ReportAccelerationState::PresenceVector::YawAcceleration) &&
00305         mAccelerationState.IsFieldPresent(ReportAccelerationState::PresenceVector::YawAcceleration))
00306     {
00307         report.SetYawAcceleration(mAccelerationState.GetYawAcceleration());
00308     }
00309    if(query->IsFieldPresent(ReportAccelerationState::PresenceVector::RotationalAccelerationRMS) &&
00310         mAccelerationState.IsFieldPresent(ReportAccelerationState::PresenceVector::RotationalAccelerationRMS))
00311     {
00312         report.SetRotationalAccelerationRMS(mAccelerationState.GetRotationalAccelerationRMS());
00313     }
00314    if(query->IsFieldPresent(ReportAccelerationState::PresenceVector::TimeStamp) &&
00315         mAccelerationState.IsFieldPresent(ReportAccelerationState::PresenceVector::TimeStamp))
00316     {
00317         report.SetTimeStamp(mAccelerationState.GetTimeStamp());
00318     }
00319 }
00320 
00321 
00327 void AccelerationStateSensor::PrintStatus() const
00328 {
00329     //Mutex::ScopedLock lock(&mAccelerationStateMutex);
00330     mAccelerationStateMutex.Lock();
00331     JAUS::AccelerationState acc = mAccelerationState;
00332     mAccelerationStateMutex.Unlock();
00333 
00334     if(GetSynchronizeID().IsValid())
00335     {
00336         std::cout << "[" << GetServiceID().ToString() << "] - Synchronized to [" << GetSynchronizeID().ToString() << "]:\n";
00337     }
00338     else
00339     {
00340         std::cout << "[" << GetServiceID().ToString() << "] - Current Acceleration State:\n";
00341     }
00342 
00343     acc.PrintMessageBody();
00344 }
00345 
00346 
00356 void AccelerationStateSensor::CheckServiceSynchronization(const unsigned int timeSinceLastCheckMs)
00357 {
00358     Address syncID = GetSynchronizeID();
00359     if(syncID.IsValid())
00360     {
00361         // See if we have an active subscription, and if not create it.
00362         if(EventsService()->HaveSubscription(REPORT_ACCELERATION_STATE, syncID) == false)
00363         {
00364             QueryAccelerationState query(syncID, GetComponentID());
00365             query.SetPresenceVector(query.GetPresenceVectorMask());
00366             EventsService()->RequestEveryChangeEvent(syncID, &query);
00367         }
00368     }
00369 }
00370 
00371 
00372 /*  End of File */