Exemple #1
0
 /**
  * Close all resources acquired.
  * Called when initialization failed or during destruction.
  */
 void close()
 {
   if(udp)
     delete udp;
   if(proxy)
   {
     proxy->getGenericProxy()->getModule()->removeAllPreProcess();
     proxy->getGenericProxy()->getModule()->removeAllPostProcess();
     delete proxy;
   }
   if(memory)
     delete memory;
 }
Exemple #2
0
  /** Close all resources acquired. Called when initialization failed or during destruction. */
  void close()
  {
    fprintf(stderr, "libbhuman: Stopping.\n");

    if(proxy)
    {
      proxy->getGenericProxy()->getModule()->removeAllPreProcess();
      proxy->getGenericProxy()->getModule()->removeAllPostProcess();
      delete proxy;
    }
    if(memory)
      delete memory;
    if(sem != SEM_FAILED)
      sem_close(sem);
    if(data != MAP_FAILED)
      munmap(data, sizeof(LBHData));

    fprintf(stderr, "libbhuman: Stopped.\n");
  }
Exemple #3
0
  /**
   * The constructor sets up the structures required to communicate with NAOqi.
   * @param pBroker A NAOqi broker that allows accessing other NAOqi modules.
   */
  GameCtrl(boost::shared_ptr<AL::ALBroker> pBroker)
    : ALModule(pBroker, "GameCtrl"),
      proxy(0),
      memory(0),
      udp(0),
      teamNumber(0)
  {
    setModuleDescription("A module that provides packets from the GameController.");

    assert(numOfButtons == sizeof(buttonNames) / sizeof(*buttonNames));
    assert(numOfLEDs == sizeof(ledNames) / sizeof(*ledNames));

    init();

    try
    {
      memory = new AL::ALMemoryProxy(pBroker);
      proxy = new AL::DCMProxy(pBroker);

      AL::ALValue params;
      AL::ALValue result;
      params.arraySetSize(1);
      params.arraySetSize(2);

      params[0] = std::string("leds");
      params[1].arraySetSize(numOfLEDs);
      for(int i = 0; i < numOfLEDs; ++i)
        params[1][i] = std::string(ledNames[i]);
      result = proxy->createAlias(params);
      assert(result == params);

      ledRequest.arraySetSize(6);
      ledRequest[0] = std::string("leds");
      ledRequest[1] = std::string("ClearAll");
      ledRequest[2] = std::string("time-separate");
      ledRequest[3] = 0;
      ledRequest[4].arraySetSize(1);
      ledRequest[5].arraySetSize(numOfLEDs);
      for(int i = 0; i < numOfLEDs; ++i)
        ledRequest[5][i].arraySetSize(1);

      for(int i = 0; i < numOfButtons; ++i)
        buttons[i] = (float*) memory->getDataPtr(buttonNames[i]);

      // If no color was set, set it to black (no LED).
      // This actually has a race condition.
      if(memory->getDataList("GameCtrl/teamColour").empty())
        memory->insertData("GameCtrl/teamColour", TEAM_BLACK);

      playerNumber = (int*) memory->getDataPtr("GameCtrl/playerNumber");
      teamNumberPtr = (int*) memory->getDataPtr("GameCtrl/teamNumber");
      defaultTeamColour = (int*) memory->getDataPtr("GameCtrl/teamColour");

      // register "onPreProcess" and "onPostProcess" callbacks
      theInstance = this;
      proxy->getGenericProxy()->getModule()->atPreProcess(&onPreProcess);
      proxy->getGenericProxy()->getModule()->atPostProcess(&onPostProcess);

      udp = new UdpComm();
      if(!udp->setBlocking(false) ||
         !udp->setBroadcast(true) ||
         !udp->bind("0.0.0.0", GAMECONTROLLER_DATA_PORT) ||
         !udp->setLoopback(false))
      {
        fprintf(stderr, "libgamectrl: Could not open UDP port\n");
        delete udp;
        udp = 0;
        // continue, because button interface will still work
      }

      publish();
    }
    catch(AL::ALError& e)
    {
      fprintf(stderr, "libgamectrl: %s\n", e.what());
      close();
    }
  }
