JAUS++: jaus++/2.0/src/jaus/mobility/sensors/reportglobalpose.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 #include "jaus/mobility/sensors/reportglobalpose.h"
00041 #include "jaus/core/scaledinteger.h"
00042 #include <cxutils/math/cxmath.h>
00043 #include <iomanip>
00044 
00045 const double JAUS::ReportGlobalPose::Limits::MinLatitude = -90.0;
00046 const double JAUS::ReportGlobalPose::Limits::MaxLatitude = 90.0;
00047 const double JAUS::ReportGlobalPose::Limits::MinLongitude = -180.0;
00048 const double JAUS::ReportGlobalPose::Limits::MaxLongitude = 180.0;
00049 const double JAUS::ReportGlobalPose::Limits::MinElevation = -10000.0;
00050 const double JAUS::ReportGlobalPose::Limits::MaxElevation = 35000.0;
00051 const double JAUS::ReportGlobalPose::Limits::MinPositionRMS = 0.0;
00052 const double JAUS::ReportGlobalPose::Limits::MaxPositionRMS = 100.0;
00053 const double JAUS::ReportGlobalPose::Limits::MinAngle = -CxUtils::PI;
00054 const double JAUS::ReportGlobalPose::Limits::MaxAngle = CxUtils::PI;
00055 const double JAUS::ReportGlobalPose::Limits::MinAttitudeRMS = 0.0;
00056 const double JAUS::ReportGlobalPose::Limits::MaxAttitudeRMS = CxUtils::PI;
00057 
00058 using namespace JAUS;
00059 
00060 
00069 ReportGlobalPose::ReportGlobalPose(const Address& dest, const Address& src) : Message(REPORT_GLOBAL_POSE, dest, src)
00070 {
00071     mPresenceVector = 0;
00072     mLatitude = 0;
00073     mLongitude = 0;
00074     mAltitude = 0;
00075     mRoll = 0;
00076     mPositionRMS = 0;
00077     mPitch = 0;
00078     mYaw = 0;
00079     mAttitudeRMS = 0;
00080     mTimeStamp.SetCurrentTime();
00081 }
00082 
00083 
00089 ReportGlobalPose::ReportGlobalPose(const ReportGlobalPose& message) : Message(REPORT_GLOBAL_POSE)
00090 {
00091     *this = message;
00092 }
00093 
00094 
00100 ReportGlobalPose::~ReportGlobalPose()
00101 {
00102 }
00103 
00104 
00114 bool ReportGlobalPose::SetLatitude(const double degrees)
00115 {
00116     if(degrees >= Limits::MinLatitude && degrees <= Limits::MaxLatitude)
00117     {
00118         mLatitude = degrees;
00119         mPresenceVector |= PresenceVector::Latitude;
00120         return true;
00121     }
00122     return false;
00123 }
00124 
00125 
00135 bool ReportGlobalPose::SetLongitude(const double degrees)
00136 {
00137     if(degrees >= Limits::MinLongitude && degrees <= Limits::MaxLongitude)
00138     {
00139         mLongitude = degrees;
00140         mPresenceVector |= PresenceVector::Longitude;
00141         return true;
00142     }
00143     return false;
00144 }
00145 
00146 
00157 bool ReportGlobalPose::SetAltitude(const double value)
00158 {
00159     if(value >= Limits::MinElevation && value <= Limits::MaxElevation)
00160     {
00161         mAltitude = value;
00162         mPresenceVector |= PresenceVector::Altitude;
00163         return true;
00164     }
00165     return false;
00166 }
00167 
00168 
00179 bool ReportGlobalPose::SetPositionRMS(const double value)
00180 {
00181     if(value >= Limits::MinPositionRMS && value <= Limits::MaxPositionRMS)
00182     {
00183         mPositionRMS = value;
00184         mPresenceVector |= PresenceVector::PositionRMS;
00185         return true;
00186     }
00187     return false;
00188 }
00189 
00190 
00200 bool ReportGlobalPose::SetRoll(const double radians)
00201 {
00202     if(radians >= Limits::MinAngle && radians <= Limits::MaxAngle)
00203     {
00204         mRoll = radians;
00205         mPresenceVector |= PresenceVector::Roll;
00206         return true;
00207     }
00208     return false;
00209 }
00210 
00211 
00221 bool ReportGlobalPose::SetPitch(const double radians)
00222 {
00223     if(radians >= Limits::MinAngle && radians <= Limits::MaxAngle)
00224     {
00225         mPitch = radians;
00226         mPresenceVector |= PresenceVector::Pitch;
00227         return true;
00228     }
00229     return false;
00230 }
00231 
00232 
00242 bool ReportGlobalPose::SetYaw(const double radians)
00243 {
00244     if(radians >= Limits::MinAngle && radians <= Limits::MaxAngle)
00245     {
00246         mYaw = radians;
00247         mPresenceVector |= PresenceVector::Yaw;
00248         return true;
00249     }
00250     return false;
00251 }
00252 
00253 
00264 bool ReportGlobalPose::SetAttitudeRMS(const double radians)
00265 {
00266     if(radians >= Limits::MinAttitudeRMS && radians <= Limits::MaxAttitudeRMS)
00267     {
00268         mAttitudeRMS = radians;
00269         mPresenceVector |= PresenceVector::AttitudeRMS;
00270         return true;
00271     }
00272     return false;
00273 }
00274 
00275 
00286 bool ReportGlobalPose::SetTimeStamp(const JAUS::Time& time)
00287 {
00288     mTimeStamp = time;
00289     mPresenceVector |= PresenceVector::TimeStamp;
00290     return true;
00291 }
00292 
00293 
00308 bool ReportGlobalPose::SetPose(const Wgs& position,
00309                             const Point3D& orientation,
00310                             const Time& time)
00311 {
00312     //Check bounds before proceeding.
00313     if(position.mLatitude >= Limits::MinLatitude && position.mLatitude <= Limits::MaxLatitude &&
00314        position.mLongitude >= Limits::MinLongitude && position.mLongitude <= Limits::MaxLongitude && 
00315        position.mElevation >= Limits::MinElevation && position.mElevation <= Limits::MaxElevation &&
00316        orientation.mX >= Limits::MinAngle && orientation.mX <= Limits::MaxAngle && 
00317        orientation.mY >= Limits::MinAngle && orientation.mY <= Limits::MaxAngle && 
00318        orientation.mZ >= Limits::MinAngle && orientation.mZ <= Limits::MaxAngle)
00319     {
00320         mLatitude = position.mLatitude;
00321         mLongitude = position.mLongitude;
00322         mAltitude = position.mElevation;
00323         mRoll = orientation.mX;
00324         mPitch = orientation.mY;
00325         mYaw = orientation.mZ;
00326         mTimeStamp = time;
00327         mPresenceVector |= PresenceVector::Latitude | PresenceVector::Longitude | PresenceVector::Altitude |
00328                            PresenceVector::Roll | PresenceVector::Pitch | PresenceVector::Yaw |
00329                            PresenceVector::TimeStamp;
00330         return true;
00331     }
00332     //Parameters are out of bound.
00333     return false;
00334 }
00335 
00336 
00350 bool ReportGlobalPose::SetPosition(const Wgs& position, const Time& time)
00351 {
00352     //Check bounds before proceeding.
00353     if(position.mLatitude >= Limits::MinLatitude && position.mLatitude <= Limits::MaxLatitude &&
00354        position.mLongitude >= Limits::MinLongitude && position.mLongitude <= Limits::MaxLongitude && 
00355        position.mElevation >= Limits::MinElevation && position.mElevation <= Limits::MaxElevation)
00356     {
00357         mLatitude = position.mLatitude;
00358         mLongitude = position.mLongitude;
00359         mAltitude = position.mElevation;
00360         mTimeStamp = time;
00361         mPresenceVector |= PresenceVector::Latitude | PresenceVector::Longitude | PresenceVector::Altitude |
00362                            PresenceVector::TimeStamp;
00363         return true;
00364     }
00365     //Parameters are out of bound.
00366     return false;
00367 }
00368 
00369 
00381 bool ReportGlobalPose::SetOrientation(const Point3D& orientation, const Time& time)
00382 {
00383     //Check bounds before proceeding.
00384     if(orientation.mX >= Limits::MinAngle && orientation.mX <= Limits::MaxAngle && 
00385        orientation.mY >= Limits::MinAngle && orientation.mY <= Limits::MaxAngle && 
00386        orientation.mZ >= Limits::MinAngle && orientation.mZ <= Limits::MaxAngle)
00387     {
00388         mRoll = orientation.mX;
00389         mPitch = orientation.mY;
00390         mYaw = orientation.mZ;
00391         mTimeStamp = time;
00392         mPresenceVector |= PresenceVector::Roll | PresenceVector::Pitch | PresenceVector::Yaw |
00393                            PresenceVector::TimeStamp;
00394         return true;
00395     }
00396     //Parameters are out of bound.
00397     return false;
00398 }
00399 
00400 
00401 
00410 Wgs ReportGlobalPose::GetPosition() const
00411 {
00412     Wgs position;
00413     position.Set(mLatitude, mLongitude, mAltitude);
00414     return position;
00415 }
00416 
00417 
00425 Point3D ReportGlobalPose::GetOrientation() const
00426 {
00427     Point3D orientation;
00428     orientation.Set(mRoll, mPitch, mYaw);
00429     return orientation;
00430 }
00431 
00432 
00444 int ReportGlobalPose::WriteMessageBody(Packet& packet) const
00445 {
00446     int expected = USHORT_SIZE;
00447     int written = 0;
00448 
00449     written += packet.Write(mPresenceVector);
00450 
00451     if((mPresenceVector & PresenceVector::Latitude) > 0)
00452     {
00453         expected += UINT_SIZE;
00454         written += ScaledInteger::Write(packet, mLatitude, Limits::MaxLatitude, Limits::MinLatitude, ScaledInteger::UInt);
00455     }
00456     if((mPresenceVector & PresenceVector::Longitude) > 0)
00457     {
00458         expected += UINT_SIZE;
00459         written += ScaledInteger::Write(packet, mLongitude, Limits::MaxLongitude, Limits::MinLongitude, ScaledInteger::UInt);
00460     }
00461     if((mPresenceVector & PresenceVector::Altitude) > 0)
00462     {
00463         expected += UINT_SIZE;
00464         written += ScaledInteger::Write(packet, mAltitude, Limits::MaxElevation, Limits::MinElevation, ScaledInteger::UInt);
00465     }
00466     if((mPresenceVector & PresenceVector::PositionRMS) > 0)
00467     {
00468         expected += UINT_SIZE;
00469         written += ScaledInteger::Write(packet, mPositionRMS, Limits::MaxPositionRMS, Limits::MinPositionRMS, ScaledInteger::UInt);
00470     }
00471     if((mPresenceVector & PresenceVector::Roll) > 0)
00472     {
00473         expected += USHORT_SIZE;
00474         written += ScaledInteger::Write(packet, mRoll, Limits::MaxAngle, Limits::MinAngle, ScaledInteger::UShort);
00475     }
00476     if((mPresenceVector & PresenceVector::Pitch) > 0)
00477     {
00478         expected += USHORT_SIZE;
00479         written += ScaledInteger::Write(packet, mPitch, Limits::MaxAngle, Limits::MinAngle, ScaledInteger::UShort);
00480     }
00481     if((mPresenceVector & PresenceVector::Yaw) > 0)
00482     {
00483         expected += USHORT_SIZE;
00484         written += ScaledInteger::Write(packet, mYaw, Limits::MaxAngle, Limits::MinAngle, ScaledInteger::UShort);
00485     }
00486     if((mPresenceVector & PresenceVector::AttitudeRMS) > 0)
00487     {
00488         expected += USHORT_SIZE;
00489         written += ScaledInteger::Write(packet, mAttitudeRMS, Limits::MaxAttitudeRMS, Limits::MinAttitudeRMS, ScaledInteger::UShort);
00490     }
00491     if((mPresenceVector & PresenceVector::TimeStamp) > 0)
00492     {
00493         expected += UINT_SIZE;
00494         written += packet.Write(mTimeStamp.ToUInt());
00495     }
00496 
00497     return expected == written ? written : -1;
00498 }
00499 
00500 
00512 int ReportGlobalPose::ReadMessageBody(const Packet& packet)
00513 {
00514     int expected = USHORT_SIZE;
00515     int read = 0;
00516 
00517     read += packet.Read(mPresenceVector);
00518 
00519     if((mPresenceVector & PresenceVector::Latitude) > 0)
00520     {
00521         expected += UINT_SIZE;
00522         read += ScaledInteger::Read(packet, mLatitude, Limits::MaxLatitude, Limits::MinLatitude, ScaledInteger::UInt);
00523     }
00524     if((mPresenceVector & PresenceVector::Longitude) > 0)
00525     {
00526         expected += UINT_SIZE;
00527         read += ScaledInteger::Read(packet, mLongitude, Limits::MaxLongitude, Limits::MinLongitude, ScaledInteger::UInt);
00528     }
00529     if((mPresenceVector & PresenceVector::Altitude) > 0)
00530     {
00531         expected += UINT_SIZE;
00532         read += ScaledInteger::Read(packet, mAltitude, Limits::MaxElevation, Limits::MinElevation, ScaledInteger::UInt);
00533     }
00534     if((mPresenceVector & PresenceVector::PositionRMS) > 0)
00535     {
00536         expected += UINT_SIZE;
00537         read += ScaledInteger::Read(packet, mPositionRMS, Limits::MaxPositionRMS, Limits::MinPositionRMS, ScaledInteger::UInt);
00538     }
00539     if((mPresenceVector & PresenceVector::Roll) > 0)
00540     {
00541         expected += USHORT_SIZE;
00542         read += ScaledInteger::Read(packet, mRoll, Limits::MaxAngle, Limits::MinAngle, ScaledInteger::UShort);
00543     }
00544     if((mPresenceVector & PresenceVector::Pitch) > 0)
00545     {
00546         expected += USHORT_SIZE;
00547         read += ScaledInteger::Read(packet, mPitch, Limits::MaxAngle, Limits::MinAngle, ScaledInteger::UShort);
00548     }
00549     if((mPresenceVector & PresenceVector::Yaw) > 0)
00550     {
00551         expected += USHORT_SIZE;
00552         read += ScaledInteger::Read(packet, mYaw, Limits::MaxAngle, Limits::MinAngle, ScaledInteger::UShort);
00553     }
00554     if((mPresenceVector & PresenceVector::AttitudeRMS) > 0)
00555     {
00556         expected += USHORT_SIZE;
00557         read += ScaledInteger::Read(packet, mAttitudeRMS, Limits::MaxAttitudeRMS, Limits::MinAttitudeRMS, ScaledInteger::UShort);
00558     }
00559 
00560     if((mPresenceVector & PresenceVector::TimeStamp) > 0)
00561     {
00562         UInt time = 0;
00563         expected += UINT_SIZE;
00564         read += packet.Read(time);
00565         mTimeStamp.SetTime(time);
00566     }
00567 
00568     return expected == read ? read : -1;
00569 }
00570 
00571 
00577 void ReportGlobalPose::ClearMessageBody()
00578 {
00579     mPresenceVector = 0;
00580     mLatitude = 0;
00581     mLongitude = 0;
00582     mAltitude = 0;
00583     mPositionRMS = 0;
00584     mRoll = 0;
00585     mPitch = 0;
00586     mYaw = 0;
00587     mAttitudeRMS = 0;
00588     mTimeStamp.SetCurrentTime();
00589 }
00590 
00591 
00597 void ReportGlobalPose::PrintMessageBody() const
00598 {
00599     std::cout << "WGS: [" << std::fixed << std::setprecision(5) << mLatitude << ", " 
00600                           << std::fixed << std::setprecision(5) << mLongitude << ", " 
00601                           << std::fixed << std::setprecision(2) << mAltitude << "]\n";
00602     std::cout << "Orientation: [" << std::fixed << std::setprecision(1) << CxUtils::CxToDegrees(mRoll) << ", " 
00603                                   << std::fixed << std::setprecision(1) << CxUtils::CxToDegrees(mPitch) << ", " 
00604                                   << std::fixed << std::setprecision(1) << CxUtils::CxToDegrees(mYaw) << "]\n";
00605     std::cout << "Timestamp: " << mTimeStamp.ToString() << std::endl;
00606 }
00607 
00608 
00616 int ReportGlobalPose::RunTestCase() const
00617 {
00618     int result = 0;
00619 
00620     Packet packet;
00621 
00622     ReportGlobalPose msg1, msg2;
00623     JAUS::Time time;
00624     time.SetCurrentTime();
00625 
00626     msg1.SetAltitude(-1023);
00627     msg1.SetTimeStamp(time);
00628 
00629     if((msg1.WriteMessageBody(packet) != -1) &&
00630         (msg2.ReadMessageBody(packet) != -1))
00631     {
00632         if(msg2.AreFieldsPresent(PresenceVector::Altitude | PresenceVector::TimeStamp) )
00633          {
00634                 result = 1;
00635          }
00636     }
00637 
00638     return result;
00639 }
00640 
00641 
00647 ReportGlobalPose& ReportGlobalPose::operator=(const ReportGlobalPose& message)
00648 {
00649     if(this != &message)
00650     {
00651         CopyHeaderData(&message);
00652         mPresenceVector = message.mPresenceVector;
00653         mLatitude = message.mLatitude;
00654         mLongitude = message.mLongitude;
00655         mAltitude = message.mAltitude;
00656         mPositionRMS = message.mPositionRMS;
00657         mRoll = message.mRoll;
00658         mPitch = message.mPitch;
00659         mYaw = message.mYaw;
00660         mAttitudeRMS = message.mAttitudeRMS;
00661         mTimeStamp = message.mTimeStamp;
00662         
00663     }
00664     return *this;
00665 }
00666 
00667 
00668 /*  End of File */