JAUS++: jaus++/2.0/src/jaus/mobility/sensors/setlocalpose.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/setlocalpose.h"
00041 #include "jaus/core/scaledinteger.h"
00042 #include <cxutils/math/cxmath.h>
00043 
00044 const double JAUS::SetLocalPose::Limits::MinPoint = -100000.0;
00045 const double JAUS::SetLocalPose::Limits::MaxPoint = 100000.0;
00046 const double JAUS::SetLocalPose::Limits::MinPositionRMS = 0.0;
00047 const double JAUS::SetLocalPose::Limits::MaxPositionRMS = 100.0;
00048 const double JAUS::SetLocalPose::Limits::MinAngle = -CxUtils::PI;
00049 const double JAUS::SetLocalPose::Limits::MaxAngle = CxUtils::PI;
00050 const double JAUS::SetLocalPose::Limits::MinAttitudeRMS = 0.0;
00051 const double JAUS::SetLocalPose::Limits::MaxAttitudeRMS = CxUtils::PI;
00052 
00053 using namespace JAUS;
00054 
00055 
00064 SetLocalPose::SetLocalPose(const Address& dest, const Address& src) : Message(SET_LOCAL_POSE, dest, src)
00065 {
00066     mPresenceVector = 0;
00067     mX = 0;
00068     mY = 0;
00069     mZ = 0;
00070     mRoll = 0;
00071     mPositionRMS = 0;
00072     mPitch = 0;
00073     mYaw = 0;
00074     mAttitudeRMS = 0;
00075     mTimeStamp.SetCurrentTime();
00076 }
00077 
00078 
00084 SetLocalPose::SetLocalPose(const SetLocalPose& message) : Message(SET_LOCAL_POSE)
00085 {
00086     *this = message;
00087 }
00088 
00089 
00095 SetLocalPose::~SetLocalPose()
00096 {
00097 }
00098 
00099 
00109 bool SetLocalPose::SetX(const double value)
00110 {
00111     if(value >= Limits::MinPoint && value <= Limits::MaxPoint)
00112     {
00113         mX = value;
00114         mPresenceVector |= PresenceVector::X;
00115         return true;
00116     }
00117     return false;
00118 }
00119 
00120 
00130 bool SetLocalPose::SetY(const double value)
00131 {
00132     if(value >= Limits::MinPoint && value <= Limits::MaxPoint)
00133     {
00134         mY = value;
00135         mPresenceVector |= PresenceVector::Y;
00136         return true;
00137     }
00138     return false;
00139 }
00140 
00141 
00152 bool SetLocalPose::SetZ(const double value)
00153 {
00154     if(value >= Limits::MinPoint && value <= Limits::MaxPoint)
00155     {
00156         mZ = value;
00157         mPresenceVector |= PresenceVector::Z;
00158         return true;
00159     }
00160     return false;
00161 }
00162 
00163 
00174 bool SetLocalPose::SetPositionRMS(const double value)
00175 {
00176     if(value >= Limits::MinPositionRMS && value <= Limits::MaxPositionRMS)
00177     {
00178         mPositionRMS = value;
00179         mPresenceVector |= PresenceVector::PositionRMS;
00180         return true;
00181     }
00182     return false;
00183 }
00184 
00185 
00195 bool SetLocalPose::SetRoll(const double radians)
00196 {
00197     if(radians >= Limits::MinAngle && radians <= Limits::MaxAngle)
00198     {
00199         mRoll = radians;
00200         mPresenceVector |= PresenceVector::Roll;
00201         return true;
00202     }
00203     return false;
00204 }
00205 
00206 
00216 bool SetLocalPose::SetPitch(const double radians)
00217 {
00218     if(radians >= Limits::MinAngle && radians <= Limits::MaxAngle)
00219     {
00220         mPitch = radians;
00221         mPresenceVector |= PresenceVector::Pitch;
00222         return true;
00223     }
00224     return false;
00225 }
00226 
00227 
00237 bool SetLocalPose::SetYaw(const double radians)
00238 {
00239     if(radians >= Limits::MinAngle && radians <= Limits::MaxAngle)
00240     {
00241         mYaw = radians;
00242         mPresenceVector |= PresenceVector::Yaw;
00243         return true;
00244     }
00245     return false;
00246 }
00247 
00248 
00259 bool SetLocalPose::SetAttitudeRMS(const double radians)
00260 {
00261     if(radians >= Limits::MinAttitudeRMS && radians <= Limits::MaxAttitudeRMS)
00262     {
00263         mAttitudeRMS = radians;
00264         mPresenceVector |= PresenceVector::AttitudeRMS;
00265         return true;
00266     }
00267     return false;
00268 }
00269 
00270 
00281 bool SetLocalPose::SetTimeStamp(const JAUS::Time& time)
00282 {
00283     mTimeStamp = time;
00284     mPresenceVector |= PresenceVector::TimeStamp;
00285     return true;
00286 }
00287 
00288 
00302 bool SetLocalPose::SetPose(const Point3D& position,
00303                            const Point3D& orientation,
00304                            const Time& time)
00305 {
00306     //Check bounds before proceeding.
00307     if(position.mX >= Limits::MinPoint && position.mX <= Limits::MaxPoint &&
00308        position.mY >= Limits::MinPoint && position.mY <= Limits::MaxPoint && 
00309        position.mZ >= Limits::MinPoint && position.mZ <= Limits::MaxPoint &&
00310        orientation.mX >= Limits::MinAngle && orientation.mX <= Limits::MaxAngle && 
00311        orientation.mY >= Limits::MinAngle && orientation.mY <= Limits::MaxAngle && 
00312        orientation.mZ >= Limits::MinAngle && orientation.mZ <= Limits::MaxAngle)
00313     {
00314         mX = position.mX;
00315         mY = position.mY;
00316         mZ = position.mZ;
00317         mRoll = orientation.mX;
00318         mPitch = orientation.mY;
00319         mYaw = orientation.mZ;
00320         mTimeStamp = time;
00321         mPresenceVector |= PresenceVector::X | PresenceVector::Y | PresenceVector::Z |
00322                            PresenceVector::Roll | PresenceVector::Pitch | PresenceVector::Yaw |
00323                            PresenceVector::TimeStamp;
00324         return true;
00325     }
00326     //Parameters are out of bound.
00327     return false;
00328 }
00329 
00330 
00342 bool SetLocalPose::SetPosition(const Point3D& position, const Time& time)
00343 {
00344     //Check bounds before proceeding.
00345     if(position.mX >= Limits::MinPoint && position.mX <= Limits::MaxPoint &&
00346        position.mY >= Limits::MinPoint && position.mY <= Limits::MaxPoint && 
00347        position.mZ >= Limits::MinPoint && position.mZ <= Limits::MaxPoint)
00348     {
00349         mX = position.mX;
00350         mY = position.mY;
00351         mZ = position.mZ;
00352         mTimeStamp = time;
00353         mPresenceVector |= PresenceVector::X | PresenceVector::Y | PresenceVector::Z |
00354                            PresenceVector::TimeStamp;
00355         return true;
00356     }
00357     //Parameters are out of bound.
00358     return false;
00359 }
00360 
00361 
00373 bool SetLocalPose::SetOrientation(const Point3D& orientation, const Time& time)
00374 {
00375     //Check bounds before proceeding.
00376     if(orientation.mX >= Limits::MinAngle && orientation.mX <= Limits::MaxAngle && 
00377        orientation.mY >= Limits::MinAngle && orientation.mY <= Limits::MaxAngle && 
00378        orientation.mZ >= Limits::MinAngle && orientation.mZ <= Limits::MaxAngle)
00379     {
00380         mRoll = orientation.mX;
00381         mPitch = orientation.mY;
00382         mYaw = orientation.mZ;
00383         mTimeStamp = time;
00384         mPresenceVector |= PresenceVector::Roll | PresenceVector::Pitch | PresenceVector::Yaw |
00385                            PresenceVector::TimeStamp;
00386         return true;
00387     }
00388     //Parameters are out of bound.
00389     return false;
00390 }
00391 
00392 
00407 bool SetLocalPose::AddToPose(const Point3D& position,
00408                              const Point3D& orientation,
00409                              const Time& time)
00410 {
00411     double newX = position.mX + mX;
00412     double newY = position.mY + mY;
00413     double newZ = position.mZ + mZ;
00414     double newRoll = orientation.mX + mRoll;
00415     double newPitch = orientation.mY + mPitch;
00416     double newYaw = orientation.mZ + mYaw;
00417 
00418     //Check bounds before proceeding.
00419     if(newX >= Limits::MinPoint && newX <= Limits::MaxPoint &&
00420        newY >= Limits::MinPoint && newY <= Limits::MaxPoint && 
00421        newZ >= Limits::MinPoint && newZ <= Limits::MaxPoint &&
00422        newRoll >= Limits::MinAngle && newRoll <= Limits::MaxAngle && 
00423        newPitch >= Limits::MinAngle && newPitch <= Limits::MaxAngle && 
00424        newYaw >= Limits::MinAngle && newYaw <= Limits::MaxAngle)
00425     {
00426         mX = newX;
00427         mY = newY;
00428         mZ = newZ;
00429         mRoll = newRoll;
00430         mPitch = newPitch;
00431         mYaw = newYaw;
00432         mTimeStamp = time;
00433         mPresenceVector |= PresenceVector::X | PresenceVector::Y | PresenceVector::Z |
00434                            PresenceVector::Roll | PresenceVector::Pitch | PresenceVector::Yaw |
00435                            PresenceVector::TimeStamp;
00436         return true;
00437     }
00438     //Parameters are out of bound.
00439     return false;
00440 }
00441 
00442 
00455 bool SetLocalPose::AddToPosition(const Point3D& position, const Time& time)
00456 {    
00457     double newX = position.mX + mX;
00458     double newY = position.mY + mY;
00459     double newZ = position.mZ + mZ;
00460 
00461     //Check bounds before proceeding.
00462     if(newX >= Limits::MinPoint && newX <= Limits::MaxPoint &&
00463        newY >= Limits::MinPoint && newY <= Limits::MaxPoint && 
00464        newZ >= Limits::MinPoint && newZ <= Limits::MaxPoint)
00465     {
00466         mX = newX;
00467         mY = newY;
00468         mZ = newZ;
00469         mTimeStamp = time;
00470         mPresenceVector |= PresenceVector::X | PresenceVector::Y | PresenceVector::Z |
00471                            PresenceVector::TimeStamp;
00472         return true;
00473     }
00474     //Parameters are out of bound.
00475     return false;
00476 }
00477 
00478 
00492 bool SetLocalPose::AddToOrientation(const Point3D& orientation, const Time& time)
00493 {    
00494     double newRoll = orientation.mX + mRoll;
00495     double newPitch = orientation.mY + mPitch;
00496     double newYaw = orientation.mZ + mYaw;
00497 
00498     //Check bounds before proceeding.
00499     if(newRoll >= Limits::MinAngle && newRoll <= Limits::MaxAngle && 
00500        newPitch >= Limits::MinAngle && newPitch <= Limits::MaxAngle && 
00501        newYaw >= Limits::MinAngle && newYaw <= Limits::MaxAngle)
00502     {
00503         mRoll = newRoll;
00504         mPitch = newPitch;
00505         mYaw = newYaw;
00506         mTimeStamp = time;
00507         mPresenceVector |= PresenceVector::Roll | PresenceVector::Pitch | PresenceVector::Yaw |
00508                            PresenceVector::TimeStamp;
00509         return true;
00510     }
00511     //Parameters are out of bound.
00512     return false;
00513 }
00514 
00515 
00527 int SetLocalPose::WriteMessageBody(Packet& packet) const
00528 {
00529     int expected = USHORT_SIZE;
00530     int written = 0;
00531 
00532     written += packet.Write(mPresenceVector);
00533 
00534     if((mPresenceVector & PresenceVector::X) > 0)
00535     {
00536         expected += UINT_SIZE;
00537         written += ScaledInteger::Write(packet, mX, Limits::MaxPoint, Limits::MinPoint, ScaledInteger::UInt);
00538     }
00539     if((mPresenceVector & PresenceVector::Y) > 0)
00540     {
00541         expected += UINT_SIZE;
00542         written += ScaledInteger::Write(packet, mY, Limits::MaxPoint, Limits::MinPoint, ScaledInteger::UInt);
00543     }
00544     if((mPresenceVector & PresenceVector::Z) > 0)
00545     {
00546         expected += UINT_SIZE;
00547         written += ScaledInteger::Write(packet, mZ, Limits::MaxPoint, Limits::MinPoint, ScaledInteger::UInt);
00548     }
00549     if((mPresenceVector & PresenceVector::PositionRMS) > 0)
00550     {
00551         expected += UINT_SIZE;
00552         written += ScaledInteger::Write(packet, mPositionRMS, Limits::MaxPositionRMS, Limits::MinPositionRMS, ScaledInteger::UInt);
00553     }
00554     if((mPresenceVector & PresenceVector::Roll) > 0)
00555     {
00556         expected += USHORT_SIZE;
00557         written += ScaledInteger::Write(packet, mRoll, Limits::MaxAngle, Limits::MinAngle, ScaledInteger::UShort);
00558     }
00559     if((mPresenceVector & PresenceVector::Pitch) > 0)
00560     {
00561         expected += USHORT_SIZE;
00562         written += ScaledInteger::Write(packet, mPitch, Limits::MaxAngle, Limits::MinAngle, ScaledInteger::UShort);
00563     }
00564     if((mPresenceVector & PresenceVector::Yaw) > 0)
00565     {
00566         expected += USHORT_SIZE;
00567         written += ScaledInteger::Write(packet, mYaw, Limits::MaxAngle, Limits::MinAngle, ScaledInteger::UShort);
00568     }
00569     if((mPresenceVector & PresenceVector::AttitudeRMS) > 0)
00570     {
00571         expected += USHORT_SIZE;
00572         written += ScaledInteger::Write(packet, mAttitudeRMS, Limits::MaxAttitudeRMS, Limits::MinAttitudeRMS, ScaledInteger::UShort);
00573     }
00574     if((mPresenceVector & PresenceVector::TimeStamp) > 0)
00575     {
00576         expected += UINT_SIZE;
00577         written += packet.Write(mTimeStamp.ToUInt());
00578     }
00579 
00580     return expected == written ? written : -1;
00581 }
00582 
00583 
00595 int SetLocalPose::ReadMessageBody(const Packet& packet)
00596 {
00597     int expected = USHORT_SIZE;
00598     int read = 0;
00599 
00600     read += packet.Read(mPresenceVector);
00601 
00602     if((mPresenceVector & PresenceVector::X) > 0)
00603     {
00604         expected += UINT_SIZE;
00605         read += ScaledInteger::Read(packet, mX, Limits::MaxPoint, Limits::MinPoint, ScaledInteger::UInt);
00606     }
00607     if((mPresenceVector & PresenceVector::Y) > 0)
00608     {
00609         expected += UINT_SIZE;
00610         read += ScaledInteger::Read(packet, mY, Limits::MaxPoint, Limits::MinPoint, ScaledInteger::UInt);
00611     }
00612     if((mPresenceVector & PresenceVector::Z) > 0)
00613     {
00614         expected += UINT_SIZE;
00615         read += ScaledInteger::Read(packet, mZ, Limits::MaxPoint, Limits::MinPoint, ScaledInteger::UInt);
00616     }
00617     if((mPresenceVector & PresenceVector::PositionRMS) > 0)
00618     {
00619         expected += UINT_SIZE;
00620         read += ScaledInteger::Read(packet, mPositionRMS, Limits::MaxPositionRMS, Limits::MinPositionRMS, ScaledInteger::UInt);
00621     }
00622     if((mPresenceVector & PresenceVector::Roll) > 0)
00623     {
00624         expected += USHORT_SIZE;
00625         read += ScaledInteger::Read(packet, mRoll, Limits::MaxAngle, Limits::MinAngle, ScaledInteger::UShort);
00626     }
00627     if((mPresenceVector & PresenceVector::Pitch) > 0)
00628     {
00629         expected += USHORT_SIZE;
00630         read += ScaledInteger::Read(packet, mPitch, Limits::MaxAngle, Limits::MinAngle, ScaledInteger::UShort);
00631     }
00632     if((mPresenceVector & PresenceVector::Yaw) > 0)
00633     {
00634         expected += USHORT_SIZE;
00635         read += ScaledInteger::Read(packet, mYaw, Limits::MaxAngle, Limits::MinAngle, ScaledInteger::UShort);
00636     }
00637     if((mPresenceVector & PresenceVector::AttitudeRMS) > 0)
00638     {
00639         expected += USHORT_SIZE;
00640         read += ScaledInteger::Read(packet, mAttitudeRMS, Limits::MaxAttitudeRMS, Limits::MinAttitudeRMS, ScaledInteger::UShort);
00641     }
00642 
00643     if((mPresenceVector & PresenceVector::TimeStamp) > 0)
00644     {
00645         UInt time = 0;
00646         expected += UINT_SIZE;
00647         read += packet.Read(time);
00648         mTimeStamp.SetTime(time);
00649     }
00650 
00651     return expected == read ? read : -1;
00652 }
00653 
00654 
00660 void SetLocalPose::ClearMessageBody()
00661 {
00662     mPresenceVector = 0;
00663     mX = 0;
00664     mY = 0;
00665     mZ = 0;
00666     mPositionRMS = 0;
00667     mRoll = 0;
00668     mPitch = 0;
00669     mYaw = 0;
00670     mAttitudeRMS = 0;
00671     mTimeStamp.SetCurrentTime();
00672 }
00673 
00674 
00682 int SetLocalPose::RunTestCase() const
00683 {
00684     int result = 0;
00685 
00686     Packet packet;
00687 
00688     SetLocalPose msg1, msg2;
00689 
00690     msg1.SetPositionRMS(.54);
00691     msg1.SetY(1.002);
00692 
00693     if((msg1.WriteMessageBody(packet) != -1) &&
00694         (msg2.ReadMessageBody(packet) != -1))
00695     {
00696         if(msg2.AreFieldsPresent(PresenceVector::PositionRMS| PresenceVector::Y) )
00697          {
00698                 result = 1;
00699          }
00700     }
00701 
00702     return result;
00703 }
00704 
00705 
00711 SetLocalPose& SetLocalPose::operator=(const SetLocalPose& message)
00712 {
00713     if(this != &message)
00714     {
00715         CopyHeaderData(&message);
00716         mPresenceVector = message.mPresenceVector;
00717         mX = message.mX;
00718         mY = message.mY;
00719         mZ = message.mZ;
00720         mPositionRMS = message.mPositionRMS;
00721         mRoll = message.mRoll;
00722         mPitch = message.mPitch;
00723         mYaw = message.mYaw;
00724         mAttitudeRMS = message.mAttitudeRMS;
00725         mTimeStamp = message.mTimeStamp;
00726         
00727     }
00728     return *this;
00729 }
00730 
00731 
00732 /*  End of File */