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