C++ ConstString::c_str方法代码示例

    virtual bool configure(ResourceFinder &rf)
    {
        string slash = "/";
        string ctrlName;
        string robotName;
       // string remoteName;
        string localName;

        Time::turboBoost();

        // get params from the RF
        ctrlName = rf.check("local", Value("robotJoystickControl")).asString();
        robotName = rf.check("robot", Value("cer")).asString();

        localName = "/" + ctrlName;

        //reads the configuration file
        Property ctrl_options;

        ConstString configFile = rf.findFile("from");
        if (configFile == "") //--from torsoJoystickControl.ini
        {
            yWarning("Cannot find .ini configuration file. By default I'm searching for torsoJoystickControl.ini");
            //return false;
        }
        else
        {
            ctrl_options.fromConfigFile(configFile.c_str());
        }

        ctrl_options.put("robot", robotName.c_str());
        ctrl_options.put("local", localName.c_str());

        //check for robotInterface availablity
        yInfo("Checking for robotInterface availability");
        Port startport;
        startport.open(localName+"/robotInterfaceCheck:rpc");


        Bottle cmd; cmd.addString("is_ready");
        Bottle response;
        int rc_count = 0;
        int rp_count = 0;
        int rf_count = 0;
        double start_time = yarp::os::Time::now();
        bool not_yet_connected = true;

        bool skip_robot_interface_check = rf.check("skip_robot_interface_check");
        if (skip_robot_interface_check)
        {
            yInfo("skipping robotInterface check");
        }
        else
        {
            do
            {
                if (not_yet_connected)
                {
                    bool rc = yarp::os::Network::connect(localName + "/robotInterfaceCheck:rpc", "/" + robotName + "/robotInterface");
                    if (rc == false)
                    {
                        yWarning("Problems trying to connect to RobotInterface %d", rc_count++);
                        yarp::os::Time::delay(1.0);
                        continue;
                    }
                    else
                    {
                        not_yet_connected = false;
                        yDebug("Connection established with robotInterface");
                    }
                }

                bool rp = startport.write(cmd, response);
                if (rp == false)
                {
                    yWarning("Problems trying to connect to RobotInterface %d", rp_count++);
                    if (yarp::os::Time::now() - start_time > 30)
                    {
                        yError("Timeout expired while trying to connect to robotInterface");
                        return false;
                    }
                    yarp::os::Time::delay(1.0);
                    continue;
                }
                else
                {
                    if (response.get(0).asString() != "ok")
                    {
                        yWarning("RobotInterface is not ready yet, retrying... %d", rf_count++);
                        if (yarp::os::Time::now() - start_time > 30)
                        {
                            yError("Timeout expired while waiting for robotInterface availability");
                            return false;
                        }
                        yarp::os::Time::delay(1.0);
                        continue;
                    }
                    else
                    {
                        yInfo("RobotInterface is ready");
//.........这里部分代码省略.........

String NameConfig::getHostName(bool prefer_loopback) {
    // try to pick a good host identifier

    ConstString result = "127.0.0.1";
    bool loopback = true;

    // Pick an IPv4 address.
    // Prefer non-local addresses, then shorter addresses.
    // Avoid IPv6.
#ifdef YARP_HAS_ACE
    ACE_INET_Addr *ips = NULL;
    size_t count = 0;
    if (ACE::get_ip_interfaces(count,ips)>=0) {
        for (size_t i=0; i<count; i++) {
            ConstString ip = ips[i].get_host_addr();
#else
    int family, s;
    char hostname[NI_MAXHOST]; hostname[NI_MAXHOST-1] = '\0';
    ConstString ip;
    struct ifaddrs *ifaddr, *ifa;
    if (getifaddrs(&ifaddr) == -1) {
    	perror("getifaddrs in getIps");
    	exit(EXIT_FAILURE);
    }
    for (ifa = ifaddr; ifa != NULL; ifa = ifa->ifa_next) {
    	if (ifa->ifa_addr == NULL) continue;
    	family = ifa->ifa_addr->sa_family;
    	if (family == AF_INET || family == AF_INET6) {
    		s = getnameinfo(ifa->ifa_addr,
    				(family == AF_INET) ? sizeof(struct sockaddr_in) :
    						sizeof(struct sockaddr_in6),
    						hostname, NI_MAXHOST, NULL, 0, NI_NUMERICHOST);
    		if (s != 0) {
    			printf("getnameinfo() failed: %s\n", gai_strerror(s));
    			exit(EXIT_FAILURE);
    		}
    		ip = ConstString(hostname);
#endif

            YARP_DEBUG(Logger::get(), String("scanning network interface ") +
                       ip.c_str());
            bool take = prefer_loopback;
            if (ip.find(":")!=ConstString::npos) continue;
            if (!take) {
                if (result=="localhost") {
                    take = true; // can't be worse
                }
                if (loopback) {
                    take = true; // can't be worse
                } else if (ip.length()<result.length()) {
                    take = true;
                }
            }
            if (take) {
                if (!prefer_loopback) result = ip;
                loopback = false;
                if (ip == "127.0.0.1" || ip == "127.1.0.1" ||
                    ip == "127.0.1.1") {
                    loopback = true;
                }
#ifdef YARP_HAS_ACE
#ifdef YARP_ACE_ADDR_HAS_LOOPBACK_METHOD
                loopback = ips[i].is_loopback();
#endif
#endif

                if (prefer_loopback && loopback) {
                    result = ip;
                }
            }
        }
#ifdef YARP_HAS_ACE
        delete[] ips;
#endif
    }

    return result.c_str();
}


bool NameConfig::isLocalName(const String& name) {
    bool result = false;

#ifdef YARP_HAS_ACE
    ACE_INET_Addr *ips = NULL;
    size_t count = 0;
    if (ACE::get_ip_interfaces(count,ips)>=0) {
        for (size_t i=0; i<count; i++) {
            String ip = ips[i].get_host_addr();
            if (ip==name) {
                result = true;
                break;
            }
        }
        delete[] ips;
    }
#else
    /**
     * If this does not work properly, use a more sophisticated way
     * instead of just gethostname.
//.........这里部分代码省略.........

bool SubscriberOnSql::hookup(const ConstString& port) {
    if (getDelegate()) {
        NestedContact nc(port);
        if (nc.getNestedName().size()>0) {
            return false;
        }
    }
    mutex.wait();
    sqlite3_stmt *statement = NULL;
    char *query = NULL;
    //query = sqlite3_mprintf("SELECT * FROM subscriptions WHERE src = %Q OR dest= %Q",port, port);
    query = sqlite3_mprintf("SELECT src,dest,srcFull,destFull FROM subscriptions WHERE (src = %Q OR dest= %Q) AND EXISTS (SELECT NULL FROM live WHERE name=src) AND EXISTS (SELECT NULL FROM live WHERE name=dest) UNION SELECT s1.src, s2.dest, s1.srcFull, s2.destFull FROM subscriptions s1, subscriptions s2, topics t WHERE (s1.dest = t.topic AND s2.src = t.topic) AND (s1.src = %Q OR s2.dest = %Q) AND EXISTS (SELECT NULL FROM live WHERE name=s1.src) AND EXISTS (SELECT NULL FROM live WHERE name=s2.dest)",port.c_str(), port.c_str(), port.c_str(), port.c_str());
    // 
    if (verbose) {
        printf("Query: %s\n", query);
    }
    int result = sqlite3_prepare_v2(SQLDB(implementation),query,-1,&statement,
                                    NULL);
    if (result!=SQLITE_OK) {
        const char *msg = sqlite3_errmsg(SQLDB(implementation));
        if (msg!=NULL) {
            fprintf(stderr,"Error: %s\n", msg);
        }
    }
    while (result == SQLITE_OK && sqlite3_step(statement) == SQLITE_ROW) {
        char *src = (char *)sqlite3_column_text(statement,0);
        char *dest = (char *)sqlite3_column_text(statement,1);
        char *srcFull = (char *)sqlite3_column_text(statement,2);
        char *destFull = (char *)sqlite3_column_text(statement,3);
        char *mode = (char *)sqlite3_column_text(statement,4);
        checkSubscription(src,dest,srcFull,destFull,mode?mode:"");
    }
    sqlite3_finalize(statement);
    sqlite3_free(query);
    mutex.post();

    return false;
}

bool file::read(ImageOf<PixelMono> & dest, const ConstString& src)
{
    return ImageReadMono(dest,src.c_str());
}

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;
            rosrpc.setCarrier("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.toURI());
            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";
                }
                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.toURI());
            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 name = contact.getName();
    size_t pub_idx = name.find("+#");
    size_t sub_idx = name.find("-#");

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

//.........这里部分代码省略.........
    int dummyInt=0;
    fwrite(&dummyInt,sizeof(int),1,fid2);
    fclose(fid2);
    
    //make a "file" that holds a single int to keep index of the last sample written and initialize with a zero
    FILE *fid3=fopen("/tmp/lastSampleIndex.dat","w");
    int dummyInt2=0;
    fwrite(&dummyInt2,sizeof(int),1,fid3);
    fclose(fid3);
    
	//memory map the index file
	int lastFrameFileID = open("/tmp/lastFrameIndex.dat",O_RDWR);
   	lastFrameIndex=(int *)mmap(0, sizeof(int), PROT_READ | PROT_WRITE, MAP_SHARED , lastFrameFileID, 0);
	close(lastFrameFileID);

	//memory map the last sample index file
	int lastSampleFileID = open("/tmp/lastSampleIndex.dat",O_RDWR);
   	lastSampleIndex=(int *)mmap(0, sizeof(int), PROT_READ | PROT_WRITE, MAP_SHARED , lastSampleFileID, 0);
	close(lastSampleFileID);


	//****now you are ready to write frames into this memory****//
    
     

    // see if user has supplied audio device
    Property p;
    if (argc>1) {
        p.fromCommand(argc,argv);
    }

    // otherwise default device is "portaudio"
    if (!p.check("device")) {
        p.put("device","portaudio");
        p.put("write",1);
        p.put("delay",1);
    }

    // start the echo service running
    Echo echo;
    echo.open(p);

    // process the keyboard
    bool muted = false;
    bool saving = false;
    bool help = false;
    ConstString fname = "audio_%06d.wav";
    int ct = 0;
    bool done = false;
    while (!done) {
        if (help) {
            printf("  Press return to mute/unmute\n");
            printf("  Type \"s\" to set start/stop saving audio in memory\n");
            printf("  Type \"write filename.wav\" to write saved audio to a file\n");
            printf("  Type \"buf NUMBER\" to set buffering delay (default is 0)\n");
            printf("  Type \"write\" or \"w\" to write saved audio with same/default name\n");
            printf("  Type \"q\" to quit\n");
            printf("  Type \"help\" to see this list again\n");
            help = false;
        } else {
            printf("Type \"help\" for usage\n");
        }
        
        ConstString keys = Network::readString();
        Bottle b(keys);
        ConstString cmd = b.get(0).asString();
        if (b.size()==0) {
            muted = !muted;
            echo.mute(muted);
            printf("%s\n", muted?"Muted":"Audible again");
        } else if (cmd=="help") {
            help = true;
        } else if (cmd=="s") {

            saving = !saving;
            echo.save(saving);
            printf("%s\n", saving?"Saving":"Stopped saving");
            if (saving) {
                printf("  Type \"s\" again to stop saving\n");
            }
        } else if (cmd=="write"||cmd=="w") {
            if (b.size()==2) {
                fname = b.get(1).asString();
            }
            char buf[2560];
            sprintf(buf,fname.c_str(),ct);
            echo.saveFile(buf);
            ct++;
        } else if (cmd=="q"||cmd=="quit") {
            done = true;
        } else if (cmd=="buf"||cmd=="b") {
            padding = b.get(1).asInt();
            printf("Buffering at %d\n", padding);
        }
    }

    echo.close();

    return 0;
}

    void setInputPort(ResourceFinder *rf, int index, ConstString local)
    {
        ConstString **remote;
        ConstString *localTmp;
        bool connectionDone = false;
        remote = new ConstString*[nPlots];

        //local/remote port connection
        if(rf->find("remote").isString())
            {
                nPortInputsInPlots[index]  = 1;
                remote[index] = new ConstString[nPortInputsInPlots[index]];
                remote[index][0] = resFind->find("remote").toString();
                plot[index]->setInputPortName(remote[index][0].c_str());
                //connect
                plot[index]->setPorts(&local, nPortInputsInPlots[index]);
                yarpConnectRemoteLocal(remote[index][0], local);
                connectionDone = true;
            }
        else if(rf->find("remote").isList())
            {
                Bottle *rrBot;
                //if (VERBOSE) fprintf(stderr, "MESSAGE: Option remote is a list\n");
                rrBot = resFind->find("remote").asList();
                int listSize = rrBot->size();
                if (rrBot->get(index).isString() && listSize==1)
                    {
                        nPortInputsInPlots[index]  = 1;
                        remote[index] = new ConstString[nPortInputsInPlots[index]];
                        remote[index][0] = rrBot->get(0).toString();
                        plot[index]->setInputPortName(remote[index][0].c_str());
                        //connect
                        plot[index]->setPorts(&local, nPortInputsInPlots[index]);
                        yarpConnectRemoteLocal(remote[index][0], local);
                        connectionDone = true;
                    }
                if (rrBot->get(index).isString() && listSize==nPlots)
                    {
                        nPortInputsInPlots[index]  = 1;
                        remote[index] = new ConstString[nPortInputsInPlots[index]];
                        remote[index][0] = rrBot->get(index).toString();
                        plot[index]->setInputPortName(remote[index][0].c_str());
                        //connect
                        plot[index]->setPorts(&local, nPortInputsInPlots[index]);
                        yarpConnectRemoteLocal(remote[index][0], local);
                        connectionDone = true;
                    }
                if (rrBot->get(index).isList() && listSize==nPlots)
                    {
                        //if (VERBOSE) fprintf(stderr, "MESSAGE: Getting a double list of remote ports! \n");
                        Bottle *rrrBot;
                        rrrBot = rrBot->get(index).asList();
                        nPortInputsInPlots[index]  = rrrBot->size();
                        remote[index] = new ConstString[nPortInputsInPlots[index]];
                        localTmp = new ConstString[nPortInputsInPlots[index]];
                        for (int j = 0; j < nPortInputsInPlots[index]; j++)
                            {
                                char stringN[64];
                                sprintf(stringN, "/input%d", j);
                                ConstString sN(stringN);

                                remote[index][j] = rrrBot->get(j).toString();
                                localTmp[j] = sN;
                                localTmp[j] = localTmp[j] + local;
                            }
                        ConstString sumRemote = remote[index][0];
                        sumRemote = sumRemote + " ";
                        for (int j = 1; j < nPortInputsInPlots[index]; j++)
                            {
                                sumRemote = sumRemote + remote[index][j];
                                sumRemote = sumRemote + " ";
                            }
                        plot[index]->setInputPortName(sumRemote.c_str());
                        //connect
                        plot[index]->setPorts(localTmp, nPortInputsInPlots[index]);
                        for (int j = 0; j < nPortInputsInPlots[index]; j++)
                            yarpConnectRemoteLocal(remote[index][j], localTmp[j]);
                        connectionDone = true;
                    }
                
            }
        if (!connectionDone)
            fprintf(stderr, "WARNING: no input port connected. Waiting explicit connection from user.\n");
    }

int main(int argc, char *argv[]) {
    Property p;
    p.fromCommand(argc,argv);

    // check where to put description of device
    ConstString dest = "";
    dest = p.check("doc",Value("")).toString();

    ConstString fileName = p.check("file",Value("default.ini")).asString();

    if (p.check("file")) {
        p.fromConfigFile(fileName);
    }

    ConstString deviceName = p.check("device",Value("")).asString();

    // if no device given, we should be operating a completely
    // standard test harness like for libYARP_OS and libYARP_sig
    if (deviceName=="") {
        return harness_main(argc,argv);
    }

    // device name given - use special device testing procedure

#ifdef CHECK_FOR_LEAKS
    mtrace();
#endif

    int result = 0;

    Network::init();
    Network::setLocalMode(true);

    String seek = fileName.c_str();
    ConstString exampleName = "";
    int pos = seek.rfind('/');
    if (pos==-1) {
        pos = seek.rfind('\\');
    }
    if (pos==-1) {
        pos = 0;
    } else {
        pos++;
    }
    int len = seek.find('.',pos);
    if (len==-1) {
        len = seek.length();
    } else {
        len -= pos;
    }
    exampleName = seek.substr(pos,len).c_str();
    ConstString shortFileName = seek.substr(pos,seek.length()).c_str();

    PolyDriver dd;
	YARP_DEBUG(Logger::get(), "harness opening...");
	
    bool ok = dd.open(p);
    YARP_DEBUG(Logger::get(), "harness opened.");
    result = ok?0:1;

    ConstString wrapperName = "";
    ConstString codeName = "";

    DriverCreator *creator = 
        Drivers::factory().find(deviceName.c_str());
    if (creator!=NULL) {
        wrapperName = creator->getWrapper();
        codeName = creator->getCode();
    }


    if (dest!="") {
        String dest2 = dest.c_str();
        if (result!=0) {
            dest2 += ".fail";
        }
        FILE *fout = fopen(dest2.c_str(),"w");
        if (fout==NULL) {
            printf("Problem writing to %s\n", dest2.c_str());
            exit(1);
        }
        fprintf(fout,"/**\n");
        fprintf(fout," * \\ingroup dev_examples\n");
        fprintf(fout," *\n");
        fprintf(fout," * \\defgroup %s Example for %s (%s)\n\n",
                exampleName.c_str(),
                deviceName.c_str(),
                exampleName.c_str());
        fprintf(fout, "Instantiates \\ref cmd_device_%s \"%s\" device implemented by yarp::dev::%s.\n",
                deviceName.c_str(), deviceName.c_str(), codeName.c_str());
        fprintf(fout, "\\verbatim\n%s\\endverbatim\n",
                getFile(fileName).c_str());
        fprintf(fout, "If this text is saved in a file called %s then the device can be created by doing:\n",
                shortFileName.c_str());
        fprintf(fout, "\\verbatim\nyarpdev --file %s\n\\endverbatim\n",
                shortFileName.c_str());
        fprintf(fout, "Of course, the configuration could be passed just as command line options, or as a yarp::os::Property object in a program:\n");
        fprintf(fout, "\\code\n");
        fprintf(fout, "Property p;\n");
        fprintf(fout, "p.fromConfigFile(\"%s\");\n",
//.........这里部分代码省略.........

bool Port::open(const Contact& contact, bool registerName,
                const char *fakeName) {
    Contact contact2 = contact;

    if (!NetworkBase::initialized()) {
        YARP_ERROR(Logger::get(), "YARP not initialized; create a yarp::os::Network object before using ports");
        return false;
    }

    ConstString n = contact2.getName();
    if (n!="..." && n!="" && n[0]!='/') {
        if (fakeName==NULL) {
            YARP_SPRINTF1(Logger::get(),error,
                          "Port name '%s' needs to start with a '/' character",
                          n.c_str());
            return false;
        }
    }
    if (n!="..." && n!="") {
        if (fakeName==NULL) {
            ConstString prefix = NetworkBase::getEnvironment("YARP_PORT_PREFIX");
            if (prefix!="") {
                n = prefix + n;
                contact2 = contact2.addName(n);
            }
        }
    }

    // Allow for open() to be called safely many times on the same Port
    PortCoreAdapter *currentCore = &(HELPER(implementation));
    if (currentCore->isOpened()) {
        PortCoreAdapter *newCore = new PortCoreAdapter(*this);
        YARP_ASSERT(newCore!=NULL);
        // copy state that should survive in a new open()
        if (currentCore->checkPortReader()!=NULL) {
            newCore->configReader(*(currentCore->checkPortReader()));
        }
        if (currentCore->checkReadCreator()!=NULL) {
            newCore->configReadCreator(*(currentCore->checkReadCreator()));
        }
        if (currentCore->checkWaitAfterSend()>=0) {
            newCore->configWaitAfterSend(currentCore->checkWaitAfterSend());
        }
        close();
        delete ((PortCoreAdapter*)implementation);
        implementation = newCore;
    }

    PortCoreAdapter& core = HELPER(implementation);

    core.openable();

    bool local = false;
    if (NetworkBase::localNetworkAllocation()&&contact2.getPort()<=0) {
        YARP_DEBUG(Logger::get(),"local network allocation needed");
        local = true;
    }

    bool success = true;
    Address caddress(contact2.getHost().c_str(),
                     contact2.getPort(),
                     contact2.getCarrier().c_str(),
                     contact2.getName().c_str());
    Address address = caddress;

    core.setReadHandler(core);
    if (contact2.getPort()>0 && contact2.getHost()!="") {
        registerName = false;
    }
    if (registerName&&!local) {
        Contact contactFull = NetworkBase::registerContact(contact2);
        address = Address::fromContact(contactFull);
    }

    core.setControlRegistration(registerName);
    success = (address.isValid()||local)&&(fakeName==NULL);

    ConstString blame = "invalid address";
    if (success) {
        success = core.listen(address,registerName);
        blame = "address conflict";
        if (success) {
            success = core.start();
            blame = "manager did not start";
        }
    }
    if (success) {
        address = core.getAddress();
        if (registerName&&local) {
            contact2 = contact2.addSocket(address.getCarrierName().c_str(),
                                          address.getName().c_str(),
                                          address.getPort());
            contact2 = contact2.addName(address.getRegName().c_str());
            Contact newName = NetworkBase::registerContact(contact2);
            core.resetPortName(newName.getName());
            address = core.getAddress();
        }

        if (core.getVerbosity()>=1) {
            YARP_INFO(Logger::get(),
//.........这里部分代码省略.........

bool velImpControlThread::init(PolyDriver *d, ConstString partName, ConstString robotName, bool _impedance_enabled)
{
	impedance_enabled = _impedance_enabled;
	char tmp[255];
	
	limbName = partName;

	yarp::os::Time::turboBoost();

    nb_void_loops = 0;
    
	///opening port for fast transfer of position command
	sprintf(tmp,"/%s/vc/%s/fastCommand", robotName.c_str(), partName.c_str());
	fprintf(stderr,"opening port for part %s\n",tmp);
	command_port.open(tmp);
	

	std::string tmp2;
	tmp2="/";
	tmp2+=robotName.c_str();
	tmp2+="/vc/";
	tmp2+=partName.c_str();

	command_port2.open(std::string(tmp2+"/command").c_str());
	stiffness_port.open(std::string(tmp2+"/stiffness:o").c_str());
	damping_port.open(std::string(tmp2+"/damping:o").c_str());
	velocity_port.open(std::string(tmp2+"/velocity:o").c_str());

	if (d==0)
		return false;

	driver=d;

	driver->view(ivel);
	driver->view(ienc);
    driver->view(ipid);
	driver->view(ictrl);
	driver->view(iimp);
	driver->view(itime);
	

	if ( (ivel==0)||(ienc==0) || (iimp==0)||(ictrl==0)||(ipid==0))
	{
		printf("velContr::init >> failed to open a device\n"); 
		return false;
	}

	ivel->getAxes(&nJoints);

	fprintf(stderr,"controlling %d DOFs\n",nJoints);

	Vector accs;
	accs.resize(nJoints);
	ivel->getRefAccelerations(accs.data());
	accs=ACCELERATIONS;
	ivel->setRefAccelerations(accs.data());
	
	
	for(int i=0;i<nJoints;i++)
	{
		ictrl->setPositionMode(i);	//we set the position mode to be sure to have high stiffness
	}
	
	for(int i=0;i<nJoints;i++)
	{
		if(impContr[i]==1)
		{
			requestedStiff[i]=currStiff[i]=stanceStiff[i];
			requestedDamp[i]=currDamp[i]=stanceDamp[i];
			ictrl->setImpedancePositionMode(i);	//we set the position mode to be sure to have high stiffness
			iimp->setImpedance(i, requestedStiff[i], requestedDamp[i]);
		}
	}


	encoders.resize(nJoints);
    encoders_ref.resize(nJoints);
	encoders_speed.resize(nJoints);
	command.resize(nJoints);
	targets.resize(nJoints);
	ffVelocities.resize(nJoints);
	command=0;
	targets=0;
	ffVelocities = 0;
	Kp.resize(nJoints);
	Kp=0;
	Kd.resize(nJoints);
	Kd=0;
	error.resize(nJoints);
	error=0;
	error_d.resize(nJoints);
	error_d=0;
	state = INIT;

	maxVel.resize(nJoints);
	maxVel = 0.0;
	

	
#if 0 
//.........这里部分代码省略.........

	bool MarmaladeRenderProgram::initialize( const ConstString & _name, const MarmaladeRenderVertexShaderPtr & _vertexShader, const MarmaladeRenderFragmentShaderPtr & _fragmentShader, uint32_t _samplerCount )
	{
		m_name = _name;
		m_samplerCount = _samplerCount;

		if( m_samplerCount > MENGE_MAX_TEXTURE_STAGES )
		{
			LOGGER_ERROR( m_serviceProvider )("MarmaladeProgram::initialize %s don't support sampler count %d max %d"
				, _name.c_str()
				, m_samplerCount
				, MENGE_MAX_TEXTURE_STAGES
				);

			return false;
		}

		GLuint program;
		GLCALLR( m_serviceProvider, program, glCreateProgram, () );

		if( program == 0 )
		{
			LOGGER_ERROR( m_serviceProvider )("MarmaladeProgram::initialize %s invalid create program"
				, _name.c_str()
				);

			return false;
		}

		if( _vertexShader != nullptr )
		{
			m_vertexShader = _vertexShader;

			m_vertexShader->attach( program );
		}

		if( _fragmentShader != nullptr )
		{
			m_fragmentShader = _fragmentShader;

			m_fragmentShader->attach( program );
		}
		
		GLCALL( m_serviceProvider, glBindAttribLocation, ( program, VERTEX_POSITION_ARRAY, "inVert" ) );
		GLCALL( m_serviceProvider, glBindAttribLocation, ( program, VERTEX_COLOR_ARRAY, "inCol" ) );

		for( uint32_t i = 0; i != MENGINE_RENDER_VERTEX_UV_COUNT; ++i )
		{
			char attrib[16];
			sprintf( attrib, "inUV%d", i );
		
			GLCALL( m_serviceProvider, glBindAttribLocation, (program, VERTEX_UV0_ARRAY + i, attrib) );
		}

		GLCALL( m_serviceProvider, glLinkProgram, ( program ) );

		GLint linked;
		GLCALL( m_serviceProvider, glGetProgramiv, ( program, GL_LINK_STATUS, &linked ) );

		if( linked == GL_FALSE )
		{
			GLchar errorLog[1024] = {0};
			GLCALL( m_serviceProvider, glGetProgramInfoLog, ( program, 1023, NULL, errorLog ) );

			LOGGER_ERROR( m_serviceProvider )("MarmaladeProgram::shaderProgram - shader linking error '%s'"
				, errorLog
				);

			return false;
		}
				
		int transformLocation;
		GLCALLR( m_serviceProvider, transformLocation, glGetUniformLocation, (program, "mvpMat") );

		m_transformLocation = transformLocation;

		for( uint32_t index = 0; index != m_samplerCount; ++index )
		{
			char samplerVar[16];
			sprintf( samplerVar, "inSampler%d", index );

			int location;
			GLCALLR( m_serviceProvider, location, glGetUniformLocation, (program, samplerVar) );

			m_samplerLocation[index] = location;
		}

		m_program = program;

		return true;
    }

bool positionDirectControlThread::init(PolyDriver *d, ConstString moduleName, ConstString partName, ConstString robotName, Bottle* jointsList)
{
    yarp::os::Time::turboBoost();

    ///opening port command input
    char tmp[255];
    sprintf(tmp, "/%s/%s/%s/command:i", moduleName.c_str(), robotName.c_str(), partName.c_str());
    yInfo("opening port for part %s\n",tmp);
    command_port.open(tmp);

    if (d==0)
    {
        yError ("Invalid device driver pointer");
        return false;
    }

    driver=d;
    driver->view(idir);
    driver->view(ipos);
    driver->view(ienc);
    driver->view(imod);
    driver->view(ilim);

    if ( (idir==0)||(ienc==0) || (imod==0) || (ipos==0) || (ilim==0))
    {
        yError ("Failed to view motor interfaces");
        return false;
    }

    int tmpj=0;
    ipos->getAxes(&tmpj);
    part_joints=tmpj;
    control_joints= jointsList->size();
    if (control_joints>part_joints)
    {
        yError ("you cannot control more of %d joints for this robot part", part_joints);
        return false;
    }
    else if (control_joints<=0)
    {
        yError ("invalid number of control joints (%d)", control_joints);
        return false;
    }
    else
    {
        control_joints_list = new int [control_joints];
        for (unsigned int i=0; i< control_joints; i++)
        {
            if (jointsList->get(i).isInt() && jointsList->get(i).asInt()>=0)
            {
                control_joints_list[i] = jointsList->get(i).asInt();
            }
            else
            {
                yError ("invalid list of jonts to control");
                return false;
            }
        }
    }
    yInfo("part has %d joints, controlling %d joints\n",part_joints, control_joints);

    Vector speeds;
    speeds.resize(control_joints);
    speeds=10.0;
    for (unsigned int i=0; i<control_joints; i++)
    {
        ipos->setRefSpeed(control_joints_list[i],speeds[i]);
    }

    encoders.resize(control_joints);
    targets.resize(control_joints);
    prev_targets.resize(control_joints);
    error.resize(control_joints);
    encoders.zero();
    targets.zero();
    prev_targets.zero();
    error.zero();

    min_limits.resize(control_joints);
    max_limits.resize(control_joints);
    for (unsigned int i=0; i<control_joints; i++)
    {
        double min=0;
        double max=0;
        ilim->getLimits(control_joints_list[i],&min,&max);
        min_limits[i]=min;
        max_limits[i]=max;
    }

    for (unsigned int i=0; i<control_joints; i++)
    {
        imod->setControlMode(control_joints_list[i],VOCAB_CM_POSITION_DIRECT);
    }

    //get the current position
    for (unsigned int i=0; i< control_joints; i++)
    {
        double val =0;
        ienc->getEncoder(control_joints_list[i],&val);
        targets[i] = encoders[i] = val;
//.........这里部分代码省略.........

ConstString NetworkBase::getNameServerName() {
    NameConfig nc;
    ConstString name = nc.getNamespace(false);
    return name.c_str();
}

 void yarpConnectRemoteLocal(ConstString remote, ConstString local)
 {
     if(!Network::connect(remote.c_str(), local.c_str(), "udp"))
         fprintf(stderr, "WARNING: Connection to %s  was NOT successfull. Waiting from explicit connection from user.\n", remote.c_str());
 }

Contact RosNameSpace::unregisterAdvanced(const ConstString& name, NameStore *store) {
    NestedContact nc;
    nc.fromString(name);
    ConstString cat = nc.getCategory();

    if (nc.getNestedName()!="") {
        if (cat == "-1") {
            Nodes& nodes = NameClient::getNameClient().getNodes();
            Contact c = nodes.getURI(name);
            c.setCarrier("rosrpc");
            c = rosify(c);
            Bottle cmd, reply;
            cmd.clear();
            cmd.addString("unregisterService");
            cmd.addString(toRosNodeName(nc.getNodeName()));
            cmd.addString(nc.getNestedName());
            cmd.addString(c.toURI());
            bool ok = NetworkBase::write(getNameServerContact(),
                                         cmd, reply);
            if (!ok) return Contact();
        } else if (cat == "+" || cat== "-") {
            Bottle cmd, reply;
            cmd.clear();
            cmd.addString((cat=="+")?"unregisterPublisher":"unregisterSubscriber");
            cmd.addString(toRosNodeName(nc.getNodeName()));
            cmd.addString(nc.getNestedName());
            Contact c;
            if (store) {
                c = rosify(store->query(nc.getNodeName()));
            } else {
                Nodes& nodes = NameClient::getNameClient().getNodes();
                c = rosify(nodes.getParent(name));
            }
            cmd.addString(c.toString());
            bool ok = NetworkBase::write(getNameServerContact(),
                                         cmd, reply);
            if (!ok) return Contact();
        }
        return Contact();
    }

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

    size_t pub_idx = name.find("+#");
    size_t sub_idx = name.find("-#");

    ConstString node = "";
    ConstString pub = "";
    ConstString sub = "";
    if (pub_idx!=ConstString::npos) {
        node = name.substr(0, pub_idx);
        pub = name.substr(pub_idx+2, name.length());
    }
    if (sub_idx!=ConstString::npos) {
        node = name.substr(0, sub_idx);
        sub = name.substr(sub_idx+2, name.length());
    }
    if (node=="") {
        node = name;
    }
    YARP_SPRINTF3(Logger::get(), debug, "Name [%s] sub [%s] pub [%s]\n",
                  name.c_str(), sub.c_str(), pub.c_str());

    if (pub!="") {
        NetworkBase::disconnect(name, ConstString("topic:/") + pub);
    }
    if (sub!="") {
        NetworkBase::disconnect(ConstString("topic:/") + sub, name);
    }

    Contact contact = NetworkBase::queryName(name);
    Bottle cmd, reply;
    cmd.addString("unregisterPublisher");
    cmd.addString(name);
    cmd.addString("/yarp/registration");
    Contact c("http", contact.getHost().c_str(), contact.getPort());
    cmd.addString(c.toString());
    bool ok = NetworkBase::write(getNameServerContact(),
                                 cmd, reply);
    if (!ok) return Contact();

    return Contact();
}