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