bool RosType::cache(const char *tname, RosTypeSearch& env,
RosTypeCodeGen& gen) {
std::string rosType = tname;
size_t at = rosType.rfind('.');
if (at == std::string::npos) {
return false;
}
std::string ext = rosType.substr(at+1);
rosType = rosType.substr(0,at);
std::string pkg;
at = rosType.rfind('/');
if (at != std::string::npos && at > 0) {
size_t pkg_at = rosType.rfind('/', at-1);
pkg = rosType.substr(pkg_at+1, at-pkg_at-1);
if (pkg == "srv" || pkg == "msg") {
pkg_at = rosType.rfind('/', pkg_at-1);
pkg = rosType.substr(pkg_at+1, at-pkg_at-5); // -1 -("/msg" or "/srv") = -5
}
}
rosType = rosType.substr(rosType.rfind('/')+1);
std::string outfile = rosType + "." + ext;
if (!pkg.empty()) {
outfile = pkg + "/" + outfile;
}
// If input and output are the same there is nothing to do
if (tname == outfile) {
return true;
}
FILE *fin = fopen((env.getTargetDirectory() + "/" + tname).c_str(),"r");
yarp::os::mkdir_p((env.getTargetDirectory() + "/" + pkg).c_str(),1);
FILE *fout = fopen((env.getTargetDirectory() + "/" + outfile).c_str(),"w");
if (!fin) {
fin = fopen(tname,"r");
}
if (!fin) {
fprintf(stderr, "[type] FAILED to open input file \"%s\"\n", (env.getTargetDirectory() + "/" + tname).c_str());
std::exit(1);
}
if (!fout) {
fprintf(stderr, "[type] FAILED to open output file \"%s\"\n", (env.getTargetDirectory() + "/" + outfile).c_str());
std::exit(1);
}
do {
char buf[2048];
char *result = fgets(buf,sizeof(buf),fin);
if (result==nullptr) break;
fputs(result,fout);
} while (true);
fclose(fout);
fclose(fin);
return true;
}
void configure_search(RosTypeSearch& env, Searchable& p) {
if (p.check("out")) {
env.setTargetDirectory(p.find("out").toString().c_str());
}
if (p.check("web",Value(0)).asInt()!=0 || p.findGroup("web").size()==1) {
env.allowWeb();
}
if (p.check("soft",Value(0)).asInt()!=0 || p.findGroup("soft").size()==1) {
env.softFail();
}
env.lookForService(p.check("service"));
}
int generate_cpp(int argc, char *argv[])
{
bool is_service = false;
Property p;
string fname;
p.fromCommand(argc,argv);
bool verbose = p.check("verbose");
fname = argv[argc-1];
if (fname.rfind(".")!=string::npos) {
string ext = fname.substr(fname.rfind("."),fname.length());
if (ext==".srv" || ext==".SRV") {
is_service = true;
}
}
if (is_service) {
p.put("service",1);
}
RosTypeSearch env;
RosType t;
RosTypeCodeGenYarp gen;
if (verbose) {
env.setVerbose();
t.setVerbose();
gen.setVerbose();
}
if (p.check("out")) {
gen.setTargetDirectory(p.find("out").toString().c_str());
}
configure_search(env,p);
if (t.read(fname.c_str(),env,gen)) {
RosTypeCodeGenState state;
t.emitType(gen,state);
t.cache(fname.c_str(),env,gen);
}
return 0;
}
int main(int argc, char *argv[]) {
if (argc<=1) {
show_usage();
return 0;
}
if (std::string("help")==argv[1] || std::string("--help")==argv[1]) {
show_usage();
return 0;
}
bool is_service = false;
Property p;
string fname;
p.fromCommand(argc-1,argv);
fname = argv[argc-1];
if (fname.rfind(".")!=string::npos) {
string ext = fname.substr(fname.rfind("."),fname.length());
if (ext==".srv" || ext==".SRV") {
is_service = true;
}
}
RosTypeSearch env;
RosType t;
RosTypeCodeGenYarp gen;
if (p.check("out")) {
gen.setTargetDirectory(p.find("out").toString().c_str());
env.setTargetDirectory(gen.getTargetDirectory().c_str());
}
env.lookForService(is_service);
if (t.read(fname.c_str(),env,gen)) {
RosTypeCodeGenState state;
t.emitType(gen,state);
}
return 0;
}
bool RosType::read(const char *tname, RosTypeSearch& env, RosTypeCodeGen& gen,
int nesting) {
string indent = "";
for (int i=0; i<nesting; i++) {
indent += " ";
}
if (nesting>0) env.lookForService(false); // no srv nesting allowed in ros
//printf("Checking %s\n", tname);
clear();
string base = tname;
rosType = base;
if (base.length()==0) return false;
if (base[base.length()-1]==']') {
char ch = '\0';
if (base.length()>=2) {
ch = base[base.length()-2];
}
if (ch!='[') {
fprintf(stderr,"dodgy array? %s\n", base.c_str());
return false;
}
isArray = true;
base = base.substr(0,base.length()-2);
rosType = base;
}
if (base[0]>='a'&&base[0]<='z'&&base.find("/")==string::npos&&base.find(".")==string::npos) {
if (base=="time"||base=="duration") {
if (gen.hasNativeTimeClass()) {
isPrimitive = true;
} else {
rosType = (base=="time")?"TickTime":"TickDuration";
isPrimitive = false;
RosType t1;
t1.rosType = "uint32";
t1.rosName = "sec";
t1.isPrimitive = true;
t1.isValid = true;
subRosType.push_back(t1);
RosType t2;
t2.rosType = "uint32";
t2.rosName = "nsec";
t2.isPrimitive = true;
t2.isValid = true;
subRosType.push_back(t2);
}
} else {
isPrimitive = true;
}
isValid = true;
return true;
}
if (rosType.find(".")!=string::npos) {
rosType = rosType.substr(0,rosType.rfind("."));
}
bool ok = true;
string path = env.findFile(base.c_str());
rosPath = path;
FILE *fin = fopen((env.getTargetDirectory() + "/" + path).c_str(),"r");
if (!fin) {
fin = fopen(path.c_str(),"r");
}
if (!fin) {
fprintf(stderr, "[type] FAILED to open %s\n", path.c_str());
exit(1);
}
fprintf(stderr,"[type]%s BEGIN %s\n", indent.c_str(), path.c_str());
char *result = NULL;
txt = "";
RosType *cursor = this;
do {
char buf[2048];
result = fgets(buf,sizeof(buf),fin);
if (result==NULL) break;
txt += "// ";
txt += result;
int len = (int)strlen(result);
for (int i=0; i<len; i++) {
if (result[i]=='\n') {
result[i] = '\0';
break;
}
}
string row = result;
vector<string> msg = normalizedMessage(row);
if (msg.size()==0) { continue; }
if (msg[0] == "---") {
printf("--- reply ---\n");
cursor->isValid = ok;
ok = true;
cursor->reply = new RosType();
cursor = cursor->reply;
cursor->rosType = rosType + "Reply";
continue;
}
bool have_const = false;
string const_txt = "";
if (msg.size()>2) {
if (msg[2]=="=") {
have_const = true;
//printf("Not worrying about: %s\n", row.c_str());
//continue;
const_txt = msg[3];
}
}
if (msg.size()!=2 && !have_const) {
if (msg.size()>0) {
if (msg[0][0]!='[') {
fprintf(stderr,"[type] skip %s\n", row.c_str());
ok = false;
}
}
continue;
}
string t = msg[0];
string n = msg[1];
fprintf(stderr,"[type]%s %s %s [%s]\n", indent.c_str(), t.c_str(),
n.c_str(), const_txt.c_str());
RosType sub;
if (!sub.read(t.c_str(),env,gen,nesting+1)) {
fprintf(stderr, "[type]%s Type not complete: %s\n",
indent.c_str(),
row.c_str());
ok = false;
}
sub.rosName = n;
sub.initializer = const_txt;
cursor->subRosType.push_back(sub);
} while (result!=NULL);
fprintf(stderr,"[type]%s END %s\n", indent.c_str(), path.c_str());
fclose(fin);
cursor->isValid = ok;
return isValid;
}
bool RosType::read(const char *tname, RosTypeSearch& env, RosTypeCodeGen& gen,
int nesting) {
std::string indent;
for (int i=0; i<nesting; i++) {
indent += " ";
}
if (nesting>0) {
env.lookForService(false); // no srv nesting allowed in ros
}
if (verbose) {
printf("[type]%s Checking %s\n", indent.c_str(), tname);
}
clear();
std::string base = tname;
rosType = base;
if (base.length()==0) {
return false;
}
if (base[base.length()-1]==']') {
size_t at = base.rfind('[');
if (at==std::string::npos) {
fprintf(stderr,"dodgy array? %s\n", base.c_str());
return false;
}
std::string idx = base.substr(at+1,base.length()-at-2);
if (idx!="") {
yarp::os::Bottle b(idx);
arrayLength = b.get(0).asInt32();
} else {
arrayLength = -1;
}
isArray = true;
base = base.substr(0,at);
rosType = base;
}
isStruct = true;
rosRawType = base;
if (base[0]>='a'&&base[0]<='z'&&base.find('/')==std::string::npos&&base.find('.')==std::string::npos) {
isStruct = false;
isRosPrimitive = true;
if (base=="time"||base=="duration") {
if (gen.hasNativeTimeClass()) {
isPrimitive = true;
} else {
rosType = (base=="time")?"TickTime":"TickDuration";
isStruct = true;
isPrimitive = false;
RosType t1;
t1.rosType = "uint32";
t1.rosRawType = "uint32";
t1.rosName = "sec";
t1.isPrimitive = true;
t1.isRosPrimitive = true;
t1.isValid = true;
t1.verbose = verbose;
subRosType.push_back(t1);
RosType t2;
t2.rosType = "uint32";
t2.rosRawType = "uint32";
t2.rosName = "nsec";
t2.isPrimitive = true;
t2.isRosPrimitive = true;
t2.isValid = true;
t2.verbose = verbose;
subRosType.push_back(t2);
}
} else {
isPrimitive = true;
isRosPrimitive = true;
}
isValid = true;
return true;
}
if (rosType.find('.')!=std::string::npos) {
rosType = rosType.substr(0,rosType.rfind('.'));
}
if (isStruct) {
size_t at = rosType.rfind('/');
if (at != std::string::npos) {
std::string temp = rosType.substr(0, at);
at = temp.rfind('/');
if (at != std::string::npos) {
package = temp.substr(at+1);
} else {
package = temp;
}
if (package == "srv" || package == "msg")
{
at = temp.rfind('/');
if (at != std::string::npos) {
temp = temp.substr(0, at);
at = temp.rfind('/');
if (at != std::string::npos) {
package = temp.substr(at+1);
} else {
package = temp;
}
}
}
} else if (rosType == "Header" || rosType == "std_msgs/Header") {
rosType = "std_msgs/Header";
base = "std_msgs/Header";
} else if (package != "" && package != ".") {
rosType = package + "/" + rosType;
base = package + "/" + base;
}
}
bool ok = true;
std::string path = env.findFile(base.c_str());
rosPath = path;
FILE *fin = fopen((env.getTargetDirectory() + "/" + path).c_str(),"r");
if (!fin) {
fin = fopen(path.c_str(),"r");
}
if (!fin) {
fprintf(stderr, "[type] FAILED to open %s\n", path.c_str());
std::exit(1);
}
if (verbose) {
fprintf(stderr,"[type]%s BEGIN %s\n", indent.c_str(), path.c_str());
}
char *result = nullptr;
txt = "";
source = "";
RosType *cursor = this;
char buf[2048];
do {
result = fgets(buf,sizeof(buf),fin);
if (result==nullptr) break;
txt += "// ";
txt += result;
source += result;
int len = (int)strlen(result);
for (int i=0; i<len; i++) {
if (result[i]=='\n') {
result[i] = '\0';
break;
}
}
std::string row = result;
std::vector<std::string> msg = normalizedMessage(row);
if (msg.size()==0) { continue; }
if (msg[0] == "---") {
if (verbose) {
printf("--- reply ---\n");
}
cursor->isValid = ok;
ok = true;
cursor->reply = new RosType();
cursor->reply->verbose = verbose;
cursor = cursor->reply;
cursor->rosType = rosType + "Reply";
continue;
}
bool have_const = false;
std::string const_txt;
if (msg.size()>2) {
if (msg[2]=="=") {
have_const = true;
//printf("Not worrying about: %s\n", row.c_str());
//continue;
const_txt = msg[3];
}
}
if (msg.size()!=2 && !have_const) {
if (msg.size()>0) {
if (msg[0][0]!='[') {
if (verbose) {
fprintf(stderr,"[type] skip %s\n", row.c_str());
}
ok = false;
}
}
continue;
}
std::string t = msg[0];
std::string n = msg[1];
if (verbose) {
fprintf(stderr,"[type]%s %s %s", indent.c_str(), t.c_str(), n.c_str());
if (const_txt!="") {
fprintf(stderr," = %s", const_txt.c_str());
}
fprintf(stderr,"\n");
}
RosType sub;
sub.verbose = verbose;
sub.package = package;
if (!sub.read(t.c_str(), env, gen, nesting+1)) {
fprintf(stderr, "[type]%s Type not complete: %s\n",
indent.c_str(),
row.c_str());
ok = false;
}
sub.rosName = n;
const_txt.erase(0,const_txt.find_first_not_of(" \t"));
if (const_txt.find_first_of(" \t#")!=std::string::npos) {
const_txt = const_txt.substr(0,const_txt.find_first_of(" \t#"));
}
sub.initializer = const_txt;
cursor->subRosType.push_back(sub);
} while (result!=nullptr);
if (verbose) {
fprintf(stderr,"[type]%s END %s\n", indent.c_str(), path.c_str());
}
fclose(fin);
if (rosType == "Header" || rosType == "std_msgs/Header") {
std::string preamble = "[std_msgs/Header]:";
if (source.find(preamble)==0) {
source = source.substr(preamble.length()+1,source.length());
}
}
isValid = ok;
if (!isValid) {
fprintf(stderr, "[type]%s Check failed: %s\n", indent.c_str(), tname);
}
return isValid;
}
int main(int argc, char *argv[])
{
if (argc<=1) {
show_usage();
return 0;
}
if (std::string("help")==argv[1] || std::string("--help")==argv[1]) {
show_usage();
return 0;
}
Property p;
p.fromCommand(argc,argv);
if (!(p.check("name")||p.check("cmd"))) {
return generate_cpp(argc,argv);
}
if (!p.check("soft")) {
p.put("soft",1);
}
if (!p.check("web")) {
p.put("web",1);
}
bool has_cmd = p.check("cmd");
bool verbose = p.check("verbose");
RosTypeSearch env;
configure_search(env,p);
Network yarp;
Port port;
if (!has_cmd) {
// Borrow an accidentally-available service type on ROS, in
// order to avoid adding build dependencies for now.
port.promiseType(Type::byNameOnWire("test_roscpp/TestStringString"));
port.setRpcServer();
if (!port.open(p.find("name").asString())) {
return 1;
}
}
while (true) {
Bottle req;
if (has_cmd) {
req = p.findGroup("cmd").tail();
} else {
if (!port.read(req,true)) {
continue;
}
}
if (req.size()==1) {
req.fromString(req.get(0).asString());
}
if (verbose) {
printf("Request: %s\n", req.toString().c_str());
}
Bottle resp;
ConstString tag = req.get(0).asString();
string fname0 = req.get(1).asString().c_str();
string fname = env.findFile(fname0.c_str());
string txt = "";
if (tag=="raw") {
txt = env.readFile(fname.c_str());
resp.addString(txt);
} else if (tag=="twiddle") {
RosTypeCodeGenYarp gen;
RosType t;
if (t.read(fname0.c_str(),env,gen)) {
ConstString txt;
generateTypeMap(t,txt);
resp.addString(txt);
} else {
resp.addString("?");
}
} else {
resp.addString("?");
}
if (!has_cmd) port.reply(resp);
if (verbose||has_cmd) {
printf("Response: %s\n", resp.toString().c_str());
}
if (has_cmd) break;
}
return 0;
}
