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;
}
}
}
}