void initialize()
{
ALValue angles;
ALValue names;
// Create a proxy to ALVideoDevice on the robot.
float fractionMaxSpeed = 0.1f;
names = ALValue::array("HeadYaw","HeadPitch","LShoulderPitch","LShoulderRoll","LElbowYaw","LElbowRoll");
angles = ALValue::array(0.0f, -0.0f, 1.4f, 0.35f, -1.4f, -1.05f);
motionProxy->setAngles(names, angles, fractionMaxSpeed);
names = ALValue::array("LHipYawPitch","LHipRoll","LHipPitch","LKneePitch","LAnklePitch","LAnkleRoll");
angles = ALValue::array(0.0f, -0.0f, -0.44f, 0.7f, -0.35, 0.0f);
motionProxy->setAngles(names, angles, fractionMaxSpeed);
names = ALValue::array("RHipYawPitch","RHipRoll","RHipPitch","RKneePitch","RAnklePitch","RAnkleRoll");
angles = ALValue::array(0.0f, -0.0f, -0.44f, 0.7f, -0.35f, 0.0f);
motionProxy->setAngles(names, angles, fractionMaxSpeed);
names = ALValue::array("RShoulderPitch","RShoulderRoll","RElbowYaw","RElbowRoll");
angles = ALValue::array(1.4f, -0.35f, 1.4f, 1.05f);
motionProxy->setAngles(names, angles, fractionMaxSpeed);
}
int main(int argc, char *argv[]) {
std::cout << "..::: starting NAOJI" << std::endl;
std::cout << "Copyright (c) 2009, Asura-FIT" << std::endl;
std::cout << "Copyright (c) 2007, Aldebaran-robotics" << std::endl
<< std::endl;
int i = 1;
std::string brokerName = "naoji_main";
std::string brokerIP = "127.0.0.1";
int brokerPort = 0;
// Default parent broker IP
std::string parentBrokerIP = "127.0.0.1";
// Default parent broker port
int parentBrokerPort = kBrokerPort;
// checking options
while (i < argc) {
if (argv[i][0] != '-')
return usage(argv[0]);
else if (std::string(argv[i]) == "-b")
brokerIP = std::string(argv[++i]);
else if (std::string(argv[i]) == "-p")
brokerPort = atoi(argv[++i]);
else if (std::string(argv[i]) == "-pip")
parentBrokerIP = std::string(argv[++i]);
else if (std::string(argv[i]) == "-pport")
parentBrokerPort = atoi(argv[++i]);
else if (std::string(argv[i]) == "-h")
return usage(argv[0]);
i++;
}
std::cout << "Try to connect to parent Broker at ip :" << parentBrokerIP
<< " and port : " << parentBrokerPort << std::endl;
std::cout << "Start the server bind on this ip : " << brokerIP
<< " and port : " << brokerPort << std::endl;
// Starting Broker
ALPtr<ALBroker> pBroker = ALBroker::createBroker(brokerName, brokerIP, brokerPort, parentBrokerIP, parentBrokerPort);
// Starting Broker ALPtr<ALBroker> pBroker = ALBroker::createBroker(brokerName, brokerIP, brokerPort, parentBrokerIP, parentBrokerPort);
pBroker->setBrokerManagerInstance(ALBrokerManager::getInstance());
ALModule::createModule<NaojiModule>(pBroker, "NaojiModule");
struct sigaction new_action;
/* Set up the structure to specify the new action. */
new_action.sa_handler = _terminationHandler;
sigemptyset(&new_action.sa_mask);
new_action.sa_flags = 0;
sigaction(SIGINT, &new_action, NULL);
pBroker.reset(); // because of while( 1 ), broker counted by brokermanager
while (1) {
SleepMs(100);
}
}
ALImageTranscriber::ALImageTranscriber(shared_ptr<Sensors> s,
ALPtr<ALBroker> broker)
: ThreadedImageTranscriber(s,"ALImageTranscriber"),
log(), camera(), lem_name(""), camera_active(false),
image(reinterpret_cast<uint16_t*>(new uint8_t[IMAGE_BYTE_SIZE])),
table(new unsigned char[yLimit * uLimit * vLimit]),
params(y0, u0, v0, y1, u1, v1, yLimit, uLimit, vLimit)
{
try {
log = broker->getLoggerProxy();
// Possible values are
// lowDebug, debug, lowInfo, info, warning, error, fatal
log->setVerbosity("error");
}catch (ALError &e) {
cout << "Could not create a proxy to ALLogger module" << endl;
}
#ifdef USE_VISION
registerCamera(broker);
if(camera_active) {
try{
initCameraSettings(BOTTOM_CAMERA);
}catch(ALError &e){
cout << "Failed to init the camera settings:"<<
e.toString()<< endl;
camera_active = false;
}
} else {
cout << "\tCamera is inactive!" << endl;
}
#endif
initTable("/home/nao/naoqi/lib/naoqi/table.mtb");
}
ALImageTranscriber::ALImageTranscriber(shared_ptr<Synchro> synchro,
shared_ptr<Sensors> s,
ALPtr<ALBroker> broker)
: ThreadedImageTranscriber(s,synchro,"ALImageTranscriber"),
log(), camera(), lem_name(""), camera_active(false),
image(new unsigned char[IMAGE_BYTE_SIZE])
{
try {
log = broker->getLoggerProxy();
// Possible values are
// lowDebug, debug, lowInfo, info, warning, error, fatal
log->setVerbosity("error");
}catch (ALError &e) {
std::cerr << "Could not create a proxy to ALLogger module" << std::endl;
}
#ifdef USE_VISION
registerCamera(broker);
if(camera_active) {
try{
initCameraSettings(BOTTOM_CAMERA);
}catch(ALError &e){
cout << "Failed to init the camera settings:"<<e.toString()<<endl;
camera_active = false;
}
}
else
cout << "\tCamera is inactive!" << endl;
#endif
}
void ALImageTranscriber::registerCamera(ALPtr<ALBroker> broker) {
try {
camera = broker->getProxy("NaoCam");
camera_active =true;
}catch (ALError &e) {
log->error("ALImageTranscriber",
"Could not create a proxy to NaoCam module");
camera_active =false;
return;
}
lem_name = "ALImageTranscriber_LEM";
int format = NAO_IMAGE_SIZE;
int colorSpace = NAO_COLOR_SPACE;
int fps = DEFAULT_CAMERA_FRAMERATE;
int resolution = format;
#ifdef DEBUG_MAN_INITIALIZATION
printf(" Registering LEM with format=%i colorSpace=%i fps=%i\n", format,
colorSpace, fps);
#endif
try {
lem_name = camera->call<std::string>("register", lem_name, format,
colorSpace, fps);
cout << "Registered Camera: " << lem_name << " successfully"<<endl;
} catch (ALError &e) {
cout << "Failed to register camera" << lem_name << endl;
camera_active = false;
// SleepMs(500);
// try {
// printf("LEM failed once, trying again\n");
// lem_name = camera->call<std::string>("register", lem_name, format,
// colorSpace, fps);
// }catch (ALError &e2) {
// log->error("ALImageTranscriber", "Could not call the register method of the NaoCam "
// "module\n" + e2.toString());
// return;
// }
}
}
void shoot()
{
ALValue angles;
ALValue names;
//Right Foot support
float fractionMaxSpeed = 0.07f;
names = ALValue::array("RAnkleRoll","LAnkleRoll","RHipRoll","LHipRoll");
angles = ALValue::array(-0.33f, -0.33f, 0.25f, 0.33f);
motionProxy->angleInterpolationWithSpeed(names, angles, fractionMaxSpeed);
//Rise Left Foot
names = ALValue::array("LHipPitch", "LKneePitch","LAnklePitch");
angles = ALValue::array(-0.523f, 1.398f, -0.875f);
motionProxy->angleInterpolationWithSpeed(names, angles, fractionMaxSpeed);
//Get Left Leg Position
std::string effector = "LLeg";
int space = 2;
bool useSensorValues = true;
std::vector<float> legPos = motionProxy->getPosition(effector, space, useSensorValues);
//Move Left Leg Forward
legPos[0] += 0.20f;
legPos[1] -= 0.05f;
legPos[2] -= 0.05f;
ALValue times = 0.5f; // seconds
bool isAbsolute = false;
int axisMask = 63;
motionProxy->positionInterpolation(effector, space, legPos, axisMask, times, isAbsolute);
//Get Left Leg Position
legPos = motionProxy->getPosition(effector, space, useSensorValues);
//Move Left Leg Backward
legPos[0] -= 0.2f;
legPos[2] -= 0.095f;
legPos[4] -= 0.4f;
times = 2.0f; // seconds
motionProxy->positionInterpolation(effector, space, legPos, axisMask, times, isAbsolute);
}
void ALRosGatherer::init(ALPtr<ALBroker> broker) {
fDataGathererProxy = broker->getProxy("ALDataGatherer");
}
int main( int argc, char *argv[] )
{
#ifdef REDIRECT_C_STDERR
// Redirect stderr to stdout
FILE *_syderr = stderr;
stderr = stdout;
#endif
std::cout << "..::: starting MANMODULE revision " << MANMODULE_VERSION_REVISION << " :::.." << std::endl;
std::cout << "Copyright (c) 2007, Aldebaran-robotics" << std::endl << std::endl;
int i = 1;
std::string brokerName = "manmodule";
std::string brokerIP = "0.0.0.0";
int brokerPort = 0 ;
// Default parent broker IP
std::string parentBrokerIP = "127.0.0.1";
// Default parent broker port
int parentBrokerPort = kBrokerPort;
// checking options
while( i < argc ) {
if ( argv[i][0] != '-' ) return usage( argv[0] );
else if ( std::string( argv[i] ) == "-b" ) brokerIP = std::string( argv[++i] );
else if ( std::string( argv[i] ) == "-p" ) brokerPort = atoi( argv[++i] );
else if ( std::string( argv[i] ) == "-pip" ) parentBrokerIP = std::string( argv[++i] );
else if ( std::string( argv[i] ) == "-pport" ) parentBrokerPort = atoi( argv[++i] );
else if ( std::string( argv[i] ) == "-h" ) return usage( argv[0] );
i++;
}
// If server port is not set
if ( !brokerPort )
brokerPort = FindFreePort( brokerIP );
std::cout << "Try to connect to parent Broker at ip :" << parentBrokerIP
<< " and port : " << parentBrokerPort << std::endl;
std::cout << "Start the server bind on this ip : " << brokerIP
<< " and port : " << brokerPort << std::endl;
// Starting Broker
ALPtr<ALBroker> pBroker = ALBroker::createBroker(brokerName, brokerIP, brokerPort, parentBrokerIP, parentBrokerPort);
pBroker->setBrokerManagerInstance(ALBrokerManager::getInstance());
//<OGETINSTANCE> don't remove this comment
//ALPtr<Man> manptr = ALModule::createModule<Man>(pBroker,"Man" );
//</OGETINSTANCE> don't remove this comment
#ifndef _WIN32
struct sigaction new_action;
/* Set up the structure to specify the new action. */
new_action.sa_handler = _terminationHandler;
sigemptyset( &new_action.sa_mask );
new_action.sa_flags = 0;
sigaction( SIGINT, &new_action, NULL );
#endif
//man = boost::shared_ptr<Man>(new Man(pBroker,"Man"));
ALCreateMan(pBroker);
//man->getTrigger()->await_off();
// // Not sure what the purpose of this modulegenerator code is: //EDIT -JS
pBroker.reset(); // because of while( 1 ), broker counted by brokermanager
while( 1)
{
SleepMs( 100 );
}
cout << "Main method finished" <<endl;
#ifdef _WIN32
_terminationHandler( 0 );
#endif
exit( 0 );
}