Exemple #4
0
  /**
   * Handles the button interface.
   * Resets the internal state when a new team number was set.
   * Receives packets from the GameController.
   * Initializes gameCtrlData when teamNumber and playerNumber are available.
   */
  void handleInput()
  {
    unsigned now = (unsigned) proxy->getTime(0);

    if(*teamNumberPtr != 0)
    {
      // new team number was set -> reset internal structure
      teamNumber = *teamNumberPtr;
      memory->insertData("GameCtrl/teamNumber", 0);
      init();
    }

    if(receive())
    {
      if(!whenPacketWasReceived)
        previousState = (uint8_t) -1; // force LED update on first packet received
      whenPacketWasReceived = now;
      publish();
    }

    if(teamNumber && *playerNumber)
    {
      // init gameCtrlData if invalid
      if(gameCtrlData.teams[0].teamNumber != teamNumber &&
         gameCtrlData.teams[1].teamNumber != teamNumber)
      {
        uint8_t teamColour = (uint8_t) *defaultTeamColour;
        if(teamColour != TEAM_BLUE && teamColour != TEAM_RED && teamColour != TEAM_YELLOW &&
           teamColour != TEAM_WHITE && teamColour != TEAM_GREEN && teamColour != TEAM_ORANGE &&
           teamColour != TEAM_PURPLE && teamColour != TEAM_BROWN && teamColour != TEAM_GRAY)
          teamColour = TEAM_BLACK;
        gameCtrlData.teams[0].teamNumber = (uint8_t) teamNumber;
        gameCtrlData.teams[0].teamColour = teamColour;
        gameCtrlData.teams[1].teamColour = teamColour ^ 1; // we don't know better
        if(!gameCtrlData.playersPerTeam)
          gameCtrlData.playersPerTeam = (uint8_t) *playerNumber; // we don't know better
        publish();
      }
      TeamInfo& team = gameCtrlData.teams[gameCtrlData.teams[0].teamNumber == teamNumber ? 0 : 1];

      if(*playerNumber <= gameCtrlData.playersPerTeam)
      {
        bool chestButtonPressed = *buttons[chest] != 0.f;
        if(chestButtonPressed != previousChestButtonPressed && now - whenChestButtonStateChanged >= BUTTON_DELAY)
        {
          if(chestButtonPressed && whenChestButtonStateChanged && now - whenPacketWasReceived >= GAMECONTROLLER_TIMEOUT) // ignore first press, e.g. for getting up
          {
            RobotInfo& player = team.players[*playerNumber - 1];
            if(player.penalty == PENALTY_NONE)
            {
              player.penalty = PENALTY_MANUAL;
            }
            else
            {
              player.penalty = PENALTY_NONE;
              gameCtrlData.state = STATE_PLAYING;
            }
            publish();
          }

          previousChestButtonPressed = chestButtonPressed;
          whenChestButtonStateChanged = now;
        }

        if(gameCtrlData.state == STATE_INITIAL)
        {
          bool leftFootButtonPressed = *buttons[leftFootLeft] != 0.f || *buttons[leftFootRight] != 0.f;
          if(leftFootButtonPressed != previousLeftFootButtonPressed && now - whenLeftFootButtonStateChanged >= BUTTON_DELAY)
          {
            if(leftFootButtonPressed)
            {
              team.teamColour = (team.teamColour + 1) % 10; // cycle between TEAM_BLUE .. TEAM_GRAY
              publish();
            }
            previousLeftFootButtonPressed = leftFootButtonPressed;
            whenLeftFootButtonStateChanged = now;
          }

          bool rightFootButtonPressed = *buttons[rightFootLeft] != 0.f || *buttons[rightFootRight] != 0.f;
          if(rightFootButtonPressed != previousRightFootButtonPressed && now - whenRightFootButtonStateChanged >= BUTTON_DELAY)
          {
            if(rightFootButtonPressed)
            {
              if(gameCtrlData.gamePhase == GAME_PHASE_NORMAL)
              {
                gameCtrlData.gamePhase = GAME_PHASE_PENALTYSHOOT;
                gameCtrlData.kickingTeam = team.teamNumber;
              }
              else if(gameCtrlData.kickingTeam == team.teamNumber)
                gameCtrlData.kickingTeam = 0;
              else
                gameCtrlData.gamePhase = GAME_PHASE_NORMAL;
              publish();
            }
            previousRightFootButtonPressed = rightFootButtonPressed;
            whenRightFootButtonStateChanged = now;
          }
        }
      }
      else
        fprintf(stderr, "Player number %d too big. Maximum number is %d.\n", *playerNumber, gameCtrlData.playersPerTeam);
    }
  }
