JAUS++: jaus++/2.0/src/jaus/mobility/sensors/globalposesensor.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/globalposesensor.h"
00042 #include "jaus/core/events/createevent.h"
00043 
00044 using namespace JAUS;
00045 
00046 const std::string GlobalPoseSensor::Name = "urn:jaus:jss:mobility:GlobalPoseSensor";
00047 
00053 GlobalPoseSensor::GlobalPoseSensor(const double updateRate) : AccessControl::Child(Service::ID(GlobalPoseSensor::Name), Service::ID(AccessControl::Name))
00054 {
00055     mMaxUpdateRate = 10.0;
00056     SetSensorUpdateRate(updateRate);
00057 }
00058 
00059 
00065 GlobalPoseSensor::~GlobalPoseSensor()
00066 {
00067 }
00068 
00069 
00077 void GlobalPoseSensor::SetGlobalPose(const JAUS::SetGlobalPose& globalPose)
00078 {
00079     Mutex::ScopedLock lock(&mGlobalPoseMutex);
00080     mGlobalPose.ClearMessage();
00081     UInt pv = globalPose.GetPresenceVector();
00082     if( (pv & GlobalPose::PresenceVector::Latitude) > 0) { mGlobalPose.SetLatitude(globalPose.GetLatitude()); }
00083     if( (pv & GlobalPose::PresenceVector::Longitude) > 0) { mGlobalPose.SetLongitude(globalPose.GetLongitude()); }
00084     if( (pv & GlobalPose::PresenceVector::Altitude) > 0) { mGlobalPose.SetAltitude(globalPose.GetAltitude()); }
00085     if( (pv & GlobalPose::PresenceVector::PositionRMS) > 0) { mGlobalPose.SetPositionRMS(globalPose.GetPositionRMS()); }
00086     if( (pv & GlobalPose::PresenceVector::Roll) > 0) { mGlobalPose.SetRoll(globalPose.GetRoll()); }
00087     if( (pv & GlobalPose::PresenceVector::Pitch) > 0) { mGlobalPose.SetPitch(globalPose.GetPitch()); }
00088     if( (pv & GlobalPose::PresenceVector::Yaw) > 0) { mGlobalPose.SetYaw(globalPose.GetYaw()); }
00089     if( (pv & GlobalPose::PresenceVector::AttitudeRMS) > 0) { mGlobalPose.SetAttitudeRMS(globalPose.GetAttitudeRMS()); }
00090     if( (pv & GlobalPose::PresenceVector::TimeStamp) > 0) { mGlobalPose.SetTimeStamp(globalPose.GetTimeStamp()); }
00091 
00092     SignalEvent(REPORT_GLOBAL_POSE);
00093 }
00094 
00095 
00103 void GlobalPoseSensor::SetGlobalPose(const JAUS::GlobalPose& globalPose)
00104 {
00105     Mutex::ScopedLock lock(&mGlobalPoseMutex);
00106     mGlobalPose = globalPose;
00107     SignalEvent(REPORT_GLOBAL_POSE);
00108 }
00109 
00110 
00123 bool GlobalPoseSensor::SetGlobalPose(const Wgs& position,
00124                                      const Point3D& orientation,
00125                                      const Time& time)
00126 {
00127     Mutex::ScopedLock lock(&mGlobalPoseMutex);
00128     if(mGlobalPose.SetPose(position, orientation, time))
00129     {
00130         SignalEvent(REPORT_GLOBAL_POSE);
00131         return true;
00132     }
00133     return false;
00134 }
00135 
00136 
00149 bool GlobalPoseSensor::SetGlobalPosition(const Wgs& position, const Time& time)
00150 {
00151     Mutex::ScopedLock lock(&mGlobalPoseMutex);
00152     if(mGlobalPose.SetPosition(position, time))
00153     {
00154         SignalEvent(REPORT_GLOBAL_POSE);
00155         return true;
00156     }
00157     return false;
00158 }
00159 
00160 
00173 bool GlobalPoseSensor::SetGlobalOrientation(const Point3D& orientation, const Time& time)
00174 {
00175     Mutex::ScopedLock lock(&mGlobalPoseMutex);
00176     if(mGlobalPose.SetOrientation(orientation, time))
00177     {
00178         SignalEvent(REPORT_GLOBAL_POSE);
00179         return true;
00180     }
00181     return false;
00182 }
00183 
00184 
00193 void GlobalPoseSensor::SetGeomagneticVariation(const double magneticVariation)
00194 {
00195     Mutex::ScopedLock lock(&mGlobalPoseMutex);
00196     if(magneticVariation >= SetGeomagneticProperty::Limits::MinMagneticVariation &&
00197        magneticVariation <= SetGeomagneticProperty::Limits::MaxMagneticVariation)
00198     {
00199         mGeomagneticProperty = magneticVariation;
00200         SignalEvent(REPORT_GEOMAGNETIC_PROPERTY);
00201     }
00202 }
00203 
00204 
00212 bool GlobalPoseSensor::SetSensorUpdateRate(const double rate)
00213 {
00214     if(rate >= CreateEvent::Limits::MinUpdateRate &&
00215        rate <= CreateEvent::Limits::MaxUpdateRate)
00216     {
00217         mMaxUpdateRate = rate;
00218         return true;
00219     }
00220     return false;      
00221 }
00222 
00223 
00231 GlobalPose GlobalPoseSensor::GetGlobalPose() const
00232 {
00233     Mutex::ScopedLock lock(&mGlobalPoseMutex);
00234     return mGlobalPose;
00235 }
00236 
00237 
00245 double GlobalPoseSensor::GetGeomagneticVariation() const
00246 {
00247     Mutex::ScopedLock lock(&mGlobalPoseMutex);
00248     return mGeomagneticProperty;
00249 }
00250 
00251 
00261 bool GlobalPoseSensor::GenerateEvent(const Events::Subscription& info) const
00262 {
00263     if(info.mpQueryMessage->GetMessageCode() == QUERY_GLOBAL_POSE)
00264     {
00265         const QueryGlobalPose* query = dynamic_cast<const QueryGlobalPose*>(info.mpQueryMessage);
00266 
00267         if(query == NULL)
00268         {
00269             return false;
00270         }
00271 
00272         GlobalPose report;
00273         CreateReportFromQuery(query, report);
00274         SendEvent(info, &report);
00275 
00276         return true;
00277     }
00278     else if(info.mpQueryMessage->GetMessageCode() == QUERY_GEOMAGNETIC_PROPERTY)
00279     {
00280         ReportGeomagneticProperty report;
00281         report.SetMagneticVariation(mGeomagneticProperty);
00282         SendEvent(info, &report);
00283 
00284         return true;
00285     }
00286     return false;
00287 }
00288 
00289 
00305 bool GlobalPoseSensor::IsEventSupported(const Events::Type type,
00306                                         const double requestedPeriodicRate,
00307                                         const Message* queryMessage,
00308                                         double& confirmedPeriodicRate,
00309                                         std::string& errorMessage) const
00310 {
00311     if(queryMessage->GetMessageCode() == QUERY_GLOBAL_POSE)
00312     {
00313         if(requestedPeriodicRate <= mMaxUpdateRate)
00314         {
00315             confirmedPeriodicRate = requestedPeriodicRate;
00316         }
00317         else
00318         {
00319             confirmedPeriodicRate = mMaxUpdateRate;
00320         }
00321         return true;
00322     }
00323     else if(queryMessage->GetMessageCode() == QUERY_GEOMAGNETIC_PROPERTY)
00324     {
00325         if(requestedPeriodicRate <= mMaxUpdateRate)
00326         {
00327             confirmedPeriodicRate = requestedPeriodicRate;
00328         }
00329         else
00330         {
00331             confirmedPeriodicRate = mMaxUpdateRate;
00332         }
00333         return true;
00334     }
00335     return false;
00336 }
00337 
00338 
00350 void GlobalPoseSensor::Receive(const JAUS::Message *message)
00351 {
00352     switch(message->GetMessageCode())
00353     {
00354     case QUERY_GLOBAL_POSE:
00355         {
00356             const QueryGlobalPose* query = dynamic_cast<const QueryGlobalPose*>(message);
00357             if(query)
00358             {
00359                 ReportGlobalPose report;
00360                 CreateReportFromQuery(query, report);
00361                 Send(&report);
00362             }
00363         }
00364         break;
00365     case REPORT_GLOBAL_POSE:
00366         {
00367             const JAUS::ReportGlobalPose* report = dynamic_cast<const JAUS::ReportGlobalPose*>(message);
00368             if(report && report->GetSourceID() == GetSynchronizeID())
00369             {
00370                 SetGlobalPose(*report);
00371             }
00372         }
00373         break;
00374     case SET_GLOBAL_POSE:
00375         {
00376             const JAUS::SetGlobalPose* command = dynamic_cast<const JAUS::SetGlobalPose*>(message);
00377             if(command)
00378             {
00379                 SetGlobalPose(*command);
00380             }
00381         }
00382         break;
00383     case QUERY_GEOMAGNETIC_PROPERTY:
00384         {
00385 
00386             ReportGeomagneticProperty report(message->GetSourceID(), GetComponentID());
00387             report.SetMagneticVariation(GetGeomagneticVariation());
00388             Send(&report);
00389         }
00390         break;
00391     case SET_GEOMAGNETIC_PROPERTY:
00392         {
00393             const SetGeomagneticProperty* command = dynamic_cast<const SetGeomagneticProperty*>(message);
00394             if(command)
00395             {
00396                 SetGeomagneticVariation(command->GetMagneticVariation());
00397             }
00398         }
00399         break;
00400     case REPORT_GEOMAGNETIC_PROPERTY:
00401         {
00402             const JAUS::ReportGeomagneticProperty* report = dynamic_cast<const JAUS::ReportGeomagneticProperty*>(message);
00403             if(report && report->GetSourceID() == GetSynchronizeID())
00404             {
00405                 SetGeomagneticVariation(report->GetMagneticVariation());
00406             }
00407         }
00408         break;
00409     default:
00410         break;
00411     };
00412 }
00413 
00414 
00426 Message* GlobalPoseSensor::CreateMessage(const UShort messageCode) const
00427 {
00428     Message* message = NULL;
00429     switch(messageCode)
00430     {
00431     case QUERY_GLOBAL_POSE:
00432         message = new QueryGlobalPose();
00433         break;
00434     case QUERY_GEOMAGNETIC_PROPERTY:
00435         message = new QueryGeomagneticProperty();
00436         break;
00437     case REPORT_GLOBAL_POSE:
00438         message = new ReportGlobalPose();
00439         break;
00440     case SET_GLOBAL_POSE:
00441         message = new JAUS::SetGlobalPose();
00442         break;
00443     case REPORT_GEOMAGNETIC_PROPERTY:
00444         message = new ReportGeomagneticProperty();
00445         break;
00446     case SET_GEOMAGNETIC_PROPERTY:
00447         message = new SetGeomagneticProperty();
00448         break;
00449     default:
00450         message = NULL;
00451         break;
00452     };
00453     return message;
00454 }
00455 
00456 
00462 void GlobalPoseSensor::PrintStatus() const
00463 {
00464     //Mutex::ScopedLock lock(&mGlobalPoseMutex);
00465     if(GetSynchronizeID().IsValid())
00466     {
00467         std::cout << "[" << GetServiceID().ToString() << "] - Synchronized to [" << GetSynchronizeID().ToString() << "]:\n";
00468     }
00469     else
00470     {
00471         std::cout << "[" << GetServiceID().ToString() << "] - Current Global Pose:\n";
00472     }
00473     GlobalPose pose;
00474     mGlobalPoseMutex.Lock();
00475     pose = mGlobalPose;
00476     mGlobalPoseMutex.Unlock();
00477     pose.PrintMessageBody();
00478 }
00479 
00480 
00489 void GlobalPoseSensor::CreateReportFromQuery(const QueryGlobalPose* query, GlobalPose& report) const
00490 {
00491     Mutex::ScopedLock lock(&mGlobalPoseMutex);
00492     report.ClearMessage();
00493     report.SetDestinationID(query->GetSourceID());
00494     report.SetSourceID(GetComponentID());
00495     UInt pv1 = query->GetPresenceVector();
00496     UInt pv2 = mGlobalPose.GetPresenceVector();
00497 
00498     if( (pv2 & (pv1 & GlobalPose::PresenceVector::Latitude)) > 0) { report.SetLatitude(mGlobalPose.GetLatitude()); }
00499     if( (pv2 & (pv1 & GlobalPose::PresenceVector::Longitude)) > 0) { report.SetLongitude(mGlobalPose.GetLongitude()); }
00500     if( (pv2 & (pv1 & GlobalPose::PresenceVector::Altitude)) > 0) { report.SetAltitude(mGlobalPose.GetAltitude()); }
00501     if( (pv2 & (pv1 & GlobalPose::PresenceVector::PositionRMS)) > 0) { report.SetPositionRMS(mGlobalPose.GetPositionRMS()); }
00502     if( (pv2 & (pv1 & GlobalPose::PresenceVector::Roll)) > 0) { report.SetRoll(mGlobalPose.GetRoll()); }
00503     if( (pv2 & (pv1 & GlobalPose::PresenceVector::Pitch)) > 0) { report.SetPitch(mGlobalPose.GetPitch()); }
00504     if( (pv2 & (pv1 & GlobalPose::PresenceVector::Yaw)) > 0) { report.SetYaw(mGlobalPose.GetYaw()); }
00505     if( (pv2 & (pv1 & GlobalPose::PresenceVector::AttitudeRMS)) > 0) { report.SetAttitudeRMS(mGlobalPose.GetAttitudeRMS()); }
00506     if( (pv2 & (pv1 & GlobalPose::PresenceVector::TimeStamp)) > 0) { report.SetTimeStamp(mGlobalPose.GetTimeStamp()); }
00507 }
00508 
00509 
00519 void GlobalPoseSensor::CheckServiceSynchronization(const unsigned int timeSinceLastCheckMs)
00520 {
00521     Address syncID = GetSynchronizeID();
00522     if(syncID.IsValid())
00523     {
00524         // See if we have an active subscription, and if not create it.
00525         if(EventsService()->HaveSubscription(REPORT_GLOBAL_POSE, syncID) == false)
00526         {
00527             QueryGlobalPose query(syncID, GetComponentID());
00528             query.SetPresenceVector(query.GetPresenceVectorMask());
00529             EventsService()->RequestEveryChangeEvent(syncID, &query);
00530         }
00531         if(EventsService()->HaveSubscription(REPORT_GEOMAGNETIC_PROPERTY, syncID) == false)
00532         {
00533             QueryGeomagneticProperty query(syncID, GetComponentID());
00534             EventsService()->RequestEveryChangeEvent(syncID, &query);
00535         }
00536     }
00537 }
00538 
00539 
00540 /*  End of File */