00001 /* --------------------------------------------------------------------------- 00002 Phission : 00003 Realtime Vision Processing System 00004 00005 Copyright (C) 2003 Philip D.S. Thoren (pthoren@cs.uml.edu) 00006 University of Massachusetts at Lowell, 00007 Laboratory for Artificial Intelligence and Robotics 00008 00009 This file is part of Phission. 00010 00011 ---------------------------------------------------------------------------*/ 00012 #include <phRequestDB.h> 00013 00014 /* -------------------------------------------------------------------------- */ 00015 /* phMessageNode: */ 00016 /* -------------------------------------------------------------------------- */ 00017 phMessageNode::phMessageNode( ) 00018 { 00019 phFUNCTION("phMessageNode::phMessageNode") 00020 00021 this->setName("phMessageNode"); 00022 00023 } 00024 00025 /* -------------------------------------------------------------------------- */ 00026 phMessageNode::~phMessageNode() 00027 { 00028 phFUNCTION("phMessageNode::~phMessageNode") 00029 } 00030 00031 /* -------------------------------------------------------------------------- */ 00032 int phMessageNode::addReply( phMessage reply ) 00033 { 00034 phFUNCTION("phMessageNode::addReply") 00035 00036 phMessage *copyreply = NULL; 00037 phObjectNode *node = NULL; 00038 00039 copyreply = new phMessage(reply); 00040 00041 node = this->m_list.pushObject(copyreply); 00042 phCHECK_NULLPTR(node,NULL,"this->m_list.pushObject(copyreply:%p) failed.", 00043 copyreply); 00044 00045 rc = node->enableAutoDelete(); 00046 phPRINT_RC(rc,NULL,"node->enableAutoDelete failed."); 00047 00048 return phSUCCESS; 00049 error: 00050 return phFAIL; 00051 } 00052 00053 /* -------------------------------------------------------------------------- */ 00054 uint32_t phMessageNode::getTotalReplies( ) 00055 { 00056 return this->m_list.length(); 00057 } 00058 00059 /* -------------------------------------------------------------------------- */ 00060 int phMessageNode::setMessage( phMessage message ) 00061 { 00062 return this->m_message.set(message); 00063 } 00064 00065 /* -------------------------------------------------------------------------- */ 00066 phMessage &phMessageNode::getMessage() 00067 { 00068 return this->m_message; 00069 } 00070 00071 /* -------------------------------------------------------------------------- */ 00072 int phMessageNode::messageRelease() 00073 { 00074 return this->m_semaphore.post(); 00075 } 00076 00077 /* -------------------------------------------------------------------------- */ 00078 int phMessageNode::messageWait() 00079 { 00080 return this->m_semaphore.take(); 00081 } 00082 00083 /* -------------------------------------------------------------------------- */ 00084 /* phRequestList: */ 00085 /* -------------------------------------------------------------------------- */ 00086 phRequestList::phRequestList(uint32_t id) 00087 { 00088 phFUNCTION("phRequestList::phRequestList") 00089 00090 this->setName("phRequestList"); 00091 00092 this->setId(id); 00093 00094 this->m_clock = 0; 00095 } 00096 00097 /* -------------------------------------------------------------------------- */ 00098 phRequestList::~phRequestList() 00099 { 00100 phFUNCTION("phRequestList::~phRequestList") 00101 } 00102 00103 /* -------------------------------------------------------------------------- */ 00104 uint32_t phRequestList::tick( uint32_t compare_ts ) 00105 { 00106 uint32_t value = phTIME_INVALID; 00107 00108 if (compare_ts != phTIME_INVALID) 00109 { 00110 value = compare_ts + 1; 00111 if (value > this->m_clock) 00112 { 00113 this->m_clock = value; 00114 } 00115 } 00116 else 00117 { 00118 this->m_clock++; 00119 } 00120 00121 return this->m_clock; 00122 } 00123 00124 /* -------------------------------------------------------------------------- */ 00125 uint32_t phRequestList::getTimestamp( ) 00126 { 00127 return this->m_clock; 00128 } 00129 00130 /* -------------------------------------------------------------------------- */ 00131 int phRequestList::setId( uint32_t id ) 00132 { 00133 this->m_id = id; 00134 return phSUCCESS; 00135 } 00136 00137 /* -------------------------------------------------------------------------- */ 00138 uint32_t phRequestList::getId( ) 00139 { 00140 return this->m_id; 00141 } 00142 00143 /* -------------------------------------------------------------------------- */ 00144 phMessageNode *phRequestList::addMessage( phMessage message, 00145 uint32_t system_nodes ) 00146 { 00147 phFUNCTION("phRequestList::addMessage") 00148 phObjectNode *node = NULL; 00149 phMessageNode *msgNode = NULL; 00150 uint32_t i = 0; 00151 uint32_t insertAt = 0; 00152 phMessage msg; 00153 int done = 0; 00154 uint32_t node_id = 0; 00155 00156 /* Find the first message with less than the total system 00157 * nodes for the total replies */ 00158 if (message.getMessageId() == phNODE_REPLY) 00159 { 00160 DEBUG_PRINT("REPLY\n"); 00161 node = this->m_list.getHeadObject(); 00162 00163 /* Loop through all the displays in the list */ 00164 /* for (uint32_t i = 0; i < total_displays; i++ ) */ 00165 DEBUG_PRINT("\n"); 00166 while ((node != NULL) && (!done)) 00167 { 00168 msgNode = (phMessageNode *)node->getObject(); 00169 00170 DEBUG_PRINT("\n"); 00171 if (msgNode != NULL) 00172 { 00173 msg = msgNode->getMessage(); 00174 node_id = msg.getSrcId(); 00175 00176 DEBUG_PRINT("totalReplies:%u ? nodes:%u\n", 00177 msgNode->getTotalReplies(), 00178 system_nodes); 00179 DEBUG_PRINT("node_id:%u ? message.getDstId():%u\n", 00180 node_id,message.getDstId()); 00181 00182 if ((msgNode->getTotalReplies() < system_nodes) && 00183 (node_id == message.getDstId())) 00184 { 00185 rc = msgNode->addReply(message); 00186 phPRINT_RC(rc,NULL,"msgNode->addReply failed."); 00187 00188 done = 1; 00189 } 00190 } 00191 00192 node = node->getNextObject(); 00193 i++; 00194 } 00195 } 00196 /* Create a new message node that copies the one being 00197 * added */ 00198 else if ((message.getMessageId() == phCLIENT_REQUEST) || 00199 (message.getMessageId() == phNODE_REQUEST)) 00200 { 00201 DEBUG_PRINT("REQUEST\n"); 00202 node = this->m_list.getHeadObject(); 00203 00204 /* Loop through all the displays in the list */ 00205 done = 0; 00206 while ((node != NULL) && (!done)) 00207 { 00208 msgNode = (phMessageNode *)node->getObject(); 00209 00210 if (msgNode != NULL) 00211 { 00212 msg = msgNode->getMessage(); 00213 00214 DEBUG_PRINT("ti-new:%u ? node:%u\n", 00215 message.getTimestamp(), 00216 msg.getTimestamp()); 00217 /* compare the timestamps */ 00218 if (message.getTimestamp() > msg.getTimestamp()) 00219 { 00220 DEBUG_PRINT("ti-new:%u < node:%u\n", 00221 message.getTimestamp(), 00222 msg.getTimestamp()); 00223 insertAt++; 00224 } 00225 else if (message.getTimestamp() == msg.getTimestamp()) 00226 { 00227 DEBUG_PRINT("ti-new:%u == node:%u\n", 00228 message.getTimestamp(), 00229 msg.getTimestamp()); 00230 /* if the SrcId of the current node is greater than 00231 * the message we'll be inserting, then were done 00232 * here.... */ 00233 if (message.getSrcId() <= msg.getSrcId()) 00234 { 00235 DEBUG_PRINT("id-new:%u < node:%u\n", 00236 message.getSrcId(), 00237 msg.getSrcId()); 00238 done = 1; 00239 DEBUG_PRINT("done:%d\n",done); 00240 } 00241 /* ... otherwise we need to make sure there aren't 00242 * any more nodes with the same timestamp. 00243 * so we continue looping until we've reached 00244 * the point where we can exit. */ 00245 else 00246 { 00247 insertAt++; 00248 DEBUG_PRINT("insertAt:%u\n",insertAt); 00249 } 00250 } 00251 else 00252 { 00253 done = 1; 00254 DEBUG_PRINT("done:%d\n",done); 00255 } 00256 } 00257 00258 node = node->getNextObject(); 00259 i++; 00260 } 00261 00262 msgNode = new phMessageNode( ); 00263 phCHECK_NULLPTR(msgNode,"new","new phMessageNode failed."); 00264 00265 rc = msgNode->setMessage( message ); 00266 phPRINT_RC(rc,NULL,"msgNode->setMessage failed."); 00267 00268 /* 00269 phPROGRESS("msgNode:%p insertAt:%u length:%u\n", 00270 msgNode,insertAt, this->m_list.length()); 00271 msgNode->getMessage().print(stderr); 00272 */ 00273 00274 node = this->m_list.insertObject( msgNode, insertAt ); 00275 phCHECK_NULLPTR(node,NULL,"insertObject(msg,%u) failed.", 00276 insertAt); 00277 } 00278 00279 return msgNode; 00280 error: 00281 return NULL; 00282 } 00283 00284 /* -------------------------------------------------------------------------- */ 00285 phMessageNode *phRequestList::getHeadRequest() 00286 { 00287 phFUNCTION("phRequestList::getHeadRequest") 00288 00289 phObjectNode *node = NULL; 00290 phMessageNode *msgnode = NULL; 00291 00292 node = this->m_list.getHeadObject(); 00293 00294 if (node != NULL) 00295 { 00296 msgnode = (phMessageNode *)node->getObject(); 00297 phCHECK_NULLPTR(msgnode,NULL,"node->getObject() returned NULL"); 00298 } 00299 00300 return msgnode; 00301 error: 00302 return NULL; 00303 } 00304 00305 /* -------------------------------------------------------------------------- */ 00306 int phRequestList::removeHeadRequest() 00307 { 00308 phFUNCTION("phRequestList::removeHeadRequest") 00309 00310 phObject *obj = NULL; 00311 phMessageNode *msgnode = NULL; 00312 phMessage msg; 00313 00314 if (this->m_list.length() > 0) 00315 { 00316 obj = this->m_list.shiftObject(); 00317 00318 if (obj != NULL) 00319 { 00320 msgnode = (phMessageNode *)obj; 00321 00322 /* 00323 phPROGRESS("Removing node:\n"); 00324 msg = msgnode->getMessage(); 00325 msg.print(stderr); 00326 */ 00327 phDelete(msgnode); 00328 } 00329 } 00330 00331 return phSUCCESS; 00332 error: 00333 return phFAIL; 00334 } 00335 00336 /* -------------------------------------------------------------------------- */ 00337 int phRequestList::getTotalRequests() 00338 { 00339 return this->m_list.length(); 00340 } 00341 00342 /* -------------------------------------------------------------------------- */ 00343 void phRequestList::print(FILE *fd,char *msg) 00344 { 00345 phFUNCTION("phRequestList::print") 00346 uint32_t i = 0; 00347 00348 phObjectNode *node = NULL; 00349 phMessageNode *msgNode = NULL; 00350 00351 node = this->m_list.getHeadObject(); 00352 00353 fprintf(fd,"--------------------------------------------------------------------------\n"); 00354 fprintf(fd,"RequestList[id:%3u c:%4u len:%3u] - %s\n", 00355 this->m_id,this->m_clock,this->m_list.length(),msg ); 00356 00357 /* Loop through all the displays in the list */ 00358 while (node != NULL) 00359 { 00360 msgNode = (phMessageNode *)node->getObject(); 00361 00362 if (msgNode != NULL) 00363 { 00364 fprintf(fd,"%u:",i); 00365 msgNode->print(fd); 00366 i++; 00367 } 00368 00369 node = node->getNextObject(); 00370 } 00371 00372 fprintf(fd,"--------------------------------------------------------------------------\n\n"); 00373 fflush(fd); 00374 } 00375 /* -------------------------------------------------------------------------- */ 00376 void phMessageNode::print(FILE *fd) 00377 { 00378 phFUNCTION("phMessageNode::print") 00379 uint32_t i = 0; 00380 00381 phObjectNode *node = NULL; 00382 phMessage *msg = NULL; 00383 00384 fprintf(fd,"\tphMessageNode[replies:%2u]\t", 00385 this->m_list.length()); 00386 00387 this->m_message.print(fd,2); 00388 00389 if (this->m_list.length() > 0) 00390 { 00391 fprintf(fd,"\t\tReplies:\n"); 00392 } 00393 00394 node = this->m_list.getHeadObject(); 00395 00396 /* Loop through all the displays in the list */ 00397 while (node != NULL) 00398 { 00399 msg= (phMessage *)node->getObject(); 00400 00401 if (msg != NULL) 00402 { 00403 fprintf(fd,"\t\t\t%u:",i); 00404 msg->print(fd,2); 00405 i++; 00406 } 00407 00408 node = node->getNextObject(); 00409 } 00410 } 00411 00412 /* -------------------------------------------------------------------------- */ 00413 /* phRequestDB: */ 00414 /* -------------------------------------------------------------------------- */ 00415 phRequestDB::phRequestDB() 00416 { 00417 phFUNCTION("phRequestDB::phRequestDB") 00418 this->setName("phRequestDB"); 00419 } 00420 00421 /* -------------------------------------------------------------------------- */ 00422 phRequestDB::~phRequestDB() 00423 { 00424 phFUNCTION("phRequestDB::~phRequestDB") 00425 } 00426 00427 /* -------------------------------------------------------------------------- */ 00428 phRequestList *phRequestDB::getRequestList( uint32_t id ) 00429 { 00430 phFUNCTION("phRequestDB::getRequestList") 00431 00432 phObjectNode *node = NULL; 00433 phRequestList *reqnode = NULL; 00434 phRequestList *ret = NULL; 00435 uint32_t i = 0; 00436 uint32_t insertAt = 0; 00437 00438 if (id == phMUTEX_INVALID) return NULL; 00439 00440 node = this->m_list.getHeadObject(); 00441 00442 /* Loop through all the displays in the list */ 00443 while ((node != NULL) && (ret == NULL)) 00444 { 00445 reqnode = (phRequestList *)node->getObject(); 00446 00447 if (reqnode != NULL) 00448 { 00449 if (reqnode->getId() == id) 00450 { 00451 ret = reqnode; 00452 } 00453 /* count the number of nodes that we'll want to insert 00454 * after if we don't find the node... this way we 00455 * only search the list once */ 00456 else if (reqnode->getId() > id) 00457 { 00458 insertAt++; 00459 } 00460 } 00461 00462 node = node->getNextObject(); 00463 i++; 00464 } 00465 00466 /* if the return list == NULL then we need to create one */ 00467 if (ret == NULL) 00468 { 00469 ret = new phRequestList(id); 00470 phCHECK_NULLPTR(ret,"new","new phRequestList(id:%d) failed.", 00471 id); 00472 00473 node = this->m_list.insertObject( ret, insertAt ); 00474 phCHECK_NULLPTR(node,NULL,"insertObject(reqlist,%u) failed.", 00475 insertAt); 00476 00477 rc = node->enableAutoDelete(); 00478 phPRINT_RC(rc,NULL,"node->enableAutoDelete failed."); 00479 } 00480 00481 return ret; 00482 error: 00483 return NULL; 00484 } 00485 00486 /* -------------------------------------------------------------------------- */ 00487 uint32_t phRequestDB::getTotalLists() 00488 { 00489 return this->m_list.length(); 00490 } 00491
Copyright (C) 2002 - 2007 |
Philip D.S. Thoren ( pthoren@users.sourceforge.net ) University Of Massachusetts at Lowell Robotics Lab |