std::string TcpRosStream::rosToKind(const char *rosname) { if (ConstString(rosname)=="") return ""; std::map<std::string, std::string> kinds = rosToKind(); if (kinds.find(rosname)!=kinds.end()) { return kinds[rosname]; } Port port; port.openFake("yarpidl_rosmsg"); if (port.addOutput("/typ")) { Bottle cmd, resp; cmd.addString(ConstString("twiddle ") + rosname); dbg_printf("QUERY yarpidl_rosmsg %s\n", cmd.toString().c_str()); port.write(cmd,resp); dbg_printf("GOT yarpidl_rosmsg %s\n", resp.toString().c_str()); ConstString txt = resp.get(0).asString(); if (txt!="?") return txt; } port.close(); if (ConstString(rosname)!="") { fprintf(stderr, "Do not know anything about type '%s'\n", rosname); fprintf(stderr, "Could not connect to a type server to look up type '%s'\n", rosname); ::exit(1); } return ""; }
int main() { yarp::os::Network yarp; yarp::os::Bottle reply; Port port; port.open("/moveBall:o"); port.addOutput("/icubSim/world"); Bottle del_all; del_all.addString("world"); del_all.addString("del"); del_all.addString("all"); port.write(del_all, reply); Bottle create_obj("world mk ssph"); create_obj.addDouble(0.1); //radius create_obj.addInt(1); //x create_obj.addInt(1); //y create_obj.addInt(1); //z create_obj.addInt(0); //r create_obj.addInt(0); //g create_obj.addInt(1); //b port.write(create_obj, reply); if (reply.get(0).asVocab()!=Vocab::encode("ok")) fprintf(stderr, "Error setting sphere\n"); double start = Time::now(); while (true) { double t = Time::now() - start; double dx = cos(t); double dy = sin(t); Bottle move_obj; move_obj.addString("world"); move_obj.addString("set"); move_obj.addString("ssph"); move_obj.addInt(1); //radius move_obj.addDouble(dx/2); //x move_obj.addDouble(dy/2+1); //y move_obj.addDouble(1); //z port.write(move_obj, reply); if (reply.get(0).asVocab()!=Vocab::encode("ok")) fprintf(stderr, "Error moving sphere\n"); Time::delay(0.1); } return 0; }
int main(int argc, char *argv[]) { Network yarp; int r; SkinCommand skin; Port port; skin.yarp().attachAsClient(port); if (!port.open("/skin/client")) { return 1; } if (!port.addOutput("/skin/server")) { return 1; } skin.calib(); vector<double> thresholds = skin.get_touch_thresholds(); for (vector<double>::iterator it = thresholds.begin(); it != thresholds.end(); it++) { printf("Threshold: %g\n", *it); } skin.set_smooth_factor(0.42); printf("Smooth factor is %g\n", skin.get_smooth_factor()); map<string,string> info = skin.get_info(); for (map<string,string>::iterator it = info.begin(); it != info.end(); it++) { printf("Info: %s -> %s\n", it->first.c_str(), it->second.c_str()); } vector<TaxelPose> poses = skin.get_poses_all(); printf("%d poses found\n", poses.size()); for (vector<TaxelPose>::iterator it = poses.begin(); it != poses.end(); it++) { // Place yarp::sig::Vectors into bottles from printing Bottle pos, angle; pos.read(it->position); angle.read(it->normal); printf("Pose: %s / %s\n", pos.toString().c_str(), angle.toString().c_str()); } port.close(); return 0; }
bool NetworkBase::write(const Contact& contact, PortWriter& cmd, PortReader& reply, const ContactStyle& style) { if (!getNameSpace().serverAllocatesPortNumbers()) { // switch to more up-to-date method Port port; port.setAdminMode(style.admin); port.openFake("network_write"); Contact ec = contact; if (style.carrier!="") { ec.setCarrier(style.carrier); } if (!port.addOutput(ec)) { if (!style.quiet) { ACE_OS::fprintf(stderr, "Cannot make connection to '%s'\n", ec.toString().c_str()); } return false; } bool ok = port.write(cmd,reply); return ok; } const char *connectionName = "admin"; ConstString name = contact.getName(); const char *targetName = name.c_str(); // use carefully! Contact address = contact; if (!address.isValid()) { address = getNameSpace().queryName(targetName); } if (!address.isValid()) { if (!style.quiet) { YARP_SPRINTF1(Logger::get(),error, "cannot find port %s", targetName); } return false; } if (style.timeout>0) { address.setTimeout((float)style.timeout); } OutputProtocol *out = Carriers::connect(address); if (out==YARP_NULLPTR) { if (!style.quiet) { YARP_SPRINTF1(Logger::get(),error, "Cannot connect to port %s", targetName); } return false; } if (style.timeout>0) { out->setTimeout(style.timeout); } Route r(connectionName,targetName, (style.carrier!="")?style.carrier.c_str():"text_ack"); out->open(r); PortCommand pc(0,style.admin?"a":"d"); BufferedConnectionWriter bw(out->getConnection().isTextMode(), out->getConnection().isBareMode()); bool ok = true; if (out->getConnection().canEscape()) { ok = pc.write(bw); } if (!ok) { if (!style.quiet) { YARP_ERROR(Logger::get(),"could not write to connection"); } if (out!=YARP_NULLPTR) delete out; return false; } ok = cmd.write(bw); if (!ok) { if (!style.quiet) { YARP_ERROR(Logger::get(),"could not write to connection"); } if (out!=YARP_NULLPTR) delete out; return false; } if (style.expectReply) { bw.setReplyHandler(reply); } out->write(bw); if (out!=YARP_NULLPTR) { delete out; out = YARP_NULLPTR; } return true; }