C++ Contact::addCarrier方法代码示例

        bool configure(ResourceFinder &rf)
        {
            Property config;
            config.fromConfigFile(rf.getContext() + "config.ini");
            
            Bottle &bGeneral = config.findGroup("host_computer");

            ip_address = bGeneral.find("ip_address").asString().c_str();
            cout << "Host computer ip address: " << ip_address << endl;

            gtw_contactTactileLeftInputPort = gtw_contactTactileLeftInputPort.addName("/gtw/telepresence/tactile/left:i");
            gtw_contactTactileLeftInputPort = gtw_contactTactileLeftInputPort.addCarrier("tcp");
            gtw_contactTactileLeftInputPort = gtw_contactTactileLeftInputPort.addHost(ip_address);
            gtw_contactTactileLeftInputPort = gtw_contactTactileLeftInputPort.addPort(80005);

            gtw_contactTactileRightInputPort = gtw_contactTactileRightInputPort.addName("/gtw/telepresence/tactile/right:i");
            gtw_contactTactileRightInputPort = gtw_contactTactileRightInputPort.addCarrier("tcp");
            gtw_contactTactileRightInputPort = gtw_contactTactileRightInputPort.addHost(ip_address);
            gtw_contactTactileRightInputPort = gtw_contactTactileRightInputPort.addPort(80006);

            gtw_contactTactileLeftOutputPort = gtw_contactTactileLeftOutputPort.addName("/gtw/telepresence/tactile/left:o");
            gtw_contactTactileLeftOutputPort = gtw_contactTactileLeftOutputPort.addCarrier("tcp");
            gtw_contactTactileLeftOutputPort = gtw_contactTactileLeftOutputPort.addHost(ip_address);
            gtw_contactTactileLeftOutputPort = gtw_contactTactileLeftOutputPort.addPort(80007);

            gtw_contactTactileRightOutputPort = gtw_contactTactileRightOutputPort.addName("/gtw/telepresence/tactile/right:o");
            gtw_contactTactileRightOutputPort = gtw_contactTactileRightOutputPort.addCarrier("tcp");
            gtw_contactTactileRightOutputPort = gtw_contactTactileRightOutputPort.addHost(ip_address);
            gtw_contactTactileRightOutputPort = gtw_contactTactileRightOutputPort.addPort(80008);


            Network::registerContact(gtw_contactTactileLeftInputPort);
            Network::registerContact(gtw_contactTactileRightInputPort);
            Network::registerContact(gtw_contactTactileLeftOutputPort);
            Network::registerContact(gtw_contactTactileRightOutputPort);

            gtw_tactileLeftInputPort.open(gtw_contactTactileLeftInputPort, true); // give the port a name
            gtw_tactileRightInputPort.open(gtw_contactTactileRightInputPort, true); // give the port a name
            gtw_tactileLeftOutputPort.open(gtw_contactTactileLeftOutputPort, true); // give the port a name
            gtw_tactileRightOutputPort.open(gtw_contactTactileRightOutputPort, true); // give the port a name


            cout << "Waiting for connection: /icub/skin/left_hand_comp -> /gtw/telepresence/tactile/left:i" << endl;
            while( !Network::isConnected("/icub/skin/left_hand_comp", "/gtw/telepresence/tactile/left:i") )
            {}
            cout << "Connection ready: /icub/skin/left_hand_comp -> /gtw/telepresence/tactile/left:i" << endl;

            cout << "Waiting for connection: /icub/skin/right_hand_comp -> /gtw/telepresence/tactile/right:i" << endl;
            while( !Network::isConnected("/icub/skin/right_hand_comp", "/gtw/telepresence/tactile/right:i") )
            {}
            cout << "Connection ready: /icub/skin/right_hand_comp -> /gtw/telepresence/tactile/right:i" << endl;

            return true;
        }

Contact RosNameSpace::detectNameServer(bool useDetectedServer,
                                       bool& scanNeeded,
                                       bool& serverUsed) {
    NameConfig nc;
    nc.fromFile();
    Contact c = nc.getAddress();
    scanNeeded = false;
    serverUsed = false;

    if (!c.isValid()) {
        scanNeeded = true;
        fprintf(stderr,"Checking for ROS_MASTER_URI...\n");
        ConstString addr = NetworkBase::getEnvironment("ROS_MASTER_URI");
        c = Contact::fromString(addr.c_str());
        if (c.isValid()) {
            c = c.addCarrier("xmlrpc");
            c = c.addName(nc.getNamespace().c_str());
            NameConfig nc;
            nc.setAddress(c);
            nc.setMode("ros");
            nc.toFile();
            serverUsed = true;
         }
    }
    return c;
}

