/**
* 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;
}
/** 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");
}
/**
* 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();
}
}
/**
* 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);
}
}
/**
* 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;
}
}
/**
* 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 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);
}
}
/*!
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;
}