Exemple #5
0
  /**
   * Sets the LEDs whenever the state they visualize changes.
   * Regularily sends the return packet to the GameController.
   */
  void handleOutput()
  {
    unsigned now = (unsigned) proxy->getTime(0);

    if(teamNumber && *playerNumber &&
       *playerNumber <= gameCtrlData.playersPerTeam &&
       (gameCtrlData.teams[0].teamNumber == teamNumber ||
        gameCtrlData.teams[1].teamNumber == teamNumber))
    {
      const TeamInfo& team = gameCtrlData.teams[gameCtrlData.teams[0].teamNumber == teamNumber ? 0 : 1];
      if(gameCtrlData.state != previousState ||
         gameCtrlData.gamePhase != previousGamePhase ||
         gameCtrlData.kickingTeam != previousKickingTeam ||
         team.teamColour != previousTeamColour ||
         team.players[*playerNumber - 1].penalty != previousPenalty)
      {
        switch(team.teamColour)
        {
          case TEAM_BLUE:
            setLED(leftFootRed, 0.f, 0.f, 1.f);
            break;
          case TEAM_RED:
            setLED(leftFootRed, 1.f, 0.f, 0.f);
            break;
          case TEAM_YELLOW:
            setLED(leftFootRed, 1.f, 1.f, 0.f);
            break;
          case TEAM_WHITE:
            setLED(leftFootRed, 1.f, 1.f, 1.f);
            break;
          case TEAM_GREEN:
            setLED(leftFootRed, 0.f, 1.f, 0.f);
            break;
          case TEAM_ORANGE:
            setLED(leftFootRed, 1.f, 0.5f, 0.f);
            break;
          case TEAM_PURPLE:
            setLED(leftFootRed, 1.f, 0.f, 1.f);
            break;
          case TEAM_BROWN:
            setLED(leftFootRed, 0.2f, 0.1f, 0.f);
            break;
          case TEAM_GRAY:
            setLED(leftFootRed, 0.2f, 0.2f, 0.2f);
            break;
          case TEAM_BLACK:
          default:
            setLED(leftFootRed, 0.f, 0.f, 0.f);
        }

        if(gameCtrlData.state == STATE_INITIAL &&
           gameCtrlData.gamePhase == GAME_PHASE_PENALTYSHOOT &&
           gameCtrlData.kickingTeam == team.teamNumber)
          setLED(rightFootRed, 0.f, 1.f, 0.f);
        else if(gameCtrlData.state == STATE_INITIAL &&
                gameCtrlData.gamePhase == GAME_PHASE_PENALTYSHOOT &&
                gameCtrlData.kickingTeam != team.teamNumber)
          setLED(rightFootRed, 1.f, 1.0f, 0.f);
        else if(now - whenPacketWasReceived < GAMECONTROLLER_TIMEOUT &&
                gameCtrlData.state <= STATE_SET &&
                gameCtrlData.kickingTeam == team.teamNumber)
          setLED(rightFootRed, 1.f, 1.f, 1.f);
        else
          setLED(rightFootRed, 0.f, 0.f, 0.f);

        if(team.players[*playerNumber - 1].penalty != PENALTY_NONE)
          setLED(chestRed, 1.f, 0.f, 0.f);
        else
          switch(gameCtrlData.state)
          {
            case STATE_READY:
              setLED(chestRed, 0.f, 0.f, 1.f);
              break;
            case STATE_SET:
              setLED(chestRed, 1.f, 0.4f, 0.f);
              break;
            case STATE_PLAYING:
              setLED(chestRed, 0.f, 1.f, 0.f);
              break;
            default:
              setLED(chestRed, 0.f, 0.f, 0.f);
          }

        ledRequest[4][0] = (int) now;
        proxy->setAlias(ledRequest);

        previousState = gameCtrlData.state;
        previousGamePhase = gameCtrlData.gamePhase;
        previousKickingTeam = gameCtrlData.kickingTeam;
        previousTeamColour = team.teamColour;
        previousPenalty = team.players[*playerNumber - 1].penalty;
      }

      if(now - whenPacketWasReceived < GAMECONTROLLER_TIMEOUT &&
         now - whenPacketWasSent >= ALIVE_DELAY &&
         send(GAMECONTROLLER_RETURN_MSG_ALIVE))
        whenPacketWasSent = now;
    }
  }