bool RosNameSpace::connectTopic(Bottle& cmd,
                                bool srcIsTopic,
                                const Contact& src,
                                const Contact& dest,
                                ContactStyle style,
                                bool activeRegistration) {
    Bottle reply;
    Contact dynamicSrc = src;
    Contact dynamicDest = dest;
    if (style.carrier!="") {
        if (srcIsTopic) {
            dynamicDest = dynamicDest.addCarrier(style.carrier);
        } else {
            dynamicSrc = dynamicSrc.addCarrier(style.carrier);
        }
    }
    Contact base = getNameServerContact();
    bool ok = NetworkBase::write(base,
                                    cmd,
                                    reply);
    bool fail = (reply.check("faultCode",Value(0)).asInt()!=0)||!ok;
    if (fail) {
        if (!style.quiet) {
            fprintf(stderr,"Failure: name server did not accept connection to topic.\n");
            if (reply.check("faultString")) {
                fprintf(stderr,"Cause: %s\n", reply.check("faultString",Value("")).asString().c_str());
            }
        }
    }
    if (!fail) {
        if (activeRegistration) {
            Bottle *lst = reply.get(2).asList();
            Bottle cmd2;
            if (lst!=NULL) {
                cmd2.addString("publisherUpdate");
                cmd2.addString("/yarp");
                cmd2.addString(dynamicSrc.getName());
                cmd2.addList() = *lst;
                NetworkBase::write(dynamicDest,
                                    cmd2,
                                    reply,
                                    true);
            }
        }
    }
    return !fail;
}

yarp::os::Contact RosLookup::getRosCoreAddressFromEnv() {
    ConstString addr = NetworkBase::getEnvironment("ROS_MASTER_URI");
    Contact c = Contact::fromString(addr.c_str());
    if (c.isValid()) {
        c = c.addCarrier("xmlrpc");
    }
    return c;
}

Contact RosNameSpace::queryName(const ConstString& name) {
    dbg_printf("ROSNameSpace queryName(%s)\n", name.c_str());
    NestedContact nc(name);
    ConstString full = name;
    ConstString node = nc.getNodeName();
    ConstString srv = nc.getNestedName();
    ConstString cat = nc.getCategory();
    bool is_service = false;

    Bottle cmd,reply;
    if (cat.find("-1")==ConstString::npos) {
        cmd.addString("lookupNode");
        cmd.addString("dummy_id");
        cmd.addString(toRosNodeName(node));
        NetworkBase::write(getNameServerContact(),
                           cmd, reply);
    }
    Contact contact;
    if (reply.get(0).asInt()!=1) {
        cmd.clear();
        reply.clear();
        cmd.addString("lookupService");
        cmd.addString("dummy_id");
        cmd.addString(toRosNodeName(node));
        NetworkBase::write(getNameServerContact(),
                           cmd, reply);
        is_service = true;
    }
    contact = Contact::fromString(reply.get(2).asString());
    // unfortunate differences in labeling carriers
    if (contact.getCarrier()=="rosrpc") {
        contact = contact.addCarrier(ConstString("rossrv+service.") + name);
    } else {
        contact = contact.addCarrier("xmlrpc");
    }
    contact = contact.addName(name);

    if (srv == "" || !is_service) return contact;

    return Contact();
}

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 = ec.addCarrier(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==NULL) {
        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!=NULL) delete out;
        return false;
    }
    ok = cmd.write(bw);
    if (!ok) {
        if (!style.quiet) {
            YARP_ERROR(Logger::get(),"could not write to connection");
        }
        if (out!=NULL) delete out;
        return false;
    }
    if (style.expectReply) {
        bw.setReplyHandler(reply);
    }
    out->write(bw);
    if (out!=NULL) {
        delete out;
        out = NULL;
    }
    return true;
}

