JAUS++: jaus++/2.0/src/jaus/mobility/sensors/localposesensor.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/localposesensor.h"
00042 #include "jaus/core/events/createevent.h"
00043 #include "jaus/core/component.h"
00044 
00045 #include <cxutils/math/cxmath.h>
00046 
00047 using namespace JAUS;
00048 
00049 const std::string LocalPoseSensor::Name = "urn:jaus:jss:mobility:LocalPoseSensor";
00050 
00056 LocalPoseSensor::LocalPoseSensor(const double updateRate)
00057                                         : AccessControl::Child(Service::ID(LocalPoseSensor::Name), 
00058                                                                Service::ID(Events::Name))
00059 {
00060     mMaxUpdateRate = 10.0;
00061     SetSensorUpdateRate(updateRate);
00062 }
00063 
00064 
00070 LocalPoseSensor::~LocalPoseSensor()
00071 {
00072 }
00073 
00074 
00082 void LocalPoseSensor::SetLocalPose(const JAUS::SetLocalPose& localPose)
00083 {
00084     Mutex::ScopedLock lock(&mLocalPoseMutex);
00085     mLocalPose.ClearMessage();
00086     UInt pv = localPose.GetPresenceVector();
00087     if( (pv & LocalPose::PresenceVector::X) > 0) { mLocalPose.SetX(localPose.GetX()); }
00088     if( (pv & LocalPose::PresenceVector::Y) > 0) { mLocalPose.SetY(localPose.GetY()); }
00089     if( (pv & LocalPose::PresenceVector::Z) > 0) { mLocalPose.SetZ(localPose.GetZ()); }
00090     if( (pv & LocalPose::PresenceVector::PositionRMS) > 0) { mLocalPose.SetPositionRMS(localPose.GetPositionRMS()); }
00091     if( (pv & LocalPose::PresenceVector::Roll) > 0) { mLocalPose.SetRoll(localPose.GetRoll()); }
00092     if( (pv & LocalPose::PresenceVector::Pitch) > 0) { mLocalPose.SetPitch(localPose.GetPitch()); }
00093     if( (pv & LocalPose::PresenceVector::Yaw) > 0) { mLocalPose.SetYaw(localPose.GetYaw()); }
00094     if( (pv & LocalPose::PresenceVector::AttitudeRMS) > 0) { mLocalPose.SetAttitudeRMS(localPose.GetAttitudeRMS()); }
00095     if( (pv & LocalPose::PresenceVector::TimeStamp) > 0) { mLocalPose.SetTimeStamp(localPose.GetTimeStamp()); }
00096 
00097     SignalEvent(REPORT_LOCAL_POSE);
00098 }
00099 
00100 
00108 void LocalPoseSensor::SetLocalPose(const JAUS::LocalPose& localPose)
00109 {
00110     Mutex::ScopedLock lock(&mLocalPoseMutex);
00111     mLocalPose = localPose;
00112     SignalEvent(REPORT_LOCAL_POSE);
00113 }
00114 
00115 
00128 bool LocalPoseSensor::SetLocalPose(const Point3D& position,
00129                                    const Point3D& orientation,
00130                                    const Time& time)
00131 {
00132     Mutex::ScopedLock lock(&mLocalPoseMutex);
00133     if(mLocalPose.SetPose(position, orientation, time))
00134     {
00135         SignalEvent(REPORT_LOCAL_POSE);
00136         return true;
00137     }
00138     return false;
00139 }
00140 
00141 
00155 bool LocalPoseSensor::SetLocalPose(const JAUS::GlobalPose& globalPose)
00156 {
00157     Mutex::ScopedLock lock(&mLocalPoseMutex);
00158 
00159     UInt required = ReportGlobalPose::PresenceVector::Latitude |
00160                     ReportGlobalPose::PresenceVector::Longitude |
00161                     ReportGlobalPose::PresenceVector::Altitude |
00162                     ReportGlobalPose::PresenceVector::Roll |
00163                     ReportGlobalPose::PresenceVector::Pitch |
00164                     ReportGlobalPose::PresenceVector::Yaw;
00165     if(mGlobalPoseReference.AreFieldsPresent(required) == false)
00166     {
00167         // Try lookup global pose data if it is available.
00168         GlobalPoseSensor* globalPoseSensor = (GlobalPoseSensor*)GetComponent()->GetService(GlobalPoseSensor::Name);
00169         if(globalPoseSensor)
00170         {
00171             mGlobalPoseReference = globalPoseSensor->GetGlobalPose();
00172         }
00173         else
00174         {
00175             Address::List id = GetComponent()->DiscoveryService()->GetComponentsWithService(GlobalPoseSensor::Name);
00176             for(unsigned int i = 0; i < (unsigned int)id.size(); i++)
00177             {
00178                 if(id[i].mSubsystem == GetComponentID().mSubsystem)
00179                 {
00180                     QueryGlobalPose query(id.front(), GetComponentID());
00181                     query.SetPresenceVector(query.GetPresenceVectorMask());
00182                     ReportGlobalPose report;
00183                     if(Send(&query, &report))
00184                     {
00185                         mGlobalPoseReference = report;
00186                         break;
00187                     }
00188                 }
00189             }
00190         }
00191     }
00192     // If we have the data we need, calculate local pose.
00193     if(mGlobalPoseReference.AreFieldsPresent(required) && globalPose.AreFieldsPresent(required))
00194     {
00195         CxUtils::Utm origin(CxUtils::Wgs(mGlobalPoseReference.GetLatitude(),
00196                                          mGlobalPoseReference.GetLongitude(),
00197                                          mGlobalPoseReference.GetAltitude()));
00198         CxUtils::Utm pos(CxUtils::Wgs(globalPose.GetLatitude(),
00199                                       globalPose.GetLongitude(),
00200                                       globalPose.GetAltitude()));
00201         CxUtils::Point3D localPos(pos.mNorthing - origin.mNorthing,
00202                                   pos.mEasting - origin.mEasting,
00203                                   pos.mElevation - origin.mElevation);
00204         double angleDiff = CxUtils::Orientation::AngleDiff(mGlobalPoseReference.GetYaw(), globalPose.GetYaw());
00205         localPos = localPos.Rotate(-mGlobalPoseReference.GetYaw(), Point3D::Z);
00206         CxUtils::Point3D localOr(CxUtils::Orientation::AngleDiff(mGlobalPoseReference.GetRoll(), globalPose.GetRoll()),
00207                                  CxUtils::Orientation::AngleDiff(mGlobalPoseReference.GetPitch(), globalPose.GetPitch()),
00208                                  CxUtils::Orientation::AngleDiff(mGlobalPoseReference.GetYaw(), globalPose.GetYaw()));
00209         mLocalPose.SetPosition(localPos);
00210         mLocalPose.SetOrientation(localOr);
00211         mLocalPose.SetAttitudeRMS(globalPose.GetAttitudeRMS());
00212         mLocalPose.SetPositionRMS(globalPose.GetPositionRMS());
00213         if(globalPose.IsFieldPresent(ReportGlobalPose::PresenceVector::TimeStamp))
00214         {
00215             mLocalPose.SetTimeStamp(globalPose.GetTimeStamp());
00216         }
00217         else
00218         {
00219             mLocalPose.SetTimeStamp(Time(true));
00220         }
00221         SignalEvent(REPORT_LOCAL_POSE);
00222         return true;
00223     }
00224 
00225     return false;
00226 }
00227 
00228 
00244 void LocalPoseSensor::SetLocalPoseReference(const GlobalPose& globalPose)
00245 {
00246     Mutex::ScopedLock lock(&mLocalPoseMutex);
00247     mGlobalPoseReference = globalPose;
00248 }
00249 
00250 
00263 bool LocalPoseSensor::SetLocalPosition(const Point3D& position, const Time& time)
00264 {
00265     Mutex::ScopedLock lock(&mLocalPoseMutex);
00266     if(mLocalPose.SetPosition(position, time))
00267     {
00268         SignalEvent(REPORT_LOCAL_POSE);
00269         return true;
00270     }
00271     return false;
00272 }
00273 
00274 
00287 bool LocalPoseSensor::SetLocalOrientation(const Point3D& orientation, const Time& time)
00288 {
00289     Mutex::ScopedLock lock(&mLocalPoseMutex);
00290     if(mLocalPose.SetOrientation(orientation, time))
00291     {
00292         SignalEvent(REPORT_LOCAL_POSE);
00293         return true;
00294     }
00295     return false;
00296 }
00297 
00298 
00314 bool LocalPoseSensor::AddToLocalPose(const Point3D& position,
00315                                      const Point3D& orientation,
00316                                      const Time& time)
00317 {
00318     Mutex::ScopedLock lock(&mLocalPoseMutex);
00319     if(mLocalPose.AddToPose(position, orientation, time))
00320     {
00321         SignalEvent(REPORT_LOCAL_POSE);
00322         return true;
00323     }
00324     return false;
00325 }
00326 
00327 
00341 bool LocalPoseSensor::AddToLocalPosition(const Point3D& position, const Time& time)
00342 {
00343     Mutex::ScopedLock lock(&mLocalPoseMutex);
00344     if(mLocalPose.AddToPosition(position, time))
00345     {
00346         SignalEvent(REPORT_LOCAL_POSE);
00347         return true;
00348     }
00349     return false;
00350 }
00351 
00352 
00367 bool LocalPoseSensor::AddtoLocalOrientation(const Point3D& orientation, const Time& time)
00368 {
00369     Mutex::ScopedLock lock(&mLocalPoseMutex);
00370     if(mLocalPose.AddToOrientation(orientation, time))
00371     {
00372         SignalEvent(REPORT_LOCAL_POSE);
00373         return true;
00374     }
00375     return false;
00376 }
00377 
00378 
00386 bool LocalPoseSensor::SetSensorUpdateRate(const double rate)
00387 {
00388     if(rate >= CreateEvent::Limits::MinUpdateRate &&
00389        rate <= CreateEvent::Limits::MaxUpdateRate)
00390     {
00391         mMaxUpdateRate = rate;
00392         return true;
00393     }
00394     return false;
00395 }
00396 
00397 
00405 LocalPose LocalPoseSensor::GetLocalPose() const
00406 {
00407     Mutex::ScopedLock lock(&mLocalPoseMutex);
00408     return mLocalPose;
00409 }
00410 
00411 
00424 GlobalPose LocalPoseSensor::GetLocalPoseReference() const
00425 {
00426     Mutex::ScopedLock lock(&mLocalPoseMutex);
00427     return mGlobalPoseReference;
00428 }
00429 
00430 
00440 bool LocalPoseSensor::GenerateEvent(const Events::Subscription& info) const
00441 {
00442     if(info.mpQueryMessage->GetMessageCode() == QUERY_LOCAL_POSE)
00443     {
00444         const QueryLocalPose* query = dynamic_cast<const QueryLocalPose*>(info.mpQueryMessage);
00445 
00446         if(query == NULL)
00447         {
00448             return false;
00449         }
00450 
00451         LocalPose report;
00452         CreateReportFromQuery(query, report);
00453         SendEvent(info, &report);
00454 
00455         return true;
00456     }
00457     return false;
00458 }
00459 
00460 
00476 bool LocalPoseSensor::IsEventSupported(const Events::Type type,
00477                                         const double requestedPeriodicRate,
00478                                         const Message* queryMessage,
00479                                         double& confirmedPeriodicRate,
00480                                         std::string& errorMessage) const
00481 {
00482     if(queryMessage->GetMessageCode() == QUERY_LOCAL_POSE)
00483     {
00484         if(requestedPeriodicRate <= mMaxUpdateRate)
00485         {
00486             confirmedPeriodicRate = requestedPeriodicRate;
00487         }
00488         else
00489         {
00490             confirmedPeriodicRate = mMaxUpdateRate;
00491         }
00492         return true;
00493     }
00494     return false;
00495 }
00496 
00497 
00509 void LocalPoseSensor::Receive(const JAUS::Message *message)
00510 {
00511     switch(message->GetMessageCode())
00512     {
00513     case QUERY_LOCAL_POSE:
00514         {
00515             const QueryLocalPose* query = dynamic_cast<const QueryLocalPose*>(message);
00516             if(query)
00517             {
00518                 ReportLocalPose report(query->GetSourceID(), GetComponentID());
00519                 CreateReportFromQuery(query, report);
00520                 Send(&report);
00521             }
00522         }
00523         break;
00524     case SET_LOCAL_POSE:
00525         {
00526             const JAUS::SetLocalPose* command = dynamic_cast<const JAUS::SetLocalPose*>(message);
00527             if(command)
00528             {
00529                 SetLocalPose(*command);
00530                 // Try lookup global pose data if it is available.
00531                 GlobalPoseSensor* globalPoseSensor = (GlobalPoseSensor*)GetComponent()->GetService(GlobalPoseSensor::Name);
00532                 if(globalPoseSensor)
00533                 {
00534                     mGlobalPoseReference = globalPoseSensor->GetGlobalPose();
00535                 }
00536                 else
00537                 {
00538                     Address::List id = GetComponent()->DiscoveryService()->GetComponentsWithService(GlobalPoseSensor::Name);
00539                     for(unsigned int i = 0; i < (unsigned int)id.size(); i++)
00540                     {
00541                         if(id[i].mSubsystem == GetComponentID().mSubsystem)
00542                         {
00543                             QueryGlobalPose query(id.front(), GetComponentID());
00544                             query.SetPresenceVector(query.GetPresenceVectorMask());
00545                             ReportGlobalPose report;
00546                             if(Send(&query, &report))
00547                             {
00548                                 mGlobalPoseReference = report;
00549                                 break;
00550                             }
00551                         }
00552                     }
00553                 }
00554             }
00555         }
00556         break;
00557     case REPORT_LOCAL_POSE:
00558         {
00559             const JAUS::ReportLocalPose* report = dynamic_cast<const JAUS::ReportLocalPose*>(message);
00560             if(report && report->GetSourceID() == GetSynchronizeID())
00561             {
00562                 SetLocalPose(*report);
00563             }
00564         }
00565         break;
00566     default:
00567         break;
00568     };
00569 }
00570 
00571 
00583 Message* LocalPoseSensor::CreateMessage(const UShort messageCode) const
00584 {
00585     Message* message = NULL;
00586     switch(messageCode)
00587     {
00588     case QUERY_LOCAL_POSE:
00589         message = new QueryLocalPose();
00590         break;
00591     case REPORT_LOCAL_POSE:
00592         message = new ReportLocalPose();
00593         break;
00594     case SET_LOCAL_POSE:
00595         message = new JAUS::SetLocalPose();
00596         break;
00597     default:
00598         message = NULL;
00599         break;
00600     };
00601     return message;
00602 }
00603 
00604 
00610 void LocalPoseSensor::PrintStatus() const
00611 {
00612     //Mutex::ScopedLock lock(&mLocalPoseMutex);
00613     if(GetSynchronizeID().IsValid())
00614     {
00615         std::cout << "[" << GetServiceID().ToString() << "] - Synchronized to [" << GetSynchronizeID().ToString() << "]:\n";
00616     }
00617     else
00618     {
00619         std::cout << "[" << GetServiceID().ToString() << "] - Current Local Pose:\n";
00620     }
00621     LocalPose pose;
00622     mLocalPoseMutex.Lock();
00623     pose = mLocalPose;
00624     mLocalPoseMutex.Unlock();
00625     pose.PrintMessageBody();
00626 }
00627 
00628 
00637 void LocalPoseSensor::CreateReportFromQuery(const QueryLocalPose* query, LocalPose& report) const
00638 {
00639     Mutex::ScopedLock lock(&mLocalPoseMutex);
00640     report.ClearMessage();
00641     report.SetDestinationID(query->GetSourceID());
00642     report.SetSourceID(GetComponentID());
00643     UInt pv1 = query->GetPresenceVector();
00644     UInt pv2 = mLocalPose.GetPresenceVector();
00645 
00646     if( (pv2 & (pv1 & LocalPose::PresenceVector::X)) > 0) { report.SetX(mLocalPose.GetX()); }
00647     if( (pv2 & (pv1 & LocalPose::PresenceVector::Y)) > 0) { report.SetY(mLocalPose.GetY()); }
00648     if( (pv2 & (pv1 & LocalPose::PresenceVector::Z)) > 0) { report.SetZ(mLocalPose.GetZ()); }
00649     if( (pv2 & (pv1 & LocalPose::PresenceVector::PositionRMS)) > 0) { report.SetPositionRMS(mLocalPose.GetPositionRMS()); }
00650     if( (pv2 & (pv1 & LocalPose::PresenceVector::Roll)) > 0) { report.SetRoll(mLocalPose.GetRoll()); }
00651     if( (pv2 & (pv1 & LocalPose::PresenceVector::Pitch)) > 0) { report.SetPitch(mLocalPose.GetPitch()); }
00652     if( (pv2 & (pv1 & LocalPose::PresenceVector::Yaw)) > 0) { report.SetYaw(mLocalPose.GetYaw()); }
00653     if( (pv2 & (pv1 & LocalPose::PresenceVector::AttitudeRMS)) > 0) { report.SetAttitudeRMS(mLocalPose.GetAttitudeRMS()); }
00654     if( (pv2 & (pv1 & LocalPose::PresenceVector::TimeStamp)) > 0) { report.SetTimeStamp(mLocalPose.GetTimeStamp()); }
00655 }
00656 
00657 
00667 void LocalPoseSensor::CheckServiceSynchronization(const unsigned int timeSinceLastCheckMs)
00668 {
00669     Address syncID = GetSynchronizeID();
00670     if(syncID.IsValid())
00671     {
00672         // See if we have an active subscription, and if not create it.
00673         if(EventsService()->HaveSubscription(REPORT_LOCAL_POSE, syncID) == false)
00674         {
00675             QueryLocalPose query(syncID, GetComponentID());
00676             query.SetPresenceVector(query.GetPresenceVectorMask());
00677             EventsService()->RequestEveryChangeEvent(syncID, &query);
00678         }
00679     }
00680 }
00681 
00682 
00683 /*  End of File */