Exemple #6
0
  /**
   * The constructor initializes the shared memory for communicating with bhuman.
   * It also establishes a communication with NaoQi and prepares all data structures
   * required for this communication.
   * @param pBroker A NaoQi broker that allows accessing other NaoQi modules.
   */
  BHuman(boost::shared_ptr<AL::ALBroker> pBroker) :
    ALModule(pBroker, "BHuman"),
    data((LBHData*) MAP_FAILED),
    sem(SEM_FAILED),
    proxy(0),
    memory(0),
    dcmTime(0),
    lastReadingActuators(-1),
    actuatorDrops(0),
    frameDrops(allowedFrameDrops + 1),
    state(sitting),
    phase(0.f),
    ledIndex(0),
    rightEarLEDsChangedTime(0),
    startPressedTime(0),
    lastBHumanStartTime(0)
  {
    setModuleDescription("A module that provides basic ipc NaoQi DCM access using shared memory.");
    fprintf(stderr, "libbhuman: Starting.\n");

    assert(lbhNumOfSensorIds == sizeof(sensorNames) / sizeof(*sensorNames));
    assert(lbhNumOfActuatorIds == sizeof(actuatorNames) / sizeof(*actuatorNames));
    assert(lbhNumOfTeamInfoIds == sizeof(teamInfoNames) / sizeof(*teamInfoNames));

    // create shared memory
    memoryHandle = shm_open(LBH_MEM_NAME, O_CREAT | O_RDWR, S_IRUSR | S_IWUSR);
    if(memoryHandle == -1)
      perror("libbhuman: shm_open");
    else if(ftruncate(memoryHandle, sizeof(LBHData)) == -1)
      perror("libbhuman: ftruncate");
    else
    {
      // map the shared memory
      data = (LBHData*) mmap(NULL, sizeof(LBHData), PROT_READ | PROT_WRITE, MAP_SHARED, memoryHandle, 0);
      if(data == MAP_FAILED)
        perror("libbhuman: mmap");
      else
      {
        memset(data, 0, sizeof(LBHData));

        // open semaphore
        sem = sem_open(LBH_SEM_NAME, O_CREAT | O_RDWR, S_IRUSR | S_IWUSR, 0);
        if(sem == SEM_FAILED)
          perror("libbhuman: sem_open");
        else
          try
          {
            // get the robot name
            memory = new AL::ALMemoryProxy(pBroker);

            std::string robotName = (std::string) memory->getData("Device/DeviceList/ChestBoard/BodyNickName", 0);
            strncpy(data->robotName, robotName.c_str(), sizeof(data->robotName));

            // create "positionRequest" and "hardnessRequest" alias
            proxy = new AL::DCMProxy(pBroker);

            AL::ALValue params;
            AL::ALValue result;
            params.arraySetSize(1);
            params.arraySetSize(2);

            params[0] = std::string("positionActuators");
            params[1].arraySetSize(lbhNumOfPositionActuatorIds);
            for(int i = 0; i < lbhNumOfPositionActuatorIds; ++i)
              params[1][i] = std::string(actuatorNames[i]);
            result = proxy->createAlias(params);

            params[0] = std::string("hardnessActuators");
            params[1].arraySetSize(lbhNumOfHardnessActuatorIds);
            for(int i = 0; i < lbhNumOfHardnessActuatorIds; ++i)
              params[1][i] = std::string(actuatorNames[headYawHardnessActuator + i]);
            result = proxy->createAlias(params);

            params[0] = std::string("usRequest");
            params[1].arraySetSize(1);
            params[1][0] = std::string(actuatorNames[usActuator]);
            result = proxy->createAlias(params);

            // prepare positionRequest
            positionRequest.arraySetSize(6);
            positionRequest[0] = std::string("positionActuators");
            positionRequest[1] = std::string("ClearAll");
            positionRequest[2] = std::string("time-separate");
            positionRequest[3] = 0;
            positionRequest[4].arraySetSize(1);
            positionRequest[5].arraySetSize(lbhNumOfPositionActuatorIds);
            for(int i = 0; i < lbhNumOfPositionActuatorIds; ++i)
              positionRequest[5][i].arraySetSize(1);

            // prepare hardnessRequest
            hardnessRequest.arraySetSize(6);
            hardnessRequest[0] = std::string("hardnessActuators");
            hardnessRequest[1] = std::string("ClearAll");
            hardnessRequest[2] = std::string("time-separate");
            hardnessRequest[3] = 0;
            hardnessRequest[4].arraySetSize(1);
            hardnessRequest[5].arraySetSize(lbhNumOfHardnessActuatorIds);
            for(int i = 0; i < lbhNumOfHardnessActuatorIds; ++i)
              hardnessRequest[5][i].arraySetSize(1);

            // prepare usRequest
            usRequest.arraySetSize(6);
            usRequest[0] = std::string("usRequest");
            usRequest[1] = std::string("Merge"); // doesn't work with "ClearAll"
            usRequest[2] = std::string("time-separate");
            usRequest[3] = 0;
            usRequest[4].arraySetSize(1);
            usRequest[5].arraySetSize(1);
            usRequest[5][0].arraySetSize(1);

            // prepare ledRequest
            ledRequest.arraySetSize(3);
            ledRequest[1] = std::string("ClearAll");
            ledRequest[2].arraySetSize(1);
            ledRequest[2][0].arraySetSize(2);
            ledRequest[2][0][1] = 0;

            // prepare sensor pointers
            for(int i = 0; i < lbhNumOfSensorIds; ++i)
              sensorPtrs[i] = (float*) memory->getDataPtr(sensorNames[i]);
            resetUsMeasurements();

            // initialize requested actuators
            memset(requestedActuators, 0, sizeof(requestedActuators));
            for(int i = faceLedRedLeft0DegActuator; i < chestBoardLedRedActuator; ++i)
              requestedActuators[i] = -1.f;

            // register "onPreProcess" and "onPostProcess" callbacks
            theInstance = this;
            proxy->getGenericProxy()->getModule()->atPreProcess(&onPreProcess);
            proxy->getGenericProxy()->getModule()->atPostProcess(&onPostProcess);

            fprintf(stderr, "libbhuman: Started!\n");
            return; // success
          }
          catch(AL::ALError& e)
          {
            fprintf(stderr, "libbhuman: %s\n", e.toString().c_str());
          }
      }
    }
    close(); // error
  }
