C++ Port::addOutput方法代码示例

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