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");
break;
}
}
} while (1);
}
//set the thread rate
int rate = rf.check("rate",Value(10)).asInt();
yInfo("baseCtrl thread rate: %d ms.",rate);
//start the control thread
control_thr = new ControlThread(rate, rf, ctrl_options);
if (!control_thr->start())
{
delete control_thr;
return false;
}
//check for debug mode
if (rf.check("no_motors"))
{
this->control_thr->motors_enabled = false;
yInfo() << "Motors disabled";
}
rpcPort.open((localName+"/rpc").c_str());
attach(rpcPort);
return true;
}
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.
*/
char hostname[NI_MAXHOST]; hostname[NI_MAXHOST-1] = '\0';
gethostname(hostname, NI_MAXHOST);
if (strcmp(hostname, name.c_str()) == 0) result = true;
#endif
// just in case
if (name=="localhost"||name=="127.0.0.1") { result = true; }
return result;
}
yarp::os::Bottle NameConfig::getIpsAsBottle() {
yarp::os::Bottle result;
#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();
result.addString(ip.c_str());
}
delete[] ips;
}
#else
int family, s;
char host[NI_MAXHOST];
struct ifaddrs *ifaddr, *ifa;
if (getifaddrs(&ifaddr) == -1) {
perror("getifaddrs in getIpsAsBottle");
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),
host, NI_MAXHOST, NULL, 0, NI_NUMERICHOST);
if (s != 0) {
printf("getnameinfo() failed: %s\n", gai_strerror(s));
exit(EXIT_FAILURE);
}
result.addString(host);
}
}
#endif
return result;
}
String NameConfig::getIps() {
yarp::os::Bottle bot = getIpsAsBottle();
ConstString result = "";
for (int i=0; i<bot.size(); i++) {
ConstString ip = bot.get(i).asString();
if (i>0) {
result += " ";
}
result += ip;
}
return result.c_str();
}
void NameConfig::setAddress(const Address& address) {
this->address = address;
}
void NameConfig::setNamespace(const String& ns) {
space = ns;
}
String NameConfig::getNamespace(bool refresh) {
if (space==""||refresh) {
ConstString senv = NetworkBase::getEnvironment("YARP_NAMESPACE");
if (senv!="") {
spaces.fromString(senv.c_str());
} else {
String fname = getConfigFileName(YARP_CONFIG_NAMESPACE_FILENAME);
spaces.fromString(readConfig(fname).c_str());
}
space = spaces.get(0).asString().c_str();
if (space=="") {
space = "/root";
}
if (spaces.size()==0) {
spaces.addString("/root");
}
}
return space;
}
Bottle NameConfig::getNamespaces(bool refresh) {
getNamespace(refresh);
return spaces;
}
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());
}
bool file::read(ImageOf<PixelFloat>& dest, const ConstString& src)
{
int hh = 0, ww = 0;
FILE *fp = fopen(src.c_str(), "r");
if (fp==NULL) {
return false;
}
int blank = 1;
int curr = 0;
while (!feof(fp)) {
int ch = fgetc(fp);
if (ch==' ' || ch == '\t' || ch == '\r' ||ch == '\n'|| feof(fp)){
if (!blank) {
if (curr==0) {
hh++;
}
curr++;
if (curr>ww) {
ww = curr;
}
}
blank = 1;
if (ch=='\n') {
curr = 0;
}
} else {
blank = 0;
}
}
fclose(fp);
fp = fopen(src.c_str(), "rb");
if (fp==NULL) {
return false;
}
dest.resize(ww,hh);
hh = 0; ww = 0;
{
char buf[256];
int idx = 0;
int blank = 1;
int curr = 0;
while (!feof(fp)) {
int ch = fgetc(fp);
if (ch==' ' || ch == '\t' ||ch == '\r'||ch == '\n' || feof(fp)){
if (!blank) {
if (curr==0) {
hh++;
}
curr++;
if (curr>ww) {
ww = curr;
}
buf[idx] = '\0';
dest(curr-1,hh-1) = float(atof(buf));
idx = 0;
}
blank = 1;
if (ch=='\n') {
curr = 0;
}
} else {
buf[idx] = ch;
idx++;
yAssert(((unsigned int)idx)<sizeof(buf));
blank = 0;
}
}
}
fclose(fp);
return true;
}
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 = "";
if (pub_idx!=ConstString::npos) {
node = name.substr(0, pub_idx);
pub = name.substr(pub_idx+2, name.length());
YARP_SPRINTF1(Logger::get(), debug, "Publish to %s", pub.c_str());
}
if (sub_idx!=ConstString::npos) {
node = name.substr(0, sub_idx);
sub = name.substr(sub_idx+2, name.length());
YARP_SPRINTF1(Logger::get(), debug, "Subscribe to %s", sub.c_str());
}
if (node=="") {
node = name;
}
YARP_SPRINTF4(Logger::get(), debug, "Name [%s] Node [%s] sub [%s] pub [%s]",
name.c_str(), node.c_str(), sub.c_str(), pub.c_str());
{
Bottle cmd, reply;
// for ROS, we fake port name registrations by
// registering them as nodes that publish to an arbitrary
// topic
cmd.clear();
cmd.addString("registerPublisher");
cmd.addString(toRosNodeName(node));
cmd.addString("/yarp/registration");
cmd.addString("*");
Contact c = rosify(contact);
cmd.addString(c.toString());
bool ok = NetworkBase::write(getNameServerContact(),
cmd, reply);
if (!ok) {
return Contact();
}
}
if (pub!="") {
NetworkBase::connect(node, ConstString("topic:/") + pub);
}
if (sub!="") {
NetworkBase::connect(ConstString("topic:/") + sub, node);
}
Contact c = contact;
c.setName(node);
return c;
}
bool file::read(ImageOf<PixelBgr> & dest, const ConstString& src)
{
return ImageReadBGR(dest,src.c_str());
}
int main(int argc, char *argv[]) {
yarp::os::Network yarp;
//pre-setup some memory mapping stuff right away
printf("\n\nInitializing memory mapped files in the /tmp directory to hold audio data\n\n");
//build a vector of short zeros
short *initialAudioArray;
initialAudioArray = (short *)malloc(kMappedFileSize_samples*sizeof(short));
for (int i=0;i<kMappedFileSize_samples;i++){
initialAudioArray[i]=(short)0;
}
//write the zeros to a file to initialize it
FILE *fid=fopen("/tmp/AudioDataDump.dat","w");
fwrite(initialAudioArray,sizeof(short),kMappedFileSize_samples,fid);
fclose(fid);
//memory map the file
//these were declared globally so that we can get to them from the callback
int mappedFileID=open("/tmp/AudioDataDump.dat",O_RDWR);
mappedAudioData=(short *)mmap(0, kMappedFileSize_bytes, PROT_READ | PROT_WRITE, MAP_SHARED , mappedFileID, 0);
close(mappedFileID); //the double colon forces to call the close() from outside of this namespace (because it's obscured by the method close()
//make a "file" that holds a single int to keep index of frames written and initialize with a zero
FILE *fid2=fopen("/tmp/lastFrameIndex.dat","w");
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",
shortFileName.c_str());
fprintf(fout, "// of course you could construct the Property object on-the-fly\n");
fprintf(fout, "PolyDriver dev;\n");
fprintf(fout, "dev.open(p);\n");
fprintf(fout, "if (dev.isValid()) { /* use the device via view method */ }\n" );
fprintf(fout, "\\endcode\n");
fprintf(fout, "Here is a list of properties checked when starting up a device based on this configuration file. Note that which properties are checked can depend on whether other properties are present. In some cases properties can also vary between operating systems. So this is just an example\n\n");
toDox(dd,fout);
fprintf(fout, "\n\\sa yarp::dev::%s\n\n",
codeName.c_str());
fprintf(fout, " */\n");
fclose(fout);
fout = NULL;
}
if (ok) {
YARP_DEBUG(Logger::get(), "harness closing...");
dd.close();
YARP_DEBUG(Logger::get(), "harness closed.");
}
// just checking for crashes, not device creation
return result; //result;
}
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(),
String("Port ") +
address.getRegName() +
" active at " +
address.toString());
}
}
if (fakeName!=NULL) {
success = core.manualStart(fakeName);
blame = "unmanaged port failed to start";
}
if (!success) {
YARP_ERROR(Logger::get(),
String("Port ") +
(address.isValid()?(address.getRegName().c_str()):(contact2.getName().c_str())) +
" failed to activate" +
(address.isValid()?" at ":"") +
(address.isValid()?address.toString():String("")) +
" (" +
blame.c_str() +
")");
}
return success;
}
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
sprintf(tmp,"%s_target_speed.dat",partName.c_str());
targetSpeedFile = fopen(tmp,"w");
sprintf(tmp,"%s_current_speed.dat",partName.c_str());
currentSpeedFile = fopen(tmp,"w");
#endif
char file_name[255];
sprintf(file_name, "%s_impedance.dat", partName.c_str());
stiffFile = fopen(file_name, "w");
return true;
}
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;
prev_targets[i] = encoders[i];
}
return true;
}
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();
}
SoundBufferInterfacePtr SoundEngine::createSoundBufferFromFile( const ConstString& _pakName, const FilePath & _fileName, const ConstString & _codecType, bool _streamable )
{
if( m_supportStream == false && _streamable == true )
{
LOGGER_WARNING(m_serviceProvider)("SoundEngine::createSoundBufferFromFile: unsupport stream sound %s:%s"
, _pakName.c_str()
, _fileName.c_str()
);
_streamable = false;
}
SoundDecoderInterfacePtr soundDecoder;
if( _streamable == false )
{
if( PREFETCHER_SERVICE(m_serviceProvider)
->getSoundDecoder( _pakName, _fileName, soundDecoder ) == false )
{
soundDecoder = this->createSoundDecoder_( _pakName, _fileName, _codecType, false );
}
else
{
if( soundDecoder->rewind() == false )
{
LOGGER_ERROR(m_serviceProvider)("RenderTextureManager::loadTexture invalid rewind decoder '%s':'%s'"
, _pakName.c_str()
, _fileName.c_str()
);
return nullptr;
}
}
}
else
{
soundDecoder = this->createSoundDecoder_( _pakName, _fileName, _codecType, true );
}
if( soundDecoder == nullptr )
{
LOGGER_ERROR(m_serviceProvider)("SoundEngine::createSoundBufferFromFile invalid create decoder '%s':'%s' type %s"
, _pakName.c_str()
, _fileName.c_str()
, _codecType.c_str()
);
return nullptr;
}
SoundBufferInterfacePtr buffer = SOUND_SYSTEM(m_serviceProvider)
->createSoundBuffer( soundDecoder, _streamable );
if( buffer == nullptr )
{
LOGGER_ERROR(m_serviceProvider)("SoundEngine::createSoundBufferFromFile: Can't create sound buffer for file %s:%s"
, _pakName.c_str()
, _fileName.c_str()
);
return nullptr;
}
return buffer;
}
bool file::read(ImageOf<PixelRgb> & dest, const ConstString& src)
{
return ImageReadRGB(dest,src.c_str());
}
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;
}
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")) {
ConstString carrier = reply.find("carrier").asString();
if (!style.quiet) {
printf("Connection found between %s and %s using carrier %s\n",
src.getName().c_str(), dest.getName().c_str(), carrier.c_str());
}
if (mode==YARP_ENACT_EXISTS) {
return (carrier == style.carrier) ? 0 : 1;
}
// This is either a connect or a disconnect command, but the current
// connection is connectionless, the other side will not know that we
// are closing the connection and therefore will continue sending data.
// Therefore we send an explicit disconnect here.
bool currentIsConnectionLess = false;
bool currentIsPush = true;
if (reply.check("push")) {
currentIsPush = reply.find("push").asBool();
}
if (reply.check("connectionless")) {
currentIsConnectionLess = reply.find("connectionless").asBool();
}
if (currentIsConnectionLess && ((reversed && currentIsPush) || (!reversed && !currentIsPush))) {
enactConnection(dest, src, style, YARP_ENACT_DISCONNECT, !reversed);
}
}
if (mode==YARP_ENACT_EXISTS) {
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.
cmd.clear();
reply.clear();
cmd.addVocab(act);
Contact c = dest;
if (style.carrier!="") {
c.setCarrier(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());
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";
}
if (mode==YARP_ENACT_CONNECT && !ok) {
noteDud(dest);
}
if (!style.quiet) {
if (style.verboseOnSuccess||!ok) {
fprintf(stderr,"%s %s",
ok?"Success:":"Failure:",
msg.c_str());
}
}
return ok ? 0 : 1;
}
bool file::write(const ImageOf<PixelRgba> & src, const ConstString& dest)
{
ImageOf<PixelRgb> img2;
img2.copy(src);
return ImageWriteRGB(const_cast<ImageOf<PixelRgb> &>(img2), dest.c_str());
}
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.setCarrier("tcp");
}
if (staticDest.getCarrier()=="") {
staticDest.setCarrier("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.setCarrier("tcp");
staticDest.setCarrier("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 = YARP_NULLPTR;
if (staticSrc.getCarrier()!="") {
srcCarrier = Carriers::chooseCarrier(staticSrc.getCarrier().c_str());
}
if (srcCarrier!=YARP_NULLPTR) {
ConstString srcBootstrap = srcCarrier->getBootstrapCarrierName();
if (srcBootstrap!="") {
srcIsCompetent = true;
} else {
carrierConstraint = staticSrc.getCarrier();
}
delete srcCarrier;
srcCarrier = YARP_NULLPTR;
}
}
} else {
srcIsTopic = true;
}
// see if we can do business with the destination port
bool destIsCompetent = false;
bool destIsTopic = false;
if (staticDest.getCarrier()!="topic") {
if (!topical) {
Carrier *destCarrier = YARP_NULLPTR;
if (staticDest.getCarrier()!="") {
destCarrier = Carriers::chooseCarrier(staticDest.getCarrier().c_str());
}
if (destCarrier!=YARP_NULLPTR) {
ConstString destBootstrap = destCarrier->getBootstrapCarrierName();
if (destBootstrap!="") {
destIsCompetent = true;
} else {
carrierConstraint = staticDest.getCarrier();
}
delete destCarrier;
destCarrier = YARP_NULLPTR;
}
}
} else {
destIsTopic = true;
}
if (srcIsTopic||destIsTopic) {
Bottle cmd, reply;
NameSpace& ns = getNameSpace();
bool ok = false;
if (srcIsTopic) {
if (mode==YARP_ENACT_CONNECT) {
ok = ns.connectTopicToPort(staticSrc,staticDest,style);
} else if (mode==YARP_ENACT_DISCONNECT) {
ok = ns.disconnectTopicFromPort(staticSrc,staticDest,style);
} else {
fprintf(stderr,"Failure: cannot check subscriptions yet\n");
return 1;
}
} else {
if (mode==YARP_ENACT_CONNECT) {
ok = ns.connectPortToTopic(staticSrc,staticDest,style);
} else if (mode==YARP_ENACT_DISCONNECT) {
ok = ns.disconnectPortFromTopic(staticSrc,staticDest,style);
} else {
fprintf(stderr,"Failure: cannot check subscriptions yet\n");
return 1;
}
}
if (!ok) {
return 1;
}
if (!style.quiet) {
if (style.verboseOnSuccess) {
fprintf(stderr,"Success: connection to topic %s.\n", mode==YARP_ENACT_CONNECT ? "added" : "removed");
}
}
return 0;
}
if (dynamicSrc.getCarrier()!="") {
style.carrier = dynamicSrc.getCarrier();
}
if (dynamicDest.getCarrier()!="") {
style.carrier = dynamicDest.getCarrier();
}
if (style.carrier!="" && carrierConstraint!="") {
if (style.carrier!=carrierConstraint) {
fprintf(stderr,"Failure: conflict between %s and %s\n",
style.carrier.c_str(),
carrierConstraint.c_str());
return 1;
}
}
if (carrierConstraint!="") {
style.carrier = carrierConstraint;
}
if (style.carrier=="") {
style.carrier = staticDest.getCarrier();
}
if (style.carrier=="") {
style.carrier = staticSrc.getCarrier();
}
bool connectionIsPush = false;
bool connectionIsPull = false;
Carrier *connectionCarrier = YARP_NULLPTR;
if (style.carrier!="topic") {
connectionCarrier = Carriers::chooseCarrier(style.carrier.c_str());
if (connectionCarrier!=YARP_NULLPTR) {
connectionIsPush = connectionCarrier->isPush();
connectionIsPull = !connectionIsPush;
}
}
int result = -1;
if ((srcIsCompetent&&connectionIsPush)||topical) {
// Classic case.
Contact c = Contact::fromString(dest);
if (connectionCarrier!=YARP_NULLPTR) delete connectionCarrier;
return enactConnection(staticSrc,c,style,mode,false);
}
if (destIsCompetent&&connectionIsPull) {
Contact c = Contact::fromString(src);
if (connectionCarrier!=YARP_NULLPTR) delete connectionCarrier;
return enactConnection(staticDest,c,style,mode,true);
}
if (connectionCarrier!=YARP_NULLPTR) {
if (!connectionIsPull) {
Contact c = Contact::fromString(dest);
result = connectionCarrier->connect(staticSrc,c,style,mode,false);
} else {
Contact c = Contact::fromString(src);
result = connectionCarrier->connect(staticDest,c,style,mode,true);
}
}
if (connectionCarrier!=YARP_NULLPTR) {
delete connectionCarrier;
connectionCarrier = YARP_NULLPTR;
}
if (result!=-1) {
if (!style.quiet) {
if (result==0) {
if (style.verboseOnSuccess) {
printf("Success: added connection using custom carrier method\n");
}
} else {
printf("Failure: custom carrier method did not work\n");
}
}
return result;
}
if (mode!=YARP_ENACT_DISCONNECT) {
fprintf(stderr,"Failure: no way to make connection %s->%s\n", src.c_str(), dest.c_str());
}
return 1;
}
bool file::write(const ImageOf<PixelMono> & src, const ConstString& dest)
{
return ImageWriteMono(const_cast<ImageOf<PixelMono> &>(src), dest.c_str());
}
Contact NetworkBase::registerName(const ConstString& name) {
YARP_SPRINTF1(Logger::get(),debug,"register name %s",name.c_str());
return getNameSpace().registerName(name);
}
int main(int argc, char *argv[]) {
yarp::os::Network yarp;
// 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;
}
bool NetworkBase::setConnectionQos(const ConstString& src, const ConstString& dest,
const QosStyle& srcStyle, const QosStyle &destStyle,
bool quiet) {
//e.g., prop set /portname (sched ((priority 30) (policy 1))) (qos ((tos 0)))
yarp::os::Bottle cmd, reply;
// ignore if everything left as default
if(srcStyle.getPacketPriorityAsTOS()!=-1 || srcStyle.getThreadPolicy() !=-1) {
// set the source Qos
cmd.addString("prop");
cmd.addString("set");
cmd.addString(dest.c_str());
Bottle& sched = cmd.addList();
sched.addString("sched");
Property& sched_prop = sched.addDict();
sched_prop.put("priority", srcStyle.getThreadPriority());
sched_prop.put("policy", srcStyle.getThreadPolicy());
Bottle& qos = cmd.addList();
qos.addString("qos");
Property& qos_prop = qos.addDict();
qos_prop.put("tos", srcStyle.getPacketPriorityAsTOS());
Contact srcCon = Contact::fromString(src);
bool ret = write(srcCon, cmd, reply, true, true, 2.0);
if(!ret) {
if(!quiet)
ACE_OS::fprintf(stderr, "Cannot write to '%s'\n",src.c_str());
return false;
}
if(reply.get(0).asString() != "ok") {
if(!quiet)
ACE_OS::fprintf(stderr, "Cannot set qos properties of '%s'. (%s)\n",
src.c_str(), reply.toString().c_str());
return false;
}
}
// ignore if everything left as default
if(destStyle.getPacketPriorityAsTOS()!=-1 || destStyle.getThreadPolicy() !=-1) {
// set the destination Qos
cmd.clear();
reply.clear();
cmd.addString("prop");
cmd.addString("set");
cmd.addString(src.c_str());
Bottle& sched2 = cmd.addList();
sched2.addString("sched");
Property& sched_prop2 = sched2.addDict();
sched_prop2.put("priority", destStyle.getThreadPriority());
sched_prop2.put("policy", destStyle.getThreadPolicy());
Bottle& qos2 = cmd.addList();
qos2.addString("qos");
Property& qos_prop2 = qos2.addDict();
qos_prop2.put("tos", destStyle.getPacketPriorityAsTOS());
Contact destCon = Contact::fromString(dest);
bool ret = write(destCon, cmd, reply, true, true, 2.0);
if(!ret) {
if(!quiet)
ACE_OS::fprintf(stderr, "Cannot write to '%s'\n",dest.c_str());
return false;
}
if(reply.get(0).asString() != "ok") {
if(!quiet)
ACE_OS::fprintf(stderr, "Cannot set qos properties of '%s'. (%s)\n",
dest.c_str(), reply.toString().c_str());
return false;
}
}
return true;
}
bool SubscriberOnSql::welcome(const ConstString& port, int activity) {
mutex.wait();
NameSpace *ns = getDelegate();
if (ns) {
NestedContact nc(port);
if (nc.getNestedName().size()>0) {
NameStore *store = getStore();
if (store!=NULL) {
Contact node = store->query(nc.getNodeName());
Contact me = store->query(port);
if (node.isValid() && me.isValid()) {
if (activity>0) {
ns->registerAdvanced(me,store);
} else {
ns->unregisterAdvanced(port,store);
}
}
}
}
}
char *msg = NULL;
char *query;
if (activity>0) {
query = sqlite3_mprintf("INSERT OR IGNORE INTO live (name,stamp) VALUES(%Q,DATETIME('now'))",
port.c_str());
} else {
// Port not responding. Mark as non-live.
if (activity==0) {
query = sqlite3_mprintf("DELETE FROM live WHERE name=%Q AND stamp < DATETIME('now','-30 seconds')",
port.c_str());
} else {
// activity = -1 -- definite dodo
query = sqlite3_mprintf("DELETE FROM live WHERE name=%Q",
port.c_str());
}
}
if (verbose) {
printf("Query: %s\n", query);
}
bool ok = true;
int result = sqlite3_exec(SQLDB(implementation), query,
NULL, NULL, &msg);
if (result!=SQLITE_OK) {
ok = false;
if (msg!=NULL) {
fprintf(stderr,"Error: %s\n", msg);
sqlite3_free(msg);
}
}
sqlite3_free(query);
mutex.post();
if (activity>0) {
hookup(port);
} else if (activity<0) {
breakdown(port);
}
return ok;
}
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;
}
bool SubscriberOnSql::open(const ConstString& filename, bool fresh) {
sqlite3 *db = NULL;
if (fresh) {
int result = access(filename.c_str(),F_OK);
if (result==0) {
fprintf(stderr,"Database needs to be recreated.\n");
fprintf(stderr,"Please move %s out of the way.\n", filename.c_str());
return false;
}
}
int result = sqlite3_open_v2(filename.c_str(),
&db,
SQLITE_OPEN_READWRITE|SQLITE_OPEN_CREATE|
SQLITE_OPEN_NOMUTEX,
NULL);
if (result!=SQLITE_OK) {
fprintf(stderr,"Failed to open database %s\n", filename.c_str());
if (db!=NULL) {
sqlite3_close(db);
}
return false;
}
const char *create_subscribe_table = "CREATE TABLE IF NOT EXISTS subscriptions (\n\
id INTEGER PRIMARY KEY,\n\
src TEXT,\n\
dest TEXT,\n\
srcFull TEXT,\n\
destFull TEXT,\n\
mode TEXT);";
result = sqlite3_exec(db, create_subscribe_table, NULL, NULL, NULL);
if (result!=SQLITE_OK) {
sqlite3_close(db);
fprintf(stderr,"Failed to set up subscriptions table\n");
exit(1);
}
const char *check_subscriptions_size = "PRAGMA table_info(subscriptions)";
sqlite3_stmt *statement = NULL;
result = sqlite3_prepare_v2(db, check_subscriptions_size, -1,
&statement, NULL);
if (result!=SQLITE_OK) {
fprintf(stderr,"Failed to set up subscriptions table\n");
exit(1);
}
int count = 0;
while (sqlite3_step(statement) == SQLITE_ROW) {
count++;
}
sqlite3_finalize(statement);
if (count==5) {
const char *add_structure = "ALTER TABLE subscriptions ADD COLUMN mode";
result = sqlite3_exec(db, add_structure, NULL, NULL, NULL);
if (result!=SQLITE_OK) {
sqlite3_close(db);
fprintf(stderr,"Failed to set up subscriptions table\n");
exit(1);
}
}
const char *create_topic_table = "CREATE TABLE IF NOT EXISTS topics (\n\
id INTEGER PRIMARY KEY,\n\
topic TEXT,\n\
structure TEXT);";
result = sqlite3_exec(db, create_topic_table, NULL, NULL, NULL);
if (result!=SQLITE_OK) {
sqlite3_close(db);
fprintf(stderr,"Failed to set up topics table\n");
exit(1);
}
const char *check_topic_size = "PRAGMA table_info(topics)";
statement = NULL;
result = sqlite3_prepare_v2(db, check_topic_size, -1,
&statement, NULL);
if (result!=SQLITE_OK) {
fprintf(stderr,"Failed to set up topics table\n");
exit(1);
}
count = 0;
while (sqlite3_step(statement) == SQLITE_ROW) {
//sqlite3_column_text(statement,1);
count++;
}
sqlite3_finalize(statement);
if (count==2) {
const char *add_structure = "ALTER TABLE topics ADD COLUMN structure";
result = sqlite3_exec(db, add_structure, NULL, NULL, NULL);
if (result!=SQLITE_OK) {
sqlite3_close(db);
fprintf(stderr,"Failed to set up topics table\n");
exit(1);
}
}
const char *create_live_table = "CREATE TABLE IF NOT EXISTS live (\n\
id INTEGER PRIMARY KEY,\n\
name TEXT UNIQUE,\n\
stamp DATETIME);";
result = sqlite3_exec(db, create_live_table, NULL, NULL, NULL);
if (result!=SQLITE_OK) {
sqlite3_close(db);
fprintf(stderr,"Failed to set up live table\n");
exit(1);
}
const char *create_struct_table = "CREATE TABLE IF NOT EXISTS structures (\n\
name TEXT PRIMARY KEY,\n\
yarp TEXT);";
result = sqlite3_exec(db, create_struct_table, NULL, NULL, NULL);
if (result!=SQLITE_OK) {
sqlite3_close(db);
fprintf(stderr,"Failed to set up structures table\n");
exit(1);
}
implementation = db;
return true;
}
bool yarp::os::impl::HttpCarrier::reply(Protocol& proto, SizedWriter& writer) {
DummyConnector con;
con.setTextMode(true);
for (size_t i=writer.headerLength(); i<writer.length(); i++) {
con.getWriter().appendBlock(writer.data(i),writer.length(i));
}
Bottle b;
b.read(con.getReader());
ConstString mime = b.check("mime",Value("text/html")).asString();
ConstString body;
bool using_json = false;
if (stream!=NULL) {
if (stream->useJson()) {
mime = "text/json";
asJson(body,&b,stream->typeHint());
using_json = true;
}
}
if (b.check("web")&&!using_json) {
body = b.find("web").toString();
}
if (b.check("stream")&&!using_json) {
String header("HTTP/1.1 200 OK\r\nContent-Type: ");
header += mime;
header += "\r\n";
header += "Transfer-Encoding: chunked\r\n";
header += "\r\n";
int N = 2*1024;
header += NetType::toHexString(body.length()+N);
header += "\r\n";
Bytes b2((char*)header.c_str(),header.length());
proto.os().write(b2);
// chrome etc won't render until enough chars are received.
for (int i=0; i<N; i++) {
proto.os().write(' ');
}
Bytes b3((char*)body.c_str(),body.length());
proto.os().write(b3);
proto.os().write('\r');
proto.os().write('\n');
if (stream!=NULL) {
stream->flip();
}
return true;
}
if (stream!=NULL) {
stream->finish();
}
// Could check response codes, mime types here.
if (body.length()!=0 || using_json) {
ConstString mime = b.check("mime",Value("text/html")).asString();
String header("HTTP/1.1 200 OK\nContent-Type: ");
header += mime;
header += "\n";
header += "Access-Control-Allow-Origin: *\n";
header += "\n";
Bytes b2((char*)header.c_str(),header.length());
proto.os().write(b2);
//body = b.toString();
Bytes b3((char*)body.c_str(),body.length());
proto.os().write(b3);
} else {
writer.write(proto.os());
}
proto.os().flush();
return proto.os().isOk();
}