JAUS++: jaus++/2.0/src/jaus/mobility/drivers/globalwaypoint.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/globalwaypoint.h"
00041 #include "jaus/mobility/drivers/setglobalwaypoint.h"
00042 #include "jaus/mobility/drivers/reportglobalwaypoint.h"
00043 #include <iomanip>
00044 #include <cxutils/math/cxmath.h>
00045 
00046 const double JAUS::GlobalWaypoint::Limits::MinLatitude = -90.0;
00047 const double JAUS::GlobalWaypoint::Limits::MaxLatitude = 90.0;
00048 const double JAUS::GlobalWaypoint::Limits::MinLongitude = -180.0;
00049 const double JAUS::GlobalWaypoint::Limits::MaxLongitude = 180.0;
00050 const double JAUS::GlobalWaypoint::Limits::MinElevation = -10000;
00051 const double JAUS::GlobalWaypoint::Limits::MaxElevation = 35000;
00052 const double JAUS::GlobalWaypoint::Limits::MinAngle = -CxUtils::PI;
00053 const double JAUS::GlobalWaypoint::Limits::MaxAngle = CxUtils::PI;
00054 const double JAUS::GlobalWaypoint::Limits::MinWaypointTolerance = 0.0;
00055 const double JAUS::GlobalWaypoint::Limits::MaxWaypointTolerance = 100.0;
00056 const double JAUS::GlobalWaypoint::Limits::MinPathTolerance = 0.0;
00057 const double JAUS::GlobalWaypoint::Limits::MaxPathTolerance = 100000.0;
00058 
00059 using namespace JAUS;
00060 
00061 
00067 GlobalWaypoint::GlobalWaypoint()
00068 {
00069     mPresenceVector = 0;
00070     mLatitude = 0;
00071     mLongitude = 0;
00072     mAltitude = 0;
00073     mRoll = 0;
00074     mPitch = 0;
00075     mYaw = 0;
00076     mWaypointTolerance = 0;
00077     mPathTolerance = 0;
00078 }
00079 
00080 
00086 GlobalWaypoint::GlobalWaypoint(const ReportGlobalWaypoint& waypoint)
00087 {
00088     mPresenceVector = 0;
00089     mLatitude = 0;
00090     mLongitude = 0;
00091     mAltitude = 0;
00092     mRoll = 0;
00093     mPitch = 0;
00094     mYaw = 0;
00095     mWaypointTolerance = 0;
00096     mPathTolerance = 0;
00097     *this = waypoint;
00098 }
00099 
00105 GlobalWaypoint::GlobalWaypoint(const SetGlobalWaypoint& waypoint)
00106 {
00107     mPresenceVector = 0;
00108     mLatitude = 0;
00109     mLongitude = 0;
00110     mAltitude = 0;
00111     mRoll = 0;
00112     mPitch = 0;
00113     mYaw = 0;
00114     mWaypointTolerance = 0;
00115     mPathTolerance = 0;
00116     *this = waypoint;
00117 }
00118 
00124 GlobalWaypoint::GlobalWaypoint(const GlobalWaypoint& waypoint)
00125 {
00126     *this = waypoint;
00127 }
00128 
00129 
00135 GlobalWaypoint::~GlobalWaypoint()
00136 {
00137 }
00138 
00139 
00149 bool GlobalWaypoint::SetPosition(const Wgs& position)
00150 {
00151     bool result = true;
00152     result &= SetLatitude(position.mLatitude);
00153     result &= SetLongitude(position.mLongitude);
00154     result &= SetAltitude(position.mElevation);
00155     return result;
00156 }
00157 
00158 
00168 bool GlobalWaypoint::SetLatitude(const double degrees)
00169 {
00170     if(degrees >= Limits::MinLatitude && degrees <= Limits::MaxLatitude)
00171     {
00172         mLatitude = degrees;
00173         return true;
00174     }
00175     return false;
00176 }
00177 
00178 
00188 bool GlobalWaypoint::SetLongitude(const double degrees)
00189 {
00190     if(degrees >= Limits::MinLongitude && degrees <= Limits::MaxLongitude)
00191     {
00192         mLongitude = degrees;
00193         return true;
00194     }
00195     return false;
00196 }
00197 
00198 
00209 bool GlobalWaypoint::SetAltitude(const double value)
00210 {
00211     if(value >= Limits::MinElevation && value <= Limits::MaxElevation)
00212     {
00213         mAltitude = value;
00214         mPresenceVector |= PresenceVector::Altitude;
00215         return true;
00216     }
00217     return false;
00218 }
00219 
00220 
00230 bool GlobalWaypoint::SetRoll(const double radians)
00231 {
00232     if(radians >= Limits::MinAngle && radians <= Limits::MaxAngle)
00233     {
00234         mRoll = radians;
00235         mPresenceVector |= PresenceVector::Roll;
00236         return true;
00237     }
00238     return false;
00239 }
00240 
00241 
00251 bool GlobalWaypoint::SetPitch(const double radians)
00252 {
00253     if(radians >= Limits::MinAngle && radians <= Limits::MaxAngle)
00254     {
00255         mPitch = radians;
00256         mPresenceVector |= PresenceVector::Pitch;
00257         return true;
00258     }
00259     return false;
00260 }
00261 
00262 
00272 bool GlobalWaypoint::SetYaw(const double radians)
00273 {
00274     if(radians >= Limits::MinAngle && radians <= Limits::MaxAngle)
00275     {
00276         mYaw = radians;
00277         mPresenceVector |= PresenceVector::Yaw;
00278         return true;
00279     }
00280     return false;
00281 }
00282 
00283 
00294 bool GlobalWaypoint::SetWaypointTolerance(const double value)
00295 {
00296     if(value >= Limits::MinWaypointTolerance && value <= Limits::MaxWaypointTolerance)
00297     {
00298         mWaypointTolerance= value;
00299         mPresenceVector |= PresenceVector::WaypointTolerance;
00300         return true;
00301     }
00302     return false;
00303 }
00304 
00305 
00316 bool GlobalWaypoint::SetPathTolerance(const double value)
00317 {
00318     if(value >= Limits::MinPathTolerance && value <= Limits::MaxPathTolerance)
00319     {
00320         mPathTolerance= value;
00321         mPresenceVector |= PresenceVector::PathTolerance;
00322         return true;
00323     }
00324     return false;
00325 }
00326 
00327 
00333 Wgs GlobalWaypoint::GetPosition() const
00334 {
00335     return Wgs(mLatitude, mLongitude, mAltitude);
00336 }
00337 
00338 
00346 Point3D GlobalWaypoint::GetOrientation() const
00347 {
00348     Point3D orientation;
00349     orientation.Set(mRoll, mPitch, mYaw);
00350     return orientation;
00351 }
00352 
00353 
00359 void GlobalWaypoint::Print() const
00360 {
00361     std::cout << "WGS: [" << std::fixed << std::setprecision(5) << mLatitude << ", " 
00362                           << std::fixed << std::setprecision(5) << mLongitude << ", " 
00363                           << std::fixed << std::setprecision(2) << mAltitude << "]\n";
00364     if( (mPresenceVector & (PresenceVector::Yaw | PresenceVector::Pitch | PresenceVector::Roll)) > 0)
00365     {
00366         std::cout << "Orientation: [" << std::fixed << std::setprecision(1) << CxUtils::CxToDegrees(mRoll) << ", " 
00367                                       << std::fixed << std::setprecision(1) << CxUtils::CxToDegrees(mPitch) << ", " 
00368                                       << std::fixed << std::setprecision(1) << CxUtils::CxToDegrees(mYaw) << "]\n";
00369     }
00370     if( (mPresenceVector & (PresenceVector::PathTolerance | PresenceVector::WaypointTolerance)) > 0)
00371     {
00372         std::cout << "Tolerance (Waypoint, Path): [" << mWaypointTolerance << ", " << mPathTolerance << "]" << std::endl;
00373     }
00374 }
00375 
00376 
00388 bool GlobalWaypoint::IsSameAs(const Waypoint* waypoint, const double errorInMeters) const
00389 {
00390     const GlobalWaypoint* gp = dynamic_cast<const GlobalWaypoint*>(waypoint);
00391     if(gp)
00392     {
00393         Wgs a(mLatitude, mLongitude, mAltitude), b(gp->mLatitude, gp->mLongitude, gp->mAltitude);
00394         if(CxUtils::Wgs::GreatCircleDistance(a, b) <= errorInMeters)
00395         {
00396             return true;
00397         }
00398     }
00399     return false;
00400 }
00401 
00402 
00408 GlobalWaypoint& GlobalWaypoint::operator=(const GlobalWaypoint& waypoint)
00409 {
00410     if(this != &waypoint)
00411     {
00412         mActiveFlag = waypoint.mActiveFlag;
00413         mFinishedFlag = waypoint.mFinishedFlag;
00414 
00415         mPresenceVector = waypoint.mPresenceVector;
00416         mLatitude = waypoint.mLatitude;
00417         mLongitude = waypoint.mLongitude;
00418         mAltitude = waypoint.mAltitude;
00419         mRoll = waypoint.mRoll;
00420         mPitch = waypoint.mPitch;
00421         mYaw = waypoint.mYaw;
00422         mWaypointTolerance = waypoint.mWaypointTolerance;
00423         mPathTolerance = waypoint.mPathTolerance;
00424     }
00425     return *this;
00426 }
00427 
00428 
00434 GlobalWaypoint& GlobalWaypoint::operator=(const SetGlobalWaypoint& waypoint)
00435 {
00436     mActiveFlag = waypoint.mActiveFlag;
00437     mFinishedFlag = waypoint.mFinishedFlag;
00438 
00439     mPresenceVector = (Byte)waypoint.GetPresenceVector();
00440     mLatitude = waypoint.GetLatitude();
00441     mLongitude = waypoint.GetLongitude();
00442     mAltitude = waypoint.GetAltitude();
00443     mRoll = waypoint.GetRoll();
00444     mPitch = waypoint.GetPitch();
00445     mYaw = waypoint.GetYaw();
00446     mWaypointTolerance = waypoint.GetWaypointTolerance();
00447     mPathTolerance = waypoint.GetPathTolerance();
00448     return *this;
00449 }
00450 
00451 
00457 GlobalWaypoint& GlobalWaypoint::operator=(const ReportGlobalWaypoint& waypoint)
00458 {
00459     mActiveFlag = waypoint.mActiveFlag;
00460     mFinishedFlag = waypoint.mFinishedFlag;
00461 
00462     mPresenceVector = (Byte)waypoint.GetPresenceVector();
00463     mLatitude = waypoint.GetLatitude();
00464     mLongitude = waypoint.GetLongitude();
00465     mAltitude = waypoint.GetAltitude();
00466     mRoll = waypoint.GetRoll();
00467     mPitch = waypoint.GetPitch();
00468     mYaw = waypoint.GetYaw();
00469     mWaypointTolerance = waypoint.GetWaypointTolerance();
00470     mPathTolerance = waypoint.GetPathTolerance();
00471     return *this;
00472 }
00473 
00474 
00475 /*  End of File */