static int metaConnect(const ConstString& src,
                       const ConstString& dest,
                       ContactStyle style,
                       int mode) {
    YARP_SPRINTF3(Logger::get(),debug,
                  "working on connection %s to %s (%s)",
                  src.c_str(),
                  dest.c_str(),
                  (mode==YARP_ENACT_CONNECT)?"connect":((mode==YARP_ENACT_DISCONNECT)?"disconnect":"check")
                  );

    // get the expressed contacts, without name server input
    Contact dynamicSrc = Contact::fromString(src);
    Contact dynamicDest = Contact::fromString(dest);

    bool topical = style.persistent;
    if (dynamicSrc.getCarrier()=="topic" ||
        dynamicDest.getCarrier()=="topic") {
        topical = true;
    }

    bool topicalNeedsLookup = !getNameSpace().connectionHasNameOfEndpoints();

    // fetch completed contacts from name server, if needed
    Contact staticSrc;
    Contact staticDest;
    if (needsLookup(dynamicSrc)&&(topicalNeedsLookup||!topical)) {
        staticSrc = NetworkBase::queryName(dynamicSrc.getName());
        if (!staticSrc.isValid()) {
            if (!style.persistent) {
                if (!style.quiet) {
                    fprintf(stderr, "Failure: could not find source port %s\n",
                            src.c_str());
                }
                return 1;
            } else {
                staticSrc = dynamicSrc;
            }
        }
    } else {
        staticSrc = dynamicSrc;
    }
    if (staticSrc.getCarrier()=="") {
        staticSrc = staticSrc.addCarrier("tcp");
    }
    if (staticDest.getCarrier()=="") {
        staticDest = staticDest.addCarrier("tcp");
    }

    if (needsLookup(dynamicDest)&&(topicalNeedsLookup||!topical)) {
        staticDest = NetworkBase::queryName(dynamicDest.getName());
        if (!staticDest.isValid()) {
            if (!style.persistent) {
                if (!style.quiet) {
                    fprintf(stderr, "Failure: could not find destination port %s\n",
                            dest.c_str());
                }
                return 1;
            } else {
                staticDest = dynamicDest;
            }
        }
    } else {
        staticDest = dynamicDest;
    }

    if (staticSrc.getCarrier()=="xmlrpc" &&
        (staticDest.getCarrier()=="xmlrpc"||(staticDest.getCarrier().find("rossrv")==0))&&
        mode==YARP_ENACT_CONNECT) {
        // Unconnectable in general
        // Let's assume the first part is a YARP port, and use "tcp" instead
        staticSrc = staticSrc.addCarrier("tcp");
        staticDest = staticDest.addCarrier("tcp");
    }

    ConstString carrierConstraint = "";

    // see if we can do business with the source port
    bool srcIsCompetent = false;
    bool srcIsTopic = false;
    if (staticSrc.getCarrier()!="topic") {
        if (!topical) {
            Carrier *srcCarrier = NULL;
            if (staticSrc.getCarrier()!="") {
                srcCarrier = Carriers::chooseCarrier(staticSrc.getCarrier().c_str());
            }
            if (srcCarrier!=NULL) {
                String srcBootstrap = srcCarrier->getBootstrapCarrierName();
                if (srcBootstrap!="") {
                    srcIsCompetent = true;
                } else {
                    carrierConstraint = staticSrc.getCarrier();
                }
                delete srcCarrier;
                srcCarrier = NULL;
            }
        }
    } else {
        srcIsTopic = true;
    }
//.........这里部分代码省略.........

static int enactConnection(const Contact& src,
                           const Contact& dest,
                           const ContactStyle& style,
                           int mode,
                           bool reversed) {
    ContactStyle rpc;
    rpc.admin = true;
    rpc.quiet = style.quiet;
    rpc.timeout = style.timeout;

    if (style.persistent) {
        bool ok = false;
        // we don't talk to the ports, we talk to the nameserver
        NameSpace& ns = getNameSpace();
        if (mode==YARP_ENACT_CONNECT) {
            ok = ns.connectPortToPortPersistently(src,dest,style);
        } else if (mode==YARP_ENACT_DISCONNECT) {
            ok = ns.disconnectPortToPortPersistently(src,dest,style);
        } else {
            fprintf(stderr,"Failure: cannot check subscriptions yet\n");
            return 1;
        }
        if (!ok) {
            return 1;
        }
        if (!style.quiet) {
            fprintf(stderr,"Success: port-to-port persistent connection added.\n");
        }
        return 0;
    }

    if (mode==YARP_ENACT_EXISTS) {
        Bottle cmd, reply;
        cmd.addVocab(Vocab::encode("list"));
        cmd.addVocab(Vocab::encode(reversed?"in":"out"));
        cmd.addString(dest.getName().c_str());
        YARP_SPRINTF2(Logger::get(),debug,"asking %s: %s",
                      src.toString().c_str(), cmd.toString().c_str());
        bool ok = NetworkBase::write(src,cmd,reply,rpc);
        if (!ok) {
            noteDud(src);
            return 1;
        }
        if (reply.check("carrier")) {
            if (!style.quiet) {
                printf("Connection found between %s and %s\n",
                       src.getName().c_str(), dest.getName().c_str());
            }
            return 0;
        }
        return 1;
    }

    int act = (mode==YARP_ENACT_DISCONNECT)?VOCAB3('d','e','l'):VOCAB3('a','d','d');

    // Let's ask the destination to connect/disconnect to the source.
    // We assume the YARP carrier will reverse the connection if
    // appropriate when connecting.
    Bottle cmd, reply;
    cmd.addVocab(act);
    Contact c = dest;
    if (style.carrier!="") {
        c = c.addCarrier(style.carrier);
    }
    if (mode!=YARP_ENACT_DISCONNECT) {
        cmd.addString(c.toString());
    } else {
        cmd.addString(c.getName());
    }

    Contact c2 = src;
    if (c2.getPort()<=0) {
        c2 = NetworkBase::queryName(c2.getName());
    }
    if (c2.getCarrier()!="tcp") {
        YARP_SPRINTF2(Logger::get(),debug,"would have asked %s: %s",
                      src.toString().c_str(), cmd.toString().c_str());
        return 1;
    }

    YARP_SPRINTF2(Logger::get(),debug,"** asking %s: %s",
                  src.toString().c_str(), cmd.toString().c_str());
    bool ok = NetworkBase::write(c2,cmd,reply,rpc);
    if (!ok) {
        noteDud(src);
        return 1;
    }
    ok = false;
    ConstString msg = "";
    if (reply.get(0).isInt()) {
        ok = (reply.get(0).asInt()==0);
        msg = reply.get(1).asString();
    } else {
        // older protocol
        msg = reply.get(0).asString();
        ok = msg[0]=='A'||msg[0]=='R';
    }
    if (mode==YARP_ENACT_DISCONNECT && !ok) {
        msg = "no such connection\n";
    }
//.........这里部分代码省略.........

Contact RosNameSpace::registerAdvanced(const Contact& contact, NameStore *store) {
    dbg_printf("ROSNameSpace registerContact(%s / %s)\n", 
               contact.toString().c_str(),
               contact.toURI().c_str());
    NestedContact nc = contact.getNested();
    if (nc.getNestedName()=="") {
        nc.fromString(contact.getName());
    }
    ConstString cat = nc.getCategory();
    if (nc.getNestedName()!="") {
        if (cat == "-1") {
            Bottle cmd, reply;
            cmd.clear();
            cmd.addString("registerService");
            cmd.addString(toRosNodeName(nc.getNodeName()));
            cmd.addString(toRosName(nc.getNestedName()));
            Contact rosrpc = contact.addCarrier("rosrpc");
            cmd.addString(rosrpc.toURI());
            Contact c;
            if (store) {
                c = rosify(store->query(nc.getNodeName()));
            } else {
                Nodes& nodes = NameClient::getNameClient().getNodes();
                c = rosify(nodes.getParent(contact.getName()));
            }
            cmd.addString(c.toString());
            bool ok = NetworkBase::write(getNameServerContact(),
                                         cmd, reply);
            if (!ok) return Contact();
        } else if (cat == "+" || cat== "-") {
            Bottle cmd, reply;
            cmd.clear();
            cmd.addString((cat=="+")?"registerPublisher":"registerSubscriber");
            cmd.addString(toRosNodeName(nc.getNodeName()));
            cmd.addString(toRosName(nc.getNestedName()));
            ConstString typ = nc.getTypeNameStar();
            if (typ!="*"&&typ!="") {
                // remap some basic native YARP types
                if (typ=="yarp/image") {
                    typ = "sensor_msgs/Image";
                } else if (typ=="yarp/vector") {
                    typ = "std_msgs/Float64MultiArray";
                }
                if (typ.find("/")==ConstString::npos) {
                    typ = ConstString("yarp/") + typ;
                }
            }
            cmd.addString(typ);
            Contact c;
            if (store) {
                c = rosify(store->query(nc.getNodeName()));
            } else {
                Nodes& nodes = NameClient::getNameClient().getNodes();
                c = rosify(nodes.getParent(contact.getName()));
            }
            //Contact c = rosify(contact);
            cmd.addString(c.toString());
            bool ok = NetworkBase::write(getNameServerContact(),
                                         cmd, reply);
            if (!ok) {
                fprintf(stderr, "ROS registration error: %s\n", reply.toString().c_str());
                return Contact();
            }
            if (cat=="-") {
                Bottle *publishers = reply.get(2).asList();
                if (publishers && publishers->size()>=1) {
                    cmd.clear();
                    cmd.addString(contact.toURI());
                    cmd.addString("publisherUpdate");
                    cmd.addString("/yarp/RosNameSpace");
                    cmd.addString(toRosName(nc.getNestedName()));
                    cmd.addList() = *publishers;

                    mutex.wait();
                    bool need_start = false;
                    if (pending.size()==0) {
                        mutex.post();
                        stop();
                        need_start = true;
                        mutex.wait();
                    }
                    pending.addList() = cmd;
                    if (need_start) {
                        start();
                    }
                    mutex.post();
                }
            }
        }
        return contact;
    }

    // Remainder of method is supporting older /name+#/foo syntax

    ConstString full = contact.getName();
    ConstString name = full;
    size_t pub_idx = name.find("+#");
    size_t sub_idx = name.find("-#");

    ConstString node = "";
//.........这里部分代码省略.........