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