home *** CD-ROM | disk | FTP | other *** search
/ DOS/V Power Report 2003 May / VPR0305.ISO / AIBO / MoNet / MotionAgents / MoNetAgentManager.cc < prev    next >
Encoding:
C/C++ Source or Header  |  2002-12-20  |  4.9 KB  |  173 lines
  1. //
  2. // Copyright 2002 Sony Corporation 
  3. //
  4. // Permission to use, copy, modify, and redistribute this software for
  5. // non-commercial use is hereby granted.
  6. //
  7. // This software is provided "as is" without warranty of any kind,
  8. // either expressed or implied, including but not limited to the
  9. // implied warranties of fitness for a particular purpose.
  10. //
  11.  
  12. #include <OPENR/OPENRAPI.h>
  13. #include <OPENR/OSyslog.h>
  14. #include "MoNetAgentManager.h"
  15. #include "MoNetAgent.h"
  16.  
  17. MoNetAgentManager::MoNetAgentManager() : activeMoNetAgent(0),
  18.                                          activeAgentCommand(),
  19.                                          moNetAgentList(),
  20.                                          effectorSubject(0)
  21. {
  22.     for (int i = 0; i < NUM_JOINTS; i++) primitiveID[i] = oprimitiveID_UNDEF;
  23.     for (int i = 0; i < NUM_COMMAND_VECTOR; i++) commandRegions[i] = 0;
  24. }
  25.  
  26. void
  27. MoNetAgentManager::Init()
  28. {
  29.     OSYSDEBUG(("MoNetAgentManager::Init()\n"));
  30.     OpenPrimitives();
  31.     NewCommandVectorData();
  32.  
  33.     list<MoNetAgent*>::iterator iter = moNetAgentList.begin();
  34.     list<MoNetAgent*>::iterator last = moNetAgentList.end();
  35.     while (iter != last) {
  36.         (*iter)->Init();
  37.         ++iter;
  38.     }
  39. }
  40.  
  41. void
  42. MoNetAgentManager::Start(OSubject* effector)
  43. {
  44.     OSYSDEBUG(("MoNetAgentManager::Start()\n"));
  45.     effectorSubject = effector;
  46. }
  47.  
  48. void
  49. MoNetAgentManager::RegisterMoNetAgent(MoNetAgent* m)
  50. {
  51.     OSYSDEBUG(("MoNetAgentManager::RegisterMoNetAgent()\n"));
  52.     moNetAgentList.push_back(m);
  53.     m->SetMoNetAgentManager(this);
  54. }
  55.  
  56. void
  57. MoNetAgentManager::NotifyCommand(const ONotifyEvent& event,
  58.                                  MoNetAgentResult* result)
  59. {
  60.     MoNetAgentCommand* cmd = (MoNetAgentCommand*)event.Data(0);
  61.  
  62.     if (activeMoNetAgent != 0) {
  63.         // BUSY
  64.         result->agent      = cmd->agent;
  65.         result->index      = cmd->index;
  66.         result->status     = monetBUSY;
  67.         result->endPosture = monetpostureUNDEF;
  68.         return;
  69.     }
  70.     
  71.     list<MoNetAgent*>::iterator iter = moNetAgentList.begin();
  72.     list<MoNetAgent*>::iterator last = moNetAgentList.end();
  73.     while (iter != last) {
  74.         if ((*iter)->AreYou(cmd->agent) == true) {
  75.             (*iter)->NotifyCommand(*cmd, result);
  76.             if (result->status != monetSUCCESS) return;
  77.             activeMoNetAgent   = *iter;
  78.             activeAgentCommand = *cmd;
  79.             return;
  80.         }
  81.         ++iter;
  82.     }
  83.  
  84.     // INVALID_ARG
  85.     result->agent      = cmd->agent;
  86.     result->index      = cmd->index;
  87.     result->status     = monetINVALID_ARG;
  88.     result->endPosture = monetpostureUNDEF;
  89. }
  90.  
  91. void
  92. MoNetAgentManager::ReadyEffector(const OReadyEvent& event,
  93.                                  MoNetAgentResult* result)
  94.     
  95. {
  96.     if (activeMoNetAgent == 0) {
  97.         result->agent      = monetagentUNDEF;
  98.         result->index      = -1;
  99.         result->status     = monetSUCCESS;
  100.         result->endPosture = monetpostureUNDEF;
  101.         return;
  102.     }
  103.  
  104.     activeMoNetAgent->ReadyEffector(activeAgentCommand, result);
  105.     if (result->status != monetSUCCESS) {
  106.         activeMoNetAgent = 0;
  107.         activeAgentCommand.Clear();
  108.     }
  109. }
  110.  
  111. void
  112. MoNetAgentManager::OpenPrimitives()
  113. {
  114.     OSYSDEBUG(("MoNetAgentManager::OpenPrimitives()\n"));
  115.  
  116.     OStatus result;
  117.  
  118.     for (int i = 0; i < NUM_JOINTS; i++) {
  119.         result = OPENR::OpenPrimitive(JOINT_LOCATOR[i], &primitiveID[i]);
  120.         if (result != oSUCCESS) {
  121.             OSYSLOG1((osyslogERROR, "%s : %s %d",
  122.                       "MoNetAgentManager::OpenPrimitives()",
  123.                       "OPENR::OpenPrimitive() FAILED", result));
  124.         }
  125.     }
  126. }
  127.  
  128. void
  129. MoNetAgentManager::NewCommandVectorData()
  130. {
  131.     OSYSDEBUG(("MoNetAgentManager::NewCommandVectorData()\n"));
  132.  
  133.     OStatus result;
  134.     MemoryRegionID      cmdVecDataID;
  135.     OCommandVectorData* cmdVecData;
  136.     OCommandInfo*       info;
  137.  
  138.     for (int i = 0; i < NUM_COMMAND_VECTOR; i++) {
  139.  
  140.         result = OPENR::NewCommandVectorData(NUM_JOINTS, 
  141.                                              &cmdVecDataID, &cmdVecData);
  142.         if (result != oSUCCESS) {
  143.             OSYSLOG1((osyslogERROR, "%s : %s %d",
  144.                       "MoNetAgentManager::NewCommandVectorData()",
  145.                       "OPENR::NewCommandVectorData() FAILED", result));
  146.         }
  147.  
  148.         commandRegions[i] = new RCRegion(cmdVecData->vectorInfo.memRegionID,
  149.                                          cmdVecData->vectorInfo.offset,
  150.                                          (void*)cmdVecData,
  151.                                          cmdVecData->vectorInfo.totalSize);
  152.  
  153.         cmdVecData->SetNumData(NUM_JOINTS);
  154.  
  155.         for (int j = 0; j < NUM_JOINTS; j++) {
  156.             info = cmdVecData->GetInfo(j);
  157.             info->Set(odataJOINT_COMMAND2, primitiveID[j], NUM_FRAMES);
  158.         }
  159.     }
  160. }
  161.  
  162. RCRegion*
  163. MoNetAgentManager::FindFreeCommandRegion()
  164. {   
  165.     for (int i = 0; i < NUM_COMMAND_VECTOR; i++) {
  166.         if (commandRegions[i]->NumberOfReference() == 1) {
  167.             return commandRegions[i];
  168.         }
  169.     }
  170.  
  171.     return 0;
  172. }
  173.