JAUS++: jaus++/2.0/src/jaus/mobility/drivers/reportlocalwaypoint.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/drivers/reportlocalwaypoint.h"
00041 #include "jaus/core/scaledinteger.h"
00042 #include <cxutils/math/cxmath.h>
00043 
00044 const double JAUS::ReportLocalWaypoint::Limits::MinPoint = -100000;
00045 const double JAUS::ReportLocalWaypoint::Limits::MaxPoint = 100000;
00046 const double JAUS::ReportLocalWaypoint::Limits::MinAngle = -CxUtils::PI;
00047 const double JAUS::ReportLocalWaypoint::Limits::MaxAngle = CxUtils::PI;
00048 const double JAUS::ReportLocalWaypoint::Limits::MinWaypointTolerance = 0.0;
00049 const double JAUS::ReportLocalWaypoint::Limits::MaxWaypointTolerance = 100.0;
00050 const double JAUS::ReportLocalWaypoint::Limits::MinPathTolerance = 0.0;
00051 const double JAUS::ReportLocalWaypoint::Limits::MaxPathTolerance = 100000.0;
00052 
00053 using namespace JAUS;
00054 
00055 
00064 ReportLocalWaypoint::ReportLocalWaypoint(const Address& dest, const Address& src) : Message(REPORT_LOCAL_WAYPOINT, dest, src)
00065 {
00066     mPresenceVector = 0;
00067     mX = 0;
00068     mY = 0;
00069     mZ = 0;
00070     mRoll = 0;
00071     mPitch = 0;
00072     mYaw = 0;
00073     mWaypointTolerance = 0;
00074     mPathTolerance = 0;
00075 }
00076 
00077 
00083 ReportLocalWaypoint::ReportLocalWaypoint(const ReportLocalWaypoint& message) : Message(REPORT_LOCAL_WAYPOINT)
00084 {
00085     *this = message;
00086 }
00087 
00088 
00094 ReportLocalWaypoint::~ReportLocalWaypoint()
00095 {
00096 }
00097 
00098 
00108 bool ReportLocalWaypoint::SetX(const double value)
00109 {
00110     if(value >= Limits::MinPoint && value <= Limits::MaxPoint)
00111     {
00112         mX = value;
00113         return true;
00114     }
00115     return false;
00116 }
00117 
00118 
00128 bool ReportLocalWaypoint::SetY(const double value)
00129 {
00130     if(value >= Limits::MinPoint && value <= Limits::MaxPoint)
00131     {
00132         mY = value;
00133         return true;
00134     }
00135     return false;
00136 }
00137 
00138 
00149 bool ReportLocalWaypoint::SetZ(const double value)
00150 {
00151     if(value >= Limits::MinPoint && value <= Limits::MaxPoint)
00152     {
00153         mZ = value;
00154         mPresenceVector |= PresenceVector::Z;
00155         return true;
00156     }
00157     return false;
00158 }
00159 
00160 
00170 bool ReportLocalWaypoint::SetRoll(const double radians)
00171 {
00172     if(radians >= Limits::MinAngle && radians <= Limits::MaxAngle)
00173     {
00174         mRoll = radians;
00175         mPresenceVector |= PresenceVector::Roll;
00176         return true;
00177     }
00178     return false;
00179 }
00180 
00181 
00191 bool ReportLocalWaypoint::SetPitch(const double radians)
00192 {
00193     if(radians >= Limits::MinAngle && radians <= Limits::MaxAngle)
00194     {
00195         mPitch = radians;
00196         mPresenceVector |= PresenceVector::Pitch;
00197         return true;
00198     }
00199     return false;
00200 }
00201 
00202 
00212 bool ReportLocalWaypoint::SetYaw(const double radians)
00213 {
00214     if(radians >= Limits::MinAngle && radians <= Limits::MaxAngle)
00215     {
00216         mYaw = radians;
00217         mPresenceVector |= PresenceVector::Yaw;
00218         return true;
00219     }
00220     return false;
00221 }
00222 
00223 
00234 bool ReportLocalWaypoint::SetWaypointTolerance(const double value)
00235 {
00236     if(value >= Limits::MinWaypointTolerance && value <= Limits::MaxWaypointTolerance)
00237     {
00238         mWaypointTolerance= value;
00239         mPresenceVector |= PresenceVector::WaypointTolerance;
00240         return true;
00241     }
00242     return false;
00243 }
00244 
00245 
00256 bool ReportLocalWaypoint::SetPathTolerance(const double value)
00257 {
00258     if(value >= Limits::MinPathTolerance && value <= Limits::MaxPathTolerance)
00259     {
00260         mPathTolerance= value;
00261         mPresenceVector |= PresenceVector::PathTolerance;
00262         return true;
00263     }
00264     return false;
00265 }
00266 
00267 
00279 int ReportLocalWaypoint::WriteMessageBody(Packet& packet) const
00280 {
00281     int expected = BYTE_SIZE;
00282     int written = 0;
00283 
00284     written += packet.Write(mPresenceVector);
00285 
00286     expected += UINT_SIZE;
00287     written += ScaledInteger::Write(packet, mX, Limits::MaxPoint, Limits::MinPoint, ScaledInteger::UInt);
00288 
00289     expected += UINT_SIZE;
00290     written += ScaledInteger::Write(packet, mY, Limits::MaxPoint, Limits::MinPoint, ScaledInteger::UInt);
00291 
00292     if((mPresenceVector & PresenceVector::Z) > 0)
00293     {
00294         expected += UINT_SIZE;
00295         written += ScaledInteger::Write(packet, mZ, Limits::MaxPoint, Limits::MinPoint, ScaledInteger::UInt);
00296     }
00297     if((mPresenceVector & PresenceVector::Roll) > 0)
00298     {
00299         expected += USHORT_SIZE;
00300         written += ScaledInteger::Write(packet, mRoll, Limits::MaxAngle, Limits::MinAngle, ScaledInteger::UShort);
00301     }
00302     if((mPresenceVector & PresenceVector::Pitch) > 0)
00303     {
00304         expected += USHORT_SIZE;
00305         written += ScaledInteger::Write(packet, mPitch, Limits::MaxAngle, Limits::MinAngle, ScaledInteger::UShort);
00306     }
00307     if((mPresenceVector & PresenceVector::Yaw) > 0)
00308     {
00309         expected += USHORT_SIZE;
00310         written += ScaledInteger::Write(packet, mYaw, Limits::MaxAngle, Limits::MinAngle, ScaledInteger::UShort);
00311     }
00312     if((mPresenceVector & PresenceVector::WaypointTolerance) > 0)
00313     {
00314         expected += USHORT_SIZE;
00315         written += ScaledInteger::Write(packet, mWaypointTolerance, Limits::MaxWaypointTolerance, Limits::MinWaypointTolerance, ScaledInteger::UShort);
00316     }
00317     if((mPresenceVector & PresenceVector::PathTolerance) > 0)
00318     {
00319         expected += UINT_SIZE;
00320         written += ScaledInteger::Write(packet, mPathTolerance, Limits::MaxPathTolerance, Limits::MinPathTolerance, ScaledInteger::UInt);
00321     }
00322 
00323     return expected == written ? written : -1;
00324 }
00325 
00326 
00338 int ReportLocalWaypoint::ReadMessageBody(const Packet& packet)
00339 {
00340     int expected = BYTE_SIZE;
00341     int read = 0;
00342 
00343     read += packet.Read(mPresenceVector);
00344 
00345     expected += UINT_SIZE;
00346     read += ScaledInteger::Read(packet, mX, Limits::MaxPoint, Limits::MinPoint, ScaledInteger::UInt);
00347 
00348     expected += UINT_SIZE;
00349     read += ScaledInteger::Read(packet, mY, Limits::MaxPoint, Limits::MinPoint, ScaledInteger::UInt);
00350 
00351     if((mPresenceVector & PresenceVector::Z) > 0)
00352     {
00353         expected += UINT_SIZE;
00354         read += ScaledInteger::Read(packet, mZ, Limits::MaxPoint, Limits::MinPoint, ScaledInteger::UInt);
00355     }
00356     if((mPresenceVector & PresenceVector::Roll) > 0)
00357     {
00358         expected += USHORT_SIZE;
00359         read += ScaledInteger::Read(packet, mRoll, Limits::MaxAngle, Limits::MinAngle, ScaledInteger::UShort);
00360     }
00361     if((mPresenceVector & PresenceVector::Pitch) > 0)
00362     {
00363         expected += USHORT_SIZE;
00364         read += ScaledInteger::Read(packet, mPitch, Limits::MaxAngle, Limits::MinAngle, ScaledInteger::UShort);
00365     }
00366     if((mPresenceVector & PresenceVector::Yaw) > 0)
00367     {
00368         expected += USHORT_SIZE;
00369         read += ScaledInteger::Read(packet, mYaw, Limits::MaxAngle, Limits::MinAngle, ScaledInteger::UShort);
00370     }
00371     if((mPresenceVector & PresenceVector::WaypointTolerance) > 0)
00372     {
00373         expected += USHORT_SIZE;
00374         read += ScaledInteger::Read(packet, mWaypointTolerance, Limits::MaxWaypointTolerance, Limits::MinWaypointTolerance, ScaledInteger::UShort);
00375     }
00376     if((mPresenceVector & PresenceVector::PathTolerance) > 0)
00377     {
00378         expected += UINT_SIZE;
00379         read += ScaledInteger::Read(packet, mPathTolerance, Limits::MaxPathTolerance, Limits::MinPathTolerance, ScaledInteger::UInt);
00380     }
00381 
00382     return expected == read ? read : -1;
00383 }
00384 
00385 
00391 void ReportLocalWaypoint::ClearMessageBody()
00392 {
00393     mPresenceVector = 0;
00394     mX = 0;
00395     mY = 0;
00396     mZ = 0;
00397     mRoll = 0;
00398     mPitch = 0;
00399     mYaw = 0;
00400     mWaypointTolerance = 0;
00401     mPathTolerance = 0;
00402 }
00403 
00404 
00412 int ReportLocalWaypoint::RunTestCase() const
00413 {
00414     int result = 0;
00415 
00416     Packet packet;
00417 
00418     ReportLocalWaypoint msg1, msg2;
00419 
00420     msg1.SetPitch(-1.25);
00421     msg1.SetWaypointTolerance(50);
00422 
00423     if((msg1.WriteMessageBody(packet) != -1) &&
00424         (msg2.ReadMessageBody(packet) != -1))
00425     {
00426         if(msg2.AreFieldsPresent(PresenceVector::Pitch | PresenceVector::WaypointTolerance) )
00427          {
00428                 result = 1;
00429          }
00430     }
00431 
00432     return result;
00433 }
00434 
00435 
00441 ReportLocalWaypoint& ReportLocalWaypoint::operator=(const ReportLocalWaypoint& message)
00442 {
00443     if(this != &message)
00444     {
00445         CopyHeaderData(&message);
00446         mPresenceVector = message.mPresenceVector;
00447         mX = message.mX;
00448         mY = message.mY;
00449         mZ = message.mZ;
00450         mRoll = message.mRoll;
00451         mPitch = message.mPitch;
00452         mYaw = message.mYaw;
00453         mWaypointTolerance = message.mWaypointTolerance;
00454         mPathTolerance = message.mPathTolerance;
00455         
00456     }
00457     return *this;
00458 }
00459 
00460 
00461 /*  End of File */