void NaoMarkServiceDetection::callback(const std::string &key, const AL::ALValue &value, const AL::ALValue &msg) { AL::ALValue marks = fMemoryProxy.getData("LandmarkDetected"); if(marks.getSize() > 0 && !_isMarkFound) { int TimeStampField = marks[0][1]; if((int)marks[1][0][1][0] == _markToFind) { if((float)marks[1][0][0][3] < 0.2f) { if(_isAllowedToMove) motionProxy->moveToward(0.5f,0,marks[1][0][0][1]); _naoMarkDetected = true; } else { motionProxy->moveToward(0,0,0); _isMarkFound = true; } } else motionProxy->moveToward(0,0,0); } }
void Bumper::init() { try { /** Create a proxy to ALMemory. */ fMemoryProxy = AL::ALMemoryProxy(getParentBroker()); fState = fMemoryProxy.getData("RightBumperPressed"); /** Subscribe to event LeftBumperPressed * Arguments: * - name of the event * - name of the module to be called for the callback * - name of the bound method to be called on event */ fMemoryProxy.subscribeToEvent("RightBumperPressed", "Bumper", "onRightBumperPressed"); } catch (const AL::ALError& e) { qiLogError("module.example") << e.what() << std::endl; } }
void Bumper::onRightBumperPressed() { qiLogInfo("module.example") << "Executing callback method on right bumper event" << std::endl; /** * As long as this is defined, the code is thread-safe. */ AL::ALCriticalSection section(fCallbackMutex); /** * Check that the bumper is pressed. */ fState = fMemoryProxy.getData("RightBumperPressed"); if (fState > 0.5f) { return; } try { fTtsProxy = AL::ALTextToSpeechProxy(getParentBroker()); fTtsProxy.say("Right bumper pressed"); } catch (const AL::ALError& e) { qiLogError("module.example") << e.what() << std::endl; } }
/** * 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 }
/** * The method reads all sensors. It also detects if the chest button was pressed * for at least three seconds. In that case, it shuts down the robot. */ void readSensors() { // get new sensor values and copy them to the shared memory block try { // copy sensor values into the shared memory block int writingSensors = 0; if(writingSensors == data->newestSensors) ++writingSensors; if(writingSensors == data->readingSensors) if(++writingSensors == data->newestSensors) ++writingSensors; assert(writingSensors != data->newestSensors); assert(writingSensors != data->readingSensors); float* sensors = data->sensors[writingSensors]; for(int i = 0; i < lbhNumOfSensorIds; ++i) sensors[i] = *sensorPtrs[i]; AL::ALValue value = memory->getData("GameCtrl/RoboCupGameControlData"); if(value.isBinary() && value.getSize() == sizeof(RoboCup::RoboCupGameControlData)) memcpy(&data->gameControlData[writingSensors], value, sizeof(RoboCup::RoboCupGameControlData)); data->newestSensors = writingSensors; // detect shutdown request via chest-button if(*sensorPtrs[chestButtonSensor] == 0.f) startPressedTime = dcmTime; else if(state != shuttingDown && startPressedTime && dcmTime - startPressedTime > 3000) { if(*sensorPtrs[rBumperRightSensor] != 0.f || *sensorPtrs[rBumperLeftSensor] != 0.f || *sensorPtrs[lBumperRightSensor] != 0.f || *sensorPtrs[lBumperLeftSensor] != 0.f) (void) !system("( /home/nao/bin/bhumand stop && sudo shutdown -r now ) &"); else (void) !system("( /home/nao/bin/bhumand stop && sudo shutdown -h now ) &"); state = preShuttingDown; } } catch(AL::ALError& e) { fprintf(stderr, "libbhuman: %s\n", e.toString().c_str()); } // raise the semaphore if(sem != SEM_FAILED) { int sval; if(sem_getvalue(sem, &sval) == 0) { if(sval < 1) { sem_post(sem); frameDrops = 0; } else { if(frameDrops == 0) fprintf(stderr, "libbhuman: dropped sensor data.\n"); ++frameDrops; } } } }