00001
00029 #include <RangeFinder.H>
00030 #include <CommsManager.H>
00031 #include <Logger.H>
00032
00033
00034
00035 using namespace WURDE;
00036
00037 RangeFinder::RangeFinder(const std::string & streamname){
00038 init(streamname,STRAT_NORMAL,"Any");
00039
00040
00041 m_haveGlobalname=false;
00042 }
00043
00044 RangeFinder::RangeFinder(const ConnectionStrategy & strategy,const std::string & gblName){
00045 init("Any",strategy,gblName);
00046
00047 if(strategy==STRAT_NORMAL){
00048
00049
00050
00051 m_haveGlobalname=false;
00052 }
00053
00054 m_haveStream=false;
00055
00056 }
00057
00058 RangeFinder::RangeFinder(const std::string &streamname,const std::string &gblName){
00059 init(streamname,STRAT_NORMAL,gblName);
00060 }
00061
00062 void RangeFinder::init(const std::string &streamname,const ConnectionStrategy &strategy,const std::string &gblName){
00063 m_globalname=gblName;
00064 m_streamname=streamname;
00065
00066 myCOMObject=NULL;
00067
00068
00069 requests=info;
00070
00071 m_queue=false;
00072 m_supplier=false;
00073 m_doTimestamp=true;
00074 m_doTag=true;
00075
00076 m_doPublishData=false;
00077 m_doPublishInfo=false;
00078
00079 m_initialize=false;
00080 m_strategy=strategy;
00081
00082 m_haveStream=true;
00083 m_haveGlobalname=true;
00084 m_doPublishData=false;
00085 m_doPublishInfo=false;
00086
00087 m_doAutoPing=true;
00088
00089 info.source=streamname;
00090 requests.source=m_globalname;
00091 info.target="Any";
00092 requests.target=streamname;
00093 info.timestamp.now();
00094 requests.timestamp.now();
00095
00096 m_streamRequest=STAT_NULL;
00097
00098 m_sourceModule="Unknown";
00099 }
00100
00101
00102 RangeFinder::~RangeFinder(){
00103 myManager=NULL;
00104 if(myCOMObject){
00105
00106
00107 }
00108 }
00109
00110
00111 void RangeFinder::activateSupplier(){
00112 if(m_haveStream){
00113 m_supplier=true;
00114 m_globalname=m_streamname;
00115 m_haveGlobalname=true;
00116 m_initialize=false;
00117 if(myCOMObject){
00118
00119 myCOMObject->setGlobalName(m_globalname);
00120 myCOMObject->setStreamName(m_streamname);
00121 myCOMObject->setInfo(&requests);
00122 myCOMObject->setRequests(&info);
00123 myCOMObject->setStaging(&data);
00124 myCOMObject->activateSupplier();
00125 myCOMObject->setAutoPing(m_doAutoPing);
00126 myCOMObject->setInfoVector(&m_requestVector);
00127 myCOMObject->setRequestVector(&m_infoVector);
00128 myCOMObject->setDataVector(&m_dataVector);
00129
00130 data.source=m_streamname;
00131 info.source=m_streamname;
00132
00133 if(m_queue){
00134 myCOMObject->setQueueMode(true);
00135 }
00136
00137 }else{
00138 g_fatal("Attempted to activateSupplier before setting a COMObject Please just use registerSupplier.");
00139 exit(-1);
00140 }
00141 data.ranges.clear();
00142 if(m_supplier){
00143 info.x_offset.setCMode(C_SUPPLIER);
00144 }else{
00145 info.x_offset.setCMode(C_CONSUMER);
00146 }
00147 if(m_supplier){
00148 info.max_range.setCMode(C_SUPPLIER);
00149 }else{
00150 info.max_range.setCMode(C_CONSUMER);
00151 }
00152 if(m_supplier){
00153 info.y_offset.setCMode(C_SUPPLIER);
00154 }else{
00155 info.y_offset.setCMode(C_CONSUMER);
00156 }
00157 if(m_supplier){
00158 info.phi_separation.setCMode(C_SUPPLIER);
00159 }else{
00160 info.phi_separation.setCMode(C_CONSUMER);
00161 }
00162 if(m_supplier){
00163 info.psi_offset.setCMode(C_SUPPLIER);
00164 }else{
00165 info.psi_offset.setCMode(C_CONSUMER);
00166 }
00167 if(m_supplier){
00168 info.x_separation.setCMode(C_SUPPLIER);
00169 }else{
00170 info.x_separation.setCMode(C_CONSUMER);
00171 }
00172 if(m_supplier){
00173 info.phi_offset.setCMode(C_SUPPLIER);
00174 }else{
00175 info.phi_offset.setCMode(C_CONSUMER);
00176 }
00177 if(m_supplier){
00178 info.theta_separation.setCMode(C_SUPPLIER);
00179 }else{
00180 info.theta_separation.setCMode(C_CONSUMER);
00181 }
00182 if(m_supplier){
00183 info.half_angle.setCMode(C_SUPPLIER);
00184 }else{
00185 info.half_angle.setCMode(C_CONSUMER);
00186 }
00187 if(m_supplier){
00188 info.psi_separation.setCMode(C_SUPPLIER);
00189 }else{
00190 info.psi_separation.setCMode(C_CONSUMER);
00191 }
00192 if(m_supplier){
00193 info.y_separation.setCMode(C_SUPPLIER);
00194 }else{
00195 info.y_separation.setCMode(C_CONSUMER);
00196 }
00197 if(m_supplier){
00198 info.sensor_positions.setCMode(C_SUPPLIER);
00199 }else{
00200 info.sensor_positions.setCMode(C_CONSUMER);
00201 }
00202 if(m_supplier){
00203 info.uniform.setCMode(C_SUPPLIER);
00204 }else{
00205 info.uniform.setCMode(C_CONSUMER);
00206 }
00207 if(m_supplier){
00208 info.z_separation.setCMode(C_SUPPLIER);
00209 }else{
00210 info.z_separation.setCMode(C_CONSUMER);
00211 }
00212 if(m_supplier){
00213 info.theta_offset.setCMode(C_SUPPLIER);
00214 }else{
00215 info.theta_offset.setCMode(C_CONSUMER);
00216 }
00217 if(m_supplier){
00218 info.z_offset.setCMode(C_SUPPLIER);
00219 }else{
00220 info.z_offset.setCMode(C_CONSUMER);
00221 }
00222 if(m_supplier){
00223 info.angle_min.setCMode(C_SUPPLIER);
00224 }else{
00225 info.angle_min.setCMode(C_CONSUMER);
00226 }
00227 }else{
00228
00229
00230 m_initialize=true;
00231 m_supplier=true;
00232 }
00233
00234 requests=info;
00235
00236 }
00237
00238 void RangeFinder::activateConsumer(){
00239 std::string localname;
00240
00241 if(m_haveStream){
00242 m_initialize=false;
00243 m_supplier=false;
00244 if(myCOMObject&&myManager){
00245
00246 if(!m_haveGlobalname&&m_strategy==STRAT_NORMAL){
00247 m_globalname=myManager->getName()+RANGEFINDER+m_streamname+"Consumer";
00248 m_haveGlobalname=true;
00249 }
00250
00251 myCOMObject->setGlobalName(m_globalname);
00252 myCOMObject->setStreamName(m_streamname);
00253 requests.source=m_globalname;
00254 data.source=m_streamname;
00255 info.source=m_streamname;
00256 myCOMObject->setInfo(&info);
00257 myCOMObject->setRequests(&requests);
00258 myCOMObject->setStaging(&data);
00259 myCOMObject->setAutoPing(m_doAutoPing);
00260 myCOMObject->activateConsumer();
00261 myCOMObject->setInfoVector(&m_infoVector);
00262 myCOMObject->setRequestVector(&m_requestVector);
00263 myCOMObject->setDataVector(&m_dataVector);
00264
00265 if(m_queue){
00266 myCOMObject->setQueueMode(true);
00267 }
00268 }else{
00269 g_fatal("Attempted to activateConsumer before setting a COMObject and CommsManager. Please use registerConsumer instead.");
00270 exit(-1);
00271 }
00272
00273 data.ranges.clear();
00274 if(m_supplier){
00275 info.x_offset.setCMode(C_SUPPLIER);
00276 }else{
00277 info.x_offset.setCMode(C_CONSUMER);
00278 }
00279 if(m_supplier){
00280 info.max_range.setCMode(C_SUPPLIER);
00281 }else{
00282 info.max_range.setCMode(C_CONSUMER);
00283 }
00284 if(m_supplier){
00285 info.y_offset.setCMode(C_SUPPLIER);
00286 }else{
00287 info.y_offset.setCMode(C_CONSUMER);
00288 }
00289 if(m_supplier){
00290 info.phi_separation.setCMode(C_SUPPLIER);
00291 }else{
00292 info.phi_separation.setCMode(C_CONSUMER);
00293 }
00294 if(m_supplier){
00295 info.psi_offset.setCMode(C_SUPPLIER);
00296 }else{
00297 info.psi_offset.setCMode(C_CONSUMER);
00298 }
00299 if(m_supplier){
00300 info.x_separation.setCMode(C_SUPPLIER);
00301 }else{
00302 info.x_separation.setCMode(C_CONSUMER);
00303 }
00304 if(m_supplier){
00305 info.phi_offset.setCMode(C_SUPPLIER);
00306 }else{
00307 info.phi_offset.setCMode(C_CONSUMER);
00308 }
00309 if(m_supplier){
00310 info.theta_separation.setCMode(C_SUPPLIER);
00311 }else{
00312 info.theta_separation.setCMode(C_CONSUMER);
00313 }
00314 if(m_supplier){
00315 info.half_angle.setCMode(C_SUPPLIER);
00316 }else{
00317 info.half_angle.setCMode(C_CONSUMER);
00318 }
00319 if(m_supplier){
00320 info.psi_separation.setCMode(C_SUPPLIER);
00321 }else{
00322 info.psi_separation.setCMode(C_CONSUMER);
00323 }
00324 if(m_supplier){
00325 info.y_separation.setCMode(C_SUPPLIER);
00326 }else{
00327 info.y_separation.setCMode(C_CONSUMER);
00328 }
00329 if(m_supplier){
00330 info.sensor_positions.setCMode(C_SUPPLIER);
00331 }else{
00332 info.sensor_positions.setCMode(C_CONSUMER);
00333 }
00334 if(m_supplier){
00335 info.uniform.setCMode(C_SUPPLIER);
00336 }else{
00337 info.uniform.setCMode(C_CONSUMER);
00338 }
00339 if(m_supplier){
00340 info.z_separation.setCMode(C_SUPPLIER);
00341 }else{
00342 info.z_separation.setCMode(C_CONSUMER);
00343 }
00344 if(m_supplier){
00345 info.theta_offset.setCMode(C_SUPPLIER);
00346 }else{
00347 info.theta_offset.setCMode(C_CONSUMER);
00348 }
00349 if(m_supplier){
00350 info.z_offset.setCMode(C_SUPPLIER);
00351 }else{
00352 info.z_offset.setCMode(C_CONSUMER);
00353 }
00354 if(m_supplier){
00355 info.angle_min.setCMode(C_SUPPLIER);
00356 }else{
00357 info.angle_min.setCMode(C_CONSUMER);
00358 }
00359 }else{
00360
00361
00362 m_initialize=true;
00363 }
00364
00365 }
00366
00367 void RangeFinder::runUpdate(){
00368 if(myCOMObject){
00369
00370 myCOMObject->runUpdate(m_doPublishData,m_doPublishInfo);
00371
00372 m_doPublishData=false;
00373 m_doPublishInfo=false;
00374
00375 }
00376 }
00377
00378 void RangeFinder::setManager(CommsManager *myManager_){
00379
00380 if(!myCOMObject){
00381 myManager=myManager_;
00382 }
00383
00384 }
00385
00386 void RangeFinder::setQueueMode(const bool & val){
00387
00388 if(!myCOMObject){
00389 m_queue=val;
00390 }else if(!m_queue&&val){
00391 m_queue=val;
00392 myCOMObject->setDataVector(&m_dataVector);
00393 myCOMObject->setQueueMode(true);
00394
00395 }else if(!val&&m_queue){
00396 m_queue=val;
00397 myCOMObject->setQueueMode(false);
00398 while(!m_dataVector.empty()){
00399 m_dataVector.pop();
00400 }
00401 while(!m_infoVector.empty()){
00402 m_infoVector.pop();
00403 }
00404 while(!m_requestVector.empty()){
00405 m_requestVector.pop();
00406 }
00407 }
00408 }
00409
00410 bool RangeFinder::newData(){
00411 if(m_queue){
00412 return !m_dataVector.empty();
00413 }else{
00414 if(myCOMObject){
00415 return myCOMObject->newData();
00416 }
00417 }
00418
00419 return false;
00420 }
00421
00422 bool RangeFinder::newInfo(){
00423 if(m_queue){
00424 if(m_supplier){
00425 return !m_requestVector.empty();
00426 }
00427
00428 return !m_infoVector.empty();
00429 }else{
00430 if(myCOMObject){
00431 return myCOMObject->newInfo();
00432 }
00433 }
00434
00435
00436 return false;
00437 }
00438
00439 void RangeFinder::publishData(){
00440 if(m_supplier){
00441 if(m_doTimestamp){
00442 data.timestamp.now();
00443 }
00444 if(m_doTag){
00445 data.source=m_globalname;
00446 }
00447 if(m_queue){
00448 RangeFinderDataStruct temp;
00449 temp.ranges.clear();
00450 temp.source=data.source;
00451 temp.timestamp=data.timestamp;
00452 temp.ranges=data.ranges;
00453
00454 m_dataVector.push(temp);
00455
00456 }
00457 m_doPublishData=true;
00458 }
00459 }
00460
00461 void RangeFinder::publishInfo(){
00462 if(m_supplier){
00463
00464 info.mType=MESSAGE_INFO;
00465 if(m_doTimestamp){
00466 info.timestamp.now();
00467 }
00468
00469 if(m_doTag){
00470 info.source=m_globalname;
00471 info.target="Any";
00472 }
00473
00474 if(m_queue){
00475 RangeFinderInfoStruct temp;
00476
00477
00478 temp.x_offset=info.x_offset;
00479 temp.source=info.source;
00480 temp.max_range=info.max_range;
00481 temp.y_offset=info.y_offset;
00482 temp.phi_separation=info.phi_separation;
00483 temp.target=info.target;
00484 temp.psi_offset=info.psi_offset;
00485 temp.timestamp=info.timestamp;
00486 temp.x_separation=info.x_separation;
00487 temp.phi_offset=info.phi_offset;
00488 temp.theta_separation=info.theta_separation;
00489 temp.half_angle=info.half_angle;
00490 temp.psi_separation=info.psi_separation;
00491 temp.y_separation=info.y_separation;
00492 temp.sensor_positions=info.sensor_positions;
00493 temp.mType=info.mType;
00494 temp.uniform=info.uniform;
00495 temp.z_separation=info.z_separation;
00496 temp.theta_offset=info.theta_offset;
00497 temp.z_offset=info.z_offset;
00498 temp.angle_min=info.angle_min;
00499
00500 m_infoVector.push(temp);
00501
00502
00503 }
00504
00505 m_doPublishInfo=true;
00506 }else{
00507
00508 requests.mType=MESSAGE_REQUEST;
00509
00510 if(m_doTimestamp){
00511 requests.timestamp.now();
00512 }
00513
00514 if(m_doTag){
00515 requests.source=m_globalname;
00516 requests.target=m_streamname;
00517 }
00518
00519 if(m_queue){
00520 RangeFinderInfoStruct temp;
00521 if(m_supplier){
00522 temp.x_offset.setCMode(C_SUPPLIER);
00523 }else{
00524 temp.x_offset.setCMode(C_CONSUMER);
00525 }
00526 if(m_supplier){
00527 temp.max_range.setCMode(C_SUPPLIER);
00528 }else{
00529 temp.max_range.setCMode(C_CONSUMER);
00530 }
00531 if(m_supplier){
00532 temp.y_offset.setCMode(C_SUPPLIER);
00533 }else{
00534 temp.y_offset.setCMode(C_CONSUMER);
00535 }
00536 if(m_supplier){
00537 temp.phi_separation.setCMode(C_SUPPLIER);
00538 }else{
00539 temp.phi_separation.setCMode(C_CONSUMER);
00540 }
00541 if(m_supplier){
00542 temp.psi_offset.setCMode(C_SUPPLIER);
00543 }else{
00544 temp.psi_offset.setCMode(C_CONSUMER);
00545 }
00546 if(m_supplier){
00547 temp.x_separation.setCMode(C_SUPPLIER);
00548 }else{
00549 temp.x_separation.setCMode(C_CONSUMER);
00550 }
00551 if(m_supplier){
00552 temp.phi_offset.setCMode(C_SUPPLIER);
00553 }else{
00554 temp.phi_offset.setCMode(C_CONSUMER);
00555 }
00556 if(m_supplier){
00557 temp.theta_separation.setCMode(C_SUPPLIER);
00558 }else{
00559 temp.theta_separation.setCMode(C_CONSUMER);
00560 }
00561 if(m_supplier){
00562 temp.half_angle.setCMode(C_SUPPLIER);
00563 }else{
00564 temp.half_angle.setCMode(C_CONSUMER);
00565 }
00566 if(m_supplier){
00567 temp.psi_separation.setCMode(C_SUPPLIER);
00568 }else{
00569 temp.psi_separation.setCMode(C_CONSUMER);
00570 }
00571 if(m_supplier){
00572 temp.y_separation.setCMode(C_SUPPLIER);
00573 }else{
00574 temp.y_separation.setCMode(C_CONSUMER);
00575 }
00576 if(m_supplier){
00577 temp.sensor_positions.setCMode(C_SUPPLIER);
00578 }else{
00579 temp.sensor_positions.setCMode(C_CONSUMER);
00580 }
00581 if(m_supplier){
00582 temp.uniform.setCMode(C_SUPPLIER);
00583 }else{
00584 temp.uniform.setCMode(C_CONSUMER);
00585 }
00586 if(m_supplier){
00587 temp.z_separation.setCMode(C_SUPPLIER);
00588 }else{
00589 temp.z_separation.setCMode(C_CONSUMER);
00590 }
00591 if(m_supplier){
00592 temp.theta_offset.setCMode(C_SUPPLIER);
00593 }else{
00594 temp.theta_offset.setCMode(C_CONSUMER);
00595 }
00596 if(m_supplier){
00597 temp.z_offset.setCMode(C_SUPPLIER);
00598 }else{
00599 temp.z_offset.setCMode(C_CONSUMER);
00600 }
00601 if(m_supplier){
00602 temp.angle_min.setCMode(C_SUPPLIER);
00603 }else{
00604 temp.angle_min.setCMode(C_CONSUMER);
00605 }
00606 temp.x_offset=requests.x_offset;
00607 temp.source=requests.source;
00608 temp.max_range=requests.max_range;
00609 temp.y_offset=requests.y_offset;
00610 temp.target=requests.target;
00611 temp.phi_separation=requests.phi_separation;
00612 temp.timestamp=requests.timestamp;
00613 temp.psi_offset=requests.psi_offset;
00614 temp.x_separation=requests.x_separation;
00615 temp.phi_offset=requests.phi_offset;
00616 temp.theta_separation=requests.theta_separation;
00617 temp.half_angle=requests.half_angle;
00618 temp.psi_separation=requests.psi_separation;
00619 temp.sensor_positions=requests.sensor_positions;
00620 temp.y_separation=requests.y_separation;
00621 temp.mType=requests.mType;
00622 temp.uniform=requests.uniform;
00623 temp.z_separation=requests.z_separation;
00624 temp.theta_offset=requests.theta_offset;
00625 temp.z_offset=requests.z_offset;
00626 temp.angle_min=requests.angle_min;
00627
00628 m_requestVector.push(temp);
00629
00630
00631 }
00632
00633 m_doPublishInfo=true;
00634 }
00635 }
00636
00637 RangeFinderDataStruct RangeFinder::getNextData(){
00638 if(m_queue){
00639 RangeFinderDataStruct retval;
00640 retval.ranges.clear();
00641 retval.source="NULL";
00642 if(m_dataVector.empty()){
00643 return data;
00644 }
00645
00646 retval.source=m_dataVector.front().source;
00647 retval.ranges=m_dataVector.front().ranges;
00648 retval.timestamp=m_dataVector.front().timestamp;
00649
00650
00651 m_dataVector.pop();
00652 data=retval;
00653
00654 return retval;
00655 }else{
00656 myCOMObject->resetData();
00657 return data;
00658 }
00659 }
00660
00661 RangeFinderInfoStruct RangeFinder::getNextInfo(){
00662 if(m_queue){
00663 RangeFinderInfoStruct retval;
00664 if(m_supplier){
00665 retval.x_offset.setCMode(C_SUPPLIER);
00666 }else{
00667 retval.x_offset.setCMode(C_CONSUMER);
00668 }
00669 if(m_supplier){
00670 retval.max_range.setCMode(C_SUPPLIER);
00671 }else{
00672 retval.max_range.setCMode(C_CONSUMER);
00673 }
00674 if(m_supplier){
00675 retval.y_offset.setCMode(C_SUPPLIER);
00676 }else{
00677 retval.y_offset.setCMode(C_CONSUMER);
00678 }
00679 if(m_supplier){
00680 retval.phi_separation.setCMode(C_SUPPLIER);
00681 }else{
00682 retval.phi_separation.setCMode(C_CONSUMER);
00683 }
00684 if(m_supplier){
00685 retval.psi_offset.setCMode(C_SUPPLIER);
00686 }else{
00687 retval.psi_offset.setCMode(C_CONSUMER);
00688 }
00689 if(m_supplier){
00690 retval.x_separation.setCMode(C_SUPPLIER);
00691 }else{
00692 retval.x_separation.setCMode(C_CONSUMER);
00693 }
00694 if(m_supplier){
00695 retval.phi_offset.setCMode(C_SUPPLIER);
00696 }else{
00697 retval.phi_offset.setCMode(C_CONSUMER);
00698 }
00699 if(m_supplier){
00700 retval.theta_separation.setCMode(C_SUPPLIER);
00701 }else{
00702 retval.theta_separation.setCMode(C_CONSUMER);
00703 }
00704 if(m_supplier){
00705 retval.half_angle.setCMode(C_SUPPLIER);
00706 }else{
00707 retval.half_angle.setCMode(C_CONSUMER);
00708 }
00709 if(m_supplier){
00710 retval.psi_separation.setCMode(C_SUPPLIER);
00711 }else{
00712 retval.psi_separation.setCMode(C_CONSUMER);
00713 }
00714 if(m_supplier){
00715 retval.y_separation.setCMode(C_SUPPLIER);
00716 }else{
00717 retval.y_separation.setCMode(C_CONSUMER);
00718 }
00719 if(m_supplier){
00720 retval.sensor_positions.setCMode(C_SUPPLIER);
00721 }else{
00722 retval.sensor_positions.setCMode(C_CONSUMER);
00723 }
00724 if(m_supplier){
00725 retval.uniform.setCMode(C_SUPPLIER);
00726 }else{
00727 retval.uniform.setCMode(C_CONSUMER);
00728 }
00729 if(m_supplier){
00730 retval.z_separation.setCMode(C_SUPPLIER);
00731 }else{
00732 retval.z_separation.setCMode(C_CONSUMER);
00733 }
00734 if(m_supplier){
00735 retval.theta_offset.setCMode(C_SUPPLIER);
00736 }else{
00737 retval.theta_offset.setCMode(C_CONSUMER);
00738 }
00739 if(m_supplier){
00740 retval.z_offset.setCMode(C_SUPPLIER);
00741 }else{
00742 retval.z_offset.setCMode(C_CONSUMER);
00743 }
00744 if(m_supplier){
00745 retval.angle_min.setCMode(C_SUPPLIER);
00746 }else{
00747 retval.angle_min.setCMode(C_CONSUMER);
00748 }
00749 retval.source="NULL";
00750 if(m_supplier){
00751 if(m_requestVector.empty()){
00752 return requests;
00753 }
00754 retval.x_offset=m_requestVector.front().x_offset;
00755 retval.source=m_requestVector.front().source;
00756 retval.max_range=m_requestVector.front().max_range;
00757 retval.y_offset=m_requestVector.front().y_offset;
00758 retval.target=m_requestVector.front().target;
00759 retval.phi_separation=m_requestVector.front().phi_separation;
00760 retval.timestamp=m_requestVector.front().timestamp;
00761 retval.psi_offset=m_requestVector.front().psi_offset;
00762 retval.x_separation=m_requestVector.front().x_separation;
00763 retval.phi_offset=m_requestVector.front().phi_offset;
00764 retval.theta_separation=m_requestVector.front().theta_separation;
00765 retval.half_angle=m_requestVector.front().half_angle;
00766 retval.psi_separation=m_requestVector.front().psi_separation;
00767 retval.sensor_positions=m_requestVector.front().sensor_positions;
00768 retval.y_separation=m_requestVector.front().y_separation;
00769 retval.mType=m_requestVector.front().mType;
00770 retval.uniform=m_requestVector.front().uniform;
00771 retval.z_separation=m_requestVector.front().z_separation;
00772 retval.theta_offset=m_requestVector.front().theta_offset;
00773 retval.z_offset=m_requestVector.front().z_offset;
00774 retval.angle_min=m_requestVector.front().angle_min;
00775
00776
00777 if(m_supplier){
00778 retval.x_offset.setCMode(C_SUPPLIER);
00779 }else{
00780 retval.x_offset.setCMode(C_CONSUMER);
00781 }
00782 if(m_supplier){
00783 retval.max_range.setCMode(C_SUPPLIER);
00784 }else{
00785 retval.max_range.setCMode(C_CONSUMER);
00786 }
00787 if(m_supplier){
00788 retval.y_offset.setCMode(C_SUPPLIER);
00789 }else{
00790 retval.y_offset.setCMode(C_CONSUMER);
00791 }
00792 if(m_supplier){
00793 retval.phi_separation.setCMode(C_SUPPLIER);
00794 }else{
00795 retval.phi_separation.setCMode(C_CONSUMER);
00796 }
00797 if(m_supplier){
00798 retval.psi_offset.setCMode(C_SUPPLIER);
00799 }else{
00800 retval.psi_offset.setCMode(C_CONSUMER);
00801 }
00802 if(m_supplier){
00803 retval.x_separation.setCMode(C_SUPPLIER);
00804 }else{
00805 retval.x_separation.setCMode(C_CONSUMER);
00806 }
00807 if(m_supplier){
00808 retval.phi_offset.setCMode(C_SUPPLIER);
00809 }else{
00810 retval.phi_offset.setCMode(C_CONSUMER);
00811 }
00812 if(m_supplier){
00813 retval.theta_separation.setCMode(C_SUPPLIER);
00814 }else{
00815 retval.theta_separation.setCMode(C_CONSUMER);
00816 }
00817 if(m_supplier){
00818 retval.half_angle.setCMode(C_SUPPLIER);
00819 }else{
00820 retval.half_angle.setCMode(C_CONSUMER);
00821 }
00822 if(m_supplier){
00823 retval.psi_separation.setCMode(C_SUPPLIER);
00824 }else{
00825 retval.psi_separation.setCMode(C_CONSUMER);
00826 }
00827 if(m_supplier){
00828 retval.y_separation.setCMode(C_SUPPLIER);
00829 }else{
00830 retval.y_separation.setCMode(C_CONSUMER);
00831 }
00832 if(m_supplier){
00833 retval.sensor_positions.setCMode(C_SUPPLIER);
00834 }else{
00835 retval.sensor_positions.setCMode(C_CONSUMER);
00836 }
00837 if(m_supplier){
00838 retval.uniform.setCMode(C_SUPPLIER);
00839 }else{
00840 retval.uniform.setCMode(C_CONSUMER);
00841 }
00842 if(m_supplier){
00843 retval.z_separation.setCMode(C_SUPPLIER);
00844 }else{
00845 retval.z_separation.setCMode(C_CONSUMER);
00846 }
00847 if(m_supplier){
00848 retval.theta_offset.setCMode(C_SUPPLIER);
00849 }else{
00850 retval.theta_offset.setCMode(C_CONSUMER);
00851 }
00852 if(m_supplier){
00853 retval.z_offset.setCMode(C_SUPPLIER);
00854 }else{
00855 retval.z_offset.setCMode(C_CONSUMER);
00856 }
00857 if(m_supplier){
00858 retval.angle_min.setCMode(C_SUPPLIER);
00859 }else{
00860 retval.angle_min.setCMode(C_CONSUMER);
00861 }
00862 m_requestVector.pop();
00863 requests=retval;
00864 return retval;
00865 }else{
00866 if(m_infoVector.empty()){
00867 return info;
00868 }
00869 retval.x_offset=m_infoVector.front().x_offset;
00870 retval.source=m_infoVector.front().source;
00871 retval.max_range=m_infoVector.front().max_range;
00872 retval.y_offset=m_infoVector.front().y_offset;
00873 retval.target=m_infoVector.front().target;
00874 retval.phi_separation=m_infoVector.front().phi_separation;
00875 retval.timestamp=m_infoVector.front().timestamp;
00876 retval.psi_offset=m_infoVector.front().psi_offset;
00877 retval.x_separation=m_infoVector.front().x_separation;
00878 retval.phi_offset=m_infoVector.front().phi_offset;
00879 retval.theta_separation=m_infoVector.front().theta_separation;
00880 retval.half_angle=m_infoVector.front().half_angle;
00881 retval.psi_separation=m_infoVector.front().psi_separation;
00882 retval.sensor_positions=m_infoVector.front().sensor_positions;
00883 retval.y_separation=m_infoVector.front().y_separation;
00884 retval.mType=m_infoVector.front().mType;
00885 retval.uniform=m_infoVector.front().uniform;
00886 retval.z_separation=m_infoVector.front().z_separation;
00887 retval.theta_offset=m_infoVector.front().theta_offset;
00888 retval.z_offset=m_infoVector.front().z_offset;
00889 retval.angle_min=m_infoVector.front().angle_min;
00890
00891 if(m_supplier){
00892 retval.x_offset.setCMode(C_SUPPLIER);
00893 }else{
00894 retval.x_offset.setCMode(C_CONSUMER);
00895 }
00896 if(m_supplier){
00897 retval.max_range.setCMode(C_SUPPLIER);
00898 }else{
00899 retval.max_range.setCMode(C_CONSUMER);
00900 }
00901 if(m_supplier){
00902 retval.y_offset.setCMode(C_SUPPLIER);
00903 }else{
00904 retval.y_offset.setCMode(C_CONSUMER);
00905 }
00906 if(m_supplier){
00907 retval.phi_separation.setCMode(C_SUPPLIER);
00908 }else{
00909 retval.phi_separation.setCMode(C_CONSUMER);
00910 }
00911 if(m_supplier){
00912 retval.psi_offset.setCMode(C_SUPPLIER);
00913 }else{
00914 retval.psi_offset.setCMode(C_CONSUMER);
00915 }
00916 if(m_supplier){
00917 retval.x_separation.setCMode(C_SUPPLIER);
00918 }else{
00919 retval.x_separation.setCMode(C_CONSUMER);
00920 }
00921 if(m_supplier){
00922 retval.phi_offset.setCMode(C_SUPPLIER);
00923 }else{
00924 retval.phi_offset.setCMode(C_CONSUMER);
00925 }
00926 if(m_supplier){
00927 retval.theta_separation.setCMode(C_SUPPLIER);
00928 }else{
00929 retval.theta_separation.setCMode(C_CONSUMER);
00930 }
00931 if(m_supplier){
00932 retval.half_angle.setCMode(C_SUPPLIER);
00933 }else{
00934 retval.half_angle.setCMode(C_CONSUMER);
00935 }
00936 if(m_supplier){
00937 retval.psi_separation.setCMode(C_SUPPLIER);
00938 }else{
00939 retval.psi_separation.setCMode(C_CONSUMER);
00940 }
00941 if(m_supplier){
00942 retval.y_separation.setCMode(C_SUPPLIER);
00943 }else{
00944 retval.y_separation.setCMode(C_CONSUMER);
00945 }
00946 if(m_supplier){
00947 retval.sensor_positions.setCMode(C_SUPPLIER);
00948 }else{
00949 retval.sensor_positions.setCMode(C_CONSUMER);
00950 }
00951 if(m_supplier){
00952 retval.uniform.setCMode(C_SUPPLIER);
00953 }else{
00954 retval.uniform.setCMode(C_CONSUMER);
00955 }
00956 if(m_supplier){
00957 retval.z_separation.setCMode(C_SUPPLIER);
00958 }else{
00959 retval.z_separation.setCMode(C_CONSUMER);
00960 }
00961 if(m_supplier){
00962 retval.theta_offset.setCMode(C_SUPPLIER);
00963 }else{
00964 retval.theta_offset.setCMode(C_CONSUMER);
00965 }
00966 if(m_supplier){
00967 retval.z_offset.setCMode(C_SUPPLIER);
00968 }else{
00969 retval.z_offset.setCMode(C_CONSUMER);
00970 }
00971 if(m_supplier){
00972 retval.angle_min.setCMode(C_SUPPLIER);
00973 }else{
00974 retval.angle_min.setCMode(C_CONSUMER);
00975 }
00976 m_infoVector.pop();
00977 info=retval;
00978
00979
00980 requests=info;
00981
00982 return retval;
00983 }
00984 }else{
00985 myCOMObject->resetInfo();
00986 if(m_supplier){
00987 if(m_supplier){
00988 requests.x_offset.setCMode(C_SUPPLIER);
00989 }else{
00990 requests.x_offset.setCMode(C_CONSUMER);
00991 }
00992 if(m_supplier){
00993 requests.max_range.setCMode(C_SUPPLIER);
00994 }else{
00995 requests.max_range.setCMode(C_CONSUMER);
00996 }
00997 if(m_supplier){
00998 requests.y_offset.setCMode(C_SUPPLIER);
00999 }else{
01000 requests.y_offset.setCMode(C_CONSUMER);
01001 }
01002 if(m_supplier){
01003 requests.phi_separation.setCMode(C_SUPPLIER);
01004 }else{
01005 requests.phi_separation.setCMode(C_CONSUMER);
01006 }
01007 if(m_supplier){
01008 requests.psi_offset.setCMode(C_SUPPLIER);
01009 }else{
01010 requests.psi_offset.setCMode(C_CONSUMER);
01011 }
01012 if(m_supplier){
01013 requests.x_separation.setCMode(C_SUPPLIER);
01014 }else{
01015 requests.x_separation.setCMode(C_CONSUMER);
01016 }
01017 if(m_supplier){
01018 requests.phi_offset.setCMode(C_SUPPLIER);
01019 }else{
01020 requests.phi_offset.setCMode(C_CONSUMER);
01021 }
01022 if(m_supplier){
01023 requests.theta_separation.setCMode(C_SUPPLIER);
01024 }else{
01025 requests.theta_separation.setCMode(C_CONSUMER);
01026 }
01027 if(m_supplier){
01028 requests.half_angle.setCMode(C_SUPPLIER);
01029 }else{
01030 requests.half_angle.setCMode(C_CONSUMER);
01031 }
01032 if(m_supplier){
01033 requests.psi_separation.setCMode(C_SUPPLIER);
01034 }else{
01035 requests.psi_separation.setCMode(C_CONSUMER);
01036 }
01037 if(m_supplier){
01038 requests.y_separation.setCMode(C_SUPPLIER);
01039 }else{
01040 requests.y_separation.setCMode(C_CONSUMER);
01041 }
01042 if(m_supplier){
01043 requests.sensor_positions.setCMode(C_SUPPLIER);
01044 }else{
01045 requests.sensor_positions.setCMode(C_CONSUMER);
01046 }
01047 if(m_supplier){
01048 requests.uniform.setCMode(C_SUPPLIER);
01049 }else{
01050 requests.uniform.setCMode(C_CONSUMER);
01051 }
01052 if(m_supplier){
01053 requests.z_separation.setCMode(C_SUPPLIER);
01054 }else{
01055 requests.z_separation.setCMode(C_CONSUMER);
01056 }
01057 if(m_supplier){
01058 requests.theta_offset.setCMode(C_SUPPLIER);
01059 }else{
01060 requests.theta_offset.setCMode(C_CONSUMER);
01061 }
01062 if(m_supplier){
01063 requests.z_offset.setCMode(C_SUPPLIER);
01064 }else{
01065 requests.z_offset.setCMode(C_CONSUMER);
01066 }
01067 if(m_supplier){
01068 requests.angle_min.setCMode(C_SUPPLIER);
01069 }else{
01070 requests.angle_min.setCMode(C_CONSUMER);
01071 }
01072 return requests;
01073 }else{
01074 if(m_supplier){
01075 info.x_offset.setCMode(C_SUPPLIER);
01076 }else{
01077 info.x_offset.setCMode(C_CONSUMER);
01078 }
01079 if(m_supplier){
01080 info.max_range.setCMode(C_SUPPLIER);
01081 }else{
01082 info.max_range.setCMode(C_CONSUMER);
01083 }
01084 if(m_supplier){
01085 info.y_offset.setCMode(C_SUPPLIER);
01086 }else{
01087 info.y_offset.setCMode(C_CONSUMER);
01088 }
01089 if(m_supplier){
01090 info.phi_separation.setCMode(C_SUPPLIER);
01091 }else{
01092 info.phi_separation.setCMode(C_CONSUMER);
01093 }
01094 if(m_supplier){
01095 info.psi_offset.setCMode(C_SUPPLIER);
01096 }else{
01097 info.psi_offset.setCMode(C_CONSUMER);
01098 }
01099 if(m_supplier){
01100 info.x_separation.setCMode(C_SUPPLIER);
01101 }else{
01102 info.x_separation.setCMode(C_CONSUMER);
01103 }
01104 if(m_supplier){
01105 info.phi_offset.setCMode(C_SUPPLIER);
01106 }else{
01107 info.phi_offset.setCMode(C_CONSUMER);
01108 }
01109 if(m_supplier){
01110 info.theta_separation.setCMode(C_SUPPLIER);
01111 }else{
01112 info.theta_separation.setCMode(C_CONSUMER);
01113 }
01114 if(m_supplier){
01115 info.half_angle.setCMode(C_SUPPLIER);
01116 }else{
01117 info.half_angle.setCMode(C_CONSUMER);
01118 }
01119 if(m_supplier){
01120 info.psi_separation.setCMode(C_SUPPLIER);
01121 }else{
01122 info.psi_separation.setCMode(C_CONSUMER);
01123 }
01124 if(m_supplier){
01125 info.y_separation.setCMode(C_SUPPLIER);
01126 }else{
01127 info.y_separation.setCMode(C_CONSUMER);
01128 }
01129 if(m_supplier){
01130 info.sensor_positions.setCMode(C_SUPPLIER);
01131 }else{
01132 info.sensor_positions.setCMode(C_CONSUMER);
01133 }
01134 if(m_supplier){
01135 info.uniform.setCMode(C_SUPPLIER);
01136 }else{
01137 info.uniform.setCMode(C_CONSUMER);
01138 }
01139 if(m_supplier){
01140 info.z_separation.setCMode(C_SUPPLIER);
01141 }else{
01142 info.z_separation.setCMode(C_CONSUMER);
01143 }
01144 if(m_supplier){
01145 info.theta_offset.setCMode(C_SUPPLIER);
01146 }else{
01147 info.theta_offset.setCMode(C_CONSUMER);
01148 }
01149 if(m_supplier){
01150 info.z_offset.setCMode(C_SUPPLIER);
01151 }else{
01152 info.z_offset.setCMode(C_CONSUMER);
01153 }
01154 if(m_supplier){
01155 info.angle_min.setCMode(C_SUPPLIER);
01156 }else{
01157 info.angle_min.setCMode(C_CONSUMER);
01158 }
01159
01160
01161 requests=info;
01162 return info;
01163 }
01164 }
01165 }
01166
01167 void RangeFinder::setStreamName(const std::string & name){
01168
01169 if(m_supplier&&name=="Any"){
01170 return;
01171 }
01172
01173 m_streamname=name;
01174 m_haveStream=true;
01175
01176 if(m_supplier){
01177 m_globalname=name;
01178 }
01179
01180 if(myCOMObject){
01181 myCOMObject->setStreamName(name);
01182 }
01183
01184 if(m_initialize){
01185 if(m_supplier){
01186 activateSupplier();
01187 }else{
01188 activateConsumer();
01189 }
01190 }
01191
01192 m_streamRequest=STAT_OKAY;
01193 }
01194
01195 void RangeFinder::setGlobalName(const std::string & name){
01196
01197 if(name=="Any"){
01198 return;
01199 }
01200
01201 m_globalname=name;
01202 m_haveGlobalname=true;
01203 if(m_supplier){
01204 m_streamname=name;
01205 }
01206
01207 if(myCOMObject){
01208 myCOMObject->setGlobalName(name);
01209 }
01210 }
01211
01212 void RangeFinder::doPing(){
01213 if(myCOMObject){
01214 myCOMObject->doPing();
01215 }
01216 }
01217
01218 void RangeFinder::setAutoPing(const bool & val){
01219 m_doAutoPing=val;
01220 if(myCOMObject){
01221 myCOMObject->setAutoPing(val);
01222 }
01223 }
01224
01225