int main(int argc, char **argv)
{
std::string robotIp = "127.0.0.1";
if (argc < 2) {
std::cerr << "Usage: almotion_setBreathConfig robotIp "
<< "(optional default \"127.0.0.1\")."<< std::endl;
}
else {
robotIp = argv[1];
}
AL::ALMotionProxy motion(robotIp);
// Example showing how to change the breathing configuration
// Setting a relaxed configuration: 5 breaths per minute at max amplitude
AL::ALValue breathConfig;
breathConfig.arraySetSize(2);
AL::ALValue tmp;
tmp.arraySetSize(2);
tmp[0] = "Bpm";
tmp[1] = 5.0f;
breathConfig[0] = tmp;
tmp[0] = "Amplitude";
tmp[1] = 1.0f;
breathConfig[1] = tmp;
motion.setBreathConfig(breathConfig);
return 0;
}
void SensorsModule::initializeSonarValues()
{
// Get a proxy to the DCM.
boost::shared_ptr<AL::DCMProxy> dcmProxy = broker_->getDcmProxy();
if(dcmProxy != 0)
{
try
{
// For DCM::set() see:
// http://www.aldebaran-robotics.com/documentation/naoqi/sensors/dcm-api.html#DCMProxy::set__AL::ALValueCR
AL::ALValue dcmSonarCommand;
dcmSonarCommand.arraySetSize(3);
dcmSonarCommand[0] = std::string("Device/SubDeviceList/US/Actuator/Value"); // Device name.
dcmSonarCommand[1] = std::string("ClearAll"); // Delete all timed commands before adding this one.
dcmSonarCommand[2].arraySetSize(1); // A list of (1) timed-commands.
dcmSonarCommand[2][0].arraySetSize(2);
dcmSonarCommand[2][0][0] = 68.0; // The command itself.
dcmSonarCommand[2][0][1] = dcmProxy->getTime(0); // The DCM time for the command to be applied.
// Send the timed command to the sonars.
dcmProxy->set(dcmSonarCommand);
}
catch(AL::ALError& e)
{
std::cout << "SensorsModule : Failed to initialize sonars, "
<< e.toString() << std::endl;
}
}
}
/**
* \brief: Create the aliases for controlling the joint stiffnesses.
**/
void hal_experimental::create_actuator_stiffness_aliases()
{
LOG("[create_actuator_stiffness]", "create actuator stiffness initializing...");
AL::ALValue jointAliases;
try
{
jointAliases.clear();
jointAliases.arraySetSize(2);
jointAliases[0] = std::string("jointStiffness");
jointAliases[1].arraySetSize(25);
int j = 0;
for(int i = NumOfPositionActuatorIds; i <= rAnkleRollStiffnessActuator; ++i)
{
jointAliases[1][j] = actuatorNames[i];
++j;
}
// jointAliases[1][HEAD_PITCH] = std::string("Device/SubDeviceList/HeadPitch/Hardness/Actuator/Value");
// jointAliases[1][HEAD_YAW] = std::string("Device/SubDeviceList/HeadYaw/Hardness/Actuator/Value");
// jointAliases[1][L_ANKLE_PITCH] = std::string("Device/SubDeviceList/LAnklePitch/Hardness/Actuator/Value");
// jointAliases[1][L_ANKLE_ROLL] = std::string("Device/SubDeviceList/LAnkleRoll/Hardness/Actuator/Value");
// jointAliases[1][L_ELBOW_ROLL] = std::string("Device/SubDeviceList/LElbowRoll/Hardness/Actuator/Value");
// jointAliases[1][L_ELBOW_YAW] = std::string("Device/SubDeviceList/LElbowYaw/Hardness/Actuator/Value");
// jointAliases[1][L_HAND] = std::string("Device/SubDeviceList/LHand/Hardness/Actuator/Value");
// jointAliases[1][L_HIP_PITCH] = std::string("Device/SubDeviceList/LHipPitch/Hardness/Actuator/Value");
// jointAliases[1][L_HIP_ROLL] = std::string("Device/SubDeviceList/LHipRoll/Hardness/Actuator/Value");
// jointAliases[1][L_HIP_YAW_PITCH] = std::string("Device/SubDeviceList/LHipYawPitch/Hardness/Actuator/Value");
// jointAliases[1][L_KNEE_PITCH] = std::string("Device/SubDeviceList/LKneePitch/Hardness/Actuator/Value");
// jointAliases[1][L_SHOULDER_PITCH] = std::string("Device/SubDeviceList/LShoulderPitch/Hardness/Actuator/Value");
// jointAliases[1][L_SHOULDER_ROLL] = std::string("Device/SubDeviceList/LShoulderRoll/Hardness/Actuator/Value");
// jointAliases[1][L_WRIST_YAW] = std::string("Device/SubDeviceList/LWristYaw/Hardness/Actuator/Value");
// jointAliases[1][R_ANKLE_PITCH] = std::string("Device/SubDeviceList/RAnklePitch/Hardness/Actuator/Value");
// jointAliases[1][R_ANKLE_ROLL] = std::string("Device/SubDeviceList/RAnkleRoll/Hardness/Actuator/Value");
// jointAliases[1][R_ELBOW_ROLL] = std::string("Device/SubDeviceList/RElbowRoll/Hardness/Actuator/Value");
// jointAliases[1][R_ELBOW_YAW] = std::string("Device/SubDeviceList/RElbowYaw/Hardness/Actuator/Value");
// jointAliases[1][R_HAND] = std::string("Device/SubDeviceList/RHand/Hardness/Actuator/Value");
// jointAliases[1][R_HIP_PITCH] = std::string("Device/SubDeviceList/RHipPitch/Hardness/Actuator/Value");
// jointAliases[1][R_HIP_ROLL] = std::string("Device/SubDeviceList/RHipRoll/Hardness/Actuator/Value");
// jointAliases[1][R_KNEE_PITCH] = std::string("Device/SubDeviceList/RKneePitch/Hardness/Actuator/Value");
// jointAliases[1][R_SHOULDER_PITCH] = std::string("Device/SubDeviceList/RShoulderPitch/Hardness/Actuator/Value");
// jointAliases[1][R_SHOULDER_ROLL] = std::string("Device/SubDeviceList/RShoulderRoll/Hardness/Actuator/Value");
// jointAliases[1][R_WRIST_YAW] = std::string("Device/SubDeviceList/RWristYaw/Hardness/Actuator/Value");
}
catch(const AL::ALError &e)
{
std::cout << "[create_actuator_stiffness][ERROR]: Error setting up the aliase: " << e.toString() << std::endl;
}
try
{
dcm_proxy->createAlias(jointAliases);
log_file << "stiffness alias added to DCM!\n";
}
catch(const AL::ALError &e)
{
LOG("[create_actuator_stiffness]","[ERROR]: An error occured while creating stiffness actuator aliases: " + e.toString());
}
LOG("[create_actuator_stiffness]","create actuator stiffness initialized");
}
/**
* Creates the appropriate aliases with the DCM
*/
void NaoEnactor::initDCMAliases(){
AL::ALValue positionCommandsAlias;
positionCommandsAlias.arraySetSize(3);
positionCommandsAlias[0] = string("AllActuatorPosition");
positionCommandsAlias[1].arraySetSize(Kinematics::NUM_JOINTS);
AL::ALValue hardCommandsAlias;
hardCommandsAlias.arraySetSize(3);
hardCommandsAlias[0] = string("AllActuatorHardness");
hardCommandsAlias[1].arraySetSize(Kinematics::NUM_JOINTS);
for (unsigned int i = 0; i<Kinematics::NUM_JOINTS; i++){
positionCommandsAlias[1][i] = jointsP[i];
hardCommandsAlias[1][i] = jointsH[i];
}
dcmProxy->createAlias(positionCommandsAlias);
dcmProxy->createAlias(hardCommandsAlias);
}
/**
* \brief: Creates an alias for all the position actuators.
**/
void hal_experimental::create_actuator_position_aliases()
{
LOG("[create_actuator_pos]","create actuator position initializing...");
AL::ALValue jointAliases;
jointAliases.arraySetSize(2);
jointAliases[0] = std::string("jointActuators");
jointAliases[1].arraySetSize(NumOfPositionActuatorIds);
for(int i = 0; i < NumOfPositionActuatorIds; ++i)
{
jointAliases[1][i] = std::string(actuatorNames[i]);
}
// Joints actuator list
// jointAliases[1].arraySetSize(25);
// jointAliases[1][HEAD_PITCH] = std::string("Device/SubDeviceList/HeadPitch/Position/Actuator/Value");
// jointAliases[1][HEAD_YAW] = std::string("Device/SubDeviceList/HeadYaw/Position/Actuator/Value");
// jointAliases[1][L_ANKLE_PITCH] = std::string("Device/SubDeviceList/LAnklePitch/Position/Actuator/Value");
// jointAliases[1][L_ANKLE_ROLL] = std::string("Device/SubDeviceList/LAnkleRoll/Position/Actuator/Value");
// jointAliases[1][L_ELBOW_ROLL] = std::string("Device/SubDeviceList/LElbowRoll/Position/Actuator/Value");
// jointAliases[1][L_ELBOW_YAW] = std::string("Device/SubDeviceList/LElbowYaw/Position/Actuator/Value");
// jointAliases[1][L_HAND] = std::string("Device/SubDeviceList/LHand/Position/Actuator/Value");
// jointAliases[1][L_HIP_PITCH] = std::string("Device/SubDeviceList/LHipPitch/Position/Actuator/Value");
// jointAliases[1][L_HIP_ROLL] = std::string("Device/SubDeviceList/LHipRoll/Position/Actuator/Value");
// jointAliases[1][L_HIP_YAW_PITCH] = std::string("Device/SubDeviceList/LHipYawPitch/Position/Actuator/Value");
// jointAliases[1][L_KNEE_PITCH] = std::string("Device/SubDeviceList/LKneePitch/Position/Actuator/Value");
// jointAliases[1][L_SHOULDER_PITCH] = std::string("Device/SubDeviceList/LShoulderPitch/Position/Actuator/Value");
// jointAliases[1][L_SHOULDER_ROLL] = std::string("Device/SubDeviceList/LShoulderRoll/Position/Actuator/Value");
// jointAliases[1][L_WRIST_YAW] = std::string("Device/SubDeviceList/LWristYaw/Position/Actuator/Value");
// jointAliases[1][R_ANKLE_PITCH] = std::string("Device/SubDeviceList/RAnklePitch/Position/Actuator/Value");
// jointAliases[1][R_ANKLE_ROLL] = std::string("Device/SubDeviceList/RAnkleRoll/Position/Actuator/Value");
// jointAliases[1][R_ELBOW_ROLL] = std::string("Device/SubDeviceList/RElbowRoll/Position/Actuator/Value");
// jointAliases[1][R_ELBOW_YAW] = std::string("Device/SubDeviceList/RElbowYaw/Position/Actuator/Value");
// jointAliases[1][R_HAND] = std::string("Device/SubDeviceList/RHand/Position/Actuator/Value");
// jointAliases[1][R_HIP_PITCH] = std::string("Device/SubDeviceList/RHipPitch/Position/Actuator/Value");
// jointAliases[1][R_HIP_ROLL] = std::string("Device/SubDeviceList/RHipRoll/Position/Actuator/Value");
// jointAliases[1][R_KNEE_PITCH] = std::string("Device/SubDeviceList/RKneePitch/Position/Actuator/Value");
// jointAliases[1][R_SHOULDER_PITCH] = std::string("Device/SubDeviceList/RShoulderPitch/Position/Actuator/Value");
// jointAliases[1][R_SHOULDER_ROLL] = std::string("Device/SubDeviceList/RShoulderRoll/Position/Actuator/Value");
// jointAliases[1][R_WRIST_YAW] = std::string("Device/SubDeviceList/RWristYaw/Position/Actuator/Value");
try
{
dcm_proxy->createAlias(jointAliases);
log_file << "position alias added to DCM!\n";
}
catch(const AL::ALError &e)
{
LOG("[create_actuaor_pos]","[ERROR]: Error creating actuator position aliases: " + e.toString());
}
LOG("[create_actuator_pos]","create actuator position initialized");
}
void NaoSim::ControlSteps(const int &num)
{
for(int i=0; i<num; i++)
{
step();
fastMotionCommands->GetValues(commandValues);
//set angles
//rearrange the command
AL::ALValue dummyCommand;
dummyCommand.arraySetSize(26);
dummyCommand[HeadYaw] = commandValues[HEAD_YAW];
dummyCommand[HeadPitch] = commandValues[HEAD_PITCH];
dummyCommand[LShoulderPitch] = commandValues[L_SHOULDER_PITCH];
dummyCommand[LShoulderRoll] = commandValues[L_SHOULDER_ROLL];
dummyCommand[LElbowYaw] = commandValues[L_ELBOW_YAW];
dummyCommand[LElbowRoll] = commandValues[L_ELBOW_ROLL];
dummyCommand[LWristYaw] = commandValues[L_WRIST_YAW];
dummyCommand[LHand] = 0.0f;
dummyCommand[LHipYawPitch] = commandValues[HIP_YAW_PITCH];
dummyCommand[LHipRoll] = commandValues[L_HIP_ROLL];
dummyCommand[LHipPitch] = commandValues[L_HIP_PITCH];
dummyCommand[LKneePitch] = commandValues[L_KNEE_PITCH];
dummyCommand[LAnklePitch] = commandValues[L_ANKLE_PITCH];
dummyCommand[LAnkleRoll] = commandValues[L_ANKLE_ROLL];
dummyCommand[RHipYawPitch] = commandValues[HIP_YAW_PITCH];
dummyCommand[RHipRoll] = commandValues[R_HIP_ROLL];
dummyCommand[RHipPitch] = commandValues[R_HIP_PITCH];
dummyCommand[RKneePitch] = commandValues[R_KNEE_PITCH];
dummyCommand[RAnklePitch] = commandValues[R_ANKLE_PITCH];
dummyCommand[RAnkleRoll] = commandValues[R_ANKLE_ROLL];
dummyCommand[RShoulderPitch] = commandValues[R_SHOULDER_PITCH];
dummyCommand[RShoulderRoll] = commandValues[R_SHOULDER_ROLL];
dummyCommand[RElbowYaw] = commandValues[R_ELBOW_YAW];
dummyCommand[RElbowRoll] = commandValues[R_ELBOW_ROLL];
dummyCommand[RWristYaw] = commandValues[R_WRIST_YAW];
dummyCommand[RHand] = 0.0f;
motionProxy->setAngles("Body",dummyCommand, 1.0f);
}
}
void KmeAction::DcmInit()
{
AL::ALValue jointAliasses;
vector<std::string> PosActuatorStrings = KDeviceLists::getPositionActuatorKeys();
jointAliasses.arraySetSize(2);
jointAliasses[0] = std::string("BodyWithoutHead"); // Alias for Body joint actuators without Head joins
jointAliasses[1].arraySetSize(20);
// Joints actuator list
for (int i = KDeviceLists::L_ARM; i < KDeviceLists::NUMOFJOINTS; i++)
{
jointAliasses[1][i - KDeviceLists::L_ARM] = PosActuatorStrings[i];
}
// Create alias
try
{
dcm->createAlias(jointAliasses);
}
catch (const AL::ALError &e)
{
throw ALERROR("KmeAction", "createKmeActuatorAlias()", "Error when creating Alias : " + e.toString());
}
// Create Commands
commands.arraySetSize(6);
commands[0] = string("BodyWithoutHead");
commands[1] = string("ClearAll"); // Erase all previous commands
commands[2] = string("time-separate");
commands[3] = 0;
commands[5].arraySetSize(20); // For all joints except head
for (unsigned int i = 0; i < commands[5].getSize(); i++)
{
commands[5][i].arraySetSize(actionTimes[0].getSize()); //num of poses
for (unsigned int j = 0; j < commands[5][i].getSize(); j++)
{
commands[5][i][j] = actionAngles[i + 2][j]; // actionAngles[joints][poses], commands[5][joints][poses]
// Logger::Instance().WriteMsg("KmeACTION", "commands " + _toString(commands[5][i][j]) , Logger::ExtraInfo);
}
}
}
/**
* \brief: Set the stiffness value for every joint on the robot to a single value
**/
void hal_experimental::set_all_actuator_stiffnesses(const float &stiffnessVal)
{
log_file << "[set_all_stiffness]: << Setting All stiffness values to: " << stiffnessVal << std::endl;
AL::ALValue stiffnessCommands;
int DCMTime;
try
{
DCMTime = dcm_proxy->getTime(1000); //Time NAOQi has been running + 1000ms
}
catch(const AL::ALError &e)
{
LOG("[set_all_stiffness]","[ERROR] An error occured while setting stiffness value: " + e.toString());
}
stiffnessCommands.arraySetSize(3);
stiffnessCommands[0] = std::string("jointStiffness");
stiffnessCommands[1] = std::string("Merge");
stiffnessCommands[2].arraySetSize(1);
stiffnessCommands[2][0].arraySetSize(2);
stiffnessCommands[2][0][0] = stiffnessVal;
stiffnessCommands[2][0][1] = DCMTime;
log_file << "[set_all_stiffness] Alias set up..trying to set it now." << std::endl;
std::cout << "[set_all_stiffness] Alias set up..trying to set it now." << std::endl;
try
{
dcm_proxy->set(stiffnessCommands);
LOG("[set_all_stiffness]","Stiffness Values Set!");
}
catch(const AL::ALError &e)
{
LOG("[set_all_stiffness]","[ERROR] An error occured while setting stiffness value: " + e.toString());
}
log_file << "[set_all_stiffness]: Done Setting all stiffness values to: " << stiffnessVal << std::endl;
}
/**
* 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();
}
}
/**
* 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
}
/*!
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;
}
