jaus++/2.0/src/jaus/extras/rangesensor/reportlocalrangescan.cpp
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/extras/rangesensor/reportlocalrangescan.h" 00041 #include "jaus/core/scaledinteger.h" 00042 #include <cxutils/math/cxmath.h> 00043 00044 using namespace JAUS; 00045 00046 00058 int ReportLocalRangeScan::WriteMessageBody(Packet& packet) const 00059 { 00060 int total = 0; 00061 int expected = BYTE_SIZE + UINT_SIZE*3 + USHORT_SIZE*3 + UINT_SIZE + UINT_SIZE; 00062 00063 total += packet.Write(mSensorID); 00064 total += ScaledInteger::Write(packet, mLocation.mX, 100000.0, -100000.0, ScaledInteger::UInt); 00065 total += ScaledInteger::Write(packet, mLocation.mY, 100000.0, -100000.0, ScaledInteger::UInt); 00066 total += ScaledInteger::Write(packet, mLocation.mZ, 100000.0, -100000.0, ScaledInteger::UInt); 00067 total += ScaledInteger::Write(packet, mOrientation.mX, CxUtils::PI, -CxUtils::PI, ScaledInteger::UShort); 00068 total += ScaledInteger::Write(packet, mOrientation.mY, CxUtils::PI, -CxUtils::PI, ScaledInteger::UShort); 00069 total += ScaledInteger::Write(packet, mOrientation.mZ, CxUtils::PI, -CxUtils::PI, ScaledInteger::UShort); 00070 total += packet.Write(mTimeStamp.ToUInt()); 00071 total += packet.Write( (UInt)(mScan.Size()) ); 00072 00073 for(Scan::const_iterator s = mScan.begin(); 00074 s != mScan.end(); 00075 s++) 00076 { 00077 expected += USHORT_SIZE; 00078 total += packet.Write(*s); 00079 } 00080 00081 return total == expected ? total : -1; 00082 } 00083 00084 00096 int ReportLocalRangeScan::ReadMessageBody(const Packet& packet) 00097 { 00098 int total = 0; 00099 int expected = BYTE_SIZE + UINT_SIZE*3 + USHORT_SIZE*3 + UINT_SIZE + UINT_SIZE; 00100 00101 total += packet.Read(mSensorID); 00102 total += ScaledInteger::Read(packet, mLocation.mX, 100000.0, -100000.0, ScaledInteger::UInt); 00103 total += ScaledInteger::Read(packet, mLocation.mY, 100000.0, -100000.0, ScaledInteger::UInt); 00104 total += ScaledInteger::Read(packet, mLocation.mZ, 100000.0, -100000.0, ScaledInteger::UInt); 00105 total += ScaledInteger::Read(packet, mOrientation.mX, CxUtils::PI, -CxUtils::PI, ScaledInteger::UShort); 00106 total += ScaledInteger::Read(packet, mOrientation.mY, CxUtils::PI, -CxUtils::PI, ScaledInteger::UShort); 00107 total += ScaledInteger::Read(packet, mOrientation.mZ, CxUtils::PI, -CxUtils::PI, ScaledInteger::UShort); 00108 UInt timestamp = 0; 00109 total += packet.Read(timestamp); 00110 mTimeStamp.SetTime(timestamp); 00111 UInt size = 0; 00112 total += packet.Read(size); 00113 UShort val = 0; 00114 mScan.Reserve(size); 00115 for(UInt i = 0; i < size; i++) 00116 { 00117 expected += USHORT_SIZE; 00118 total += packet.Read(val); 00119 mScan.PushBack(val); 00120 } 00121 00122 return total == expected ? total : -1; 00123 } 00124 00125 00132 bool ReportLocalRangeScan::IsLargeDataSet(const unsigned int maxPayloadSize) const 00133 { 00134 unsigned int expected = BYTE_SIZE + UINT_SIZE*3 + USHORT_SIZE*3 + UINT_SIZE + USHORT_SIZE*mScan.Size(); 00135 00136 return expected > maxPayloadSize ? true : false; 00137 } 00138 00139 00140 00141 /* End of File */
