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