bool rd::RobotDevastation::initSound(yarp::os::ResourceFinder &rf)
{
SDLAudioManager::RegisterManager();
audioManager = AudioManager::getAudioManager("SDL");
std::string bsoStr( rf.findFileByName("../sounds/RobotDevastationBSO.mp3") );
if ( ! audioManager->load(bsoStr, "bso", 0) )
return false;
std::string shootStr( rf.findFileByName("../sounds/01_milshot.wav") );
if ( ! audioManager->load(shootStr, "shoot", 1) )
return false;
std::string explosionStr( rf.findFileByName("../sounds/15_explosion.wav") );
if ( ! audioManager->load(explosionStr, "explosion", 1) )
return false;
std::string noAmmoStr( rf.findFileByName("../sounds/03_clip.wav") );
if ( ! audioManager->load(noAmmoStr, "noAmmo", 1) )
return false;
std::string reloadStr( rf.findFileByName("../sounds/04_milaction.wav") );
if ( ! audioManager->load(reloadStr, "reload", 1) )
return false;
return true;
}
bool kinematicStructureInterface::configure(yarp::os::ResourceFinder &rf)
{
moduleName = rf.check("name", Value("kinematicStructureInterface")).asString().c_str();
setName(moduleName.c_str());
yInfo() << "findFileByName " << rf.findFileByName("kinematicStructureInterface.ini") ;
cout << moduleName << ": finding configuration files..." << endl;
period = rf.check("period", Value(0.1)).asDouble();
robot = rf.check("robot", Value("icub")).asString().c_str();
//Create the ICubclient, check client.ini to know which subsystem to load (should be ARE)
iCub = new ICubClient(moduleName, "kinematicStructureInterface", "client.ini");
iCub->opc->isVerbose = false;
if (!iCub->connect())
{
yInfo() << " iCubClient : Some dependencies are not running...";
Time::delay(1.0);
}
//rpc port
rpcPort.open(("/" + moduleName + "/rpc").c_str());
attach(rpcPort);
yInfo() << "\n \n" << "----------------------------------------------" << "\n \n" << moduleName << " ready! \n \n ";
return true;
}
bool CRMainEstimation::configure(yarp::os::ResourceFinder &rf) {
setName(rf.check("name", Value("headPoseEstimator"), "module name (string)").asString().c_str());
g_ntrees = rf.check("g_ntrees",Value(10)).asInt();
g_maxv = (float)rf.check("g_maxv",Value(800.0)).asDouble();
g_larger_radius_ratio = rf.check("g_larger_radius_ratio",Value(1.6)).asDouble();
g_smaller_radius_ratio = rf.check("g_smaller_radius_ratio",Value(5.0)).asDouble();
g_stride = rf.check("g_stride",Value(5)).asInt();
g_max_z = rf.check("g_max_z",Value(1300)).asInt();
g_th = rf.check("g_th",Value(500)).asInt();
string carrier=rf.check("carrier",Value("udp")).asString().c_str();
string remote=rf.check("remote",Value("kinectServer")).asString().c_str();
g_prob_th = 1.0f;
g_Estimate = new CRForestEstimator();
buf=new unsigned short[320*240];
string fullpath = rf.findFileByName("tree000.bin");
string relativepath = fullpath.substr(0, fullpath.find("000.bin"));
if( !g_Estimate->loadForest(relativepath.c_str(), g_ntrees) ){
cerr << "Could not read forest!" << endl;
return false;
}
cout << endl << "------------------------------------" << endl << endl;
cout << "Estimation: " << endl;
cout << "Trees: " << g_ntrees << " " << relativepath << endl;
cout << "Stride: " << g_stride << endl;
cout << "Max Variance: " << g_maxv << endl;
cout << "Max Distance: " << g_max_z << endl;
cout << "Head Threshold: " << g_th << endl;
cout << endl << "------------------------------------" << endl << endl;
depthPort.open(("/"+ getName() + "/depth:i").c_str());
while(!Network::connect(("/"+remote+"/depth:o").c_str(),depthPort.getName().c_str(),carrier.c_str())) {
cout << "Waiting for connection to kinectServer" << endl;
Time::delay(1.0);
}
if (!handlerPort.open(("/" + getName() + "/rpc").c_str())) {
cout << getName() << ": Unable to open port " << handlerPort.getName() << endl;
return false;
}
attach(handlerPort);
return true;
}
bool ears::configure(yarp::os::ResourceFinder &rf)
{
string moduleName = rf.check("name", Value("ears")).asString().c_str();
setName(moduleName.c_str());
yInfo() << moduleName << " : finding configuration files...";
period = rf.check("period", Value(0.1)).asDouble();
//Create an iCub Client and check that all dependencies are here before starting
bool isRFVerbose = false;
iCub = new ICubClient(moduleName, "ears", "client.ini", isRFVerbose);
iCub->opc->isVerbose = false;
if (!iCub->connect())
{
yInfo() << " iCubClient : Some dependencies are not running...";
Time::delay(1.0);
}
rpc.open(("/" + moduleName + "/rpc").c_str());
attach(rpc);
portToBehavior.open("/" + moduleName + "/behavior:o");
while (!Network::connect(portToBehavior.getName(),"/BehaviorManager/trigger:i ")) {
yWarning() << " Behavior is not reachable";
yarp::os::Time::delay(0.5);
}
portTarget.open("/" + moduleName + "/target:o");
MainGrammar = rf.findFileByName(rf.check("MainGrammar", Value("MainGrammar.xml")).toString());
bShouldListen = true;
yInfo() << "\n \n" << "----------------------------------------------" << "\n \n" << moduleName << " ready ! \n \n ";
return true;
}
bool faceTrackerModule::configure(yarp::os::ResourceFinder &rf) {
moduleName = rf.check("name",
Value("faceTracker"),
"module name (string)").asString();
setName(moduleName.c_str());
opcName = rf.check("opcName",
Value("OPC"),
"Opc name (string)").asString();
string xmlPath = rf.findFileByName("haarcascade_frontalface_default.xml");
// Create an iCub Client and check that all dependencies are here befor starting
opc = new OPCClient(moduleName.c_str());
opc->connect(opcName);
icub = NULL;
handlerPortName = "/";
handlerPortName += getName() + "/rpc";
if (!handlerPort.open(handlerPortName.c_str())) {
cout << getName() << ": Unable to open port " << handlerPortName << endl;
return false;
}
// ==================================================================
// image port open
imagePortLeft.open("/facetracking/image/left/in"); // give the left port a name
// ==================================================================
// robot
// ==================================================================
while(!Network::connect("/icub/camcalib/left/out", "/facetracking/image/left/in"))
{
Time::delay(3);
cout << "try to connect left camera, please wait ..." << endl;
}
Property options;
options.put("device", "remote_controlboard");
options.put("local", "/tutorial/motor/client");
options.put("remote", "/icub/head");
robotHead = new PolyDriver(options);
if(!robotHead->isValid())
{
cout << "Cannot connect to the robot head" << endl;
return false;
}
robotHead->view(pos);
robotHead->view(vel);
robotHead->view(enc);
if(pos==NULL || vel==NULL || enc==NULL)
{
cout << "Cannot get interface to robot head" << endl;
robotHead->close();
return false;
}
jnts = 0;
pos->getAxes(&jnts);
setpoints.resize(jnts);
cur_encoders.resize(jnts);
prev_encoders.resize(jnts);
/* prepare command */
for(int i=0;i<jnts;i++)
{
setpoints[i] = 0;
}
// ==================================================================
//// create a opencv window
cv::namedWindow("cvImage_Left",CV_WINDOW_NORMAL);
// ==================================================================
// face detection configuration
face_classifier_left.load(xmlPath);
attach(handlerPort); // attach to port
// ==================================================================
// Parameters
counter = 0;
x_buf = 0;
y_buf = 0;
mode = 0; // 0: going to a set position, 1: face searching, 2: face tracking, 3: face stuck,
setpos_counter = 0;
panning_counter = 0;
stuck_counter = 0;
tracking_counter = 0;
// ==================================================================
// random motion
tilt_target = 0;
pan_target = 0;
seed = 10000;
srand(seed);
pan_max = 80;
tilt_max = 20;
cvIplImageLeft = NULL;
return true ;
}
bool learnPrimitive::configure(yarp::os::ResourceFinder &rf)
{
string moduleName = rf.check("name", Value("learnPrimitive")).asString().c_str();
setName(moduleName.c_str());
GrammarYesNo = rf.findFileByName(rf.check("GrammarYesNo", Value("nodeYesNo.xml")).toString());
GrammarNameAction = rf.findFileByName(rf.check("GrammarNameAction", Value("GrammarNameAction.xml")).toString());
GrammarTypeAction = rf.findFileByName(rf.check("GrammarTypeAction", Value("GrammarTypeAction.xml")).toString());
yInfo() << "findFileByName " << rf.findFileByName("learnPrimitive.ini") ;
pathToIniFile = rf.findFileByName("learnPrimitive.ini") ;
cout << moduleName << ": finding configuration files..." << endl;
period = rf.check("period", Value(0.1)).asDouble();
//bool bEveryThingisGood = true;
//Create an iCub Client and check that all dependencies are here before starting
bool isRFVerbose = true;
iCub = new ICubClient(moduleName, "learnPrimitive", "client.ini", isRFVerbose);
iCub->opc->isVerbose &= true;
string robot = rf.check("robot", Value("icubSim")).asString().c_str();
string arm = rf.check("arm", Value("left_arm")).asString().c_str();
portToArm.open(("/" + moduleName + "/toArm:rpc").c_str());
string portRobotArmName = "/" + robot + "/" + arm + "/rpc:i";
yInfo() << "================> port controlling the arm : " << portRobotArmName;
if (!Network::connect(portToArm.getName().c_str(),portRobotArmName))
{
yWarning() << "WARNING PORT TO CONTROL ARM (" << portRobotArmName << ") IS NOT CONNECTED";
}
if (!iCub->connect())
{
cout << "iCubClient : Some dependencies are not running..." << endl;
Time::delay(1.0);
}
//rpc port
rpcPort.open(("/" + moduleName + "/rpc").c_str());
attach(rpcPort);
if (!iCub->getRecogClient())
{
yWarning() << "WARNING SPEECH RECOGNIZER NOT CONNECTED";
}
if (!iCub->getABMClient())
{
yWarning() << "WARNING ABM NOT CONNECTED";
}
updateProtoAction(rf);
updatePrimitive(rf);
updateAction(rf);
yInfo() << "\n \n" << "----------------------------------------------" << "\n \n" << moduleName << " ready ! \n \n ";
iCub->say("learn Primitive is ready", false);
return true;
}
/*
* Configure method, connect to different subsystem
*/
bool abmInteraction::configure(yarp::os::ResourceFinder &rf)
{
string moduleName = rf.check("name", Value("abmInteraction")).asString().c_str();
setName(moduleName.c_str());
nameGrammarHumanFeedback = rf.findFileByName(rf.check("nameGrammarHumanFeedback", Value("hFeedback.xml")).toString());
//nameGrammarHumanFeedback = rf.findFileByName(rf.check("nameGrammarHumanFeedback", Value("hFeedbackDescription")).toString());
nameGrammarYesNo = rf.findFileByName(rf.check("nameGrammarYesNo", Value("nodeYesNo.xml")).toString());
cout << moduleName << ": finding configuration files..." << endl;
period = rf.check("period", Value(0.1)).asDouble();
//tryAgain = false ; //at start, iCub will explain and speaks
//bestRank = 0 ;
//Create an iCub Client and check that all dependencies are here before starting
bool isRFVerbose = false;
iCub = new ICubClient(moduleName, "abmInteraction", "client.ini", isRFVerbose);
iCub->opc->isVerbose = false;
if (!iCub->connect())
{
cout << "iCubClient : Some dpeendencies are not running..." << endl;
Time::delay(1.0);
}
rpc.open(("/" + moduleName + "/rpc").c_str());
attach(rpc);
if (!iCub->getRecogClient())
{
cout << "WARNING SPEECH RECOGNIZER NOT CONNECTED" << endl;
}
if (!iCub->getABMClient())
{
cout << "WARNING ABM NOT CONNECTED" << endl;
}
rememberedInstance = rf.check("rememberedInstance", Value(1333)).asInt();
agentName = rf.check("agentName", Value("Bob")).asString().c_str();
img_provider_port = rf.check("img_provider_port", Value("/icub/camcalib/left/out/kinematic_structure")).asString().c_str();
//resume : no (take all the augmented_time from the instance)
//resume : yes (take only the augmented_time without feedback from the instance)
//resume : agent (take only the augmented_time with no feedback from the agent, from the instance)
resume = rf.check("resume", Value("agent")).asString().c_str();
it_augmentedTime = vAugmentedTime.begin();
/*pair<string, int> bestTimeAndRank("", -1);
if (createAugmentedTimeVector(bestTimeAndRank) == -1){
yError() << " Something is wrong with the augmented memories! quit";
return false;
}*/
//testing
/*Bottle bRecognized;
bool blop = true;
while (blop){
iCub->say("Check for sending a grammar");
bRecognized = iCub->getRecogClient()->recogFromGrammarLoop(grammarToString(nameGrammarHumanFeedback));
yInfo() << "bRecognized " << bRecognized.toString();
cout << bRecognized.get(1).toString() << endl;
bRecognized.clear();
} */
//end testing
return true;
}
