00001 #include "MMCombo.h"
00002 #include "Shared/WorldState.h"
00003 #include "Shared/Profiler.h"
00004 #include "Shared/debuget.h"
00005 #include "Shared/Config.h"
00006 #include "Shared/SharedObject.h"
00007 #include "Shared/ProcessID.h"
00008 #include "Events/EventRouter.h"
00009 #include "Behaviors/BehaviorBase.h"
00010 #include "Motion/MotionManager.h"
00011 #include "Motion/Kinematics.h"
00012 #include "SoundPlay/SoundManager.h"
00013 #include "Events/DataEvent.h"
00014 #include "Events/TextMsgEvent.h"
00015 #include "Events/FilterBankEvent.h"
00016 #include "Shared/WMclass.h"
00017
00018 #include "Shared/ERS210Info.h"
00019 #include "Shared/ERS220Info.h"
00020 #include "Shared/ERS7Info.h"
00021
00022 #include "Shared/ProjectInterface.h"
00023
00024 #include <OPENR/OSyslog.h>
00025 #include <OPENR/core_macro.h>
00026 #include <OPENR/OFbkImage.h>
00027
00028 using namespace std;
00029
00030 MMCombo::MMCombo()
00031 : OObject(), motmanMemRgn(NULL), worldStateMemRgn(NULL),
00032 soundManagerMemRgn(NULL), eventTranslatorQueueMemRgn(NULL),
00033 runLevel(0), num_open(0), etrans(), RPOPENR_isready(true), isStopped(true)
00034 {
00035 for(unsigned int i=0; i<NumOutputs; i++) {
00036 primIDs[i]=oprimitiveID_UNDEF;
00037 open[i]=false;
00038 }
00039 Profiler::initBuckets();
00040 }
00041
00042
00043 OStatus
00044 MMCombo::DoInit(const OSystemEvent&)
00045 {
00046 cout << objectName << "::DoInit() " << endl;
00047
00048 isStopped=false;
00049
00050 NEW_ALL_SUBJECT_AND_OBSERVER;
00051 REGISTER_ALL_ENTRY;
00052 SET_ALL_READY_AND_NOTIFY_ENTRY;
00053
00054
00055 observer[obsReceiveWorldState]->SetBufCtrlParam(0,1,1);
00056 observer[obsReceiveMotionManager]->SetBufCtrlParam(0,1,1);
00057 observer[obsReceiveSoundManager]->SetBufCtrlParam(0,1,1);
00058 observer[obsMotionManagerComm]->SetBufCtrlParam(0,1,MotionManager::MAX_MOTIONS+1);
00059
00060
00061 cout << objectName << ": sbjRegisterWorldState==" << sbjRegisterWorldState << " selector==" << subject[sbjRegisterWorldState]->GetID().GetSelector() << '\n'
00062 << objectName << ": obsReceiveWorldState==" << obsReceiveWorldState << " selector==" << observer[obsReceiveWorldState]->GetID().GetSelector() << '\n'
00063 << objectName << ": sbjRegisterMotionManager==" << sbjRegisterMotionManager << " selector==" << subject[sbjRegisterMotionManager]->GetID().GetSelector() << '\n'
00064 << objectName << ": obsReceiveMotionManager==" << obsReceiveMotionManager << " selector==" << observer[obsReceiveMotionManager]->GetID().GetSelector() << '\n'
00065 << objectName << ": sbjRegisterEventTranslatorQueue==" << sbjRegisterEventTranslatorQueue << " selector==" << subject[sbjRegisterEventTranslatorQueue]->GetID().GetSelector() << '\n'
00066 << objectName << ": obsReceiveEventTranslatorQueue==" << obsReceiveEventTranslatorQueue << " selector==" << observer[obsReceiveEventTranslatorQueue]->GetID().GetSelector() << '\n'
00067 << objectName << ": sbjMoveJoint==" << sbjMoveJoint << " selector==" << subject[sbjMoveJoint]->GetID().GetSelector() << '\n'
00068 << objectName << ": obsSensorFrame==" << obsSensorFrame << " selector==" << observer[obsSensorFrame]->GetID().GetSelector() << '\n'
00069 << objectName << ": obsImage==" << obsImage << " selector==" << observer[obsImage]->GetID().GetSelector() << '\n'
00070 << objectName << ": obsMic==" << obsMic << " selector==" << observer[obsMic]->GetID().GetSelector() << '\n'
00071 << objectName << ": sbjMotionManagerComm==" << sbjMotionManagerComm << " selector==" << subject[sbjMotionManagerComm]->GetID().GetSelector() << '\n'
00072 << objectName << ": obsMotionManagerComm==" << obsMotionManagerComm << " selector==" << observer[obsMotionManagerComm]->GetID().GetSelector() << '\n'
00073 << objectName << ": obsReceiveSoundManager==" << obsReceiveSoundManager << " selector==" << observer[obsReceiveSoundManager]->GetID().GetSelector() << '\n'
00074 << objectName << ": sbjSoundManagerComm==" << sbjSoundManagerComm << " selector==" << subject[sbjSoundManagerComm]->GetID().GetSelector() << '\n'
00075 << objectName << ": sbjRPOPENRSendString==" << sbjRPOPENRSendString << " selector==" << subject[sbjRPOPENRSendString]->GetID().GetSelector() << '\n'
00076 << objectName << ": obsRPOPENRReceiveString==" << obsRPOPENRReceiveString << " selector==" << observer[obsRPOPENRReceiveString]->GetID().GetSelector() << '\n'
00077 << flush;
00078
00079 if(strcmp(objectName,"MainObj")==0)
00080 ProcessID::setID(ProcessID::MainProcess);
00081 else if(strcmp(objectName,"MotoObj")==0)
00082 ProcessID::setID(ProcessID::MotionProcess);
00083
00084
00085 config=new Config("/ms/config/tekkotsu.cfg");
00086
00087 erouter = new EventRouter;
00088
00089 if(strcmp(objectName,"MainObj")==0) {
00090 bool isSlowOutput[NumOutputs];
00091 for(unsigned int i=0; i<NumOutputs; i++)
00092 isSlowOutput[i]=!IsFastOutput[i];
00093
00094 SetupOutputs(isSlowOutput);
00095
00096
00097 OPowerStatus observationStatus;
00098 observationStatus.Set(orsbALL,obsbALL,opsoREMAINING_CAPACITY_NOTIFY_EVERY_CHANGE,opsoTEMPERATURE_NOTIFY_EVERY_CHANGE,opsoTIME_DIF_NOTIFY_EVERY_CHANGE,opsoVOLUME_NOTIFY_EVERY_CHANGE);
00099 OServiceEntry entry(myOID_, Extra_Entry[entryGotPowerEvent]);
00100 OStatus result = OPENR::ObservePowerStatus(observationStatus, entry);
00101 if(result != oSUCCESS) {
00102 OSYSLOG1((osyslogERROR, "%s : %s %d","MMCombo::DoStart()","OPENR::ObservePowerStatus() FAILED", result));
00103 return oFAIL;
00104 }
00105
00106
00107 wireless = new Wireless();
00108 sout=wireless->socket(SocketNS::SOCK_STREAM,Wireless::WIRELESS_DEF_RECV_SIZE,Wireless::WIRELESS_DEF_SEND_SIZE*6);
00109 serr=wireless->socket(SocketNS::SOCK_STREAM,Wireless::WIRELESS_DEF_RECV_SIZE,Wireless::WIRELESS_DEF_SEND_SIZE*2);
00110 wireless->setDaemon(sout);
00111 wireless->setDaemon(serr);
00112 serr->setFlushType(SocketNS::FLUSH_BLOCKING);
00113 sout->setTextForward();
00114 serr->setForward(sout);
00115
00116
00117 worldStateMemRgn = InitRegion(sizeof(WorldState));
00118 state=new ((WorldState*)worldStateMemRgn->Base()) WorldState;
00119
00120
00121 eventTranslatorQueueMemRgn = InitRegion(sizeof(EventTranslator::Queue_t));
00122 EventTranslator::Queue_t * etransq=new ((EventTranslator::Queue_t*)eventTranslatorQueueMemRgn->Base()) EventTranslator::Queue_t;
00123 etrans.setQueue(etransq);
00124 MotionCommand::setQueue(etransq);
00125
00126 }
00127 if(strcmp(objectName,"MotoObj")==0) {
00128 SetupOutputs(IsFastOutput);
00129 OPENR::SetMotorPower(opowerON);
00130 OPENR::EnableJointGain(oprimitiveID_UNDEF);
00131
00132
00133 wireless = new Wireless();
00134 sout=wireless->socket(SocketNS::SOCK_STREAM,Wireless::WIRELESS_DEF_RECV_SIZE,Wireless::WIRELESS_DEF_SEND_SIZE*6);
00135 serr=wireless->socket(SocketNS::SOCK_STREAM,Wireless::WIRELESS_DEF_RECV_SIZE,Wireless::WIRELESS_DEF_SEND_SIZE*2);
00136 wireless->setDaemon(sout);
00137 wireless->setDaemon(serr);
00138 serr->setFlushType(SocketNS::FLUSH_BLOCKING);
00139 sout->setTextForward();
00140 serr->setForward(sout);
00141
00142
00143 motmanMemRgn = InitRegion(sizeof(MotionManager));
00144 motman = new (motmanMemRgn->Base()) MotionManager();
00145 motman->InitAccess(subject[sbjMotionManagerComm]);
00146 }
00147 kine = new Kinematics();
00148
00149 ProjectInterface::startupBehavior.AddReference();
00150
00151 cout << objectName << "::DoInit()-DONE" << endl;
00152 return oSUCCESS;
00153 }
00154
00155 OStatus
00156 MMCombo::DoStart(const OSystemEvent&)
00157 {
00158 cout << objectName << "::DoStart() " << endl;
00159
00160
00161
00162 if(strcmp(objectName,"MainObj")==0) {
00163 wireless->listen(sout, config->main.console_port);
00164 wireless->listen(serr, config->main.stderr_port);
00165 OPowerStatus power;
00166 OPENR::GetPowerStatus(&power);
00167 state->read(power,erouter);
00168 }
00169
00170 if(strcmp(objectName,"MotoObj")==0) {
00171 wireless->listen(sout, config->motion.console_port);
00172 wireless->listen(serr, config->motion.stderr_port);
00173 }
00174
00175 isStopped=false;
00176
00177 ENABLE_ALL_SUBJECT;
00178 ASSERT_READY_TO_ALL_OBSERVER;
00179
00180 addRunLevel();
00181
00182 cout << objectName << "::DoStart()-DONE" << endl;
00183 return oSUCCESS;
00184 }
00185
00186 OStatus
00187 MMCombo::DoStop(const OSystemEvent&)
00188 {
00189 cout << objectName << "::DoStop()..." << endl;
00190 if(strcmp(objectName,"MainObj")==0) {
00191 ProjectInterface::startupBehavior.DoStop();
00192 wireless->close(sout);
00193 wireless->close(serr);
00194 }
00195 DISABLE_ALL_SUBJECT;
00196 DEASSERT_READY_TO_ALL_OBSERVER;
00197 isStopped=true;
00198 cout << objectName << "::DoStop()-DONE" << endl;
00199 return oSUCCESS;
00200 }
00201
00202 OStatus
00203 MMCombo::DoDestroy(const OSystemEvent&)
00204 {
00205 cout << objectName << "::DoDestroy()..." << endl;
00206 ProjectInterface::startupBehavior.RemoveReference();
00207 if(strcmp(objectName,"MainObj")==0) {
00208 delete erouter;
00209 motmanMemRgn->RemoveReference();
00210 }
00211 if(strcmp(objectName,"MotoObj")==0) {
00212 worldStateMemRgn->RemoveReference();
00213 eventTranslatorQueueMemRgn->RemoveReference();
00214 }
00215 soundManagerMemRgn->RemoveReference();
00216 DELETE_ALL_SUBJECT_AND_OBSERVER;
00217 cout << objectName << "::DoDestroy()-DONE" << endl;
00218 return oSUCCESS;
00219 }
00220
00221
00222
00223
00224 void
00225 MMCombo::ReadyRegisterWorldState(const OReadyEvent&){
00226 static bool is_init=true;
00227 if(is_init) {
00228 is_init=false;
00229 cout << objectName << " Registering WorldState" << endl;
00230 if(strcmp(objectName,"MainObj")==0) {
00231 subject[sbjRegisterWorldState]->SetData(worldStateMemRgn);
00232 subject[sbjRegisterWorldState]->NotifyObservers();
00233 }
00234 }
00235 }
00236
00237 void
00238 MMCombo::GotWorldState(const ONotifyEvent& event){
00239 cout << objectName << "-GOTWORLDSTATE..." << flush;
00240
00241 if(strcmp(objectName,"MotoObj")==0) {
00242 ASSERT(event.NumOfData()==1,"Too many WorldStates");
00243 worldStateMemRgn = event.RCData(0);
00244 worldStateMemRgn->AddReference();
00245 state = reinterpret_cast<WorldState*>(worldStateMemRgn->Base());
00246 }
00247 observer[obsReceiveWorldState]->AssertReady();
00248 cout << "done" << endl;
00249 }
00250
00251
00252
00253
00254
00255 void
00256 MMCombo::ReadyRegisterMotionManager(const OReadyEvent&){
00257 static bool is_init=true;
00258 if(is_init) {
00259 is_init=false;
00260 cout << objectName << " Registering MotionManager" << endl;
00261 if(strcmp(objectName,"MotoObj")==0) {
00262 subject[sbjRegisterMotionManager]->SetData(motmanMemRgn);
00263 subject[sbjRegisterMotionManager]->NotifyObservers();
00264 }
00265 }
00266 }
00267
00268 void
00269 MMCombo::GotMotionManager(const ONotifyEvent& event){
00270 cout << objectName << "-GOTWORLDSTATE..." << flush;
00271
00272 if(strcmp(objectName,"MainObj")==0) {
00273 ASSERT(event.NumOfData()==1,"Too many MotionManagers");
00274 motmanMemRgn = event.RCData(0);
00275 motmanMemRgn->AddReference();
00276 motman = reinterpret_cast<MotionManager*>(motmanMemRgn->Base());
00277 cout << "MAIN INIT MOTMAN..." << flush;
00278
00279 motman->InitAccess(subject[sbjMotionManagerComm]);
00280 addRunLevel();
00281 }
00282 observer[obsReceiveMotionManager]->AssertReady();
00283 cout << "done" << endl;
00284 }
00285
00286
00287
00288
00289 void
00290 MMCombo::ReadyRegisterEventTranslatorQueue(const OReadyEvent&){
00291 static bool is_init=true;
00292 if(is_init) {
00293 is_init=false;
00294 cout << objectName << " Registering EventTranslatorQueue" << endl;
00295 if(strcmp(objectName,"MainObj")==0) {
00296 subject[sbjRegisterEventTranslatorQueue]->SetData(eventTranslatorQueueMemRgn);
00297 subject[sbjRegisterEventTranslatorQueue]->NotifyObservers();
00298 }
00299 }
00300 }
00301
00302 void
00303 MMCombo::GotEventTranslatorQueue(const ONotifyEvent& event){
00304 cout << objectName << "-GOTEventTranslatorQueue..." << flush;
00305
00306 if(strcmp(objectName,"MotoObj")==0) {
00307 ASSERT(event.NumOfData()==1,"Too many EventTranslatorQueue");
00308 eventTranslatorQueueMemRgn = event.RCData(0);
00309 eventTranslatorQueueMemRgn->AddReference();
00310 EventTranslator::Queue_t * etransq=reinterpret_cast<EventTranslator::Queue_t*>(eventTranslatorQueueMemRgn->Base());
00311 etrans.setQueue(etransq);
00312 MotionCommand::setQueue(etransq);
00313 erouter->addTrapper(&etrans);
00314 }
00315 observer[obsReceiveEventTranslatorQueue]->AssertReady();
00316 cout << "done" << endl;
00317 }
00318
00319
00320 void
00321 MMCombo::ReadySendJoints(const OReadyEvent& sysevent) {
00322
00323 if(isStopped) {
00324
00325 return;
00326 }
00327
00328 static unsigned int id=-1U;
00329 Profiler::Timer timer;
00330 if(ProcessID::getID()==ProcessID::MotionProcess) {
00331 if(state) {
00332 if(id==-1U)
00333 id=state->motionProfile.getNewID("ReadySendJoints()");
00334 timer.setID(id,&state->motionProfile);
00335 }
00336 } else if(ProcessID::getID()==ProcessID::MainProcess) {
00337 if(id==-1U)
00338 id=state->mainProfile.getNewID("ReadySendJoints()");
00339 timer.setID(id,&state->mainProfile);
00340 }
00341
00342 if(num_open==0)
00343 return;
00344
00345
00346 RCRegion* rgn=NULL;
00347 for (unsigned int i = 0; i < NUM_COMMAND_VECTOR; i++) {
00348 if (region[i]->NumberOfReference() == 1) {
00349 rgn=region[i];
00350
00351
00352
00353
00354
00355
00356 break;
00357 }
00358 }
00359 ASSERTRET(rgn!=NULL,"Could not find unused command vector");
00360 ASSERTRET(rgn->Base()!=NULL,"Bad Command Vector");
00361 OCommandVectorData* cmdVecData = reinterpret_cast<OCommandVectorData*>(rgn->Base());
00362
00363
00364
00365 bool isERS7;
00366 if(state!=NULL)
00367 isERS7=state->robotDesign&WorldState::ERS7Mask;
00368 else {
00369 char robotDesignStr[orobotdesignNAME_MAX + 1];
00370 memset(robotDesignStr, 0, sizeof(robotDesignStr));
00371 if (OPENR::GetRobotDesign(robotDesignStr) != oSUCCESS) {
00372 cout << objectName << "::SetupOutputs - OPENR::GetRobotDesign() failed." << endl;
00373 return;
00374 }
00375 isERS7=(strcmp(robotDesignStr,"ERS-7")==0);
00376 }
00377 if(ProcessID::getID()==ProcessID::MotionProcess) {
00378 float outputs[NumFrames][NumOutputs];
00379 if(state!=NULL) {
00380 motman->getOutputs(outputs);
00381 motman->updatePIDs(primIDs);
00382 motman->updateWorldState();
00383 } else {
00384 for(unsigned int f=0; f<NumFrames; f++)
00385 for(unsigned int i=0; i<NumOutputs; i++)
00386 outputs[f][i]=0;
00387 }
00388
00389
00390 unsigned int used=0;
00391 for(unsigned int i=PIDJointOffset; i<PIDJointOffset+NumPIDJoints; i++)
00392 if(open[i]) {
00393 float cal=config->motion.calibration[i-PIDJointOffset];
00394 OJointCommandValue2* jval = reinterpret_cast<OJointCommandValue2*>(cmdVecData->GetData(used)->value);
00395 for(unsigned int frame=0; frame<NumFrames; frame++)
00396 jval[frame].value = (slongword)(outputs[frame][i]*1e6*cal);
00397 used++;
00398 }
00399 if(isERS7) {
00400
00401 for(unsigned int i=LEDOffset; i<ERS7Info::FaceLEDPanelOffset; i++)
00402 if(open[i]) {
00403 OLEDCommandValue2* jval = reinterpret_cast<OLEDCommandValue2*>(cmdVecData->GetData(used)->value);
00404 for(unsigned int frame=0; frame<NumFrames; frame++)
00405 jval[frame].led = calcLEDValue(i-LEDOffset,outputs[frame][i]);
00406 used++;
00407 }
00408
00409 OLED3Mode curMode[NumFrames];
00410 for(unsigned int frame=0; frame<NumFrames; frame++)
00411 curMode[frame]=(calcLEDValue(ERS7Info::LEDABModeOffset-LEDOffset,sqrt(clipRange01(outputs[frame][ERS7Info::LEDABModeOffset])))==oledON?oled3_MODE_B:oled3_MODE_A);
00412 for(unsigned int i=ERS7Info::FaceLEDPanelOffset; i<LEDOffset+NumLEDs; i++)
00413 if(open[i]) {
00414 OLEDCommandValue3* jval = reinterpret_cast<OLEDCommandValue3*>(cmdVecData->GetData(used)->value);
00415 for(unsigned int frame=0; frame<NumFrames; frame++) {
00416 jval[frame].intensity = static_cast<sword>(255*clipRange01(outputs[frame][i]));
00417 jval[frame].mode=curMode[frame];
00418 }
00419 used++;
00420 }
00421 for(unsigned int i=BinJointOffset; i<BinJointOffset+NumBinJoints; i++)
00422 if(open[i]) {
00423 OJointCommandValue4* jval = reinterpret_cast<OJointCommandValue4*>(cmdVecData->GetData(used)->value);
00424 for(unsigned int frame=0; frame<NumSlowFrames; frame++)
00425 jval[frame].value = (outputs[frame][i]<.5?ojoint4_STATE0:ojoint4_STATE1);
00426 used++;
00427 }
00428 } else {
00429 for(unsigned int i=LEDOffset; i<LEDOffset+NumLEDs; i++)
00430 if(open[i]) {
00431 OLEDCommandValue2* jval = reinterpret_cast<OLEDCommandValue2*>(cmdVecData->GetData(used)->value);
00432 for(unsigned int frame=0; frame<NumFrames; frame++)
00433 jval[frame].led = calcLEDValue(i-LEDOffset,outputs[frame][i]);
00434 used++;
00435 }
00436 for(unsigned int i=BinJointOffset; i<BinJointOffset+NumBinJoints; i++)
00437 if(open[i]) {
00438 OJointCommandValue3* jval = reinterpret_cast<OJointCommandValue3*>(cmdVecData->GetData(used)->value);
00439 for(unsigned int frame=0; frame<NumSlowFrames; frame++)
00440 jval[frame].value = (outputs[frame][i]<.5?ojoint3_STATE1:ojoint3_STATE0);
00441 used++;
00442 }
00443 }
00444 } else if(ProcessID::getID()==ProcessID::MainProcess) {
00445
00446 unsigned int used=0;
00447 if(isERS7) {
00448 for(unsigned int i=BinJointOffset; i<BinJointOffset+NumBinJoints; i++)
00449 if(open[i]) {
00450 OJointCommandValue4* jval = reinterpret_cast<OJointCommandValue4*>(cmdVecData->GetData(used)->value);
00451 for(unsigned int frame=0; frame<NumSlowFrames; frame++)
00452 jval[frame].value = (state->outputs[i]<.5?ojoint4_STATE0:ojoint4_STATE1);
00453 used++;
00454 }
00455 } else {
00456 for(unsigned int i=BinJointOffset; i<BinJointOffset+NumBinJoints; i++)
00457 if(open[i]) {
00458 OJointCommandValue3* jval = reinterpret_cast<OJointCommandValue3*>(cmdVecData->GetData(used)->value);
00459 for(unsigned int frame=0; frame<NumSlowFrames; frame++)
00460 jval[frame].value = (state->outputs[i]<.5?ojoint3_STATE1:ojoint3_STATE0);
00461 used++;
00462 }
00463 }
00464 }
00465
00466
00467 subject[sbjMoveJoint]->SetData(rgn);
00468
00469
00470
00471
00472 static unsigned int initCount=1;
00473 if(initCount<NUM_COMMAND_VECTOR) {
00474 initCount++;
00475 ReadySendJoints(sysevent);
00476 } else
00477 subject[sbjMoveJoint]->NotifyObservers();
00478 }
00479
00480 void
00481 MMCombo::GotSensorFrame(const ONotifyEvent& event){
00482
00483
00484 if(isStopped) {
00485
00486 return;
00487 }
00488
00489 PROFSECTION("GotSensorFrame()",state->mainProfile);
00490 etrans.translateEvents();
00491
00492 OSensorFrameVectorData* rawsensor = reinterpret_cast<OSensorFrameVectorData*>(event.RCData(0)->Base());
00493 state->read(rawsensor[0],erouter);
00494 erouter->processTimers();
00495 static unsigned int throwaway=1;
00496 if(throwaway!=0) {
00497 throwaway--;
00498 if(throwaway==0)
00499 addRunLevel();
00500 }
00501 observer[obsSensorFrame]->AssertReady();
00502
00503
00504 }
00505
00506 void
00507 MMCombo::GotImage(const ONotifyEvent& event){
00508 if(isStopped) {
00509
00510 return;
00511 }
00512
00513 PROFSECTION("GotImage()",state->mainProfile);
00514 etrans.translateEvents();
00515
00516 erouter->processTimers();
00517
00518 WMvari(int, frame_counter, 0);
00519 ++frame_counter;
00520
00521 erouter->postEvent(new DataEvent<const OFbkImageVectorData*>(reinterpret_cast<const OFbkImageVectorData*>(event.Data(0)),EventBase::visOFbkEGID,0,EventBase::statusETID));
00522
00523 erouter->processTimers();
00524
00525 observer[obsImage]->AssertReady();
00526 }
00527
00528 void
00529 MMCombo::GotAudio(const ONotifyEvent& event){
00530 if(isStopped) {
00531
00532 return;
00533 }
00534
00535 PROFSECTION("GotAudio()",state->mainProfile);
00536 etrans.translateEvents();
00537
00538 for (int i = 0; i < event.NumOfData(); i++) {
00539 erouter->postEvent(new DataEvent<const OSoundVectorData*>(reinterpret_cast<const OSoundVectorData*>(event.Data(i)),EventBase::micOSndEGID,0,EventBase::statusETID));
00540 erouter->processTimers();
00541 }
00542
00543 observer[obsMic]->AssertReady();
00544 }
00545
00546 void
00547 MMCombo::GotPowerEvent(void * msg){
00548 if(isStopped) {
00549
00550 return;
00551 }
00552
00553
00554 PROFSECTION("PowerEvent()",state->mainProfile);
00555 etrans.translateEvents();
00556
00557 static bool first=true;
00558 if(first) {
00559 addRunLevel();
00560 first=false;
00561 }
00562 const OPowerStatus* result = &static_cast<OPowerStatusMessage*>(msg)->powerStatus;
00563 state->read(*result,erouter);
00564 erouter->processTimers();
00565
00566
00567
00568
00569 if(state->powerFlags[PowerSourceID::PauseSID]) {
00570 cout << "%%%%%%% Pause button was pushed! %%%%%%%" << endl;
00571 OBootCondition bc(0);
00572 OPENR::Shutdown(bc);
00573 }
00574
00575 }
00576
00577 void
00578 MMCombo::GotMotionMsg(const ONotifyEvent& event){
00579 if(isStopped) {
00580
00581 return;
00582 }
00583
00584
00585 if(motman!=NULL)
00586 motman->receivedMsg(event);
00587 else
00588 cout << "*** WARNING Main dropping MotionCommand (motman not ready) " << endl;
00589 observer[obsMotionManagerComm]->AssertReady();
00590
00591 }
00592
00593 void
00594 MMCombo::GotSoundManager(const ONotifyEvent& event) {
00595 cout << objectName << "-GOTSOUNDMANAGER..." << flush;
00596
00597 ASSERT(event.NumOfData()==1,"Too many SoundManagers");
00598 soundManagerMemRgn = event.RCData(0);
00599 soundManagerMemRgn->AddReference();
00600 sndman = reinterpret_cast<SoundManager*>(soundManagerMemRgn->Base());
00601 observer[obsReceiveSoundManager]->AssertReady();
00602 sndman->InitAccess(subject[sbjSoundManagerComm]);
00603 addRunLevel();
00604 cout << "done" << endl;
00605 }
00606
00607 void
00608 MMCombo::OpenPrimitives()
00609 {
00610 for(unsigned int i=0; i<NumOutputs; i++)
00611 if(open[i]) {
00612 OStatus result = OPENR::OpenPrimitive(PrimitiveName[i], &primIDs[i]);
00613 if (result != oSUCCESS)
00614 OSYSLOG1((osyslogERROR, "%s : %s %d","MMCombo::DoInit()","OPENR::OpenPrimitive() FAILED", result));
00615 }
00616 }
00617
00618 void
00619 MMCombo::SetupOutputs(const bool to_open[NumOutputs])
00620 {
00621 char robotDesignStr[orobotdesignNAME_MAX + 1];
00622 memset(robotDesignStr, 0, sizeof(robotDesignStr));
00623 if (OPENR::GetRobotDesign(robotDesignStr) != oSUCCESS) {
00624 cout << objectName << "::SetupOutputs - OPENR::GetRobotDesign() failed." << endl;
00625 return;
00626 } else {
00627 if(strcmp(robotDesignStr,"ERS-210")==0) {
00628 for(unsigned int j=0; j<NumOutputs; j++)
00629 open[j]=to_open[j] && ERS210Info::IsRealERS210[j];
00630 } else if(strcmp(robotDesignStr,"ERS-220")==0) {
00631 for(unsigned int j=0; j<NumOutputs; j++)
00632 open[j]=to_open[j] && ERS220Info::IsRealERS220[j];
00633 } else if(strcmp(robotDesignStr,"ERS-7")==0) {
00634 for(unsigned int j=0; j<NumOutputs; j++)
00635 open[j]=to_open[j] && ERS7Info::IsRealERS7[j];
00636 } else {
00637 cout << "MMCombo::SetupOutputs - ERROR: Unrecognized model: "<<robotDesignStr<<"\nSorry..."<<endl;
00638 return;
00639 }
00640 }
00641
00642
00643 for(unsigned int j=0; j<NumOutputs; j++)
00644 if(open[j])
00645 num_open++;
00646
00647 if(num_open==0)
00648 return;
00649
00650 OpenPrimitives();
00651
00652
00653 for (unsigned int i = 0; i < NUM_COMMAND_VECTOR; i++) {
00654 MemoryRegionID cmdVecDataID;
00655 OCommandVectorData* cmdVecData;
00656 OStatus result = OPENR::NewCommandVectorData(num_open,&cmdVecDataID,&cmdVecData);
00657 if (result != oSUCCESS)
00658 OSYSLOG1((osyslogERROR, "%s : %s %d","MMCombo::NewCommandVectorData()","OPENR::NewCommandVectorData() FAILED", result));
00659 region[i] = new RCRegion(cmdVecData->vectorInfo.memRegionID,cmdVecData->vectorInfo.offset,(void*)cmdVecData,cmdVecData->vectorInfo.totalSize);
00660 cmdVecData->SetNumData(num_open);
00661
00662
00663 unsigned int used=0;
00664 ASSERT(cmdVecData==reinterpret_cast<OCommandVectorData*>(region[i]->Base())," should be equal!?");
00665 for(unsigned int j=PIDJointOffset; j<PIDJointOffset+NumPIDJoints; j++)
00666 if(open[j]) {
00667 OCommandInfo* info = cmdVecData->GetInfo(used++);
00668 info->Set(odataJOINT_COMMAND2, primIDs[j], NumFrames);
00669 }
00670 if(strcmp(robotDesignStr,"ERS-7")==0) {
00671
00672 for(unsigned int j=LEDOffset; j<ERS7Info::FaceLEDPanelOffset; j++)
00673 if(open[j]) {
00674 OCommandInfo* info = cmdVecData->GetInfo(used);
00675 info->Set(odataLED_COMMAND2, primIDs[j], NumFrames);
00676 OLEDCommandValue2* jval = reinterpret_cast<OLEDCommandValue2*>(cmdVecData->GetData(used)->value);
00677 for(unsigned int frame=0; frame<NumFrames; frame++)
00678 jval[frame].period = 1;
00679 used++;
00680 }
00681
00682 for(unsigned int j=ERS7Info::FaceLEDPanelOffset; j<LEDOffset+NumLEDs; j++)
00683 if(open[j]) {
00684 OCommandInfo* info = cmdVecData->GetInfo(used);
00685 info->Set(odataLED_COMMAND3, primIDs[j], NumFrames);
00686 OLEDCommandValue3* jval = reinterpret_cast<OLEDCommandValue3*>(cmdVecData->GetData(used)->value);
00687 for(unsigned int frame=0; frame<NumFrames; frame++)
00688 jval[frame].period = 1;
00689 used++;
00690 }
00691
00692 for(unsigned int j=BinJointOffset; j<BinJointOffset+NumBinJoints; j++)
00693 if(open[j]) {
00694 OCommandInfo* info = cmdVecData->GetInfo(used);
00695 info->Set(odataJOINT_COMMAND4, primIDs[j], NumSlowFrames);
00696 OJointCommandValue4* jval = reinterpret_cast<OJointCommandValue4*>(cmdVecData->GetData(used)->value);
00697 for(unsigned int frame=0; frame<NumFrames; frame++)
00698 jval[frame].period = 1;
00699 used++;
00700 }
00701 } else {
00702 for(unsigned int j=LEDOffset; j<LEDOffset+NumLEDs; j++)
00703 if(open[j]) {
00704 OCommandInfo* info = cmdVecData->GetInfo(used);
00705 info->Set(odataLED_COMMAND2, primIDs[j], NumFrames);
00706 OLEDCommandValue2* jval = reinterpret_cast<OLEDCommandValue2*>(cmdVecData->GetData(used)->value);
00707 for(unsigned int frame=0; frame<NumFrames; frame++)
00708 jval[frame].period = 1;
00709 used++;
00710 }
00711 for(unsigned int j=BinJointOffset; j<BinJointOffset+NumBinJoints; j++)
00712 if(open[j]) {
00713 OCommandInfo* info = cmdVecData->GetInfo(used);
00714 info->Set(odataJOINT_COMMAND3, primIDs[j], NumSlowFrames);
00715 used++;
00716 }
00717 }
00718 }
00719 }
00720
00721
00722 RCRegion*
00723 MMCombo::InitRegion(unsigned int size) {
00724 unsigned int pagesize=4096;
00725 sError err=GetPageSize(&pagesize);
00726 ASSERT(err==sSUCCESS,"Error "<<err<<" getting page size");
00727 unsigned int pages=(size+pagesize-1)/pagesize;
00728 return new RCRegion(pages*pagesize);
00729 }
00730
00731 void
00732 MMCombo::addRunLevel() {
00733 runLevel++;
00734 if(runLevel==readyLevel) {
00735 cout << "START UP BEHAVIOR..." << flush;
00736 ProjectInterface::startupBehavior.DoStart();
00737 cout << "START UP BEHAVIOR-DONE" << endl;
00738 }
00739 }
00740
00741 void
00742 MMCombo::RPOPENR_notify(const ONotifyEvent& event) {
00743 const char *buf = (const char *)event.Data(0);
00744 observer[event.ObsIndex()]->AssertReady();
00745
00746
00747
00748
00749
00750
00751
00752
00753 erouter->postEvent(new TextMsgEvent(buf));
00754 }
00755
00756 int
00757 MMCombo::RPOPENR_send(char *buf, int bufsize) {
00758 if (RPOPENR_isready && bufsize>0) {
00759 RPOPENR_isready=false;
00760 subject[sbjRPOPENRSendString]->SetData(buf, bufsize);
00761 subject[sbjRPOPENRSendString]->NotifyObservers();
00762 return bufsize;
00763 }
00764 return 0;
00765 }
00766
00767
00768
00769
00770
00771
00772
00773
00774
00775
00776
00777
00778