JAUS++: jaus++/2.0/src/jaus/mobility/sensors/velocitystatesensor.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/velocitystatesensor.h"
00042 #include "jaus/core/events/createevent.h"
00043 
00044 using namespace JAUS;
00045 
00046 const std::string VelocityStateSensor::Name = "urn:jaus:jss:mobility:VelocityStateSensor";
00047 
00048 
00054 VelocityStateSensor::VelocityStateSensor(const double updateRate) : Events::Child(Service::ID(VelocityStateSensor::Name),
00055                                                                                   Service::ID(Events::Name))
00056 {
00057     mMaxUpdateRate = 10;
00058     SetSensorUpdateRate(updateRate);
00059 }
00060 
00061 
00067 VelocityStateSensor::~VelocityStateSensor()
00068 {
00069 }
00070 
00071 
00077 void VelocityStateSensor::SetVelocityState(const VelocityState& state)
00078 {
00079     Mutex::ScopedLock lock(&mVelocityStateMutex);
00080     mVelocityState = state;
00081     SignalEvent(REPORT_VELOCITY_STATE);
00082 }
00083 
00084 
00092 bool VelocityStateSensor::SetSensorUpdateRate(const double rate)
00093 {
00094     if(rate >= CreateEvent::Limits::MinUpdateRate &&
00095        rate <= CreateEvent::Limits::MaxUpdateRate)
00096     {
00097         mMaxUpdateRate = rate;
00098         return true;
00099     }
00100     return false;      
00101 }
00102 
00103 
00111 VelocityState VelocityStateSensor::GetVelocityState() const
00112 {
00113     Mutex::ScopedLock lock(&mVelocityStateMutex);
00114     return mVelocityState;
00115 }
00116 
00117 
00127 bool VelocityStateSensor::GenerateEvent(const Events::Subscription& info) const
00128 {
00129     if(info.mpQueryMessage->GetMessageCode() == QUERY_VELOCITY_STATE)
00130     {
00131         const QueryVelocityState* query = dynamic_cast<const QueryVelocityState*>(info.mpQueryMessage);
00132 
00133         if(query == NULL)
00134         {
00135             return false;
00136         }
00137 
00138         VelocityState report;
00139         CreateReportFromQuery(query, report);
00140         SendEvent(info, &report);
00141 
00142         return true;
00143     }
00144     return false;
00145 }
00146 
00147 
00163 bool VelocityStateSensor::IsEventSupported(const Events::Type type,
00164                                        const double requestedPeriodicRate,
00165                                        const Message* queryMessage,
00166                                        double& confirmedPeriodicRate,
00167                                        std::string& errorMessage) const
00168 {
00169     // Support any type of event for QUERY_VELOCITY_STATE data.
00170     if(queryMessage->GetMessageCode() == QUERY_VELOCITY_STATE)
00171     {
00172         if(requestedPeriodicRate >= CreateEvent::Limits::MinUpdateRate &&
00173            requestedPeriodicRate <= mMaxUpdateRate)
00174         {
00175             confirmedPeriodicRate = requestedPeriodicRate;
00176         }
00177         else
00178         {
00179             confirmedPeriodicRate = mMaxUpdateRate;
00180         }
00181         return true;
00182     }
00183     return false;
00184 }
00185 
00186 
00198 void VelocityStateSensor::Receive(const JAUS::Message *message)
00199 {
00200     switch(message->GetMessageCode())
00201     {
00202     case QUERY_VELOCITY_STATE:
00203         {
00204             const QueryVelocityState* query = dynamic_cast<const JAUS::QueryVelocityState*>(message);
00205             if(query)
00206             {
00207                 VelocityState report;
00208                 //CreateReportFromQuery already locks mVelocityStateMutex
00209                 CreateReportFromQuery(query, report);
00210                 if(Send(&report) == false)
00211                 {
00212                     std::cout << "ERROR!\n";
00213                 }
00214             }
00215         }
00216         break;
00217     case REPORT_VELOCITY_STATE:
00218         {
00219             const ReportVelocityState* report = dynamic_cast<const JAUS::ReportVelocityState*>(message);
00220             if(report && report->GetSourceID() == GetSynchronizeID())
00221             {
00222                 SetVelocityState(*report);
00223             }
00224         }
00225         break;
00226     default:
00227         break;
00228     };
00229 }
00230 
00231 
00243 Message* VelocityStateSensor::CreateMessage(const UShort messageCode) const
00244 {
00245     Message* message;
00246     switch(messageCode)
00247     {
00248     case QUERY_VELOCITY_STATE:
00249         message = new QueryVelocityState();
00250         break;
00251     case REPORT_VELOCITY_STATE:
00252         message = new ReportVelocityState();
00253         break;
00254     default:
00255         message = NULL;
00256         break;
00257     };
00258     return message;
00259 }
00260 
00261 
00270 void VelocityStateSensor::CreateReportFromQuery(const QueryVelocityState* query, VelocityState& report) const
00271 {
00272     Mutex::ScopedLock lock(&mVelocityStateMutex);
00273 
00274     report.ClearMessage();
00275     report.SetDestinationID(query->GetSourceID());
00276     report.SetSourceID(GetComponentID());
00277 
00278    if(query->IsFieldPresent(VelocityState::PresenceVector::VelocityX) &&
00279         mVelocityState.IsFieldPresent(VelocityState::PresenceVector::VelocityX))
00280     {
00281         report.SetVelocityX(mVelocityState.GetVelocityX());
00282     }
00283    if(query->IsFieldPresent(VelocityState::PresenceVector::VelocityY) &&
00284         mVelocityState.IsFieldPresent(VelocityState::PresenceVector::VelocityY))
00285     {
00286         report.SetVelocityY(mVelocityState.GetVelocityY());
00287     }
00288    if(query->IsFieldPresent(VelocityState::PresenceVector::VelocityZ) &&
00289         mVelocityState.IsFieldPresent(VelocityState::PresenceVector::VelocityZ))
00290     {
00291         report.SetVelocityZ(mVelocityState.GetVelocityZ());
00292     }
00293    if(query->IsFieldPresent(VelocityState::PresenceVector::VelocityRMS) &&
00294         mVelocityState.IsFieldPresent(VelocityState::PresenceVector::VelocityRMS))
00295     {
00296         report.SetVelocityRMS(mVelocityState.GetVelocityRMS());
00297     }
00298    if(query->IsFieldPresent(VelocityState::PresenceVector::RollRate) &&
00299         mVelocityState.IsFieldPresent(VelocityState::PresenceVector::RollRate))
00300     {
00301         report.SetRollRate(mVelocityState.GetRollRate());
00302     }
00303    if(query->IsFieldPresent(VelocityState::PresenceVector::PitchRate) &&
00304         mVelocityState.IsFieldPresent(VelocityState::PresenceVector::PitchRate))
00305     {
00306         report.SetPitchRate(mVelocityState.GetPitchRate());
00307     }
00308    if(query->IsFieldPresent(VelocityState::PresenceVector::YawRate) &&
00309         mVelocityState.IsFieldPresent(VelocityState::PresenceVector::YawRate))
00310     {
00311         report.SetYawRate(mVelocityState.GetYawRate());
00312     }
00313    if(query->IsFieldPresent(VelocityState::PresenceVector::RateRMS) &&
00314         mVelocityState.IsFieldPresent(VelocityState::PresenceVector::RateRMS))
00315     {
00316         report.SetRateRMS(mVelocityState.GetRateRMS());
00317     }
00318    if(query->IsFieldPresent(VelocityState::PresenceVector::TimeStamp) &&
00319         mVelocityState.IsFieldPresent(VelocityState::PresenceVector::TimeStamp))
00320     {
00321         report.SetTimeStamp(mVelocityState.GetTimeStamp());
00322     }
00323 }
00324 
00325 
00331 void VelocityStateSensor::PrintStatus() const
00332 {
00333     //Mutex::ScopedLock lock(&mVelocityStateMutex);
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 Velocity State:\n";
00341     }
00342     VelocityState velocity;
00343     mVelocityStateMutex.Lock();
00344     velocity = mVelocityState;
00345     mVelocityStateMutex.Unlock();
00346     velocity.PrintMessageBody();
00347 }
00348 
00349 
00359 void VelocityStateSensor::CheckServiceSynchronization(const unsigned int timeSinceLastCheckMs)
00360 {
00361     Address syncID = GetSynchronizeID();
00362     if(syncID.IsValid())
00363     {
00364         // See if we have an active subscription, and if not create it.
00365         if(EventsService()->HaveSubscription(REPORT_VELOCITY_STATE, syncID) == false)
00366         {
00367             QueryVelocityState query(syncID, GetComponentID());
00368             query.SetPresenceVector(query.GetPresenceVectorMask());
00369             EventsService()->RequestEveryChangeEvent(syncID, &query);
00370         }
00371     }
00372 }
00373 
00374 
00375 /*  End of File */