Exemple #7
0
  /** The method sets all actuators. */
  void setActuators()
  {
    // set all actuator values according to the values in the shared memory block
    try
    {
      dcmTime = proxy->getTime(0);

      data->readingActuators = data->newestActuators;
      if(data->readingActuators == lastReadingActuators)
      {
        if(actuatorDrops == 0)
          fprintf(stderr, "libbhuman: missed actuator request.\n");
        ++actuatorDrops;
      }
      else
        actuatorDrops = 0;
      lastReadingActuators = data->readingActuators;
      float* readingActuators = data->actuators[data->readingActuators];
      float* actuators = handleState(readingActuators);

      if(state != standing)
      {
        if(frameDrops > 0 || state == shuttingDown)
          setEyeLeds(actuators);
        else
          copyNonServos(readingActuators, actuators);
      }
      setBatteryLeds(actuators);

      // set position actuators
      positionRequest[4][0] = dcmTime; // 0 delay!
      for(int i = 0; i < lbhNumOfPositionActuatorIds; ++i)
        positionRequest[5][i][0] = actuators[i];
      proxy->setAlias(positionRequest);

      // set hardness actuators
      bool requestedHardness = false;
      for(int i = headYawHardnessActuator; i < headYawHardnessActuator + lbhNumOfHardnessActuatorIds; ++i)
        if(actuators[i] != requestedActuators[i])
        {
          hardnessRequest[4][0] = dcmTime; // 0 delay!
          for(int j = 0; j < lbhNumOfHardnessActuatorIds; ++j)
            hardnessRequest[5][j][0] = requestedActuators[headYawHardnessActuator + j] = actuators[headYawHardnessActuator + j];
          proxy->setAlias(hardnessRequest);
          requestedHardness = true;
          break;
        }

      // set us actuator
      bool requestedUs = false;
      if(requestedActuators[usActuator] != actuators[usActuator])
      {
        requestedActuators[usActuator] = actuators[usActuator];
        if(actuators[usActuator] >= 0.f)
        {
          resetUsMeasurements();
          usRequest[4][0] = dcmTime;
          usRequest[5][0][0] = actuators[usActuator];
          proxy->setAlias(usRequest);
          requestedUs = true;
        }
      }

      // set led
      if(!requestedHardness && !requestedUs)
        for(int i = 0; i < lbhNumOfLedActuatorIds; ++i)
        {
          int index = faceLedRedLeft0DegActuator + ledIndex;
          if(++ledIndex == lbhNumOfLedActuatorIds)
            ledIndex = 0;
          if(actuators[index] != requestedActuators[index])
          {
            ledRequest[0] = std::string(actuatorNames[index]);
            ledRequest[2][0][0] = requestedActuators[index] = actuators[index];
            ledRequest[2][0][1] = dcmTime;
            proxy->set(ledRequest);
            break;
          }
        }

      // set team info
      // since this should very rarely, we don't use a proxy here
      if(data->bhumanStartTime != lastBHumanStartTime)
      {
        for(int i = 0; i < lbhNumOfTeamInfoIds; ++i)
          memory->insertData(teamInfoNames[i], data->teamInfo[i]);
        lastBHumanStartTime = data->bhumanStartTime;
      }
    }
    catch(AL::ALError& e)
    {
      fprintf(stderr, "libbhuman: %s\n", e.toString().c_str());
    }
  }
  /**
   * Handles the button interface.
   * Resets the internal state when a new team number was set.
   * Receives packets from the GameController.
   * Initializes gameCtrlData when teamNumber and playerNumber are available.
   */
  void handleInput()
  {
    unsigned now = (unsigned) proxy->getTime(0);

    if(*teamNumberPtr != 0)
    { // new team number was set -> reset internal structure
      teamNumber = *teamNumberPtr;
      memory->insertData("GameCtrl/teamNumber", 0);
      init();
    }

    if(receive())
    {
      if(!whenPacketWasReceived)
        previousState = (uint8_t) -1; // force LED update on first packet received
      whenPacketWasReceived = now;
      publish();
    }

    if(teamNumber && *playerNumber)
    {
      // init gameCtrlData if invalid
      if(gameCtrlData.teams[0].teamNumber != teamNumber &&
         gameCtrlData.teams[1].teamNumber != teamNumber)
      {
        uint8_t teamColour = *defaultTeamColour == TEAM_RED ? 1 : 0;
        gameCtrlData.teams[teamColour].teamNumber = (uint8_t) teamNumber;
        gameCtrlData.teams[teamColour].teamColour = teamColour;
        gameCtrlData.teams[1 - teamColour].teamColour = 1 - teamColour;
        if(!gameCtrlData.playersPerTeam)
          gameCtrlData.playersPerTeam = (uint8_t) *playerNumber; // we don't know better
        publish();
      }
      TeamInfo& team = gameCtrlData.teams[gameCtrlData.teams[0].teamNumber == teamNumber ? 0 : 1];

      if(*playerNumber <= gameCtrlData.playersPerTeam)
      {
        bool chestButtonPressed = *buttons[chest] != 0.f;
        if(chestButtonPressed != previousChestButtonPressed && now - whenChestButtonStateChanged >= BUTTON_DELAY)
        {
          if(chestButtonPressed && whenChestButtonStateChanged) // ignore first press, e.g. for getting up
          {
            RobotInfo& player = team.players[*playerNumber - 1];
            if(player.penalty == PENALTY_NONE)
            {
              player.penalty = PENALTY_MANUAL;
              if(now - whenPacketWasReceived < GAMECONTROLLER_TIMEOUT &&
                 send(GAMECONTROLLER_RETURN_MSG_MAN_PENALISE))
                whenPacketWasSent = now;
            }
            else
            {
              player.penalty = PENALTY_NONE;
              gameCtrlData.state = STATE_PLAYING;
              if(now - whenPacketWasReceived < GAMECONTROLLER_TIMEOUT &&
                 send(GAMECONTROLLER_RETURN_MSG_MAN_UNPENALISE))
                whenPacketWasSent = now;
            }
            publish();
          }

          previousChestButtonPressed = chestButtonPressed;
          whenChestButtonStateChanged = now;
        }

        if(gameCtrlData.state == STATE_INITIAL)
        {
          bool leftFootButtonPressed = *buttons[leftFootLeft] != 0.f || *buttons[leftFootRight] != 0.f;
          if(leftFootButtonPressed != previousLeftFootButtonPressed && now - whenLeftFootButtonStateChanged >= BUTTON_DELAY)
          {
            if(leftFootButtonPressed)
            {
              team.teamColour ^= 1;
              gameCtrlData.kickOffTeam ^= 1;
              publish();
            }
            previousLeftFootButtonPressed = leftFootButtonPressed;
            whenLeftFootButtonStateChanged = now;
          }

          bool rightFootButtonPressed = *buttons[rightFootLeft] != 0.f || *buttons[rightFootRight] != 0.f;
          if(rightFootButtonPressed != previousRightFootButtonPressed && now - whenRightFootButtonStateChanged >= BUTTON_DELAY)
          {
            if(rightFootButtonPressed)
            {
              if(gameCtrlData.secondaryState == STATE2_NORMAL)
              {
                gameCtrlData.secondaryState = STATE2_PENALTYSHOOT;
                gameCtrlData.kickOffTeam = team.teamColour;
              }
              else if(gameCtrlData.kickOffTeam == team.teamColour)
                gameCtrlData.kickOffTeam ^= 1;
              else
                gameCtrlData.secondaryState = STATE2_NORMAL;
              publish();
            }
            previousRightFootButtonPressed = rightFootButtonPressed;
            whenRightFootButtonStateChanged = now;
          }
        }
      }
      else
        fprintf(stderr, "Player number %d too big. Maximum number is %d.\n", *playerNumber, gameCtrlData.playersPerTeam);
    }
  }
