void Eye2world::WorkerThread::run() {
Vector head6d(6);
Vector torso3d(3);
double prevZOffset = 0.0;
#ifndef DEBUG_OFFLINE
bool positionUpdated = false;
bool transformationAvailable = false;
BufferedPort < Bottle > *in = (BufferedPort<Bottle>*) dataPorts[id.Input_Coordinates2d];
BufferedPort < Bottle > *head = (BufferedPort<Bottle>*) dataPorts[id.Input_HeadPosition];
BufferedPort < Bottle > *torso = (BufferedPort<Bottle>*) dataPorts[id.Input_TorsoPosition];
#endif
BufferedPort < Bottle > *out = (BufferedPort<Bottle>*) dataPorts[id.Output_Coordinates3d];
while (!isStopping()) {
string camera = "left";
Vector object2d(2);
double zOffset = 0;
#ifdef DEBUG_OFFLINE
yarp::os::Time::delay(1);
#else
inputCoordinates = in->read(true);
if (isStopping() || inputCoordinates == NULL) {
break;
}
unsigned int coordinatesPos = 0;
if (inputCoordinates->get(0).isString()) {
camera = inputCoordinates->get(0).asString().c_str();
// optional! default: 0.0
coordinatesPos = 1;
} else {
// Assume (x,y) pair!
// This is for testing with YARP viewer click port.
}
double motor2eye = moduleOptions["motor2eye"].getValue().asDouble();
for (unsigned int i = 0; i < 2; i++) {
object2d[i] = inputCoordinates->get(coordinatesPos + i).asDouble();
}
zOffset = moduleOptions["heightOffset"].getValue().asDouble();
zOffset += motor2eye;
zOffset += inputCoordinates->get(coordinatesPos + 2).asDouble();
positionUpdated = zOffset != prevZOffset;
Bottle* headPosition = head->read(!transformationAvailable);
if (headPosition != NULL && headPosition->size() >= 6) {
//get data and convert from degrees to radiant
for (unsigned int i = 0; i < 6; i++) {
head6d[i] = headPosition->get(i).asDouble() * M_PI / 180.0;
}
positionUpdated = true;
}
if (isStopping()) {
break;
}
Bottle* torsoPosition = torso->read(!transformationAvailable);
if (torsoPosition != NULL && torsoPosition->size() >= 3) {
//get data and convert from degrees to radiant
for (unsigned int i = 0; i < 3; i++) {
torso3d[i] = torsoPosition->get(i).asDouble() * M_PI / 180.0;
}
positionUpdated = true;
}
if (isStopping()) {
break;
}
if (positionUpdated || !transformationAvailable) { // The 2nd one is just to be sure ;)
projections[camera]->setHeightOffset(zOffset, false);
projections[camera]->setBaseTransformation(torso3d, head6d);
transformationAvailable = true;
}
#endif
Vector object3d(3);
projections[camera]->project(object2d, object3d);
// Add scalor to the result
double scale = moduleOptions["scale"].getValue().asDouble();
object3d[0] = object3d[0] * scale;
object3d[1] = object3d[1] * scale;
object3d[2] = object3d[2] - motor2eye;
Bottle &outputCoordinates = out->prepare();
outputCoordinates.clear();
for (unsigned int i = 0; i < 3; i++) {
outputCoordinates.addDouble(object3d[i]);
}
cout << "out = " << outputCoordinates.toString() << endl;
out->write();
}
}
bool icubFinger::calibrateIndexMiddle(){
// Open the finger
Finger::open();
// Set the proximal to 0 and distal to 180
setAngle(_proximalJointIndex, 0, 30);
setAngle(_distalJointIndex, 180, 30);
while(!checkMotionDone()){
;
}
// TODO: remove this for the simulation. There is not such data
// available during simulation
Bottle *fingerEnc;
for(int i = 0; i <= _fingerEncoders->getPendingReads(); i++)
fingerEnc = _fingerEncoders->read();
if(!fingerEnc->isNull())
{
_maxProximal = fingerEnc->get(_proximalEncoderIndex).asDouble();
_minMiddle = fingerEnc->get(_middleEncoderIndex).asDouble();
_minDistal = fingerEnc->get(_distalEncoderIndex).asDouble();
}
setAngle(_distalJointIndex, 0, 30);
while(!checkMotionDone()){
;
}
setAngle(_proximalJointIndex, 90, 30);
while(!checkMotionDone()){
;
}
// TODO: remove this for the simulation. There is not such data
// available during simulation
for(int i = 0; i < _fingerEncoders->getPendingReads(); i++)
fingerEnc = _fingerEncoders->read();
if(!fingerEnc->isNull())
{
_minProximal = fingerEnc->get(_proximalEncoderIndex).asDouble();
_maxMiddle = fingerEnc->get(_middleEncoderIndex).asDouble();
_maxDistal = fingerEnc->get(_distalEncoderIndex).asDouble();
}
setAngle(_proximalJointIndex, 0);
while(!checkMotionDone()){
;
}
checkMinMax(_minDistal, _maxDistal);
checkMinMax(_minMiddle, _minMiddle);
checkMinMax(_minProximal, _minProximal);
cout << "Calibration mins:" <<
_minProximal << "\t" <<
_minMiddle << "\t" <<
_minDistal << "\t" << endl;
cout << "Calibration maxes:" <<
_maxProximal << "\t" <<
_maxMiddle << "\t" <<
_maxDistal << "\t" << endl;
}
/**
Add an other interaction in the adjKnowledge (event adj is not this one, this is used for a comparaison)
bInput format:
(action sAction_name)
(timing timing)
(coordinate X Y dX dY)
*/
void adjKnowledge::addOtherInteraction(Bottle bInput)
{
string sAction = bInput.check("action", Value("none")).asString();
// vdNoGnlTiming.push_back(bInput.check("timing", Value(0.)).asDouble());
// mActionTiming[sAction].second.push_back(bInput.check("timing", Value(0.)).asDouble());
}
/*
* Dump in the port DumperPort the human skeleton, and the object of the OPC: sObjectToDump
*
*/
void humanRobotDump::DumpHumanObject()
{
Agent* ag = iCub->opc->addOrRetrieveEntity<Agent>(sAgentName);
Bottle bDump;
bDump.addString(sActionName);
bDump.addString(sObjectToDump);
bDump.addList() = ag->m_body.asBottle();
iCub->opc->checkout();
list<Entity*> lEntity = iCub->opc->EntitiesCacheCopy();
for (list<Entity*>::iterator itEnt = lEntity.begin(); itEnt != lEntity.end(); itEnt++)
{
if ((*itEnt)->entity_type() == EFAA_OPC_ENTITY_AGENT ||
(*itEnt)->entity_type() == EFAA_OPC_ENTITY_OBJECT ||
(*itEnt)->entity_type() == EFAA_OPC_ENTITY_RTOBJECT)
{
if ((*itEnt)->name() != "icub")
{
Object* ob = iCub->opc->addOrRetrieveEntity<Object>((*itEnt)->name());
Bottle bObject;
bObject.addString(ob->name());
bObject.addDouble(ob->m_ego_position[0]);
bObject.addDouble(ob->m_ego_position[1]);
bObject.addDouble(ob->m_ego_position[2]);
bObject.addInt(ob->m_present);
bDump.addList() = bObject;
}
}
}
bDump.addInt(m_iterator);
DumperPort.write(bDump);
}
virtual void onRead(Bottle& b) {
// process data in b
printf("Got %s\n", b.toString().c_str());
}
bool InterpersonalDistanceRegulator::respond(const Bottle& cmd, Bottle& reply)
{
reply.addString("NACK");
return true;
}
bool AgentDetector::updateModule()
{
LockGuard lg(m);
bool isRefreshed = client.getDepthAndPlayers(depth,players);
client.getRgb(rgb);
bool tracked;
if (handleMultiplePlayers)
tracked=client.getJoints(joints);
else
tracked=client.getJoints(joint, EFAA_KINECT_CLOSEST_PLAYER);
//cout<<"Tracking value = "<<tracked<<endl;
if (tracked)
{
if (handleMultiplePlayers)
client.getSkeletonImage(joints,skeletonImage);
else
{
client.getSkeletonImage(joint,skeletonImage);
joints.clear();
joints.push_back(joint);
}
}
client.getPlayersImage(players,playersImage);
client.getDepthImage(depth,depthToDisplay);
if (depthPort.getOutputCount()>0)
{
depthPort.prepare()=depthToDisplay;
depthPort.write();
}
if (imagePort.getOutputCount()>0)
{
imagePort.prepare()=rgb;
imagePort.write();
}
if (playersPort.getOutputCount()>0)
{
playersPort.prepare()=playersImage;
playersPort.write();
}
if (skeletonPort.getOutputCount()>0)
{
skeletonPort.prepare()=skeletonImage;
skeletonPort.write();
}
if (showImages)
{
cvConvertScale((IplImage*)depthToDisplay.getIplImage(),depthTmp,1.0/255);
cvCvtColor((IplImage*)rgb.getIplImage(),rgbTmp,CV_BGR2RGB);
string mode=showMode;
string submode;
while (!mode.empty())
{
if (showImageParser(mode,submode))
{
if (submode=="rgb")
cvShowImage("rgb",rgbTmp);
else if (submode=="depth")
cvShowImage("depth",depthTmp);
else if (submode=="skeleton")
cvShowImage("skeleton",(IplImage*)skeletonImage.getIplImage());
else if (submode=="players")
cvShowImage("players",(IplImage*)playersImage.getIplImage());
else
yError("unrecognized show mode!");
}
}
cvWaitKey(1);
}
//Send the players information to the OPC
bool localIsCalibrated=checkCalibration();
//Allow click calibration
if (!localIsCalibrated)
{
if (AgentDetector::clicked==clicked_left)
{
AgentDetector::clicked=idle;
//Get the clicked point coordinate in Kinect space
Vector clickedPoint(3);
cout<<"Processing a click on ("<<AgentDetector::clickX<<" "<<AgentDetector::clickY<<") --> ";
client.get3DPoint((int)AgentDetector::clickX,(int)AgentDetector::clickY,clickedPoint);
cout<<clickedPoint.toString(3,3)<<endl;
Bottle bCond;
Bottle bObject;
Bottle bRTObject;
bObject.addString(EFAA_OPC_ENTITY_TAG);
bObject.addString("==");
bObject.addString(EFAA_OPC_ENTITY_OBJECT);
bRTObject.addString(EFAA_OPC_ENTITY_TAG);
bRTObject.addString("==");
bRTObject.addString(EFAA_OPC_ENTITY_RTOBJECT);
Bottle bPresent;
bPresent.addString(EFAA_OPC_OBJECT_PRESENT_TAG);
bPresent.addString("==");
bPresent.addInt(1);
bCond.addList()=bObject;
bCond.addString("&&");
bCond.addList()=bPresent;
bCond.addString("||");
bCond.addList()=bRTObject;
bCond.addString("&&");
bCond.addList()=bPresent;
opc->checkout();
opc->isVerbose=true;
list<Entity*> presentObjects=opc->Entities(bCond);
opc->isVerbose=false;
if (presentObjects.size()==1)
{
Object* o=(Object*)(presentObjects.front());
Bottle botRPH, botRPHRep;
botRPH.addString("add");
botRPH.addString("kinect");
Bottle &cooKinect=botRPH.addList();
cooKinect.addDouble(clickedPoint[0]);
cooKinect.addDouble(clickedPoint[1]);
cooKinect.addDouble(clickedPoint[2]);
Bottle &cooiCub=botRPH.addList();
cooiCub.addDouble(o->m_ego_position[0]);
cooiCub.addDouble(o->m_ego_position[1]);
cooiCub.addDouble(o->m_ego_position[2]);
rfh.write(botRPH,botRPHRep);
cout<<"Sent to RFH: "<<botRPH.toString().c_str()<<endl;
cout<<"Got from RFH: "<<botRPHRep.toString().c_str()<<endl;
pointsCnt++;
}
else
yWarning("There should be 1 and only 1 object on the table");
}
else if (AgentDetector::clicked==clicked_right)
{
AgentDetector::clicked=idle;
if (pointsCnt>=3)
{
Bottle calibBottle,calibReply;
calibBottle.addString("cal");
calibBottle.addString("kinect");
rfh.write(calibBottle,calibReply);
cout<<"Calibrated ! "<<calibReply.toString().c_str()<<endl;
calibBottle.clear();
calibBottle.addString("save");
rfh.write(calibBottle,calibReply);
cout<<"Saved to file ! "<<calibReply.toString().c_str()<<endl;
checkCalibration();
}
else
yWarning("Unable to calibrate with less than 3 points pairs collected");
}
}
if (isRefreshed)
{
// yInfo() << " refreshed";
//////////////////////////////////////////////////////////////////
//Clear the previous agents
//for(map<int, Agent*>::iterator pA=identities.begin(); pA!=identities.end() ; pA++)
//{
// pA->second->m_present = false;
//}
//partner->m_present = false;
// check if last apparition was more than dThreshlodDisaparition ago
if (tracked)
{
//Go through all skeletons
for(deque<Player>::iterator p=joints.begin(); p!=joints.end(); p++)
{
//check if this skeletton is really tracked
bool reallyTracked = false;
for(map<string,Joint>::iterator jnt = p->skeleton.begin() ; jnt != p->skeleton.end() ; jnt++)
{
if (jnt->second.x != 0 && jnt->second.y != 0 && jnt->second.z != 0)
{
reallyTracked = true; break;
}
}
if (reallyTracked)
{
dSince = (clock() - dTimingLastApparition) / (double) CLOCKS_PER_SEC;
//yInfo() << " is REALLY tracked";
string playerName = partner_default_name;
//If the skeleton is tracked we dont identify
if (identities.find(p->ID) != identities.end())
{
playerName = identities[p->ID];
}
else
{
//Check if we should learn this face
if (currentTrainingFace != "")
{
setIdentity(*p,currentTrainingFace);
currentTrainingFace = "";
}
//if (useFaceRecognition)
playerName = getIdentity(*p);
}
//We interact with OPC only if the calibration is done
if (localIsCalibrated)
{
//Retrieve this player in OPC or create if does not exist
opc->checkout();
partner = opc->addOrRetrieveEntity<Agent>(partner_default_name);
partner->m_present = true;
// reset the timing.
dTimingLastApparition = clock();
if (identities.find(p->ID) == identities.end())
{
cout<<"Assigning name "<<playerName<<" to skeleton "<<p->ID<<endl;
//Agent* specificAgent = opc->addEntity<Agent>(playerName);
Agent* specificAgent = opc->addOrRetrieveEntity<Agent>(playerName);
if(specificAgent == nullptr) {
yError() << "SHIT specificAgent";
} else {
identities[p->ID] = specificAgent->name();
specificAgent->m_present = true;
yInfo() << " specific agent is commited";
opc->commit(specificAgent);
yInfo() << " specific agent is commited done";
}
}
// Relation r(partner->name(),"named",playerName);
// opc->addRelation(r,1.0);
// cout<<"Commiting : "<<r.toString()<<endl;
yarp::os::Bottle &skeleton = outputSkeletonPort.prepare();
skeleton.clear();
//Convert the skeleton into efaaHelpers body. We loose orientation in the process...
for(map<string,Joint>::iterator jnt = p->skeleton.begin() ; jnt != p->skeleton.end() ; jnt++)
{
Vector kPosition(4);
kPosition[0] = jnt->second.x;
kPosition[1] = jnt->second.y;
kPosition[2] = jnt->second.z;
kPosition[3] = 1;
Vector icubPos = kinect2icub * kPosition;
Vector irPos = icubPos.subVector(0,2);
if (isMounted)
{
irPos = transform2IR(irPos);
Bottle jntBtl;
jntBtl.clear();
jntBtl.addString(jnt->first);
jntBtl.addDouble(jnt->second.x);
jntBtl.addDouble(jnt->second.y);
jntBtl.addDouble(jnt->second.z);
skeleton.addList() = jntBtl;
}
if (jnt->first == EFAA_OPC_BODY_PART_TYPE_HEAD)
{
partner->m_ego_position = irPos;
}
partner->m_body.m_parts[jnt->first] = irPos;
}
opc->commit(partner);
// cout << skeleton.toString()<< endl;
outputSkeletonPort.write();
//opc->commit(agent);
}
// cout<<'1'<<endl;
}
}
}
else
{
if (dSince > dThresholdDisparition)
{
opc->checkout();
partner = opc->addOrRetrieveEntity<Agent>(partner_default_name);
partner->m_present = false;
opc->commit(partner);
}
else
{
//yInfo() << " clock is: " << clock() << "\t last apparition: " << dTimingLastApparition << "\t dSince: " << dSince;
//yInfo() << " agent dissapeared but not for too long.";
}
}
}
return true;
}
virtual bool read(ConnectionReader& reader) {
YTRACE("NameServer::read start");
ConstString ref = "NAME_SERVER ";
bool ok = true;
ConstString msg = "?";
bool haveMessage = false;
if (ok) {
if (reader.isTextMode()) {
msg = reader.expectText().c_str();
} else {
// migrate to binary mode support, eventually optimize
Bottle b;
b.read(reader);
msg = b.toString().c_str();
}
haveMessage = (msg!="");
msg = ref + msg;
}
if (reader.isActive()&&haveMessage) {
YARP_DEBUG(Logger::get(),ConstString("name server got message ") + msg);
size_t index = msg.find("NAME_SERVER");
if (index==0) {
Contact remote = reader.getRemoteContact();
YARP_DEBUG(Logger::get(),
ConstString("name server receiving from ") +
remote.toURI());
YARP_DEBUG(Logger::get(),
ConstString("name server request is ") + msg);
ConstString result = server->apply(msg,remote);
ConnectionWriter *os = reader.getWriter();
if (os!=YARP_NULLPTR) {
if (result=="") {
result = ns_terminate(ConstString("unknown command ") +
msg + "\n");
}
// This change is just to make Microsoft Telnet happy
ConstString tmp;
for (unsigned int i=0; i<result.length(); i++) {
if (result[i]=='\n') {
tmp += '\r';
}
tmp += result[i];
}
tmp += '\r';
os->appendString(tmp.c_str(),'\n');
YARP_DEBUG(Logger::get(),
ConstString("name server reply is ") + result);
ConstString resultSparse = result;
size_t end = resultSparse.find("\n*** end of message");
if (end!=ConstString::npos) {
resultSparse[end] = '\0';
}
YARP_INFO(Logger::get(),resultSparse);
}
} else {
YARP_INFO(Logger::get(),
ConstString("Name server ignoring unknown command: ")+msg);
ConnectionWriter *os = reader.getWriter();
if (os!=YARP_NULLPTR) {
// this result is necessary for YARP1 support
os->appendString("???????????????????????????????????????",'\n');
//os->flush();
//os->close();
}
}
}
YTRACE("NameServer::read stop");
return true;
}
Contact NameServer::registerName(const ConstString& name,
const Contact& address,
const ConstString& remote) {
bool reusablePort = false;
bool reusableIp = false;
//YARP_DEBUG(Logger::get(),"in registerName...");
if (name!="...") {
unregisterName(name);
}
Contact suggestion = address;
if (!suggestion.isValid()) {
suggestion = Contact(name, "...", "...", 0);
}
ConstString portName = name;
if (portName == "...") {
portName = tmpNames.get();
}
ConstString carrier = suggestion.getCarrier();
if (carrier == "...") {
carrier = "tcp";
}
ConstString machine = suggestion.getHost();
int overridePort = 0;
if (machine == "...") {
if (carrier!="mcast") {
if (remote=="...") {
YARP_ERROR(Logger::get(),"remote machine name was not found! can only guess it is local...");
machine = "127.0.0.1";
} else {
machine = remote;
}
} else {
machine = mcastRecord.get();
overridePort = mcastRecord.lastPortNumber();
reusableIp = true;
}
}
int port = suggestion.getPort();
if (port == 0) {
if (overridePort) {
port = overridePort;
} else {
port = getHostRecord(machine).get();
reusablePort = true;
}
}
suggestion = Contact(portName, carrier, machine, port);
YARP_DEBUG(Logger::get(),ConstString("Registering ") +
suggestion.toURI() + " for " + suggestion.getRegName());
NameRecord& nameRecord = getNameRecord(suggestion.getRegName());
nameRecord.setAddress(suggestion,reusablePort,reusableIp);
Bottle event;
event.addVocab(Vocab::encode("add"));
event.addString(suggestion.getRegName().c_str());
onEvent(event);
return nameRecord.getAddress();
}
yarp::os::Bottle NameServer::ncmdVersion(int argc, char *argv[]) {
Bottle response;
response.addString("version");
response.addString(Companion::version().c_str());
return response;
}
Bottle NameServer::botify(const Contact& address) {
Bottle result;
if (address.isValid()) {
Bottle bname;
bname.addString("name");
bname.addString(address.getRegName().c_str());
Bottle bip;
bip.addString("ip");
bip.addString(address.getHost().c_str());
Bottle bnum;
bnum.addString("port_number");
bnum.addInt(address.getPort());
Bottle bcarrier;
bcarrier.addString("carrier");
bcarrier.addString(address.getCarrier().c_str());
result.addString("port");
result.addList() = bname;
result.addList() = bip;
result.addList() = bnum;
result.addList() = bcarrier;
} else {
Bottle bstate;
bstate.addString("error");
bstate.addInt(-2);
bstate.addString("port not known");
result.addString("port");
result.addList() = bstate;
}
return result;
}
bool ObjectReconstr::respond(const Bottle& command, Bottle& reply)
{
if (command.get(0).asString()=="help")
{
reply.addVocab(ACK);
reply.addString("To obtain a reconstruction, the module needs a segmentation algorithm that returns the set of pixels belonging to the object");
reply.addString("It typically uses the graphBasedSegmentation module");
reply.addString("Provide a point of the object first, in the format x y, for instance clicking on the segmentation image.");
reply.addString("Then, when you send the command 3Drec, you will be provided with the reconstructed object along with the minimum enclosing bounding box on the output port of the module -- typically /object-reconstruction/mesh:o.");
reply.addString("If you also want to visualize the result, the command is 3Drec visualize.");
reply.addString("If you want to reconstruct a single pixel, the command is get point (x y).");
return true;
}
if (command.get(0).asString()=="set")
{
if (command.get(1).asString()=="write")
{
if (command.size()>2)
{
if (command.get(2).asString()=="on")
{
reply.addVocab(ACK);
write=true;
}
else if (command.get(2).asString()=="off")
{
write=false;
reply.addVocab(ACK);
}
else
reply.addVocab(NACK);
return true;
}
}
else
{
reply.addVocab(NACK);
return true;
}
}
if (command.get(0).asString()=="3Drec")
{
if (middlex==-1 || middley==-1)
{
reply.addVocab(NACK);
reply.addString("I need a pixel of the object");
return true;
}
currentState=STATE_RECONSTRUCT;
visualizationOn=false;
if (command.size()==2)
if (command.get(1).asString()=="visualize")
visualizationOn=true;
reply.addVocab(ACK);
return true;
}
if (command.get(0).asString()=="name")
{
if (command.size()>=2)
{
fileName=command.get(1).asString().c_str();
reply.addVocab(ACK);
}
else
{
reply.addVocab(NACK);
reply.addString("No name was provided");
}
return true;
}
if (command.get(0).asString()=="get")
{
if (command.get(1).asString()=="point" && command.size()==3)
{
if (currentState!=STATE_RECONSTRUCT)
{
IplImage* imgL;
IplImage* imgR;
ImageOf<PixelRgb> *tmpL = imagePortInLeft.read(true);
ImageOf<PixelRgb> *tmpR = imagePortInRight.read(true);
if(tmpL!=NULL && tmpR!=NULL)
{
imgL= (IplImage*) tmpL->getIplImage();
imgR= (IplImage*) tmpR->getIplImage();
}
else
{
reply.addVocab(NACK);
return true;
}
yarp::sig::Vector point2D(2);
Bottle *pixel=command.get(2).asList();
point2D[0]=pixel->get(0).asDouble();
point2D[1]=pixel->get(1).asDouble();
yarp::sig::Vector point3D(3);
bool done=recRoutine.triangulateSinglePoint(imgL,imgR,point2D,point3D);
if (done)
{
Bottle &result=reply.addList();
result.addDouble(point3D[0]);
result.addDouble(point3D[1]);
result.addDouble(point3D[2]);
}
else
reply.addVocab(NACK);
}
else
{
reply.addVocab(NACK);
reply.addString("Still processing");
}
return true;
}
else
{
reply.addVocab(NACK);
return true;
}
}
if (command.size()==2)
{
if (command.get(0).asInt()!=0 && command.get(1).asInt()!=0)
{
middlex=(double)command.get(0).asInt();
middley=(double)command.get(1).asInt();
reply.addVocab(ACK);
return true;
}
else
{
reply.addVocab(NACK);
return true;
}
}
reply.addVocab(NACK);
return true;
}
bool RpLidar2::open(yarp::os::Searchable& config)
{
string serial;
int baudrate;
u_result result;
m_device_status = DEVICE_OK_STANBY;
m_min_distance = 0.1; //m
m_max_distance = 2.5; //m
m_pwm_val = 0;
bool br = config.check("GENERAL");
if (br != false)
{
yarp::os::Searchable& general_config = config.findGroup("GENERAL");
m_clip_max_enable = general_config.check("clip_max");
m_clip_min_enable = general_config.check("clip_min");
if (m_clip_max_enable) { m_max_distance = general_config.find("clip_max").asFloat64(); }
if (m_clip_min_enable) { m_min_distance = general_config.find("clip_min").asFloat64(); }
if (general_config.check("max_angle") == false) { yError() << "Missing max_angle param in GENERAL group"; return false; }
if (general_config.check("min_angle") == false) { yError() << "Missing min_angle param in GENERAL group"; return false; }
if (general_config.check("resolution") == false) { yError() << "Missing resolution param in GENERAL group"; return false; }
if (general_config.check("serial_port") == false) { yError() << "Missing serial_port param in GENERAL group"; return false; }
if (general_config.check("serial_baudrate") == false) { yError() << "Missing serial_baudrate param in GENERAL group"; return false; }
if (general_config.check("sample_buffer_life") == false) { yError() << "Missing sample_buffer_life param in GENERAL group"; return false; }
baudrate = general_config.find("serial_baudrate").asInt32();
serial = general_config.find("serial_port").asString();
m_max_angle = general_config.find("max_angle").asFloat64();
m_min_angle = general_config.find("min_angle").asFloat64();
m_resolution = general_config.find("resolution").asFloat64();
m_buffer_life = general_config.find("sample_buffer_life").asInt32();
m_do_not_clip_infinity_enable = (general_config.find("allow_infinity").asInt32()!=0);
if (general_config.check("motor_pwm"))
{
m_pwm_val = general_config.find("motor_pwm").asInt32();
}
if (general_config.check("thread_period"))
{
double thread_period = general_config.find("thread_period").asInt32() / 1000.0;
this->setPeriod(thread_period);
}
}
else
{
yError() << "Missing GENERAL section";
return false;
}
bool bs = config.check("SKIP");
if (bs != false)
{
yarp::os::Searchable& skip_config = config.findGroup("SKIP");
Bottle mins = skip_config.findGroup("min");
Bottle maxs = skip_config.findGroup("max");
size_t s_mins = mins.size();
size_t s_maxs = mins.size();
if (s_mins == s_maxs && s_maxs > 1 )
{
for (size_t s = 1; s < s_maxs; s++)
{
Range_t range;
range.max = maxs.get(s).asFloat64();
range.min = mins.get(s).asFloat64();
if (range.max >= 0 && range.max <= 360 &&
range.min >= 0 && range.min <= 360 &&
range.max > range.min)
{
m_range_skip_vector.push_back(range);
}
else
{
yError() << "Invalid range in SKIP section";
return false;
}
}
}
}
if (m_max_angle <= m_min_angle) { yError() << "max_angle should be > min_angle"; return false; }
double fov = (m_max_angle - m_min_angle);
if (fov >360) { yError() << "max_angle - min_angle <= 360"; return false; }
m_sensorsNum = (int)(fov/m_resolution);
m_laser_data.resize(m_sensorsNum, 0.0);
m_drv = RPlidarDriver::CreateDriver(RPlidarDriver::DRIVER_TYPE_SERIALPORT);
if (!m_drv)
{
yError() << "Create Driver fail, exit\n";
return false;
}
if (IS_FAIL(m_drv->connect(serial.c_str(), (_u32)baudrate)))
{
yError() << "Error, cannot bind to the specified serial port:", serial.c_str();
RPlidarDriver::DisposeDriver(m_drv);
return false;
}
m_info = deviceinfo();
yInfo("max_dist %f, min_dist %f", m_max_distance, m_min_distance);
bool m_inExpressMode=false;
result = m_drv->checkExpressScanSupported(m_inExpressMode);
if (result == RESULT_OK && m_inExpressMode==true)
{
yInfo() << "Express scan mode is supported";
}
else
{
yWarning() << "Device does not supports express scan mode";
}
result = m_drv->startMotor();
if (result != RESULT_OK)
{
handleError(result);
return false;
}
yInfo() << "Motor started successfully";
if (m_pwm_val!=0)
{
if (m_pwm_val>0 && m_pwm_val<1023)
{
result = m_drv->setMotorPWM(m_pwm_val);
if (result != RESULT_OK)
{
handleError(result);
return false;
}
yInfo() << "Motor pwm set to "<< m_pwm_val;
}
else
{
yError() << "Invalid motor pwm request " << m_pwm_val <<". It should be a value between 0 and 1023.";
return false;
}
}
else
{
yInfo() << "Motor pwm set to default value (600?)";
}
bool forceScan =false;
result = m_drv->startScan(forceScan,m_inExpressMode);
if (result != RESULT_OK)
{
handleError(result);
return false;
}
yInfo() << "Scan started successfully";
yInfo() << "Device info:" << m_info;
yInfo("max_angle %f, min_angle %f", m_max_angle, m_min_angle);
yInfo("resolution %f", m_resolution);
yInfo("sensors %d", m_sensorsNum);
PeriodicThread::start();
return true;
}
bool repeaterModule::respond(const Bottle& command, Bottle& reply)
{
bool ok = false;
bool rec = false; // is the command recognized?
string helpMessage = string(getName().c_str()) +
" commands are: \n" +
"help \n" +
"quit \n";
reply.clear();
if (command.get(0).asString()=="quit") {
reply.addString("quitting");
return false;
}
else if (command.get(0).asString()=="help") {
cout << helpMessage;
reply.addString("ok");
}
else if ((command.get(0).asString()=="sus") || (command.get(0).asString()=="\"sus\"")) {
//prioritiser->waitMotionDone();
//prioritiser->suspend();
reply.addString("ok");
}
else if (command.get(0).asString()=="res" || command.get(0).asString()=="\"res\"" ) {
//prioritiser->resume();
reply.addString("ok");
}
mutex.wait();
switch (command.get(0).asVocab()) {
case COMMAND_VOCAB_HELP:
rec = true;
{
reply.addVocab(Vocab::encode("many"));
reply.addString("help");
//reply.addString();
reply.addString("set fn \t: general set command ");
reply.addString("get fn \t: general get command ");
//reply.addString();
//reply.addString();
reply.addString("seek red \t : looking for a red color object");
reply.addString("seek rgb \t : looking for a general color object");
reply.addString("sus \t : suspending");
reply.addString("res \t : resuming");
//reply.addString();
ok = true;
}
break;
case COMMAND_VOCAB_SUSPEND:
rec = true;
{
//prioritiser->suspend();
ok = true;
}
break;
case COMMAND_VOCAB_STOP:
rec = true;
{
//prioritiser->suspend();
//prioritiser->resume();
ok = true;
}
break;
case COMMAND_VOCAB_RESUME:
rec = true;
{
//prioritiser->resume();
ok = true;
}
break;
case COMMAND_VOCAB_SEEK:
rec = true;
{
//prioritiser->suspend();
//prioritiser->seek(command);
//prioritiser->resume();
ok = true;
}
break;
case COMMAND_VOCAB_FIX:
rec = true;
{
switch (command.get(1).asVocab()) {
case COMMAND_VOCAB_CENT:
{
printf("Fixating in Center \n");
//prioritiser->fixCenter(1000);
}
break;
}
ok = true;
}
break;
case COMMAND_VOCAB_PUSH:
rec = true;
{
switch (command.get(1).asVocab()) {
case COMMAND_VOCAB_SAT:
{ int delta = command.get(2).asInt();
printf("Pushing Saturation %d \n", delta);
rThread->setSatPush(delta);
}
break;
case COMMAND_VOCAB_BRI:
{
int delta = command.get(2).asInt();
printf("Pushing Brightness %d \n", delta);
rThread->setBriPush(delta);
}
break;
case COMMAND_VOCAB_HUE:
{
int delta = command.get(2).asInt();
printf("Pushing Hue %d \n", delta);
rThread->setHuePush(delta);
}
break;
}
ok = true;
}
break;
default: {
}
break;
}
mutex.post();
if (!rec)
ok = RFModule::respond(command,reply);
if (!ok) {
reply.clear();
reply.addVocab(COMMAND_VOCAB_FAILED);
}
else {
reply.addVocab(COMMAND_VOCAB_OK);
}
return true;
}
bool parametricCalibrator::open(yarp::os::Searchable& config)
{
yTrace();
Property p;
p.fromString(config.toString());
if (p.check("GENERAL")==false)
{
yError() << "Parametric calibrator: missing [GENERAL] section";
return false;
}
if(p.findGroup("GENERAL").check("deviceName"))
{
deviceName = p.findGroup("GENERAL").find("deviceName").asString();
}
else
{
yError() << "Parametric calibrator: missing deviceName parameter";
return false;
}
std::string str;
if(config.findGroup("GENERAL").find("verbose").asInt())
{
str=config.toString().c_str();
yTrace() << deviceName.c_str() << str;
}
// Check clearHwFaultBeforeCalibration
Value val_clearHwFault = config.findGroup("GENERAL").find("clearHwFaultBeforeCalibration");
if(val_clearHwFault.isNull())
{
clearHwFault = false;
}
else
{
if(!val_clearHwFault.isBool())
{
yError() << deviceName.c_str() << ": clearHwFaultBeforeCalibration bool param is different from accepted values (true / false). Assuming false";
clearHwFault = false;
}
else
{
clearHwFault = val_clearHwFault.asBool();
if(clearHwFault)
yInfo() << deviceName.c_str() << ": clearHwFaultBeforeCalibration option enabled\n";
}
}
// Check Vanilla = do not use calibration!
skipCalibration =config.findGroup("GENERAL").find("skipCalibration").asBool() ;// .check("Vanilla",Value(1), "Vanilla config");
skipCalibration = !!skipCalibration;
if(skipCalibration )
{
yWarning() << deviceName << ": skipping calibration!! This option was set in general.xml file.";
yWarning() << deviceName << ": BE CAREFUL USING THE ROBOT IN THIS CONFIGURATION! See 'skipCalibration' param in config file";
}
int nj = 0;
if(p.findGroup("GENERAL").check("joints"))
{
nj = p.findGroup("GENERAL").find("joints").asInt();
}
else if(p.findGroup("GENERAL").check("Joints"))
{
// This is needed to be backward compatibile with old iCubInterface
nj = p.findGroup("GENERAL").find("Joints").asInt();
}
else
{
yError() << deviceName.c_str() << ": missing joints parameter" ;
return false;
}
type = new unsigned char[nj];
param1 = new double[nj];
param2 = new double[nj];
param3 = new double[nj];
maxPWM = new int[nj];
zeroPos = new double[nj];
zeroVel = new double[nj];
currPos = new double[nj];
currVel = new double[nj];
homePos = new double[nj];
homeVel = new double[nj];
zeroPosThreshold = new double[nj];
int i=0;
Bottle& xtmp = p.findGroup("CALIBRATION").findGroup("calibration1");
if (xtmp.size()-1!=nj) {yError() << deviceName << ": invalid number of Calibration1 params " << xtmp.size()<< " " << nj; return false;}
for (i = 1; i < xtmp.size(); i++) param1[i-1] = xtmp.get(i).asDouble();
xtmp = p.findGroup("CALIBRATION").findGroup("calibration2");
if (xtmp.size()-1!=nj) {yError() << deviceName << ": invalid number of Calibration2 params"; return false;}
for (i = 1; i < xtmp.size(); i++) param2[i-1] = xtmp.get(i).asDouble();
xtmp = p.findGroup("CALIBRATION").findGroup("calibration3");
if (xtmp.size()-1!=nj) {yError() << deviceName << ": invalid number of Calibration3 params"; return false;}
for (i = 1; i < xtmp.size(); i++) param3[i-1] = xtmp.get(i).asDouble();
xtmp = p.findGroup("CALIBRATION").findGroup("calibrationType");
if (xtmp.size()-1!=nj) {yError() << deviceName << ": invalid number of Calibration3 params"; return false;}
for (i = 1; i < xtmp.size(); i++) type[i-1] = (unsigned char) xtmp.get(i).asDouble();
xtmp = p.findGroup("CALIBRATION").findGroup("positionZero");
if (xtmp.size()-1!=nj) {yError() << deviceName << ": invalid number of PositionZero params"; return false;}
for (i = 1; i < xtmp.size(); i++) zeroPos[i-1] = xtmp.get(i).asDouble();
xtmp = p.findGroup("CALIBRATION").findGroup("velocityZero");
if (xtmp.size()-1!=nj) {yError() << deviceName << ": invalid number of VelocityZero params"; return false;}
for (i = 1; i < xtmp.size(); i++) zeroVel[i-1] = xtmp.get(i).asDouble();
xtmp = p.findGroup("HOME").findGroup("positionHome");
if (xtmp.size()-1!=nj) {yError() << deviceName << ": invalid number of PositionHome params"; return false;}
for (i = 1; i < xtmp.size(); i++) homePos[i-1] = xtmp.get(i).asDouble();
xtmp = p.findGroup("HOME").findGroup("velocityHome");
if (xtmp.size()-1!=nj) {yError() << deviceName << ": invalid number of VelocityHome params"; return false;}
for (i = 1; i < xtmp.size(); i++) homeVel[i-1] = xtmp.get(i).asDouble();
xtmp = p.findGroup("CALIBRATION").findGroup("maxPwm");
if (xtmp.size()-1!=nj) {yError() << deviceName << ": invalid number of MaxPwm params"; return false;}
for (i = 1; i < xtmp.size(); i++) maxPWM[i-1] = xtmp.get(i).asInt();
xtmp = p.findGroup("CALIBRATION").findGroup("posZeroThreshold");
if (xtmp.size()-1!=nj) {yError() << deviceName << ": invalid number of PosZeroThreshold params"; return false;}
for (i = 1; i < xtmp.size(); i++) zeroPosThreshold[i-1] = xtmp.get(i).asDouble();
xtmp = p.findGroup("CALIB_ORDER");
int calib_order_size = xtmp.size();
if (calib_order_size <= 1) {yError() << deviceName << ": invalid number CALIB_ORDER params"; return false;}
//yDebug() << "CALIB_ORDER: group size: " << xtmp.size() << " values: " << xtmp.toString().c_str();
std::list<int> tmp;
for(int i=1; i<xtmp.size(); i++)
{
tmp.clear();
Bottle *set;
set= xtmp.get(i).asList();
for(int j=0; j<set->size(); j++)
{
tmp.push_back(set->get(j).asInt() );
}
joints.push_back(tmp);
}
return true;
}
void DataPlot::initSignalDimensions()
{
if(VERBOSE) fprintf(stderr, "MESSAGE: Will now initialize the signal dimensions\n");
//getting info on the connected port
nonTimeBasedCurve = new QwtPlotCurve*[numberOfInputPorts];
for (int k = 0; k < numberOfInputPorts; k++)
{
Bottle *b = NULL;
int timeout = 0;
const int maxTimeout = 1000;
while(b==NULL && timeout < maxTimeout)
{
b = inputPorts[k].read(false);
Time::delay(0.005);
timeout++;
}
if (timeout==maxTimeout)
{
if(VERBOSE) fprintf(stderr, "MESSAGE: Couldn't receive data. Going to put a zero! \n");
realTime = false;
// Initialize data
for (int j = 0; j< numberOfPlots[k]; j++)
{
for (int i = 0; i < PLOT_SIZE; i++)
{
d_x[i] = i; // time axis
//if(VERBOSE) fprintf(stderr, "MESSAGE: (%d, %d)\n", j, i);
d_y[k][j][i] = 0;
}
}
}
else
{
if(VERBOSE) fprintf(stderr, "MESSAGE: Will now try real time!\n");
inputVectorSize = b->size();
Stamp stmp;
inputPorts[k].getEnvelope(stmp);
if (stmp.isValid())
{
if(VERBOSE) fprintf(stderr, "MESSAGE: will use real time!\n");
realTime = true;
initialTime = stmp.getTime();
}
// Initialize data
for (int j = 0; j< numberOfPlots[k]; j++)
{
if (b->size()-1 < index[k][j])
if(VERBOSE) fprintf(stderr, "WARNING: will plot some zeros since the accessed index exceed the input vector dimensions!\n");
for (int i = 0; i < PLOT_SIZE; i++)
{
d_x[i] = i; // time axis
#ifdef ENABLE_REALTIME
if (realTime)
d_x_real_time[i] = i;
#endif
//if(VERBOSE) fprintf(stderr, "MESSAGE: (%d, %d)\n", j, i);
d_y[k][j][i] = 0;
}
}
}
//if(VERBOSE) fprintf(stderr, "MESSAGE: initializing plot datas!\n");
// Assign a title
insertLegend(new QwtLegend(), QwtPlot::BottomLegend);
nonTimeBasedCurve[k] = new QwtPlotCurve[numberOfPlots[k]];
for(int i=0; i < numberOfPlots[k]; i++)
{
//Set title
char cTitle[256];
sprintf(cTitle, "Data(%d)", index[k][i]);
QwtText curveTitle(cTitle, QwtText::PlainText);
curveTitle.setFont(plotFont);
nonTimeBasedCurve[k][i].setTitle(curveTitle);
//if(VERBOSE) fprintf(stderr, "MESSAGE: Will now initialize the plot %d\n", index[i]);
// Insert new curves
nonTimeBasedCurve[k][i].attach(this);
// Set curve styles
nonTimeBasedCurve[k][i].setPen(coloredPens[k%NUMBER_OF_LIN][i%NUMBER_OF_COL]);
// Attach (don't copy) data. Both curves use the same x array.
nonTimeBasedCurve[k][i].setRawData(d_x, d_y[k][i], PLOT_SIZE);
}
// Axis
QwtText axisTitle("Time/seconds");
axisTitle.setFont(plotFont);
setAxisTitle(QwtPlot::xBottom, axisTitle);
setAxisScale(QwtPlot::xBottom, 0, 100);
setAxisFont(QwtPlot::xBottom, plotFont);
setAxisTitle(QwtPlot::yLeft, "Values");
//setAxisScale(QwtPlot::yLeft, -1.5, 1.5);
setAxisAutoScale(QwtPlot::yLeft);
setAxisAutoScale(QwtPlot::xBottom);
setAxisFont(QwtPlot::yLeft, plotFont);
setTimerInterval(50.0);
}
//if(VERBOSE) fprintf(stderr, "MESSAGE: plot intialized!\n");
}
void DeviceResponder::addUsage(const Bottle& bot, const char *explain) {
addUsage(bot.toString().c_str(),explain);
}
// Generate new values
void DataPlot::timerEvent(QTimerEvent *)
{
for (int k = 0; k < numberOfInputPorts; k++)
{
int timeout = 0;
int maxTimeout = 2;
Bottle *b = NULL;
while(b==NULL && timeout < maxTimeout)
{
b = inputPorts[k].read(false);
Time::delay(0.005);
timeout++;
}
if (timeout==maxTimeout)
{
if(VERBOSE) fprintf(stderr, "MESSAGE: Couldn't receive data. Going to put a zero! \n");
for(int i = 0; i < numberOfPlots[k]; i++)
{
d_y[k][i][PLOT_SIZE - 1] = 0;
for ( int j = 0; j < PLOT_SIZE - 1; j++ )
{
d_y[k][i][j] = d_y[k][i][j+1];
}
}
}
else
{
for(int i = 0; i < numberOfPlots[k]; i++)
{
Stamp stmp;
inputPorts[k].getEnvelope(stmp);
#ifdef ENABLE_REALTIME
if (stmp.isValid())
d_x_real_time[PLOT_SIZE - 1] = (stmp.getTime() - initialTime);
for ( int j = 0; j < PLOT_SIZE - 1; j++ )
{
if(realTime)
d_x_real_time[j] = d_x_real_time[j+1];
}
#endif
if (b==NULL)
d_y[k][i][PLOT_SIZE - 1] = 0;
else
if (b->size()-1 >= index[k][i])
{
d_y[k][i][PLOT_SIZE - 1] = b->get(index[k][i]).asDouble();
//if(VERBOSE) fprintf(stderr, "MESSAGE: Getting from port %d the index %d\n", k, index[k][i]);
}
// y moves from left to right:
// Shift y array right and assign new value to y[0].
for ( int j = 0; j < PLOT_SIZE - 1; j++ )
{
d_y[k][i][j] = d_y[k][i][j+1];
}
// update the display
//setAxisScale(QwtPlot::yLeft, min, max);
#ifdef ENABLE_REALTIME
if(numberAcquiredData==PLOT_SIZE && realTime)
{
if(VERBOSE) fprintf(stderr, "MESSAGE: switching to real time\n");
QwtPlotCurve *timeBasedCurve = new QwtPlotCurve("Data");
timeBasedCurve->attach(this);
timeBasedCurve->setRawData(d_x_real_time, d_y[k][i], PLOT_SIZE);
timeBasedCurve->setPen(coloredPens[k%NUMBER_OF_LIN][i%NUMBER_OF_COL]);
nonTimeBasedCurve[k][i].attach(NULL);
//Set title
char cTitle[256];
sprintf(cTitle, "Data(%d)", index[k][i]);
QwtText curveTitle(cTitle, QwtText::PlainText);
curveTitle.setFont(plotFont);
timeBasedCurve->setTitle(curveTitle);
}
#endif
}
}
}
if (acquire)
replot();
numberAcquiredData++;
//if(VERBOSE) fprintf(stderr, "Number of acquired data is %d\n", numberAcquiredData);
//QSize plotSize= this->sizeHint();
//if(VERBOSE) fprintf(stderr, "Hint is: hInt=%d, vInt=%d\n", plotSize.height(), plotSize.width());
static double before = Time::now();
double now = Time::now();
static double meanEffectiveTime = 0;
double currentEffectiveTime = (now - before)*1000.0;
if (numberAcquiredData >= 2)
meanEffectiveTime = (meanEffectiveTime*(numberAcquiredData-2) + currentEffectiveTime)/(numberAcquiredData-1);
//if(VERBOSE) fprintf(stderr, "Iteration %d: Current time is %f and mean is %f\n", numberAcquiredData, currentEffectiveTime, meanEffectiveTime);
if (currentEffectiveTime*0.5 > d_interval)
{
if(VERBOSE) fprintf(stderr, "Real Timer is %f but I was supposed to run at %d ms. Mean is: %f \n", currentEffectiveTime, d_interval, meanEffectiveTime);
if(VERBOSE) fprintf(stderr, "You should slow down to %d ms \n", (int) (meanEffectiveTime * 2));
//setTimerInterval((int) (meanEffectiveTime * 2));
}
before = now;
}
bool AgentDetector::respond(const Bottle& cmd, Bottle& reply)
{
if (cmd.get(0).asString() == "train" ) {
reply.addString("ack");
cout<<"Received a training order"<<endl;
currentTrainingFace = cmd.get(1).asString();
}
else if (cmd.get(0).asString() == "change_partner_name" ) {
if(cmd.get(1).isString()) {
reply.addString("ack");
partner_default_name=cmd.get(1).asString();
} else {
reply.addString("nack");
}
}
else if (cmd.get(0).asString() == "pause" ) {
m.lock();
reply.addString("ack");
}
else if (cmd.get(0).asString() == "resume" ) {
m.unlock();
reply.addString("ack");
}
else
{
reply.addString("nack");
}
return true;
}
ConstString toString() {
ConstString s;
Property done;
for (unsigned int i=0; i<delegates.size(); i++) {
if (delegates[i]==NULL) continue;
ConstString name = delegates[i]->getName();
done.put(name,1);
ConstString wrapper = delegates[i]->getWrapper();
s += "Device \"";
s += delegates[i]->getName();
s += "\"";
s += ",";
s += " C++ class ";
s += delegates[i]->getCode();
s += ", ";
if (wrapper=="") {
s += "has no network wrapper";
} else if (wrapper!=name) {
s += "wrapped by \"";
s += delegates[i]->getWrapper();
s += "\"";
} else {
s += "is a network wrapper.";
}
s += "\n";
}
scan();
Bottle lst = getSelectedPlugins();
for (int i=0; i<lst.size(); i++) {
Value& prop = lst.get(i);
ConstString name = prop.check("name",Value("untitled")).asString();
if (done.check(name)) continue;
SharedLibraryFactory lib;
YarpPluginSettings settings;
settings.setSelector(*this);
settings.readFromSearchable(prop,name);
settings.open(lib);
ConstString location = lib.getName().c_str();
if (location=="") continue;
ConstString cxx = prop.check("cxx",Value("unknown")).asString();
ConstString wrapper = prop.check("wrapper",Value("unknown")).asString();
s += "Device \"";
s += name;
s += "\"";
s += ",";
s += " available on request (found in ";
s += location;
s += " library)";
if (cxx!="unknown") {
s += ", C++ class ";
s += cxx;
s += " ";
}
if (wrapper=="") {
s += "no network wrapper known";
} else if (wrapper=="unknown") {
//s += "network wrapper unknown";
} else if (wrapper!=name) {
s += "wrapped by \"";
s += delegates[i]->getWrapper();
s += "\"";
} else {
s += "is a network wrapper.";
}
s += "\n";
}
return s;
}
virtual void report(const SearchReport& report, const char *context) {
String ctx = context;
String key = report.key.c_str();
String prefix = "";
prefix = ctx;
prefix += ".";
key = prefix + key;
if (key.substr(0,1)==".") {
key = key.substr(1,key.length());
}
if (!present.check(key.c_str())) {
present.put(key.c_str(),"present");
order.addString(key.c_str());
}
if (report.isFound) {
actual.put(key.c_str(),report.value);
}
if (report.isComment==true) {
comment.put(key.c_str(),report.value);
return;
}
if (report.isDefault==true) {
fallback.put(key.c_str(),report.value);
return;
}
if (comment.check(key.c_str())) {
if (!reported.check(key.c_str())) {
if (report.isFound) {
String hasValue = report.value.c_str();
if (hasValue.length()>35) {
hasValue = hasValue.substr(0,30) + " ...";
}
printf("Checking \"%s\": = %s (%s)\n", key.c_str(),
hasValue.c_str(),
comment.check(key.c_str(),
Value("")).toString().c_str());
} else {
reported.put(key.c_str(),1);
bool hasDefault = fallback.check(key.c_str());
String defString = "";
if (hasDefault) {
defString += " ";
defString += "(default ";
String theDefault =
fallback.find(key.c_str()).toString().c_str();
if (theDefault=="") {
defString += "is blank";
} else {
defString += theDefault;
}
defString += ")";
}
printf("Checking \"%s\": %s%s\n", key.c_str(),
comment.check(key.c_str(),
Value("")).toString().c_str(),
defString.c_str());
}
}
}
}
bool configure(ResourceFinder &rf, const string &part)
{
this->part=part;
Bottle &bGeneral=rf.findGroup("general");
if (bGeneral.isNull())
{
yError("group [general] is missing!");
return false;
}
Bottle &bPart=rf.findGroup(part);
if (bPart.isNull())
{
yError("group [%s] is missing!",part.c_str());
return false;
}
if (!bPart.check("device"))
{
yError("\"device\" option is missing!");
return false;
}
name=bGeneral.check("name",Value("fingersTuner")).asString();
robot=bGeneral.check("robot",Value("icub")).asString();
double ping_robot_tmo=bGeneral.check("ping_robot_tmo",Value(0.0)).asDouble();
device=bPart.find("device").asString();
if (Bottle *rj=bGeneral.find("relevantJoints").asList())
rJoints=*rj;
else
{
yError("\"relevantJoints\" option is missing!");
return false;
}
int numAlias=bGeneral.check("numAlias",Value(0)).asInt();
for (int i=0; i<numAlias; i++)
{
ostringstream item;
item<<"alias_"<<i;
Bottle &bAlias=bGeneral.findGroup(item.str());
if (Bottle *joints=bAlias.find("joints").asList())
alias[bAlias.find("tag").asString()]=*joints;
}
// special wildcard to point to all the joints
alias["*"]=rJoints;
for (int i=0; i<rJoints.size(); i++)
{
int j=rJoints.get(i).asInt();
pids[j]=getPidData(bPart,j);
}
ostringstream portsSuffix;
portsSuffix<<instances<<"/"<<device;
Property option("(device remote_controlboard)");
option.put("remote","/"+robot+"/"+device);
option.put("local","/"+name+"/"+portsSuffix.str());
if (ping_robot_tmo>0.0)
driver=waitPart(option,ping_robot_tmo);
else
driver=new PolyDriver(option);
if (!driver->isValid())
{
yError("%s device driver not available!",device.c_str());
return false;
}
return true;
}
int main(int argc, char **argv)
{
if (argc!=3)
return 0;
Network yarp;
MyPlayer *player = new MyPlayer(argv[1], PLAYER_PERIOD);
if(atoi(argv[2])==0)
player->setShooter(0);
else
player->setShooter(1);
player->start();
int count=0;
while(true)
{
int plLife;
int plX;
int plY;
count++;
plLife=player->getLife();
plX=player->getX();
plY=player->getY();
if ((count==500)||(plLife==0))
{
fprintf(stderr, "Stopping player\n");
player->stop();
fprintf(stderr, "Waiting some time\n");
Time::delay(2);
printf("Restarting the player\n");
player->start();
count=0;
}
Bottle world;
player->getWorld(world);
int i;
for (i=1; i<world.size(); i++)
{
printf("%s\n", world.get(i).asString().c_str());
}
printf("X:%d Y:%d Life:%d\n", plX, plY, plLife);
Time::delay(0.5);
}
player->stop();
delete player;
return 0;
}
bool respond(const Bottle &command, Bottle &reply)
{
int ack =Vocab::encode("ack");
int nack=Vocab::encode("nack");
if (command.size()>0)
{
if (command.get(0).asString() == "get")
{
if (command.get(1).asString() == "motionGain")
{
reply.addVocab(ack);
reply.addDouble(motionGain);
}
else
{
reply.addVocab(nack);
}
}
else if (command.get(0).asString() == "set")
{
if (command.get(1).asString() == "motionGain")
{
reply.addVocab(ack);
motionGain = command.get(2).asDouble();
reply.addDouble(motionGain);
}
else if (command.get(1).asString() == "behavior")
{
if (command.get(2).asString() == "avoidance")
{
reply.addVocab(ack);
motionGain = -1.0;
reply.addDouble(motionGain);
}
else if (command.get(2).asString() == "catching")
{
reply.addVocab(ack);
motionGain = 1.0;
reply.addDouble(motionGain);
}
else
{
reply.addVocab(nack);
}
}
else
{
reply.addVocab(nack);
}
}
}
return true;
}
bool humanRobotDump::respond(const Bottle& command, Bottle& reply) {
string helpMessage = string(getName().c_str()) +
" commands are: \n" +
"help \n" +
"off \n" +
"'humanDump' + action_name \n" +
"'robotDump' + action_name \n" +
"'subaction' + subaction_name \n" +
"'objectToDump' + object \n" +
"'agentName' + object \n" +
"quit \n";
reply.clear();
if (command.get(0).asString() == "quit") {
reply.addString("quitting");
rpcPort.reply(reply);
return false;
}
else if (command.get(0).asString() == "humanDump"){
if (command.size() != 2)
{
reply.addString("error in humanRobotDump: Botte 'humanDump' (action_name)");
}
else
{
sActionName = command.get(1).asString();
humanDump = true;
yInfo() << " start humanDumping";
reply.addString("start humanDumping");
}
}
else if (command.get(0).asString() == "robotDump"){
if (command.size() != 2)
{
reply.addString("error in humanRobotDump: Botte 'robotDump' (action_name)");
}
else
{
sActionName = command.get(1).asString();
robotDump = true;
yInfo() << " start robotDumping";
reply.addString("start robotDumping");
}
}
else if (command.get(0).asString() == "objectToDump"){
if (command.size() != 2)
{
reply.addString("error in humanRobotDump: Botte 'objectToDump' misses information (object name)");
}
else
{
sObjectToDump = command.get(1).toString();
yInfo() << " object to dump: " + sObjectToDump;
reply.addString("object to dump: " + sObjectToDump);
}
}
else if (command.get(0).asString() == "subaction"){
if (command.size() != 2)
{
reply.addString("error in humanRobotDump: Botte 'subaction' misses information (subaction name)");
}
else
{
sSubActionName = command.get(1).toString();
yInfo() << " subaction: " + sSubActionName;
reply.addString("subaction: " + sSubActionName);
}
}
else if (command.get(0).asString() == "agentName"){
if (command.size() != 2)
{
reply.addString("error in humanRobotDump: Botte 'agentName' misses information (agent name)");
}
else
{
sAgentName = command.get(1).toString();
yInfo() << " agent to dump: " + sAgentName;
reply.addString("agent to dump: " + sAgentName);
}
}
else if (command.get(0).asString() == "off"){
robotDump = false;
humanDump = false;
yInfo() << " stopDumpin";
reply.addString("stopDumping");
m_iterator++;
}
else {
cout << helpMessage;
reply.addString(helpMessage);
}
rpcPort.reply(reply);
return true;
}
bool selectiveAttentionModule::respond(const Bottle &command,Bottle &reply){
bool ok = false;
bool rec = false; // is the command recognized?
ConstString str=command.get(0).asString();
if(!strcmp(str.c_str(),"sus")) {
currentProcessor->suspend();
return true;
}
else if(!strcmp(str.c_str(),"res")) {
currentProcessor->resume();
return true;
}
else if(!strcmp(str.c_str(),"set")) {
ConstString str2=command.get(0).asString();
if(!strcmp(str2.c_str(),"def")) {
if(currentProcessor!=0) {
currentProcessor->setKMotion(0.2);
currentProcessor->setK1(0.5);
currentProcessor->setK2(0.1);
currentProcessor->setK3(0.5);
currentProcessor->setK4(0.1);
currentProcessor->setK5(0.5);
currentProcessor->setK6(1.0);
currentProcessor->setKC1(0.0);
currentProcessor->setKC2(0.0);
return true;
}
else if(!strcmp(str2.c_str(),"mot")) {
currentProcessor->setKMotion(1.0);
currentProcessor->setK1(0.0);
currentProcessor->setK2(0.0);
currentProcessor->setK3(0.0);
currentProcessor->setK4(0.0);
currentProcessor->setK5(0.0);
currentProcessor->setK6(0.0);
currentProcessor->setKC1(0.0);
currentProcessor->setKC2(0.0);
return true;
}
}
}
mutex.wait();
switch (command.get(0).asVocab()) {
case COMMAND_VOCAB_HELP:
rec = true;
{
reply.addVocab(Vocab::encode("many"));
reply.addString("help");
reply.addString("");
reply.addString("set fn \t: general set command ");
reply.addString("");
reply.addString("");
reply.addString("");
reply.addString("set time <int> \t: setting the costant time between saccadic events (default 3000) ");
reply.addString("set k1 <double> \t: setting the coefficients to the default value ");
reply.addString("set k1 <double> \t: setting of linear combination coefficient (map1) ");
reply.addString("set k2 <double> \t: setting of linear combination coefficient (map2) ");
reply.addString("set k3 <double> \t: setting of linear combination coefficient (map3) ");
reply.addString("set k4 <double> \t: setting of linear combination coefficient (map4) ");
reply.addString("set k5 <double> \t: setting of linear combination coefficient (map5) ");
reply.addString("set k6 <double> \t: setting of linear combination coefficient (map6) ");
reply.addString("set kc1 <double> \t: setting of linear combination coefficient (mapc1) ");
reply.addString("set kc2 <double> \t: setting of linear combination coefficient (mapc2) ");
reply.addString("set kmot <double> \t: setting of linear combination coefficient (flow motion) ");
reply.addString("set bu <double> \t: setting of weight of the bottom-up contribution ");
reply.addString("set td <double> \t: setting of weight of the top-down contribution ");
reply.addString("");
reply.addString("get fn \t: general get command ");
reply.addString("");
reply.addString("");
reply.addString("get time <int> \t: getting the timing between saccadic events ");
reply.addString("get k1 <double> \t: getting the coefficients to the default value ");
reply.addString("get k1 <double> \t: getting of linear combination coefficient (map1) ");
reply.addString("get k2 <double> \t: getting of linear combination coefficient (map2) ");
reply.addString("get k3 <double> \t: getting of linear combination coefficient (map3) ");
reply.addString("get k4 <double> \t: getting of linear combination coefficient (map4) ");
reply.addString("get k5 <double> \t: getting of linear combination coefficient (map5) ");
reply.addString("get k6 <double> \t: getting of linear combination coefficient (map6) ");
reply.addString("get kc1 <double> \t: getting of linear combination coefficient (mapc1) ");
reply.addString("get kc2 <double> \t: getting of linear combination coefficient (mapc2) ");
reply.addString("get kmot <double> \t: getting of linear combination coefficient (flow motion) ");
reply.addString(" ");
reply.addString(" ");
ok = true;
}
break;
case COMMAND_VOCAB_SUSPEND:
rec = true;
{
currentProcessor->suspend();
ok = true;
}
break;
case COMMAND_VOCAB_RESUME:
rec = true;
{
currentProcessor->resume();
ok = true;
}
break;
case COMMAND_VOCAB_INH:
rec = true;
{
printf("Inhibition of feature extraction activated \n");
//currentProcessor->suspend();
currentProcessor->magnoCellularSuppression(true);
ok = true;
}
break;
case COMMAND_VOCAB_NINH:
rec = true;
{
printf("Inhibition of feature extraction deactivated \n");
//currentProcessor->resume();
currentProcessor->magnoCellularSuppression(false);
//currentProcessor->setCounterMotion(0);
ok = true;
}
break;
case COMMAND_VOCAB_NAME:
rec = true;
{
// check and change filter name to pass on to the next filter
string fName(command.get(1).asString());
string subName;
Bottle subCommand;
int pos=1;
//int pos = fName.find_first_of(filter->getFilterName());
if (pos == 0){
pos = fName.find_first_of('.');
if (pos > -1){ // there is a subfilter name
subName = fName.substr(pos + 1, fName.size()-1);
subCommand.add(command.get(0));
subCommand.add(Value(subName.c_str()));
}
for (int i = 2; i < command.size(); i++)
subCommand.add(command.get(i));
//ok = filter->respond(subCommand, reply);
}
else{
printf("filter name does not match top filter name ");
ok = false;
}
}
break;
case COMMAND_VOCAB_SET:
rec = true;
{
switch(command.get(1).asVocab()) {
case COMMAND_VOCAB_SALIENCE_THRESHOLD:{
double thr = command.get(2).asDouble();
}
break;
case COMMAND_VOCAB_NUM_BLUR_PASSES:{
int nb = command.get(2).asInt();
//reply.addString("connection 2");
ok=true;
}
break;
case COMMAND_VOCAB_LHAND:{
int nb = command.get(2).asInt();
if(nb)
currentProcessor->setHandFixation(true);
else
currentProcessor->setHandFixation(false);
ok=true;
}
break;
case COMMAND_VOCAB_TIME:{
int w = command.get(2).asInt();
if(currentProcessor!=0)
currentProcessor->setSaccadicInterval(w);
ok = true;
}
break;
case COMMAND_VOCAB_K1:{
double w = command.get(2).asDouble();
if(currentProcessor!=0)
currentProcessor->setK1(w);
ok = true;
}
break;
case COMMAND_VOCAB_K2:{
double w = command.get(2).asDouble();
if(currentProcessor!=0)
currentProcessor->setK2(w);
ok = true;
}
break;
case COMMAND_VOCAB_K3:{
double w = command.get(2).asDouble();
if(currentProcessor!=0)
currentProcessor->setK3(w);
ok = true;
}
break;
case COMMAND_VOCAB_K4:{
double w = command.get(2).asDouble();
if(currentProcessor!=0)
currentProcessor->setK4(w);
ok = true;
}
break;
case COMMAND_VOCAB_K5:{
double w = command.get(2).asDouble();
if(currentProcessor!=0)
currentProcessor->setK5(w);
ok = true;
}
break;
case COMMAND_VOCAB_K6:{
double w = command.get(2).asDouble();
if(currentProcessor!=0)
currentProcessor->setK6(w);
ok = true;
}
break;
case COMMAND_VOCAB_BU:{
double w = command.get(2).asDouble();
if(currentProcessor!=0)
currentProcessor->setBU(w);
ok = true;
}
break;
case COMMAND_VOCAB_TD:{
double w = command.get(2).asDouble();
if(currentProcessor!=0)
currentProcessor->setTD(w);
ok = true;
}
break;
case COMMAND_VOCAB_KC1:{
double w = command.get(2).asDouble();
if(currentProcessor!=0)
currentProcessor->setKC1(w);
ok = true;
}
break;
case COMMAND_VOCAB_KC2:{
double w = command.get(2).asDouble();
if(currentProcessor!=0)
currentProcessor->setKC2(w);
ok = true;
}
break;
case COMMAND_VOCAB_DEF:{
if(currentProcessor!=0) {
currentProcessor->setKMotion(0.2);
currentProcessor->setK1(0.5);
currentProcessor->setK2(0.1);
currentProcessor->setK3(0.5);
currentProcessor->setK4(0.1);
currentProcessor->setK5(0.5);
currentProcessor->setK6(0.1);
currentProcessor->setKC1(0.0);
currentProcessor->setKC2(0.0);
}
ok = true;
}
break;
case COMMAND_VOCAB_KMOT:{
double w = command.get(2).asDouble();
if(currentProcessor!=0)
currentProcessor->setKMotion(w);
ok = true;
}
break;
case COMMAND_VOCAB_MOT:{
if(currentProcessor!=0) {
currentProcessor->setKMotion(1.0);
currentProcessor->setK1(0.0);
currentProcessor->setK2(0.0);
currentProcessor->setK3(0.0);
currentProcessor->setK4(0.0);
currentProcessor->setK5(0.0);
currentProcessor->setK6(0.0);
currentProcessor->setKC1(0.0);
currentProcessor->setKC2(0.0);
}
ok = true;
}
break;
case COMMAND_VOCAB_CHILD_NAME:{
int j = command.get(2).asInt();
string s(command.get(3).asString().c_str());
}
break;
case COMMAND_VOCAB_WEIGHT:{
double w = command.get(2).asDouble();
}
break;
case COMMAND_VOCAB_CHILD_WEIGHT:{
int j = command.get(2).asInt();
double w = command.get(3).asDouble();
}
break;
case COMMAND_VOCAB_CHILD_WEIGHTS:{
Bottle weights;
for (int i = 2; i < command.size(); i++)
weights.addDouble(command.get(i).asDouble());
}
break;
default: {
}
break;
}
}
break;
case COMMAND_VOCAB_GET:
rec = true;
{
reply.addVocab(COMMAND_VOCAB_IS);
reply.add(command.get(1));
switch(command.get(1).asVocab()) {
case COMMAND_VOCAB_SALIENCE_THRESHOLD:{
double thr=0.0;
reply.addDouble(thr);
ok = true;
}
break;
case COMMAND_VOCAB_NUM_BLUR_PASSES:{
int nb = 0;
reply.addInt(nb);
ok = true;
}
break;
case COMMAND_VOCAB_NAME:{
string s(" ");
reply.addString(s.c_str());
ok = true;
}
break;
case COMMAND_VOCAB_CHILD_NAME:{
int j = command.get(2).asInt();
string s(" ");
reply.addString(s.c_str());
ok = true;
}
break;
case COMMAND_VOCAB_CHILD_COUNT:{
int count =0;
reply.addInt(count);
ok = true;
}
break;
case COMMAND_VOCAB_WEIGHT:{
double w = 0.0;
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_CHILD_WEIGHT:{
int j = command.get(2).asInt();
double w = 0.0;
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_CHILD_WEIGHTS:{
Bottle weights;
//ok = filter->getChildWeights(&weights);
for (int k = 0; k < weights.size(); k++)
reply.addDouble(0.0);
}
break;
case COMMAND_VOCAB_TIME:{
int w = currentProcessor->getSaccadicInterval();
reply.addInt(w);
ok = true;
}
break;
case COMMAND_VOCAB_KMOT:{
double w = currentProcessor->getKMotion();
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_K1:{
double w = currentProcessor->getK1();
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_K2:{
double w = currentProcessor->getK2();
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_K3:{
double w = currentProcessor->getK3();
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_K4:{
double w = currentProcessor->getK4();
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_K5:{
double w = currentProcessor->getK5();
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_K6:{
double w = currentProcessor->getK6();
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_KC1:{
double w = currentProcessor->getKC1();
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_KC2:{
double w = currentProcessor->getKC2();
reply.addDouble(w);
ok = true;
}
/*
break;
case COMMAND_VOCAB_KC3:{
double w = currentProcessor->kc3;
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_KC4:{
double w = currentProcessor->kc4;
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_KC5:{
double w = currentProcessor->kc5;
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_KC6:{
double w = currentProcessor->kc6;
reply.addDouble(w);
ok = true;
}
break;
*/
default: {
}
break;
}
}
break;
}
mutex.post();
if (!rec)
ok = RFModule::respond(command,reply);
if (!ok) {
reply.clear();
reply.addVocab(COMMAND_VOCAB_FAILED);
}
else
reply.addVocab(COMMAND_VOCAB_OK);
return ok;
}
bool ObserverModule::respond(const Bottle& command, Bottle& reply)
{
string helpMessage = string(getName().c_str()) +
" commands are: \n" +
"help \n" +
"quit \n";
reply.clear();
if (command.get(0).asString()=="quit") {
reply.addString("quitting");
return false;
}
else if (command.get(0).asString()=="help") {
cout << helpMessage;
reply.addString("ok");
}
/* switch (command.get(0).asVocab()) {
case COMMAND_VOCAB_HELP:
rec = true;
{
//reply.addString("many"); // what used to work
reply.addString("help");
reply.addString("commands are:");
reply.addString(" help : to get help");
reply.addString(" quit : to quit the module");
reply.addString(" ");
reply.addString(" ");
reply.addString(" sus chr : to suspend chrom thread");
reply.addString(" sus edg : to suspend edges thread");
reply.addString(" res chr : to resume chrom thread");
reply.addString(" res chr : to resume edges thread");
reply.addString(" ");
reply.addString(" ");
reply.addString(" set w hor <float> : to change the weightage of horizontal orientation");
reply.addString(" set w o45 <float> : to change the weightage of 45 deg orientation");
reply.addString(" set w ver <float> : to change the weightage of vertical orientation");
reply.addString(" set w oM45 <float> : to change the weightage of -45 deg orientation");
reply.addString(" ");
reply.addString(" get w hor : to get the weightage of horizontal orientation");
reply.addString(" get w o45 : to get the weightage of 45 deg orientation");
reply.addString(" get w ver : to get the weightage of vertical orientation");
reply.addString(" get w oM45 : to get the weightage of -45 deg orientation");
reply.addString(" get int : to get the intensity value in fovea");
reply.addString(" get ori 0 : to get the orientation the saliency of feature in fovea ");
reply.addString(" get ori 45 : to get the orientation the saliency of feature in fovea ");
reply.addString(" get ori 90 : to get the orientation the saliency of feature in fovea ");
reply.addString(" get ori M45 : to get the orientation the saliency of feature in fovea ");
reply.addString(" get chr : get the value in thechrominance feature map in fovea ");
reply.addString(" ");
reply.addString(" ");
//reply.addString(helpMessage.c_str());
ok = true;
}
break;
case COMMAND_VOCAB_QUIT:
rec = true;
{
reply.addString("quitting");
ok = false;
}
break;
case COMMAND_VOCAB_SET:
{
switch(command.get(1).asVocab()){
case COMMAND_VOCAB_WEIGHT:
{
switch(command.get(2).asVocab()){
case COMMAND_VOCAB_HOR:
evThread->chromeThread->setWeightForOrientation(0,command.get(3).asDouble());
reply.addString("changed weight for horizontal orientation");
rec = true;
ok = true;
break;
case COMMAND_VOCAB_45:
evThread->chromeThread->setWeightForOrientation(1,command.get(3).asDouble());
reply.addString("changed weight for 45 deg orientation");
rec = true;
ok = true;
break;
case COMMAND_VOCAB_VER:
evThread->chromeThread->setWeightForOrientation(2,command.get(3).asDouble());
reply.addString("changed weight for vertical orientation");
rec = true;
ok = true;
break;
case COMMAND_VOCAB_M45:
evThread->chromeThread->setWeightForOrientation(3,command.get(3).asDouble());
reply.addString("changed weight for -45 deg orientation");
rec = true;
ok = true;
break;
case COMMAND_VOCAB_BRIGHT:
evThread->chromeThread->setBrightness((float)(command.get(3).asDouble()));
reply.addString("changed brightness for overall image");
rec = true;
ok = true;
break;
default:
rec = false;
ok = false;
}
}
break;
}
}
break;
case COMMAND_VOCAB_GET:
{
switch(command.get(1).asVocab()){
case COMMAND_VOCAB_WEIGHT:
{
switch(command.get(2).asVocab()){
case COMMAND_VOCAB_HOR:
wt = evThread->chromeThread->getWeightForOrientation(0);
reply.clear();
reply.addVocab(COMMAND_VOCAB_HOR); // ?? Needed
reply.addDouble(wt);
rec = true;
ok = true;
break;
case COMMAND_VOCAB_45:
wt = evThread->chromeThread->getWeightForOrientation(1);
reply.clear();
reply.addVocab(COMMAND_VOCAB_HOR); // ?? Needed
reply.addDouble(wt);
rec = true;
ok = true;
break;
case COMMAND_VOCAB_VER:
wt = evThread->chromeThread->getWeightForOrientation(2);
reply.clear();
reply.addVocab(COMMAND_VOCAB_HOR); // ?? Needed
reply.addDouble(wt);
rec = true;
ok = true;
break;
case COMMAND_VOCAB_M45:
wt = evThread->chromeThread->getWeightForOrientation(3);
reply.clear();
reply.addVocab(COMMAND_VOCAB_HOR); // ?? Needed
reply.addDouble(wt);
rec = true;
ok = true;
break;
case COMMAND_VOCAB_BRIGHT:
brightness = evThread->chromeThread->getBrightness();
reply.clear();
reply.addVocab(COMMAND_VOCAB_BRIGHT); // ?? Needed
reply.addDouble(brightness);
rec = true;
ok = true;
break;
default:
rec = false;
ok = false;
}
}
break;
case COMMAND_VOCAB_ORI:
{
switch(command.get(2).asVocab()){
case COMMAND_VOCAB_P45:
wt = evThread->chromeThread->getFoveaOri(0);
reply.clear();
reply.addInt(wt);
rec = true;
ok = true;
break;
case COMMAND_VOCAB_N45:
wt = evThread->chromeThread->getFoveaOri(-45);
reply.clear();
reply.addInt(wt);
rec = true;
ok = true;
break;
case COMMAND_VOCAB_P0:
wt = evThread->chromeThread->getFoveaOri(0);
reply.clear();
reply.addInt(wt);
rec = true;
ok = true;
break;
case COMMAND_VOCAB_P90:
wt = evThread->chromeThread->getFoveaOri(90);
reply.clear();
reply.addInt(wt);
rec = true;
ok = true;
break;
default:
rec = false;
ok = false;
}
}
break;
}
}
break;
case COMMAND_VOCAB_SUSPEND:
{
switch(command.get(1).asVocab()){
case COMMAND_VOCAB_CHROME_THREAD:
rThread->suspend();
reply.addString("suspending chrome thread");
rec = true;
ok = true;
break;
case COMMAND_VOCAB_EDGES_THREAD:
evThread->edThread->suspend();
reply.addString("suspending edges thread");
rec = true;
ok = true;
break;
default:
rec = false;
ok = false;
break;
}
}
break;
case COMMAND_VOCAB_RESUME:
{
switch(command.get(1).asVocab()){
case COMMAND_VOCAB_CHROME_THREAD:
evThread->chromeThread->resume();
reply.addString("resuming chrome thread");
rec = true;
ok = true;
break;
case COMMAND_VOCAB_EDGES_THREAD:
evThread->edThread->resume();
reply.addString("resuming edges thread");
rec = true;
ok = true;
break;
default:
rec = false;
ok = false;
break;
}
}
break;
default:
rec = false;
ok = false;
}
respondLock.post();
if (!rec){
ok = RFModule::respond(command,reply);
}
if (!ok) {
reply.clear();
reply.addVocab(COMMAND_VOCAB_FAILED);
}
else
reply.addVocab(COMMAND_VOCAB_OK);
return ok; */
return true;
}
void OscThread::ProcessMessage( const osc::ReceivedMessage& m,
const IpEndpointName& remoteEndpoint )
{
(void) remoteEndpoint; // suppress unused parameter warning
try{
string keyword = m.AddressPattern();
if( keyword == "/object")
{
cout<<"Received virtual object from osc"<<endl;
//if (H2ICUB != NULL)
osc::ReceivedMessage::const_iterator arg = m.ArgumentsBegin();
const char *type = (arg++)->AsString();
const char *name = (arg++)->AsString();
string typeword = type;
if (typeword == "add")
{
float x = (arg++)->AsFloat();
float y = (arg++)->AsFloat();
float dimx = (arg++)->AsFloat();
float dimy = (arg++)->AsFloat();
int r = (arg++)->AsInt32();
int g = (arg++)->AsInt32();
int b = (arg++)->AsInt32();
cout<<"Adding object "<<name<<endl;
if (!opc->isConnected())
{
cout<<"Not connected to OPC"<<endl;
return;
}
if (!isCalibrated)
{
cout<<"Not calibrated"<<endl;
return;
}
RTObject* o = opc->addRTObject(name);
opc->update(o);
Vector rtPosition(4);
rtPosition(0) = XaxisFactor * x;
rtPosition(1) = YaxisFactor * y;
rtPosition(2) = 0;
rtPosition(3) = 1;
o->m_rt_position[0] = rtPosition[0];
o->m_rt_position[1] = rtPosition[1];
o->m_rt_position[2] = rtPosition[2];
cout<<o->toString()<<endl;
Vector icubPos(4);
icubPos = H2ICUB * rtPosition;
o->m_ego_position[0] = icubPos[0];//+ idOffsets[tobj->getSymbolID()][0];
o->m_ego_position[1] = icubPos[1];//+ idOffsets[tobj->getSymbolID()][1];
o->m_ego_position[2] = icubPos[2];//+ idOffsets[tobj->getSymbolID()][2];
o->m_dimensions[0] = dimx;
o->m_dimensions[1] = dimy;
o->m_dimensions[2] = 0.01;
o->m_present = true;
o->m_ego_orientation[0] = 0.0;
o->m_ego_orientation[1] = 0.0;
o->m_ego_orientation[2] = 0.0;//tobj->getAngle();
o->m_color[0] = r;
o->m_color[1] = g;
o->m_color[2] = b;//tobj->getAngle();
//opc->isVerbose = true;
opc->commit(o);
//opc->isVerbose = false;
cout<<"Added"<<endl;
}
if (typeword == "remove")
{
if (!opc->isConnected())
{
cout<<"Not connected to OPC"<<endl;
return;
}
cout<<"Removing object"<<endl;
RTObject* o = opc->addRTObject(name);
o->m_present = false;
//opc->isVerbose = true;
opc->commit(o);
//opc->isVerbose = false;
cout<<"Removed"<<endl;
}
}
else if( keyword == "/event")
{
osc::ReceivedMessage::const_iterator arg = m.ArgumentsBegin();
const char *subject = (arg++)->AsString();
const char *verb = (arg++)->AsString();
const char *cobj = "none";
const char *cplace = "none";
const char *ctime = "none";
const char *cmanner = "none";
double timeLife = 5.0;
if (arg != m.ArgumentsEnd())
cobj = (arg++)->AsString();
if (arg != m.ArgumentsEnd())
cplace = (arg++)->AsString();
if (arg != m.ArgumentsEnd())
ctime = (arg++)->AsString();
if (arg != m.ArgumentsEnd())
cmanner = (arg++)->AsString();
if (arg != m.ArgumentsEnd())
timeLife = (arg++)->AsFloat();
Relation r(subject,verb,cobj,cplace,ctime,cmanner);
cout<<"Received relation (/Event) from OSC "<<r.toString()<< "with a lifetime of "<<timeLife<< "Trying to add : ";
opc->addRelation(r, timeLife);
cout<<endl;
}
else if( keyword == "/revent")
{
osc::ReceivedMessage::const_iterator arg = m.ArgumentsBegin();
const char *subject = (arg++)->AsString();
const char *verb = (arg++)->AsString();
const char *cobj = "none";
const char *cplace = "none";
const char *ctime = "none";
const char *cmanner = "none";
double timeLife = 5.0;
if (arg != m.ArgumentsEnd())
cobj = (arg++)->AsString();
if (arg != m.ArgumentsEnd())
cplace = (arg++)->AsString();
if (arg != m.ArgumentsEnd())
ctime = (arg++)->AsString();
if (arg != m.ArgumentsEnd())
cmanner = (arg++)->AsString();
if (arg != m.ArgumentsEnd())
timeLife = (arg++)->AsFloat();
Relation r(subject,verb,cobj,cplace,ctime,cmanner);
cout<<"Received relation remove request (/revent) from OSC "<<r.toString()<< "Trying to remove : ";
opc->removeRelation(r);
cout<<endl;
}
else if ( keyword == "/bottle")
{
Bottle bFwd;
osc::ReceivedMessage::const_iterator arg = m.ArgumentsBegin();
const char *fwdMsg = (arg++)->AsString();
cout<<"OSC input (bottle forwarding) : "<<fwdMsg<<endl;
bFwd.addString(fwdMsg);
oscFwding.write(bFwd);
}
else
{
cout<<"OSC input : Unknown format"<<endl;
}
}catch( osc::Exception& e ){
std::cout << "error while parsing message: "
<< m.AddressPattern() << ": " << e.what() << "\n";
}
}
cartesianMover::cartesianMover(GtkWidget *vbox_d, PolyDriver *partDd_d, char *partName, ResourceFinder *fnd)
{
finder = fnd;
if (!finder->isNull())
fprintf(stderr, "Setting a valid finder \n");
partLabel = partName;
partDd = partDd_d;
vbox = vbox_d;
interfaceError = false;
if (!partDd->isValid()) {
fprintf(stderr, "Device given to the cartesian interface is not available.\n");
interfaceError = true;
}
fprintf(stderr, "Opening crt interface...");
bool ok;
ok = partDd->view(crt);
if ((!ok) || (crt==0))
fprintf(stderr, "...crt was not ok...ok=%d", ok);
if (!partDd->isValid()) {
fprintf(stderr, "Cartesian device driver was not valid! \n");
dialog_severe_error(GTK_MESSAGE_ERROR,(char *) "Cartesian device not available.", (char *) "Check available devices", true);
interfaceError = true;
}
else if (!ok) {
fprintf(stderr, "Error while acquiring cartesian interfaces \n");
dialog_severe_error(GTK_MESSAGE_ERROR,(char *) "Problems acquiring cartesian interface", (char *) "Check if cartesian interface is running", true);
interfaceError = true;
}
if (interfaceError == false)
{
fprintf(stderr, "Allocating memory \n");
int j,k;
index = new int [MAX_NUMBER_OF_JOINTS];
entry_id = new guint [0];
*entry_id = -1;
frame_slider1 = new GtkWidget* [MAX_NUMBER_OF_JOINTS];
sliderArray = new GtkWidget* [NUMBER_OF_CARTESIAN_COORDINATES];
currPosArray = new GtkWidget* [NUMBER_OF_CARTESIAN_COORDINATES];
//fprintf(stderr, "sliderArray has address 0x%x\n", (unsigned int) sliderArray);
GtkWidget *top_hbox = NULL;
GtkWidget *bottom_hbox = NULL;
GtkWidget *panel_hbox = NULL;
GtkWidget *inv1 = NULL;
GtkWidget *invArray[NUMBER_OF_CARTESIAN_COORDINATES];
GtkWidget *homeArray[NUMBER_OF_CARTESIAN_COORDINATES];
GtkWidget *framesArray[NUMBER_OF_CARTESIAN_COORDINATES];
GtkWidget *sw = NULL;
//creation of the top_hbox
top_hbox = gtk_hbox_new (FALSE, 0);
gtk_container_set_border_width (GTK_CONTAINER (top_hbox), 10);
gtk_container_add (GTK_CONTAINER (vbox), top_hbox);
inv1 = gtk_fixed_new ();
gtk_container_add (GTK_CONTAINER (top_hbox), inv1);
Vector o;
Vector x;
while (!crt->getPose(x,o))
Time::delay(0.001);
Matrix R = axis2dcm(o);
Vector eu = dcm2euler(R);
//x(0) = 1; x(1) = 1; x(2) = 1;
//o(0) = 1; o(1) = 0; o(2) = 0; o(3) = 0;
char buffer[40] = {'i', 'n', 'i', 't'};
int numberOfRows = 3;
int height, width;
height = 100;
width = 180;
double min,max;
std::string limitString=partLabel;
limitString=limitString+"_workspace";
Bottle bCartesianLimits;
if (finder->check(limitString.c_str()))
{
//fprintf(stderr, "There seem limits for %s", partLabel);
bCartesianLimits = finder->findGroup(limitString.c_str());
//fprintf(stderr, "...got: %s", bCartesianLimits.toString().c_str());
}
fprintf(stderr, "Starting embedding cartesian GUI widgets \n");
for (k = 0; k<NUMBER_OF_CARTESIAN_COORDINATES; k++)
{
//fprintf(stderr, "Adding invArray \n");
invArray[k] = gtk_fixed_new ();
index[k]=k;
j = k/numberOfRows;
if (k==0)
{
sprintf(buffer, "x");
if (bCartesianLimits.check("xmin") && bCartesianLimits.check("xmax"))
{
min = bCartesianLimits.find("xmin").asDouble();
max = bCartesianLimits.find("xmax").asDouble();
}
else
{
min = x(0) - 0.1 * fabs(x(0));
max = x(0) + 0.1 * fabs(x(0));
}
}
if (k==1)
{
sprintf(buffer, "y");
if (bCartesianLimits.check("ymin") && bCartesianLimits.check("ymax"))
{
min = bCartesianLimits.find("ymin").asDouble();
max = bCartesianLimits.find("ymax").asDouble();
}
else
{
min = x(1) - 0.1 * fabs(x(1));
max = x(1) + 0.1 * fabs(x(1));
}
}
if (k==2)
{
sprintf(buffer, "z");
if (bCartesianLimits.check("zmin") && bCartesianLimits.check("zmax"))
{
min = bCartesianLimits.find("zmin").asDouble();
max = bCartesianLimits.find("zmax").asDouble();
}
else
{
min = x(2) - 0.1 * fabs(x(2));
max = x(2) + 0.1 * fabs(x(2));
}
}
if (k==3)
{
sprintf(buffer, "euler-alpha");
min = -180;
max = 180;
}
if (k==4)
{
sprintf(buffer, "euler-beta");
min = -180;
max = 180;
}
if (k==5)
{
sprintf(buffer, "euler-gamma");
min = -180;
max = 180;
}
frame_slider1[k] = gtk_frame_new ("Value:");
//fprintf(stderr, "Initializing sliders %d \n",k);
if (min<max)
{
sliderArray[k] = gtk_hscale_new_with_range(min, max, 1);
if (k<3)
gtk_scale_set_digits((GtkScale*) sliderArray[k],2);
else
gtk_scale_set_digits((GtkScale*) sliderArray[k],1);
}
else
{
sliderArray[k] = gtk_hscale_new_with_range(1, 2, 1);
if (k<3)
gtk_scale_set_digits((GtkScale*) sliderArray[k],2);
else
gtk_scale_set_digits((GtkScale*) sliderArray[k],1);
}
currPosArray[k] = gtk_entry_new();
//fprintf(stderr, "Initializing the buttons %d \n", k);
homeArray[k] = gtk_button_new_with_mnemonic ("Home");
//fprintf(stderr, "Initializing frames %d \n", k);
framesArray[k] = gtk_frame_new (buffer);
gtk_fixed_put (GTK_FIXED(inv1), invArray[k], 0+(k%numberOfRows)*width, 0+ j*height);
//Positions
//fprintf(stderr, "Positioning buttons %d \n", k);
gtk_fixed_put (GTK_FIXED(invArray[k]), frame_slider1[k], 60, 10 );
gtk_fixed_put (GTK_FIXED(invArray[k]), sliderArray[k], 65, 20 );
gtk_fixed_put (GTK_FIXED(invArray[k]), currPosArray[k], 95, 70);
int buttonDist= 24;
int buttonOffset = 13;
gtk_fixed_put (GTK_FIXED(invArray[k]), homeArray[k], 6, buttonOffset);
gtk_fixed_put (GTK_FIXED(invArray[k]), framesArray[k], 0, 0);
//Dimensions
//fprintf(stderr, "Dimensioning buttons %d \n", k);
gtk_widget_set_size_request (frame_slider1[k], 110, 50);
gtk_widget_set_size_request (sliderArray[k], 90, 40);
gtk_widget_set_size_request (currPosArray[k], 70, 20);
gtk_widget_set_size_request (homeArray[k], 50, 25);
gtk_widget_set_size_request (framesArray[k], width, height);
/*
* Positions commands
*/
//fprintf(stderr, "Assinging callback %d \n", k);
gtk_range_set_update_policy ((GtkRange *) (sliderArray[k]), GTK_UPDATE_DISCONTINUOUS);
if (k<3)
gtk_range_set_value ((GtkRange *) (sliderArray[k]), x(k));
if (k>=3 && k <= 5)
gtk_range_set_value ((GtkRange *) (sliderArray[k]), eu(k-3) * 180/M_PI);
g_signal_connect (sliderArray[k], "value-changed", G_CALLBACK(position_slider_changed), this);
}
/*
* Display current position
*/
*entry_id = gtk_timeout_add(UPDATE_TIME, (GtkFunction) display_cartesian_pose, this);
for (k = 0; k<NUMBER_OF_CARTESIAN_COORDINATES; k++)
gtk_editable_set_editable ((GtkEditable*) currPosArray[k], FALSE);
/*
* Common commands
*/
GtkWidget *frame3;
frame3 = gtk_frame_new ("Commands:");
gtk_fixed_put (GTK_FIXED(inv1), frame3, (NUMBER_OF_CARTESIAN_COORDINATES%numberOfRows)*width, (NUMBER_OF_CARTESIAN_COORDINATES/numberOfRows)*height);
gtk_widget_set_size_request (frame3, 180, 240);
//Button 0 in the panel
GtkWidget *button0 = gtk_button_new_with_mnemonic ("Open sequence tab");
gtk_fixed_put (GTK_FIXED (inv1), button0, 10+(NUMBER_OF_CARTESIAN_COORDINATES%numberOfRows)*width, 20+(NUMBER_OF_CARTESIAN_COORDINATES/numberOfRows)*height);
gtk_widget_set_size_request (button0, 150, 25);
fprintf(stderr, "Initializing the table \n");
init_cartesian_table();
fprintf(stderr, "Connecting the callbacks for the table \n");
g_signal_connect (button0, "clicked", G_CALLBACK (cartesian_table_open), this);
//Button1 in the panel
GtkWidget *button1 = gtk_button_new_with_mnemonic ("Stop");
gtk_fixed_put (GTK_FIXED (inv1), button1, 10+(NUMBER_OF_CARTESIAN_COORDINATES%numberOfRows)*width, 45+(NUMBER_OF_CARTESIAN_COORDINATES/numberOfRows)*height);
gtk_widget_set_size_request (button1, 150, 25);
//g_signal_connect (button1, "clicked", G_CALLBACK (stop_motion), crt);
//Velocity
GtkWidget *frame7;
frame7 = gtk_frame_new ("Time[sec]:");
gtk_fixed_put (GTK_FIXED(inv1), frame7, 5+(NUMBER_OF_CARTESIAN_COORDINATES%numberOfRows)*width, 70 + (NUMBER_OF_CARTESIAN_COORDINATES/numberOfRows)*height);
gtk_widget_set_size_request (frame7, 160, 50);
sliderVelocity = new GtkWidget;
sliderVelocity = gtk_hscale_new_with_range(1, 10, 1);
gtk_scale_set_digits((GtkScale*) sliderVelocity,2);
gtk_fixed_put (GTK_FIXED(inv1), sliderVelocity, 60+(NUMBER_OF_CARTESIAN_COORDINATES%numberOfRows)*width, 80 + (NUMBER_OF_CARTESIAN_COORDINATES/numberOfRows)*height);
gtk_widget_set_size_request (sliderVelocity, 90, 40);
gtk_range_set_update_policy ((GtkRange *) (sliderVelocity), GTK_UPDATE_DISCONTINUOUS);
gtk_range_set_value ((GtkRange *) (sliderVelocity), 2);
//Diplay axis
po = new GtkWidget*[4];
for (int i=0; i < 4; i++)
{
po[i] = gtk_entry_new();
}
//Display axis
GtkWidget *frame5;
frame5 = gtk_frame_new ("Axis:");
gtk_fixed_put (GTK_FIXED(inv1), frame5, 5+(NUMBER_OF_CARTESIAN_COORDINATES%numberOfRows)*width, 140 + (NUMBER_OF_CARTESIAN_COORDINATES/numberOfRows)*height);
gtk_widget_set_size_request (frame5, 80, 85);
gtk_fixed_put (GTK_FIXED(inv1), po[0], 10+(NUMBER_OF_CARTESIAN_COORDINATES%numberOfRows)*width, 155+(NUMBER_OF_CARTESIAN_COORDINATES/numberOfRows)*height);
gtk_fixed_put (GTK_FIXED(inv1), po[1], 10+(NUMBER_OF_CARTESIAN_COORDINATES%numberOfRows)*width, 175+(NUMBER_OF_CARTESIAN_COORDINATES/numberOfRows)*height);
gtk_fixed_put (GTK_FIXED(inv1), po[2], 10+(NUMBER_OF_CARTESIAN_COORDINATES%numberOfRows)*width, 195+(NUMBER_OF_CARTESIAN_COORDINATES/numberOfRows)*height);
//Display angle
GtkWidget *frame6;
frame6 = gtk_frame_new ("Angle:");
gtk_fixed_put (GTK_FIXED(inv1), frame6, 85+(NUMBER_OF_CARTESIAN_COORDINATES%numberOfRows)*width, 155 + (NUMBER_OF_CARTESIAN_COORDINATES/numberOfRows)*height);
gtk_widget_set_size_request (frame6, 80, 45);
gtk_fixed_put (GTK_FIXED(inv1), po[3], 90+(NUMBER_OF_CARTESIAN_COORDINATES%numberOfRows)*width, 175+(NUMBER_OF_CARTESIAN_COORDINATES/numberOfRows)*height);
for (int i=0; i < 4; i++)
{
gtk_widget_set_size_request (po[i], 70, 20);
gtk_editable_set_editable ((GtkEditable*) po[i], FALSE);
}
*entry_id = gtk_timeout_add(UPDATE_TIME, (GtkFunction) display_axis_pose, this);
//CheckButton in the panel
GtkWidget *check= gtk_check_button_new_with_mnemonic ("Tracking Mode");
gtk_fixed_put (GTK_FIXED (inv1), check, 10+(NUMBER_OF_CARTESIAN_COORDINATES%numberOfRows)*width, 120+(NUMBER_OF_CARTESIAN_COORDINATES/numberOfRows)*height);
gtk_widget_set_size_request (check, 150, 25);
//g_signal_connect (check, "clicked", G_CALLBACK (toggle_tracking_mode), crt);
}
}
bool Bottle::operator==(const Bottle& alt) const
{
return toString() == alt.toString();
}
