bool PlannerThread::threadInit()
{
closing = false;
startPlan = false;
resumePlan = true;
// initialize file names
openFiles();
process_string = "cd && cd .. && cd .. && cd " + PathName + " && ./planner.exe > " + pipeFileName;
if ( !openPorts() )
{
yError("problem opening ports");
return false;
}
return true;
}
void setUp()
{
if (_options.logpolar == 0)
_imgRecv.SetLogopolar(false);
else
_imgRecv.SetLogopolar(true);
if (_options.fovea == 0)
_imgRecv.SetFovea(false);
else
_imgRecv.SetFovea(true);
if (openPorts() == false)
ACE_OS::exit(1);
_inputImg.Resize(_options.windWidth, _options.windHeight);
}
bool homeostaticModule::addNewDrive(string driveName)
{
cout << "adding a default drive..." << endl;
Drive* drv = new Drive(driveName);
drv->setHomeostasisMin(drv->homeostasisMin);
drv->setHomeostasisMax(drv->homeostasisMax);
drv->setDecay(drv->decay);
drv->setValue((drv->homeostasisMax + drv->homeostasisMin) / 2.);
drv->setGradient(false);
manager.addDrive(drv);
cout << "default drive added. Opening ports..."<<endl;
openPorts(driveName);
cout << "new drive created successfully!"<<endl;
return true;
}
bool homeostaticModule::addNewDrive(string driveName, yarp::os::Bottle& grpHomeostatic)
{
Drive* drv = new Drive(driveName);
drv->setHomeostasisMin(grpHomeostatic.check((driveName + "-homeostasisMin"), Value(drv->homeostasisMin)).asDouble());
drv->setHomeostasisMax(grpHomeostatic.check((driveName + "-homeostasisMax"), Value(drv->homeostasisMax)).asDouble());
cout << "H1 " << grpHomeostatic.check((driveName + "-decay"), Value(drv->decay)).asDouble() <<endl;
drv->setDecay(grpHomeostatic.check((driveName + "-decay"), Value(drv->decay)).asDouble());
drv->setValue((drv->homeostasisMax + drv->homeostasisMin) / 2.);
drv->setGradient(grpHomeostatic.check((driveName + "-gradient"), Value(drv->gradient)).asBool());
cout << "h2 " << drv->decay << endl;
//cout << drv->name << " " << drv->homeostasisMin << " " << drv->homeostasisMax << " " << drv->decay << " " <<drv->gradient << endl;
//cout << d << endl;
//cout << grpHomeostatic.toString()<<endl;
manager.addDrive(drv);
cout << "h2 " << manager.drives[0]->decay << endl;
openPorts(driveName);
return true;
}
// device driver stuff
bool yarp::dev::OpenNI2DeviceDriverServer::open(yarp::os::Searchable& config) {
// this function is used in case of the Yarp Device being used as server
std::cout << "Starting OpenNI2 YARP Device please wait..." << endl;
string portPrefix;
double mConf;
int dMode, cMode;
bool printMode;
if(config.check("noRGB", "Use only depth sensor")) {
colorON = false;
} else {
colorON = true;
}
if(config.check("noRGBMirror", "enable RGB mirroring")) {
rgbMirrorON = false;
} else {
rgbMirrorON = true;
}
if(config.check("noDepthMirror", "enable depth mirroring")) {
depthMirrorON = false;
} else {
depthMirrorON = true;
}
if(config.check("noUserTracking", "Disable user tracking")) {
userTracking = false;
}
if(config.check("printVideoModes", "Print supported video modes")) {
printMode = true;
} else {
printMode = false;
}
if(config.check("depthVideoMode", "Depth video mode (default=0)")) {
dMode = config.find("depthVideoMode").asInt();
} else {
dMode = 0;
}
if(config.check("colorVideoMode", "Color video mode (default=0)")) {
cMode = config.find("colorVideoMode").asInt();
} else {
cMode = 0;
}
if(config.check("playback", "Play from .oni file")) {
oniPlayback = true;
fileDevice = config.find("playback").asString();
} else {
oniPlayback = false;
}
if(config.check("record", "Record to .oni file")) {
oniRecord = true;
oniOutputFile = config.find("record").asString();
} else {
oniRecord = false;
}
if(config.check("name", "Name for the port prefix (default=/OpenNI2)")) {
portPrefix = config.find("name").asString();
withOpenPorts = true;
openPorts(portPrefix, userTracking, colorON);
} else {
portPrefix = "/OpenNI2";
withOpenPorts = true;
openPorts(portPrefix, userTracking, colorON);
}
if (config.check("minConfidence", "Set minimum confidence (default=0.6)")) {
mConf = config.find("minConfidence").asDouble();
} else {
mConf = MINIMUM_CONFIDENCE;
}
if (config.check("loop", "Set playback to loop")) {
loop = true;
} else {
loop = false;
}
if (config.check("syncFrames", "Synchronize frames")) {
frameSync = true;
} else {
frameSync = false;
}
if (config.check("imageRegistration", "Register Images")) {
imageRegistration = true;
} else {
imageRegistration = false;
}
skeleton = new OpenNI2SkeletonTracker(userTracking, colorON, rgbMirrorON, depthMirrorON, mConf, oniPlayback, fileDevice, oniRecord, oniOutputFile, loop, frameSync, imageRegistration, printMode, dMode, cMode);
if (skeleton->getDeviceStatus() == 0) {
cout << "OpenNI2 Yarp Device started." << endl;
return true;
}
else if (skeleton->getDeviceStatus()!= 0) {
cout << "***ERROR*** Device could not be initialized." << endl;
close();
return false;
}
return true;
}
bool homeostaticModule::respond(const Bottle& cmd, Bottle& reply)
{
if (cmd.get(0).asString() == "help" )
{ string help = "\n";
help += " ['par'] ['drive'] ['val'/'min'/'max'/'dec'] [value] : Assigns a value to a specific parameter \n";
help += " ['delta'] ['drive'] ['val'/'min'/'max'/'dec'] [value] : Adds a value to a specific parameter \n";
help += " ['add'] ['conf'] [drive Bottle] : Adds a drive to the manager as a drive directly read from conf-file \n";
help += " ['add'] ['botl'] [drive Bottle] : Adds a drive to the manager as a Bottle of values of shape \n";
help += " ['add'] ['new'] [drive name] : Adds a default drive to the manager \n";
help += " ['rm'] [drive name] : removes a drive from the manager \n";
help += " ['names'] : returns an ordered list of the drives in the manager \n";
help += " : (string name, double value, double homeo_min, double homeo_max, double decay = 0.05, bool gradient = true) \n";
reply.addString(help);
/*cout << " [par] [drive] [val/min/max/dec] [value] : Assigns a value to a specific parameter \n"<<
" [delta] [drive] [val/min/max/dec] [value] : Adds a value to a specific parameter \n"<<
" [add] [conf] [drive Bottle] : Adds a drive to the manager as a drive directly read from conf-file \n"<<
" [add] [botl] [drive Bottle] : Adds a drive to the manager as a Bottle of values of shape \n"<<
" : (string name, double value, double homeo_min, double homeo_max, double decay = 0.05, bool gradient = true) \n"<<endl;
*/
}
else if (cmd.get(0).asString() == "par" )
{
for (unsigned int d = 0; d<manager.drives.size();d++)
{
if (cmd.get(1).asString() == manager.drives[d]->name)
{
if (cmd.get(2).asString()=="val")
{
manager.drives[d]->setValue(cmd.get(3).asDouble());
}
else if (cmd.get(2).asString()=="min")
{
manager.drives[d]->setHomeostasisMin(cmd.get(3).asDouble());
}
else if (cmd.get(2).asString()=="max")
{
manager.drives[d]->setHomeostasisMax(cmd.get(3).asDouble());
}
else if (cmd.get(2).asString()=="dec")
{
manager.drives[d]->setDecay(cmd.get(3).asDouble());
}
else
{
reply.addString("Format is: \n - ['par'] [drive_name] [val/min/max/dec] [value]");
}
reply.addString("ack");
cout<<"Received a order"<<endl;
}
}
reply.addString("nack: wrong drive name");
}
else if (cmd.get(0).asString() == "delta" )
{
for (unsigned int d = 0; d<manager.drives.size();d++)
{
if (cmd.get(1).asString() == manager.drives[d]->name)
{
if (cmd.get(2).asString()=="val")
{
manager.drives[d]->deltaValue(cmd.get(3).asDouble());
}
else if (cmd.get(2)=="min")
{
manager.drives[d]->deltaHomeostasisMin(cmd.get(3).asDouble());
}
else if (cmd.get(2)=="max")
{
manager.drives[d]->deltaHomeostasisMax(cmd.get(3).asDouble());
}
else if (cmd.get(2)=="dec")
{
manager.drives[d]->deltaDecay(cmd.get(3).asDouble());
}
else
{
reply.addString("nack");
cout << "Format is: \n - ['par'] [drive_name] [val/min/max/dec] [value]"<<endl;
}
reply.addString("ack");
cout<<"Received a order"<<endl;
}
}
}
else if (cmd.get(0).asString()=="add")
{
if (cmd.get(1).asString()=="conf")
{
Bottle *ga = cmd.get(2).asList();
// cout <<"HOMEOSTATIC"<< ga->toString() << endl;
Bottle grpAllostatic = ga->findGroup("ALLOSTATIC");
cout <<"HOMEOSTATIC"<< grpAllostatic.toString() << endl;
cout << "Bottle allo check: " << grpAllostatic.find("test-decay").asDouble() << endl;
addNewDrive(cmd.get(2).check("name",yarp::os::Value("")).asString(), grpAllostatic);
reply.addString("add drive from config bottle: ack");
}
else if (cmd.get(1).asString()=="botl")
{
Drive d = bDrive(cmd.get(2).asList());
manager.addDrive(&d);
openPorts(d.name);
reply.addString("add drive from bottle: ack");
}
else if (cmd.get(1).asString()=="new")
{
string d_name = cmd.get(2).asString();
bool b = addNewDrive(d_name);
if (b)
reply.addString("add new drive: ack");
else
reply.addString("ack. drive not created");
}
else
{
reply.addString("nack");
}
}
else if (cmd.get(0).asString()=="rm")
{
for (unsigned int d = 0; d<manager.drives.size();d++)
{
if (cmd.get(1).asString() == manager.drives[d]->name)
{
bool b = removeDrive(d);
if (b)
reply.addString("ack: Successfully removed");
else
reply.addString("ack: Could not remove the drive");
}
}
reply.addString("nack");
}
else if (cmd.get(0).asString()=="names")
{
Bottle nms;
nms.clear();
for (unsigned int i=0;i<manager.drives.size();i++)
{
nms.addString(manager.drives[i]->name);
}
reply.addList()=nms;
}
else
{
reply.addString("nack");
}
return true;
}
void AllostaticController::configureAllostatic(yarp::os::ResourceFinder &rf)
{
//The homeostatic module should be running in parallel, independent from this, so the objective of
//this config would be to have a proper list and connect to each port
homeo_name = "homeostasis";
string homeo_rpc_name = "/" + homeo_name + "/rpc";
string to_h_rpc_name="/"+moduleName+"/toHomeoRPC:o";
to_homeo_rpc.open(to_h_rpc_name);
while(!Network::connect(to_h_rpc_name,homeo_rpc_name))
{
yDebug()<<"Trying to connect to homeostasis...";
yDebug() << "from " << to_h_rpc_name << " to " << homeo_rpc_name;
yarp::os::Time::delay(0.2);
}
yInfo() << "Initializing drives...";//<<endl;
Bottle grpAllostatic = rf.findGroup("ALLOSTATIC");
drivesList = *grpAllostatic.find("drives").asList();
Bottle cmd;
priority_sum = 0.;
double priority;
for (int d = 0; d<drivesList.size(); d++)
{
cmd.clear();
string driveName = drivesList.get(d).asString();
yInfo() << ("Initializing drive " + driveName);
cmd.addString("add");
cmd.addString("conf");
Bottle drv;
drv.clear();
Bottle aux;
aux.clear();
aux.addString("name");
aux.addString(driveName);
drv.addList()=aux;
aux.clear();
drv.addList()=grpAllostatic;
cmd.addList()=drv;
Bottle rply;
rply.clear();
rply.get(0).asString();
to_homeo_rpc.write(cmd,rply);
AllostaticDrive alloDrive;
alloDrive.name = driveName;
Value cmds = grpAllostatic.check((driveName + "-sensation-on"), Value("None"));
alloDrive.sensationOnCmd = *cmds.asList();
cmds = grpAllostatic.check((driveName + "-sensation-off"), Value("None"));
alloDrive.sensationOffCmd = *cmds.asList();
cmds = grpAllostatic.check((driveName + "-before-trigger"), Value("None"));
if (!(cmds.isString() && cmds.asString() == "None")) {
alloDrive.beforeTriggerCmd = *cmds.asList();
}
cmds = grpAllostatic.check((driveName + "-after-trigger"), Value("None"));
if (!(cmds.isString() && cmds.asString() == "None")) {
alloDrive.afterTriggerCmd = *cmds.asList();
}
alloDrive.homeoPort = &to_homeo_rpc;
alloDrive.inputSensationPort = new BufferedPort<Bottle>;
string portName = "/" + moduleName + "/" + driveName + "/sensation:i";
alloDrive.inputSensationPort->open(portName);
openPorts(driveName);
string sensationPort = grpAllostatic.check((driveName + "-sensation-port"), Value("None")).asString();
string pn = "/" + moduleName + "/" + driveName + "/sensation:i";
while(!Network::connect(sensationPort, pn)) {
yDebug()<<"Connecting " << sensationPort << " to " << pn;
yarp::os::Time::delay(0.5);
}
// set drive priorities. Default to 1.
priority = grpAllostatic.check((driveName + "-priority"), Value(1.)).asDouble();
priority_sum += priority;
drivePriorities.push_back(priority);
//Under effects
string under_port_name = grpAllostatic.check((driveName + "-under-behavior-port"), Value("None")).asString();
string under_cmd_name = grpAllostatic.check((driveName + "-under-behavior"), Value("None")).asString();
bool active = false;
if (under_port_name != "None" && under_cmd_name != "None")
{
active = true;
string out_port_name = "/" + moduleName + "/" + driveName + "/under_action:o";
alloDrive.behaviorUnderPort = new Port();
alloDrive.behaviorUnderPort->open(out_port_name);
yDebug() << "trying to connect to" << under_port_name;
while(!Network::connect(out_port_name,under_port_name))
{
yarp::os::Time::delay(0.5);
}
alloDrive.behaviorUnderCmd = Bottle(under_cmd_name);
}else{
yInfo() << "No port name for" << driveName << "under-behavior-port";
}
//Over effects
string over_port_name = grpAllostatic.check((driveName + "-over-behavior-port"), Value("None")).asString();
string over_cmd_name = grpAllostatic.check((driveName + "-over-behavior"), Value("None")).asString();
if (over_port_name != "None" && over_cmd_name != "None")
{
active=true;
string out_port_name = "/" + moduleName + "/" + driveName + "/over_action:o";
alloDrive.behaviorOverPort = new Port();
alloDrive.behaviorOverPort->open(out_port_name);
yDebug() << "trying to connect to" << over_port_name;
while(!Network::connect(out_port_name, over_port_name))
{
yarp::os::Time::delay(0.5);
}
alloDrive.behaviorOverCmd = Bottle(over_cmd_name);
} else {
yInfo() << "No port name for" << driveName << "over-behavior-port";
}
alloDrive.active = active;
allostaticDrives[driveName] = alloDrive;
}
if (! Normalize(drivePriorities))
yDebug() << "Error: Drive priorities sum up to 0.";
yInfo() << "done.";
}