Exemple #9
0
/*!

   Connect toRomeo robot, and apply some motion.
   By default, this example connect to a robot with ip address: 198.18.0.1.
   If you want to connect on an other robot, run:

   ./motion --ip <robot ip address>

   Example:

   ./motion --ip 169.254.168.230
 */
int main(int argc, const char* argv[])
{
  try
  {
    std::string opt_ip = "198.18.0.1";;

    if (argc == 3) {
      if (std::string(argv[1]) == "--ip")
        opt_ip = argv[2];
    }

    // Create a general proxy to motion to use new functions not implemented in the specialized proxy

    std::string 	myIP = "";		// IP du portable (voir /etc/hosts)
    int 	myPort = 0 ;			// Default broker port

    //    AL::ALProxy *m_proxy;
    boost::shared_ptr<AL::ALBroker> broker = AL::ALBroker::createBroker("Broker", myIP, myPort, opt_ip, 9559);
    //    m_proxy = new AL::ALProxy(broker, "ALVideoDevice");

    //    m_proxy->callVoid("setCameraGroup",1 ,true);

    //AL::ALProxy* dcm = new AL::ALProxy(broker, "DCM_video");



    boost::shared_ptr<AL::ALProxy> proxy = boost::shared_ptr<AL::ALProxy>(new AL::ALProxy(broker, "DCM_video"));
    AL::DCMProxy* dcm = new AL::DCMProxy(proxy);

    //boost::shared_ptr<AL::ALProxy> dcm = boost::shared_ptr<AL::ALProxy>(new AL::ALProxy(broker, "DCM_video"));



    AL::ALValue commands;
    commands.arraySetSize(3);
    commands[0] = std::string("FaceBoard/CameraSwitch/Value");
    commands[1] = std::string("Merge");
    commands[2].arraySetSize(1);
    commands[2][0].arraySetSize(2);
    commands[2][0][0] = 1;
    //commands[2][0][1] = dcm->call<int>("getTime",0);// dcm->getTime(0);
    commands[2][0][1] = dcm->getTime(0);
    //dcm->callVoid("set",commands);
    dcm->set(commands);


    //int time = dcm->call<int>("getTime",0);
    int time = dcm->getTime(10);
    std::cout <<"Time " << time << std::endl;

    //dcm = naoqitools.myGetProxy( "DCM_video" );^M
    //dcm.set( ["FaceBoard/CameraSwitch/Value", "Merge",  [[rGroupNumber, dcm.getTime( 0 ) ]] ] );^M
    //dcm.set(\["ChestBoard/Led/Red/Actuator/Value", "Merge", \[[1.0, dcm.getTime(10000)]] ])




  }
  catch (const vpException &e)
  {
    std::cerr << "Caught exception: " << e.what() << std::endl;
  }
  catch (const AL::ALError &e)
  {
    std::cerr << "Caught exception: " << e.what() << std::endl;
  }



  return 0;
}