jaus++/2.0/src/jaus/mobility/list/setelement.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/mobility/list/setelement.h" 00041 #include "jaus/mobility/drivers/setwrencheffort.h" 00042 00043 using namespace JAUS; 00044 00053 SetElement::SetElement(const Address& dest, const Address& src) : Message(SET_ELEMENT, dest, src) 00054 { 00055 mRequestID = 0; 00056 } 00057 00058 00064 SetElement::SetElement(const SetElement& message) : Message(SET_ELEMENT) 00065 { 00066 mRequestID = 0; 00067 *this = message; 00068 } 00069 00070 00076 SetElement::~SetElement() 00077 { 00078 } 00079 00080 00092 int SetElement::WriteMessageBody(Packet& packet) const 00093 { 00094 int expected = BYTE_SIZE*2; 00095 int written = 0; 00096 00097 written += packet.Write(mRequestID); 00098 written += packet.WriteByte( (Byte)mElements.size() ); 00099 00100 Element::List::const_iterator element; 00101 00102 for(element = mElements.begin(); 00103 element != mElements.end(); 00104 element++) 00105 { 00106 expected += USHORT_SIZE*4 + BYTE_SIZE; 00107 if(element->mpElement) 00108 { 00109 Packet* payload = ((Packet *)(&(element->mPayload))); 00110 payload->Clear(); 00111 payload->Write(element->mpElement->GetMessageCode()); 00112 element->mpElement->WriteMessageBody(*payload); 00113 } 00114 else if(element->mPayload.Size() == 0) 00115 { 00116 break; 00117 } 00118 00119 expected += element->mPayload.Size(); 00120 00121 written += packet.Write(element->mID); 00122 written += packet.Write(element->mPrevID); 00123 written += packet.Write(element->mNextID); 00124 // Variable format field 00125 written += packet.WriteByte(0); // Format field 00126 written += packet.Write((UShort)(element->mPayload.Size())); // Count field 00127 written += packet.Write(element->mPayload); 00128 } 00129 00130 return expected == written ? written : -1; 00131 } 00132 00133 00145 int SetElement::ReadMessageBody(const Packet& packet) 00146 { 00147 int expected = BYTE_SIZE*2; 00148 int read = 0; 00149 00150 Byte count = 0; 00151 00152 read += packet.Read(mRequestID); 00153 read += packet.Read(count); 00154 00155 for(UShort e = 0; e < count; e++) 00156 { 00157 expected += USHORT_SIZE*4 + BYTE_SIZE; 00158 Byte formatField = 0; 00159 UShort countField = 0; 00160 Element element; 00161 00162 read += packet.Read(element.mID); 00163 read += packet.Read(element.mPrevID); 00164 read += packet.Read(element.mNextID); 00165 read += packet.Read(formatField); 00166 read += packet.Read(countField); 00167 00168 expected += countField; 00169 read += packet.Read(element.mPayload, countField); 00170 00171 mElements.push_back(element); 00172 } 00173 00174 return expected == read ? read : -1; 00175 } 00176 00177 00183 void SetElement::ClearMessageBody() 00184 { 00185 mRequestID = 0; 00186 mElements.clear(); 00187 } 00188 00189 00195 bool SetElement::IsLargeDataSet(const unsigned int maxPayloadSize) const 00196 { 00197 int expected = BYTE_SIZE*2; 00198 00199 Element::List::const_iterator element; 00200 00201 for(element = mElements.begin(); 00202 element != mElements.end(); 00203 element++) 00204 { 00205 expected += USHORT_SIZE*4; 00206 if(element->mpElement) 00207 { 00208 Packet* payload = ((Packet *)(&(element->mPayload))); 00209 payload->Clear(); 00210 payload->Write(element->mpElement->GetMessageCode()); 00211 element->mpElement->WriteMessageBody(*payload); 00212 } 00213 else if(element->mPayload.Size() == 0) 00214 { 00215 break; 00216 } 00217 00218 expected += element->mPayload.Size(); 00219 } 00220 00221 return expected > (int)maxPayloadSize ? true : false; 00222 } 00223 00224 00232 int SetElement::RunTestCase() const 00233 { 00234 int result = 0; 00235 00236 Packet packet; 00237 00238 SetElement msg1, msg2; 00239 SetWrenchEffort msg3, msg4, msg5, msg6; 00240 msg3.SetPropulsiveLinearEffortX(13.3); 00241 msg3.SetPropulsiveRotationalEffortZ(3.3); 00242 msg4.SetPropulsiveLinearEffortX(11.3); 00243 msg4.SetPropulsiveRotationalEffortZ(2.3); 00244 Element element1, element2; 00245 element1.mID = 12; 00246 element1.mNextID = 13; 00247 element1.mPrevID = 11; 00248 element1.mpElement = (Message*) msg3.Clone(); 00249 element2.mID = 13; 00250 element2.mNextID = 14; 00251 element2.mPrevID = 12; 00252 element2.mpElement = (Message*) msg4.Clone(); 00253 msg1.GetElementList()->push_back(element1); 00254 msg1.GetElementList()->push_back(element2); 00255 00256 if((msg1.WriteMessageBody(packet) != -1) && 00257 (msg2.ReadMessageBody(packet) != -1)) 00258 { 00259 CxUtils::Packet packet; 00260 msg2.GetElementList()->front().mPayload.Read(packet, 00261 msg2.GetElementList()->front().mPayload.Size() - USHORT_SIZE, USHORT_SIZE); 00262 msg5.ReadMessageBody(packet); 00263 msg2.GetElementList()->back().mPayload.Read(packet, 00264 msg2.GetElementList()->back().mPayload.Size() - USHORT_SIZE, USHORT_SIZE); 00265 msg6.ReadMessageBody(packet); 00266 00267 if((msg3.GetPropulsiveLinearEffortX() - msg5.GetPropulsiveLinearEffortX() < 0.01) && 00268 (msg3.GetPropulsiveRotationalEffortZ() - msg5.GetPropulsiveRotationalEffortZ() < 0.01) && 00269 (msg4.GetPropulsiveLinearEffortX() - msg6.GetPropulsiveLinearEffortX() < 0.01) && 00270 (msg4.GetPropulsiveRotationalEffortZ() - msg6.GetPropulsiveRotationalEffortZ() < 0.01)) 00271 { 00272 return true; 00273 } 00274 } 00275 00276 return result; 00277 } 00278 00279 00285 SetElement& SetElement::operator=(const SetElement& message) 00286 { 00287 if(this != &message) 00288 { 00289 CopyHeaderData(&message); 00290 mRequestID = message.mRequestID; 00291 mElements = message.mElements; 00292 } 00293 return *this; 00294 } 00295 00296 00297 /* End of File */
