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