int main(int argc, char *argv[]) {
Network yarp;
Carriers::addCarrierPrototype(new HumanCarrier);
if (argc<2) {
printf("Please run in two terminals as:\n");
printf(" carrier_human --server\n");
printf(" carrier_human --client\n");
exit(1);
}
string mode = argv[1];
if (mode == "--server") {
Port out;
out.open("/test/out");
bool connected = false;
while (!connected) {
connected = Network::connect("/test/out","/test/in","human");
if (!connected) Time::delay(1);
}
Bottle bot;
bot.fromString("1 2 3");
out.write(bot);
out.close();
}
if (mode == "--client") {
Port in;
in.open("/test/in");
Bottle bot;
in.read(bot);
printf("Got message %s\n", bot.toString().c_str());
in.close();
}
return 0;
}
bool yarp::os::impl::HttpCarrier::expectSenderSpecifier(Protocol& proto) {
proto.setRoute(proto.getRoute().addFromName("web"));
String remainder = NetType::readLine(proto.is());
if (!urlDone) {
for (unsigned int i=0; i<remainder.length(); i++) {
if (remainder[i]!=' ') {
url += remainder[i];
} else {
break;
}
}
}
bool done = false;
expectPost = false;
contentLength = 0;
while (!done) {
String result = NetType::readLine(proto.is());
if (result == "") {
done = true;
} else {
//printf(">>> %s\n", result.c_str());
Bottle b;
b.fromString(result.c_str());
if (b.get(0).asString()=="Content-Length:") {
//printf("]]] got length %d\n", b.get(1).asInt());
contentLength = b.get(1).asInt();
}
if (b.get(0).asString()=="Content-Type:") {
//printf("]]] got type %s\n", b.get(1).asString());
if (b.get(1).asString()=="application/x-www-form-urlencoded") {
expectPost = true;
}
}
}
}
if (expectPost) {
//printf("[[[this is a post message of length %d]]]\n", contentLength);
ManagedBytes blk(contentLength+1);
Bytes start(blk.get(),contentLength);
NetType::readFull(proto.is(),start);
blk.get()[contentLength] = '\0';
//printf("message: %s\n", blk.get());
input = blk.get();
} else {
//printf("message: %s\n", url.c_str());
input = url;
}
prop.fromQuery(input.c_str());
prop.put("REQUEST_URI",url.c_str());
//printf("Property %s\n",prop.toString().c_str());
Contact chome = NetworkBase::getNameServerContact();
Address home = Address::fromContact(chome);
Address me = proto.getStreams().getLocalAddress();
String from = "<html><head><link href=\"http://";
from += home.getName();
from += ":";
from += NetType::toString(home.getPort());
from += "/web/main.css\" rel=\"stylesheet\" type=\"text/css\"/></head><body bgcolor='#ffffcc'><h1>yarp port ";
from += proto.getRoute().getToName();
from += "</h1>\n";
from += "<p>(<a href=\"http://";
from += home.getName();
from += ":";
from += NetType::toString(home.getPort());
from += "/data=list\">All ports</a>) \n";
from += "(<a href=\"http://";
from += me.getName();
from += ":";
from += NetType::toString(me.getPort());
from += "/\">connections</a>) \n";
from += "(<a href=\"http://";
from += me.getName();
from += ":";
from += NetType::toString(me.getPort());
from += "/data=help\">help</a>) \n";
from += "(<a href=\"http://";
from += me.getName();
from += ":";
from += NetType::toString(me.getPort());
from += "/r\">read</a>) \n";
from += "</p>\n";
from += "<p>\n";
from += "<form method=\"post\" action=\"http://";
from += me.getName();
from += ":";
from += NetType::toString(me.getPort());
from += "/form\">";
prefix = from;
//Bytes b2((char*)from.c_str(),from.length());
//proto.os().write(b2);
//proto.os().flush();
// Message gets finished by the stream
return proto.os().isOk();
}
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;
}
int main(int argc, char *argv[])
{
// just list the devices if no argument given
if (argc <= 2) {
printf("You can call %s like this:\n", argv[0]);
printf(" %s --robot ROBOTNAME --OPTION VALUE ...\n", argv[0]);
printf("For example:\n");
printf(" %s --robot icub --local /talkto/james --remote /controlboard/rpc\n", argv[0]);
printf("Here are devices listed for your system:\n");
printf("%s", Drivers::factory().toString().c_str());
return 0;
}
// get command line options
Property options;
options.fromCommand(argc, argv);
if (!options.check("robot") || !options.check("part")) {
printf("Missing either --robot or --part options\n");
return 0;
}
Network::init();
Time::turboBoost();
std::string name;
Value& v = options.find("robot");
Value& part = options.find("part");
Value *val;
if (!options.check("device", val)) {
options.put("device", "remote_controlboard");
}
if (!options.check("local", val)) {
name="/"+std::string(v.asString().c_str())+"/"+std::string(part.asString().c_str())+"/simpleclient";
//sprintf(&name[0], "/%s/%s/client", v.asString().c_str(), part.asString().c_str());
options.put("local", name.c_str());
}
if (!options.check("remote", val)) {
name="/"+std::string(v.asString().c_str())+"/"+std::string(part.asString().c_str());
//sprintf(&name[0], "/%s/%s", v.asString().c_str(), part.asString().c_str());
options.put("remote", name.c_str());
}
fprintf(stderr, "%s", options.toString().c_str());
// create a device
PolyDriver dd(options);
if (!dd.isValid()) {
printf("Device not available. Here are the known devices:\n");
printf("%s", Drivers::factory().toString().c_str());
Network::fini();
return 1;
}
IPositionControl *pos;
IPositionDirect *posDir;
IVelocityControl *vel;
IEncoders *enc;
IPidControl *pid;
IAmplifierControl *amp;
IControlLimits *lim;
// IControlMode *icm;
IControlMode2 *iMode2;
ITorqueControl *itorque;
IOpenLoopControl *iopenloop;
IImpedanceControl *iimp;
IInteractionMode *iInteract;
bool ok;
ok = dd.view(pos);
ok &= dd.view(vel);
ok &= dd.view(enc);
ok &= dd.view(pid);
ok &= dd.view(amp);
ok &= dd.view(lim);
// ok &= dd.view(icm);
ok &= dd.view(itorque);
ok &= dd.view(iopenloop);
ok &= dd.view(iimp);
ok &= dd.view(posDir);
ok &= dd.view(iMode2);
ok &= dd.view(iInteract);
if (!ok) {
printf("Problems acquiring interfaces\n");
return 1;
}
pos->getAxes(&jnts);
printf("Working with %d axes\n", jnts);
double *tmp = new double[jnts];
printf("Device active...\n");
while (dd.isValid()) {
std::string s;
s.resize(1024);
printf("-> ");
char c = 0;
int i = 0;
while (c != '\n') {
c = (char)fgetc(stdin);
s[i++] = c;
}
s[i-1] = s[i] = 0;
Bottle p;
Bottle response;
bool ok=false;
bool rec=false;
p.fromString(s.c_str());
printf("Bottle: %s\n", p.toString().c_str());
switch(p.get(0).asVocab()) {
case VOCAB_HELP:
printf("\n\n");
printf("Available commands:\n");
printf("-------------------\n\n");
printf("IOpenLoop:\ntype [%s] and one of the following:\n", Vocab::decode(VOCAB_IOPENLOOP).c_str());
printf(" [set] [%s] <int> <float>\n",
Vocab::decode(VOCAB_OUTPUT).c_str());
printf(" [get] [%s] <int>\n",
Vocab::decode(VOCAB_OUTPUT).c_str());
printf(" [get] [%s]\n\n",
Vocab::decode(VOCAB_OUTPUTS).c_str());
printf("IControlMode:\ntype [%s] and one of the following:\n", Vocab::decode(VOCAB_ICONTROLMODE).c_str());
printf(" [set] [%s]|[%s]|[%s]|[%s]|[%s]|[%s]|[%s]|[%s][%s]|[%s]\n",
Vocab::decode(VOCAB_CM_POSITION).c_str(),
Vocab::decode(VOCAB_CM_POSITION_DIRECT).c_str(),
Vocab::decode(VOCAB_CM_VELOCITY).c_str(),
Vocab::decode(VOCAB_CM_MIXED).c_str(),
Vocab::decode(VOCAB_CM_TORQUE).c_str(),
Vocab::decode(VOCAB_CM_OPENLOOP).c_str(),
Vocab::decode(VOCAB_CM_IDLE).c_str(),
Vocab::decode(VOCAB_CM_FORCE_IDLE).c_str(),
Vocab::decode(VOCAB_CM_IMPEDANCE_POS).c_str(),
Vocab::decode(VOCAB_CM_IMPEDANCE_VEL).c_str());
printf(" [get] [%s] <int>\n\n",
Vocab::decode(VOCAB_CM_CONTROL_MODE).c_str());
printf("ITorqueControl:\ntype [%s] and one of the following:\n", Vocab::decode(VOCAB_TORQUE).c_str());
printf(" [get] [%s] <int> to read the measured torque for a single axis\n", Vocab::decode(VOCAB_TRQ).c_str());
printf(" [get] [%s] to read the measured torque for all axes\n", Vocab::decode(VOCAB_TRQS).c_str());
printf(" [set] [%s] <int> <float> to set the reference torque for a single axis\n", Vocab::decode(VOCAB_REF).c_str());
printf(" [set] [%s] <float list> to set the reference torque for all axes\n", Vocab::decode(VOCAB_REFS).c_str());
printf(" [get] [%s] <int> to read the reference torque for a single axis\n", Vocab::decode(VOCAB_REF).c_str());
printf(" [get] [%s] to read the reference torque for all axes\n\n", Vocab::decode(VOCAB_REFS).c_str());
printf("IImpedanceControl:\ntype [%s] and one of the following:\n", Vocab::decode(VOCAB_IMPEDANCE).c_str());
printf(" [set] [%s] <int> <float> <float> \n",
Vocab::decode(VOCAB_IMP_PARAM).c_str());
printf(" [set] [%s] <int> <float>\n\n",
Vocab::decode(VOCAB_IMP_OFFSET).c_str());
printf(" [get] [%s] <int>\n",
Vocab::decode(VOCAB_IMP_PARAM).c_str());
printf(" [get] [%s] <int>\n\n",
Vocab::decode(VOCAB_IMP_OFFSET).c_str());
printf("IInteractionMode:\ntype [%s] and one of the following:\n", Vocab::decode(VOCAB_INTERFACE_INTERACTION_MODE).c_str());
printf(" [set] [%s]|[%s] <int>\n",
Vocab::decode(VOCAB_IM_STIFF).c_str(),
Vocab::decode(VOCAB_IM_COMPLIANT).c_str());
printf(" [get] [%s] <int>\n",
Vocab::decode(VOCAB_INTERACTION_MODE).c_str());
printf(" [get] [%s] \n\n",
Vocab::decode(VOCAB_INTERACTION_MODES).c_str());
printf("Standard Interfaces:\n");
printf("type [get] and one of the following:\n");
printf(" [%s] to read the number of controlled axes\n", Vocab::decode(VOCAB_AXES).c_str());
printf(" [%s] to read the encoder value for all axes\n", Vocab::decode(VOCAB_ENCODERS).c_str());
printf(" [%s] to read the PID values for all axes\n", Vocab::decode(VOCAB_PIDS).c_str());
printf(" [%s] <int> to read the PID values for a single axis\n", Vocab::decode(VOCAB_PID).c_str());
printf(" [%s] <int> to read the limit values for a single axis\n", Vocab::decode(VOCAB_LIMITS).c_str());
printf(" [%s] to read the PID error for all axes\n", Vocab::decode(VOCAB_ERRS).c_str());
printf(" [%s] to read the PID output for all axes\n", Vocab::decode(VOCAB_OUTPUTS).c_str());
printf(" [%s] to read the reference position for all axes\n", Vocab::decode(VOCAB_REFERENCES).c_str());
printf(" [%s] <int> to read the reference position for a single axis\n", Vocab::decode(VOCAB_REFERENCE).c_str());
printf(" [%s] to read the reference speed for all axes\n", Vocab::decode(VOCAB_REF_SPEEDS).c_str());
printf(" [%s] <int> to read the reference speed for a single axis\n", Vocab::decode(VOCAB_REF_SPEED).c_str());
printf(" [%s] to read the reference acceleration for all axes\n", Vocab::decode(VOCAB_REF_ACCELERATIONS).c_str());
printf(" [%s] <int> to read the reference acceleration for a single axis\n", Vocab::decode(VOCAB_REF_ACCELERATION).c_str());
printf(" [%s] to read the current consumption for all axes\n", Vocab::decode(VOCAB_AMP_CURRENTS).c_str());
printf("\n");
printf("type [set] and one of the following:\n");
printf(" [%s] <int> <double> to move a single axis\n", Vocab::decode(VOCAB_POSITION_MOVE).c_str());
printf(" [%s] <int> <double> to accelerate a single axis to a given speed\n", Vocab::decode(VOCAB_VELOCITY_MOVE).c_str());
printf(" [%s] <int> <double> to set the reference speed for a single axis\n", Vocab::decode(VOCAB_REF_SPEED).c_str());
printf(" [%s] <int> <double> to set the reference acceleration for a single axis\n", Vocab::decode(VOCAB_REF_ACCELERATION).c_str());
printf(" [%s] <list> to move multiple axes\n", Vocab::decode(VOCAB_POSITION_MOVES).c_str());
printf(" [%s] <list> to accelerate multiple axes to a given speed\n", Vocab::decode(VOCAB_VELOCITY_MOVES).c_str());
printf(" [%s] <list> to set the reference speed for all axes\n", Vocab::decode(VOCAB_REF_SPEEDS).c_str());
printf(" [%s] <list> to set the reference acceleration for all axes\n", Vocab::decode(VOCAB_REF_ACCELERATIONS).c_str());
printf(" [%s] <int> to stop a single axis\n", Vocab::decode(VOCAB_STOP).c_str());
printf(" [%s] <int> to stop all axes\n", Vocab::decode(VOCAB_STOPS).c_str());
printf(" [%s] <int> <list> to set the PID values for a single axis\n", Vocab::decode(VOCAB_PID).c_str());
printf(" [%s] <int> <list> to set the limits for a single axis\n", Vocab::decode(VOCAB_LIMITS).c_str());
printf(" [%s] <int> to disable the PID control for a single axis\n", Vocab::decode(VOCAB_DISABLE).c_str());
printf(" [%s] <int> to enable the PID control for a single axis\n", Vocab::decode(VOCAB_ENABLE).c_str());
printf(" [%s] <int> <double> to set the encoder value for a single axis\n", Vocab::decode(VOCAB_ENCODER).c_str());
printf(" [%s] <list> to set the encoder value for all axes\n", Vocab::decode(VOCAB_ENCODERS).c_str());
printf("\n");
printf("NOTES: - A list is a sequence of numbers in parenthesis, e.g. (10 2 1 10)\n");
printf(" - Pids are expressed as a list of 7 numbers, type get pid <int> to see an example\n");
printf("\n");
break;
case VOCAB_QUIT:
goto ApplicationCleanQuit;
break;
case VOCAB_ICONTROLMODE:
{
handleControlModeMsg(iMode2, p, response, &rec, &ok);
printf("%s\n", response.toString().c_str());
break;
}
case VOCAB_IMPEDANCE:
{
handleImpedanceMsg(iimp, p, response, &rec, &ok);
printf("%s\n", response.toString().c_str());
break;
}
case VOCAB_TORQUE:
{
handleTorqueMsg(itorque, p, response, &rec, &ok);
printf("%s\n", response.toString().c_str());
break;
}
case VOCAB_INTERFACE_INTERACTION_MODE:
{
handleInteractionModeMsg(iInteract, p, response, &rec, &ok);
printf("%s\n", response.toString().c_str());
break;
}
case VOCAB_GET:
switch(p.get(1).asVocab()) {
case VOCAB_AXES: {
int nj = 0;
enc->getAxes(&nj);
printf ("%s: %d\n", Vocab::decode(VOCAB_AXES).c_str(), nj);
}
break;
case VOCAB_ENCODERS: {
enc->getEncoders(tmp);
printf ("%s: (", Vocab::decode(VOCAB_ENCODERS).c_str());
for(i = 0; i < jnts; i++)
printf ("%.2f ", tmp[i]);
printf (")\n");
}
break;
case VOCAB_PID: {
Pid pd;
int j = p.get(2).asInt();
pid->getPid(j, &pd);
printf("%s: ", Vocab::decode(VOCAB_PID).c_str());
printf("kp %.2f ", pd.kp);
printf("kd %.2f ", pd.kd);
printf("ki %.2f ", pd.ki);
printf("maxi %.2f ", pd.max_int);
printf("maxo %.2f ", pd.max_output);
printf("off %.2f ", pd.offset);
printf("scale %.2f ", pd.scale);
printf("\n");
}
break;
case VOCAB_PIDS: {
Pid *p = new Pid[jnts];
ok = pid->getPids(p);
Bottle& b = response.addList();
int i;
for (i = 0; i < jnts; i++)
{
Bottle& c = b.addList();
c.addDouble(p[i].kp);
c.addDouble(p[i].kd);
c.addDouble(p[i].ki);
c.addDouble(p[i].max_int);
c.addDouble(p[i].max_output);
c.addDouble(p[i].offset);
c.addDouble(p[i].scale);
}
printf("%s\n", b.toString().c_str());
delete[] p;
}
break;
case VOCAB_LIMITS: {
double min, max;
int j = p.get(2).asInt();
lim->getLimits(j, &min, &max);
printf("%s: ", Vocab::decode(VOCAB_LIMITS).c_str());
printf("limits: (%.2f %.2f)\n", min, max);
}
break;
case VOCAB_ERRS: {
pid->getErrors(tmp);
printf ("%s: (", Vocab::decode(VOCAB_ERRS).c_str());
for(i = 0; i < jnts; i++)
printf ("%.2f ", tmp[i]);
printf (")\n");
}
break;
case VOCAB_OUTPUTS: {
iopenloop->getOutputs(tmp);
printf ("%s: (", Vocab::decode(VOCAB_OUTPUTS).c_str());
for(i = 0; i < jnts; i++)
printf ("%.2f ", tmp[i]);
printf (")\n");
}
break;
case VOCAB_OUTPUT: {
int j = p.get(2).asInt();
double v;
iopenloop->getOutput(j, &v);
printf("%s: ", Vocab::decode(VOCAB_OUTPUT).c_str());
printf("%.2f ", v);
printf("\n");
}
break;
case VOCAB_REFERENCE: {
double ref_pos;
int j = p.get(2).asInt();
pid->getReference(j,&ref_pos);
printf ("%s: (", Vocab::decode(VOCAB_REFERENCE).c_str());
printf ("%.2f ", ref_pos);
printf (")\n");
}
break;
case VOCAB_REFERENCES: {
pid->getReferences(tmp);
printf ("%s: (", Vocab::decode(VOCAB_REFERENCES).c_str());
for(i = 0; i < jnts; i++)
printf ("%.2f ", tmp[i]);
printf (")\n");
}
break;
case VOCAB_REF_SPEEDS: {
pos->getRefSpeeds(tmp);
printf ("%s: (", Vocab::decode(VOCAB_REF_SPEEDS).c_str());
for(i = 0; i < jnts; i++)
printf ("%.2f ", tmp[i]);
printf (")\n");
}
break;
case VOCAB_REF_SPEED: {
double ref_speed;
int j = p.get(2).asInt();
pos->getRefSpeed(j,&ref_speed);
printf ("%s: (", Vocab::decode(VOCAB_REF_SPEED).c_str());
printf ("%.2f ", ref_speed);
printf (")\n");
}
break;
case VOCAB_REF_ACCELERATION: {
double ref_acc;
int j = p.get(2).asInt();
pos->getRefAcceleration(j,&ref_acc);
printf ("%s: (", Vocab::decode(VOCAB_REF_ACCELERATION).c_str());
printf ("%.2f ", ref_acc);
printf (")\n");
}
break;
case VOCAB_REF_ACCELERATIONS: {
pos->getRefAccelerations(tmp);
printf ("%s: (", Vocab::decode(VOCAB_REF_ACCELERATIONS).c_str());
for(i = 0; i < jnts; i++)
printf ("%.2f ", tmp[i]);
printf (")\n");
}
break;
case VOCAB_AMP_CURRENTS: {
amp->getCurrents(tmp);
printf ("%s: (", Vocab::decode(VOCAB_AMP_CURRENTS).c_str());
for(i = 0; i < jnts; i++)
printf ("%.2f ", tmp[i]);
printf (")\n");
}
break;
}
break;
case VOCAB_SET:
switch(p.get(1).asVocab()) {
case VOCAB_POSITION_MOVE: {
int j = p.get(2).asInt();
double ref = p.get(3).asDouble();
printf("%s: moving %d to %.2f\n", Vocab::decode(VOCAB_POSITION_MOVE).c_str(), j, ref);
pos->positionMove(j, ref);
}
break;
case VOCAB_VELOCITY_MOVE: {
int j = p.get(2).asInt();
double ref = p.get(3).asDouble();
printf("%s: accelerating %d to %.2f\n", Vocab::decode(VOCAB_VELOCITY_MOVE).c_str(), j, ref);
vel->velocityMove(j, ref);
}
break;
case VOCAB_REF_SPEED: {
int j = p.get(2).asInt();
double ref = p.get(3).asDouble();
printf("%s: setting speed for %d to %.2f\n", Vocab::decode(VOCAB_REF_SPEED).c_str(), j, ref);
pos->setRefSpeed(j, ref);
}
break;
case VOCAB_REF_ACCELERATION: {
int j = p.get(2).asInt();
double ref = p.get(3).asDouble();
printf("%s: setting acceleration for %d to %.2f\n", Vocab::decode(VOCAB_REF_ACCELERATION).c_str(), j, ref);
pos->setRefAcceleration(j, ref);
}
break;
case VOCAB_POSITION_MOVES: {
Bottle *l = p.get(2).asList();
for (i = 0; i < jnts; i++) {
tmp[i] = l->get(i).asDouble();
}
printf("%s: moving all joints\n", Vocab::decode(VOCAB_POSITION_MOVES).c_str());
pos->positionMove(tmp);
}
break;
case VOCAB_VELOCITY_MOVES: {
Bottle *l = p.get(2).asList();
for (i = 0; i < jnts; i++) {
tmp[i] = l->get(i).asDouble();
}
printf("%s: moving all joints\n", Vocab::decode(VOCAB_VELOCITY_MOVES).c_str());
vel->velocityMove(tmp);
}
break;
case VOCAB_REF_SPEEDS: {
Bottle *l = p.get(2).asList();
for (i = 0; i < jnts; i++) {
tmp[i] = l->get(i).asDouble();
}
printf("%s: setting speed for all joints\n", Vocab::decode(VOCAB_REF_SPEEDS).c_str());
pos->setRefSpeeds(tmp);
}
break;
case VOCAB_REF_ACCELERATIONS: {
Bottle *l = p.get(2).asList();
for (i = 0; i < jnts; i++) {
tmp[i] = l->get(i).asDouble();
}
printf("%s: setting acceleration for all joints\n", Vocab::decode(VOCAB_REF_ACCELERATIONS).c_str());
pos->setRefAccelerations(tmp);
}
break;
case VOCAB_STOP: {
int j = p.get(2).asInt();
printf("%s: stopping axis %d\n", Vocab::decode(VOCAB_STOP).c_str(), j);
pos->stop(j);
}
break;
case VOCAB_STOPS: {
printf("%s: stopping all axes\n", Vocab::decode(VOCAB_STOPS).c_str());
pos->stop();
}
break;
case VOCAB_ENCODER: {
int j = p.get(2).asInt();
double ref = p.get(3).asDouble();
printf("%s: setting the encoder value for %d to %.2f\n", Vocab::decode(VOCAB_ENCODER).c_str(), j, ref);
enc->setEncoder(j, ref);
}
break;
case VOCAB_ENCODERS: {
Bottle *l = p.get(2).asList();
for (i = 0; i < jnts; i++) {
tmp[i] = l->get(i).asDouble();
}
printf("%s: setting the encoder value for all joints\n", Vocab::decode(VOCAB_ENCODERS).c_str());
enc->setEncoders(tmp);
}
break;
case VOCAB_PID: {
Pid pd;
int j = p.get(2).asInt();
Bottle *l = p.get(3).asList();
if (l==0)
{
printf("Check you specify a 7 elements list, e.g. set pid 0 (2000 20 1 300 300 0 0)\n");
}
else
{
int elems=l->size();
if (elems>=3)
{
pd.kp = l->get(0).asDouble();
pd.kd = l->get(1).asDouble();
pd.ki = l->get(2).asDouble();
if (elems>=7)
{
pd.max_int = l->get(3).asDouble();
pd.max_output = l->get(4).asDouble();
pd.offset = l->get(5).asDouble();
pd.scale = l->get(6).asDouble();
}
printf("%s: setting PID values for axis %d\n", Vocab::decode(VOCAB_PID).c_str(), j);
pid->setPid(j, pd);
}
else
{
printf("Error, check you specify at least 7 elements, e.g. set pid 0 (2000 20 1 300 300 0 0)\n");
}
}
}
break;
case VOCAB_DISABLE: {
int j = p.get(2).asInt();
printf("%s: disabling control for axis %d\n", Vocab::decode(VOCAB_DISABLE).c_str(), j);
pid->disablePid(j);
amp->disableAmp(j);
}
break;
case VOCAB_ENABLE: {
int j = p.get(2).asInt();
printf("%s: enabling control for axis %d\n", Vocab::decode(VOCAB_ENABLE).c_str(), j);
amp->enableAmp(j);
pid->enablePid(j);
}
break;
case VOCAB_LIMITS: {
int j = p.get(2).asInt();
printf("%s: setting limits for axis %d\n", Vocab::decode(VOCAB_LIMITS).c_str(), j);
Bottle *l = p.get(3).asList();
lim->setLimits(j, l->get(0).asDouble(), l->get(1).asDouble());
}
break;
case VOCAB_OUTPUT: {
int j=p.get(2).asInt();
double v=p.get(3).asDouble();
iopenloop->setRefOutput(j,v);
printf("%s: setting output for axis %d to %f\n", Vocab::decode(VOCAB_OUTPUT).c_str(), j, v);
}
break;
}
break;
} /* switch get(0) */
} /* while () */
ApplicationCleanQuit:
dd.close();
delete[] tmp;
Network::fini();
return 0;
}
int main(int argc, char *argv[])
{
// just list the devices if no argument given
if (argc <= 2) {
printf("You can call %s like this:\n", argv[0]);
printf(" %s --robot ROBOTNAME --OPTION VALUE ...\n", argv[0]);
printf("For example:\n");
printf(" %s --robot icub --part any --remote /controlboard\n", argv[0]);
printf("Here are devices listed for your system:\n");
printf("%s", Drivers::factory().toString().c_str());
return 0;
}
// get command line options
Property options;
options.fromCommand(argc, argv);
if (!options.check("robot") || !options.check("part")) {
printf("Missing either --robot or --part options\n");
return 0;
}
Network yarp;
Time::turboBoost();
char name[1024];
Value& v = options.find("robot");
Value& part = options.find("part");
Value *val;
if (!options.check("device", val)) {
options.put("device", "remote_controlboard");
}
if (!options.check("local", val)) {
sprintf(name, "/%s/%s/client", v.asString().c_str(), part.asString().c_str());
options.put("local", name);
}
if (!options.check("remote", val)) {
sprintf(name, "/%s/%s", v.asString().c_str(), part.asString().c_str());
options.put("remote", name);
}
fprintf(stderr, "%s", options.toString().c_str());
// create a device
PolyDriver dd(options);
if (!dd.isValid()) {
printf("Device not available. Here are the known devices:\n");
printf("%s", Drivers::factory().toString().c_str());
return 1;
}
IPositionControl *pos;
IVelocityControl *vel;
IEncoders *enc;
IPidControl *pid;
IAmplifierControl *amp;
IControlLimits *lim;
bool ok;
ok = dd.view(pos);
ok &= dd.view(vel);
ok &= dd.view(enc);
ok &= dd.view(pid);
ok &= dd.view(amp);
ok &= dd.view(lim);
if (!ok) {
printf("Problems acquiring interfaces\n");
return 1;
}
int jnts = 0;
pos->getAxes(&jnts);
printf("Working with %d axes\n", jnts);
double *tmp = new double[jnts];
assert (tmp != NULL);
printf("Device active...\n");
while (dd.isValid()) {
char s[1024];
printf("-> ");
char c = 0;
int i = 0;
while (c != '\n') {
c = (char)fgetc(stdin);
s[i++] = c;
}
s[i-1] = s[i] = 0;
Bottle p;
p.fromString(s);
printf("Bottle: %s\n", p.toString().c_str());
switch(p.get(0).asVocab()) {
case VOCAB_HELP:
printf("\n\n");
printf("Available commands:\n\n");
printf("type [get] and one of the following:\n");
printf("[%s] to read the number of controlled axes\n", Vocab::decode(VOCAB_AXES).c_str());
printf("[%s] to read the encoder value for all axes\n", Vocab::decode(VOCAB_ENCODERS).c_str());
printf("[%s] <int> to read the PID values for a single axis\n", Vocab::decode(VOCAB_PID).c_str());
printf("[%s] <int> to read the limit values for a single axis\n", Vocab::decode(VOCAB_LIMITS).c_str());
printf("[%s] to read the PID error for all axes\n", Vocab::decode(VOCAB_ERRS).c_str());
printf("[%s] to read the PID output for all axes\n", Vocab::decode(VOCAB_OUTPUTS).c_str());
printf("[%s] to read the reference position for all axes\n", Vocab::decode(VOCAB_REFERENCES).c_str());
printf("[%s] to read the reference speed for all axes\n", Vocab::decode(VOCAB_REF_SPEEDS).c_str());
printf("[%s] to read the reference acceleration for all axes\n", Vocab::decode(VOCAB_REF_ACCELERATIONS).c_str());
printf("[%s] to read the current consumption for all axes\n", Vocab::decode(VOCAB_AMP_CURRENTS).c_str());
printf("\n");
printf("type [set] and one of the following:\n");
printf("[%s] <int> <double> to move a single axis\n", Vocab::decode(VOCAB_POSITION_MOVE).c_str());
printf("[%s] <int> <double> to accelerate a single axis to a given speed\n", Vocab::decode(VOCAB_VELOCITY_MOVE).c_str());
printf("[%s] <int> <double> to set the reference speed for a single axis\n", Vocab::decode(VOCAB_REF_SPEED).c_str());
printf("[%s] <int> <double> to set the reference acceleration for a single axis\n", Vocab::decode(VOCAB_REF_ACCELERATION).c_str());
printf("[%s] <list> to move multiple axes\n", Vocab::decode(VOCAB_POSITION_MOVES).c_str());
printf("[%s] <list> to accelerate multiple axes to a given speed\n", Vocab::decode(VOCAB_VELOCITY_MOVES).c_str());
printf("[%s] <list> to set the reference speed for all axes\n", Vocab::decode(VOCAB_REF_SPEEDS).c_str());
printf("[%s] <list> to set the reference acceleration for all axes\n", Vocab::decode(VOCAB_REF_ACCELERATIONS).c_str());
printf("[%s] <int> to stop a single axis\n", Vocab::decode(VOCAB_STOP).c_str());
printf("[%s] <int> to stop all axes\n", Vocab::decode(VOCAB_STOPS).c_str());
printf("[%s] <int> <list> to set the PID values for a single axis\n", Vocab::decode(VOCAB_PID).c_str());
printf("[%s] <int> <list> to set the limits for a single axis\n", Vocab::decode(VOCAB_LIMITS).c_str());
printf("[%s] <int> to disable the PID control for a single axis\n", Vocab::decode(VOCAB_DISABLE).c_str());
printf("[%s] <int> to enable the PID control for a single axis\n", Vocab::decode(VOCAB_ENABLE).c_str());
printf("[%s] <int> <double> to set the encoder value for a single axis\n", Vocab::decode(VOCAB_ENCODER).c_str());
printf("[%s] <list> to set the encoder value for all axes\n", Vocab::decode(VOCAB_ENCODERS).c_str());
printf("\n");
break;
case VOCAB_QUIT:
goto ApplicationCleanQuit;
break;
case VOCAB_GET:
switch(p.get(1).asVocab()) {
case VOCAB_AXES: {
int nj = 0;
enc->getAxes(&nj);
printf ("%s: %d\n", Vocab::decode(VOCAB_AXES).c_str(), nj);
}
break;
case VOCAB_ENCODERS: {
enc->getEncoders(tmp);
printf ("%s: (", Vocab::decode(VOCAB_ENCODERS).c_str());
for(i = 0; i < jnts; i++)
printf ("%.2f ", tmp[i]);
printf (")\n");
}
break;
case VOCAB_PID: {
Pid pd;
int j = p.get(2).asInt();
pid->getPid(j, &pd);
printf("%s: ", Vocab::decode(VOCAB_PID).c_str());
printf("kp %.2f ", pd.kp);
printf("kd %.2f ", pd.kd);
printf("ki %.2f ", pd.ki);
printf("maxi %.2f ", pd.max_int);
printf("maxo %.2f ", pd.max_output);
printf("off %.2f ", pd.offset);
printf("scale %.2f ", pd.scale);
printf("\n");
}
break;
case VOCAB_LIMITS: {
double min, max;
int j = p.get(2).asInt();
lim->getLimits(j, &min, &max);
printf("%s: ", Vocab::decode(VOCAB_LIMITS).c_str());
printf("limits: (%.2f %.2f)\n", min, max);
}
break;
case VOCAB_ERRS: {
pid->getErrorLimits(tmp);
printf ("%s: (", Vocab::decode(VOCAB_ERRS).c_str());
for(i = 0; i < jnts; i++)
printf ("%.2f ", tmp[i]);
printf (")\n");
}
break;
case VOCAB_OUTPUTS: {
pid->getErrors(tmp);
printf ("%s: (", Vocab::decode(VOCAB_OUTPUTS).c_str());
for(i = 0; i < jnts; i++)
printf ("%.2f ", tmp[i]);
printf (")\n");
}
break;
case VOCAB_REFERENCES: {
pid->getReferences(tmp);
printf ("%s: (", Vocab::decode(VOCAB_REFERENCES).c_str());
for(i = 0; i < jnts; i++)
printf ("%.2f ", tmp[i]);
printf (")\n");
}
break;
case VOCAB_REF_SPEEDS: {
pos->getRefSpeeds(tmp);
printf ("%s: (", Vocab::decode(VOCAB_REF_SPEEDS).c_str());
for(i = 0; i < jnts; i++)
printf ("%.2f ", tmp[i]);
printf (")\n");
}
break;
case VOCAB_REF_ACCELERATIONS: {
pos->getRefAccelerations(tmp);
printf ("%s: (", Vocab::decode(VOCAB_REF_ACCELERATIONS).c_str());
for(i = 0; i < jnts; i++)
printf ("%.2f ", tmp[i]);
printf (")\n");
}
break;
case VOCAB_AMP_CURRENTS: {
amp->getCurrents(tmp);
printf ("%s: (", Vocab::decode(VOCAB_AMP_CURRENTS).c_str());
for(i = 0; i < jnts; i++)
printf ("%.2f ", tmp[i]);
printf (")\n");
}
break;
}
break;
case VOCAB_SET:
switch(p.get(1).asVocab()) {
case VOCAB_POSITION_MOVE: {
int j = p.get(2).asInt();
double ref = p.get(3).asDouble();
printf("%s: moving %d to %.2f\n", Vocab::decode(VOCAB_POSITION_MOVE).c_str(), j, ref);
pos->positionMove(j, ref);
}
break;
case VOCAB_VELOCITY_MOVE: {
int j = p.get(2).asInt();
double ref = p.get(3).asDouble();
printf("%s: accelerating %d to %.2f\n", Vocab::decode(VOCAB_VELOCITY_MOVE).c_str(), j, ref);
vel->velocityMove(j, ref);
}
break;
case VOCAB_REF_SPEED: {
int j = p.get(2).asInt();
double ref = p.get(3).asDouble();
printf("%s: setting speed for %d to %.2f\n", Vocab::decode(VOCAB_REF_SPEED).c_str(), j, ref);
pos->setRefSpeed(j, ref);
}
break;
case VOCAB_REF_ACCELERATION: {
int j = p.get(2).asInt();
double ref = p.get(3).asDouble();
printf("%s: setting acceleration for %d to %.2f\n", Vocab::decode(VOCAB_REF_ACCELERATION).c_str(), j, ref);
pos->setRefAcceleration(j, ref);
}
break;
case VOCAB_POSITION_MOVES: {
Bottle *l = p.get(2).asList();
for (i = 0; i < jnts; i++) {
tmp[i] = l->get(i).asDouble();
}
printf("%s: moving all joints\n", Vocab::decode(VOCAB_POSITION_MOVES).c_str());
pos->positionMove(tmp);
}
break;
case VOCAB_VELOCITY_MOVES: {
Bottle *l = p.get(2).asList();
for (i = 0; i < jnts; i++) {
tmp[i] = l->get(i).asDouble();
}
printf("%s: moving all joints\n", Vocab::decode(VOCAB_VELOCITY_MOVES).c_str());
vel->velocityMove(tmp);
}
break;
case VOCAB_REF_SPEEDS: {
Bottle *l = p.get(2).asList();
for (i = 0; i < jnts; i++) {
tmp[i] = l->get(i).asDouble();
}
printf("%s: setting speed for all joints\n", Vocab::decode(VOCAB_REF_SPEEDS).c_str());
pos->setRefSpeeds(tmp);
}
break;
case VOCAB_REF_ACCELERATIONS: {
Bottle *l = p.get(2).asList();
for (i = 0; i < jnts; i++) {
tmp[i] = l->get(i).asDouble();
}
printf("%s: setting acceleration for all joints\n", Vocab::decode(VOCAB_REF_ACCELERATIONS).c_str());
pos->setRefAccelerations(tmp);
}
break;
case VOCAB_STOP: {
int j = p.get(2).asInt();
printf("%s: stopping axis %d\n", Vocab::decode(VOCAB_STOP).c_str());
pos->stop(j);
}
break;
case VOCAB_STOPS: {
printf("%s: stopping all axes %d\n", Vocab::decode(VOCAB_STOPS).c_str());
pos->stop();
}
break;
case VOCAB_ENCODER: {
int j = p.get(2).asInt();
double ref = p.get(3).asDouble();
printf("%s: setting the encoder value for %d to %.2f\n", Vocab::decode(VOCAB_ENCODER).c_str(), j, ref);
enc->setEncoder(j, ref);
}
break;
case VOCAB_ENCODERS: {
Bottle *l = p.get(2).asList();
for (i = 0; i < jnts; i++) {
tmp[i] = l->get(i).asDouble();
}
printf("%s: setting the encoder value for all joints\n", Vocab::decode(VOCAB_ENCODERS).c_str());
enc->setEncoders(tmp);
}
break;
case VOCAB_PID: {
Pid pd;
int j = p.get(2).asInt();
Bottle *l = p.get(3).asList();
pd.kp = l->get(0).asDouble();
pd.kd = l->get(1).asDouble();
pd.ki = l->get(2).asDouble();
pd.max_int = l->get(3).asDouble();
pd.max_output = l->get(4).asDouble();
pd.offset = l->get(5).asDouble();
pd.scale = l->get(6).asDouble();
printf("%s: setting PID values for axis %d\n", Vocab::decode(VOCAB_PID).c_str(), j);
pid->setPid(j, pd);
}
break;
case VOCAB_DISABLE: {
int j = p.get(2).asInt();
printf("%s: disabling control for axis %d\n", Vocab::decode(VOCAB_DISABLE).c_str(), j);
pid->disablePid(j);
amp->disableAmp(j);
}
break;
case VOCAB_ENABLE: {
int j = p.get(2).asInt();
printf("%s: enabling control for axis %d\n", Vocab::decode(VOCAB_ENABLE).c_str(), j);
amp->enableAmp(j);
pid->enablePid(j);
}
break;
case VOCAB_LIMITS: {
int j = p.get(2).asInt();
printf("%s: setting limits for axis %d\n", Vocab::decode(VOCAB_LIMITS).c_str(), j);
Bottle *l = p.get(3).asList();
lim->setLimits(j, l->get(0).asDouble(), l->get(1).asDouble());
}
break;
}
break;
} /* switch get(0) */
} /* while () */
ApplicationCleanQuit:
dd.close();
delete[] tmp;
return 0;
}
bool XmlRpcCarrier::write(Protocol& proto, SizedWriter& writer) {
//XmlRpc::setVerbosity(10);
StringOutputStream sos;
StringInputStream sis;
writer.write(sos);
sis.reset(sos.toString());
String header;
if (sender) {
header = NetType::readLine(sis);
}
String body = NetType::readLine(sis);
//printf("Asked to write: hdr %s body %s\n",
// header.c_str(), body.c_str());
Value v;
//printf("HEADER %s\n", header.c_str());
if (header[0]=='q') {
body = "yarp.quit";
// XMLRPC does not need a quit message, this should get stripped
return false;
}
Bottle *bot = v.asList();
//Bottle aux;
bot->fromString(body.c_str());
ConstString methodName;
if (sender) {
methodName = bot->get(0).toString();
*bot = bot->tail();
}
XmlRpcValue args;
if (bot->size()==1) {
toXmlRpcValue(bot->get(0),args);
} else {
toXmlRpcValue(v,args);
}
//printf("xmlrpc block to write is %s\n", args.toXml().c_str());
std::string req;
if (sender) {
const Address& addr = host.isValid()?host:proto.getStreams().getRemoteAddress();
XmlRpcClient c(addr.getName().c_str(),(addr.getPort()>0)?addr.getPort():80);
c.generateRequest(methodName.c_str(),args);
req = c.getRequest();
} else {
XmlRpcServerConnection c(0,NULL);
c.generateResponse(args.toXml());
req = c.getResponse();
}
int start = 0;
//printf("converts to %s\n", req.c_str());
if (sender) {
if (req.length()<8) {
fprintf(stderr, "XmlRpcCarrier fail, %s:%d\n", __FILE__, __LINE__);
return false;
}
for (int i=0; i<(int)req.length(); i++) {
if (req[i] == '\n') {
start++;
break;
}
start++;
}
if (!firstRound) {
Bytes b((char*)http.c_str(),http.length());
proto.os().write(b);
}
firstRound = false;
}
Bytes b((char*)req.c_str()+start,req.length()-start);
//printf("WRITING [%s]\n", req.c_str()+start);
proto.os().write(b);
return proto.os().isOk();
}
void fromString(const ConstString& txt,bool wipe=true) {
Bottle bot;
bot.fromString(txt);
fromBottle(bot,wipe);
}
void fromConfig(const char *txt,Searchable& env, bool wipe=true) {
StringInputStream sis;
sis.add(txt);
sis.add("\n");
if (wipe) {
clear();
}
String tag = "";
Bottle accum;
bool done = false;
do {
bool isTag = false;
bool including = false;
String buf;
bool good = true;
buf = sis.readLine('\n',&good);
while (good && !BottleImpl::isComplete(buf.c_str())) {
buf += sis.readLine('\n',&good);
}
if (!good) {
done = true;
}
if (!done) {
including = false;
if (buf.find("//")!=String::npos) {
bool quoted = false;
int comment = 0;
for (unsigned int i=0; i<buf.length(); i++) {
char ch = buf[i];
if (ch=='\"') { quoted = !quoted; }
if (!quoted) {
if (ch=='/') {
comment++;
if (comment==2) {
//buf = buf.substr(0,buf.strstr("//"));
buf = buf.substr(0,i);
break;
}
} else {
comment = 0;
}
} else {
comment = 0;
}
}
}
// expand any environment references
buf = expand(buf.c_str(),env,owner).c_str();
if (buf.length()>0 && buf[0]=='[') {
size_t stop = buf.find("]");
if (stop!=String::npos) {
buf = buf.substr(1,stop-1);
size_t space = buf.find(" ");
if (space!=String::npos) {
Bottle bot(buf.c_str());
if (bot.size()>1) {
if (bot.get(0).toString() == "include") {
including = true;
if (bot.size()>2) {
if (tag!="") {
if (accum.size()>=1) {
putBottleCompat(tag.c_str(),
accum);
}
tag = "";
}
ConstString subName, fname;
if (bot.size()==3) {
// [include section "filename"]
subName = bot.get(1).toString();
fname = bot.get(2).toString();
} else if (bot.size()==4) {
// [include type section "filename"]
ConstString key;
key = bot.get(1).toString();
subName = bot.get(2).toString();
fname = bot.get(3).toString();
Bottle *target =
getBottle(key.c_str());
if (target==NULL) {
Bottle init;
init.addString(key.c_str());
init.addString(subName.c_str());
putBottleCompat(key.c_str(),
init);
} else {
target->addString(subName.c_str());
}
} else {
YARP_ERROR(Logger::get(),
String("bad include"));
return;
}
Property p;
if (getBottle(subName)!=NULL) {
p.fromString(getBottle(subName)->tail().toString());
//printf(">>> prior p %s\n",
// p.toString().c_str());
}
p.fromConfigFile(fname.c_str(),
env, false);
accum.fromString(p.toString());
tag = subName.c_str();
//printf(">>> tag %s accum %s\n",
// tag.c_str(),
// accum.toString().c_str());
if (tag!="") {
if (accum.size()>=1) {
Bottle b;
b.addString(tag.c_str());
//Bottle& subList = b.addList();
//subList.copy(accum);
b.append(accum);
putBottleCompat(tag.c_str(),
b);
}
tag = "";
}
} else {
tag = "";
ConstString fname =
bot.get(1).toString();
//printf("Including %s\n", fname.c_str());
fromConfigFile(fname.c_str(),
env, false);
}
}
}
if (bot.size()==2 && !including) {
buf = bot.get(1).toString().c_str();
String key = bot.get(0).toString().c_str();
Bottle *target = getBottle(key.c_str());
if (target==NULL) {
Bottle init;
init.addString(key.c_str());
init.addString(buf.c_str());
putBottleCompat(key.c_str(),init);
} else {
target->addString(buf.c_str());
}
}
}
if (!including) {
isTag = true;
}
}
}
}
if (!isTag && !including) {
Bottle bot;
bot.fromString(buf.c_str());
if (bot.size()>=1) {
if (tag=="") {
putBottleCompat(bot.get(0).toString().c_str(),bot);
} else {
if (bot.get(1).asString()=="=") {
Bottle& b = accum.addList();
for (int i=0; i<bot.size(); i++) {
if (i!=1) {
b.add(bot.get(i));
}
}
} else {
accum.addList().copy(bot);
}
}
}
}
if (isTag||done) {
if (tag!="") {
if (accum.size()>=1) {
putBottleCompat(tag.c_str(),accum);
}
tag = "";
}
tag = buf;
accum.clear();
accum.addString(tag.c_str());
if (tag!="") {
if (getBottle(tag.c_str())!=NULL) {
// merge data
accum.append(getBottle(tag.c_str())->tail());
//printf("MERGE %s, got %s\n", tag.c_str(),
// accum.toString().c_str());
}
}
}
} while (!done);
}
int main(int ntargets, char *targets[]) {
Network yarp;
Port port;
string hist_file(getenv("HOME") );
hist_file += "/.yarp_write_history";
read_history(hist_file.c_str());
if (companion_active_port==NULL) {
companion_install_handler();
}
ntargets--;
targets++;
if (!port.open(targets[0])) {
printf("\nCould not open write port.\n");
return 1;
}
ntargets--;
targets++;
if (adminMode) {
port.setAdminMode();
}
bool raw = false;
for (int i=0; i<ntargets; i++) {
if (string(targets[i])=="verbatim") {
raw = true;
} else {
if (!yarp.connect(port.getName().c_str(),targets[i])) {
printf("\nCould not connect ports\n");
return 1;
}
}
}
printf("\nTo stop: CTRL+C ENTER\n\n");
rl_catch_signals = 1;
rl_set_signals();
while (!done) {
rl_on_new_line();
string txt(rl_gets());
if (!done) {
if (txt[0]<32 && txt[0]!='\n' &&
txt[0]!='\r' && txt[0]!='\0' && txt[0]!='\t') {
break; // for example, horrible windows ^D
}
Bottle bot;
if (raw) {
bot.addInt(0);
bot.addString(txt.c_str());
} else {
bot.fromString(txt.c_str());
}
//core.send(bot);
port.write(bot);
}
}
write_history(hist_file.c_str());
companion_active_port = NULL;
if (raw) {
Bottle bot;
bot.addInt(1);
bot.addString("<EOF>");
//core.send(bot);
port.write(bot);
}
port.close();
return 0;
}
ICubClient::ICubClient(const std::string &moduleName, const std::string &context, const std::string &clientConfigFile, bool isRFVerbose)
{
yarp::os::ResourceFinder rfClient;
rfClient.setVerbose(isRFVerbose);
rfClient.setDefaultContext(context.c_str());
rfClient.setDefaultConfigFile(clientConfigFile.c_str());
rfClient.configure(0,NULL);
yarp::os::ResourceFinder rfPostures;
rfPostures.setVerbose(isRFVerbose);
rfPostures.setDefaultContext(context.c_str());
rfPostures.setDefaultConfigFile(rfClient.check("posturesFile",Value("postures.ini")).asString().c_str());
rfPostures.configure(0,NULL);
yarp::os::ResourceFinder rfChoregraphies;
rfChoregraphies.setVerbose(isRFVerbose);
rfChoregraphies.setDefaultContext(context.c_str());
rfChoregraphies.setDefaultConfigFile(rfClient.check("choregraphiesFile",Value("choregraphies.ini")).asString().c_str());
rfChoregraphies.configure(0,NULL);
LoadPostures(rfPostures);
LoadChoregraphies(rfChoregraphies);
//Reaching range
Bottle defaultRangeMin ;defaultRangeMin.fromString("-0.5 -0.3 -0.15");
Bottle defaultRangeMax;defaultRangeMax.fromString("-0.1 0.3 0.5");
Bottle *rangeMin = rfClient.find("reachingRangeMin").asList();
Bottle *rangeMax = rfClient.find("reachingRangeMax").asList();
if (rangeMin == NULL) rangeMin = new Bottle(defaultRangeMin);
if (rangeMax == NULL) rangeMax = new Bottle(defaultRangeMax);
xRangeMin = defaultRangeMin.get(0).asDouble(); xRangeMax =defaultRangeMax.get(0).asDouble();
yRangeMin = defaultRangeMin.get(1).asDouble(); yRangeMax =defaultRangeMax.get(1).asDouble();
zRangeMin = defaultRangeMin.get(2).asDouble(); zRangeMax =defaultRangeMax.get(2).asDouble();
icubAgent = NULL;
//OPC
string fullName = moduleName + "/icubClient";
opc = new OPCClient(fullName);
opc->isVerbose = false;
//Susbsystems
if (Bottle* bSubsystems = rfClient.find("subsystems").asList())
{
for (int s=0; s<bSubsystems->size(); s++)
{
std::string currentSS = bSubsystems->get(s).asString();
cout<<"Trying to open subsystem : "<<currentSS<<endl;
if (currentSS == SUBSYSTEM_ATTENTION)
subSystems[SUBSYSTEM_ATTENTION] = new SubSystem_Attention(fullName);
else if (currentSS == SUBSYSTEM_EXPRESSION)
subSystems[SUBSYSTEM_EXPRESSION] = new SubSystem_Expression(fullName);
else if (currentSS == SUBSYSTEM_POSTURES)
subSystems[SUBSYSTEM_POSTURES] = new SubSystem_Postures(fullName);
else if (currentSS == SUBSYSTEM_REACTABLE)
subSystems[SUBSYSTEM_REACTABLE] = new SubSystem_Reactable(fullName);
else if (currentSS == SUBSYSTEM_IKART)
subSystems[SUBSYSTEM_IKART] = new SubSystem_iKart(fullName);
else if (currentSS == SUBSYSTEM_ABM)
subSystems[SUBSYSTEM_ABM] = new SubSystem_ABM(fullName);
else if (currentSS == SUBSYSTEM_SPEECH)
subSystems[SUBSYSTEM_SPEECH] = new SubSystem_Speech(fullName);
else if (currentSS == SUBSYSTEM_SLIDING_CONTROLLER)
subSystems[SUBSYSTEM_SLIDING_CONTROLLER] = new SubSystem_SlidingController(fullName);
else if (currentSS == SUBSYSTEM_ARE)
subSystems[SUBSYSTEM_ARE] = new SubSystem_ARE(fullName);
}
}
closed=false;
}
Bottle autobiographicalMemory::snapshot(const Bottle &bInput)
{
/*
format of input bottle :
snapshot (action name_action type_action) (time t_time) (arguments (arg1) (arg2, role) ... argN) (begin 0/1) (mental)
Arguments are from 2 types:
- something from the OPC : just argN is enough
- something external : argN + role is mandatory
*/
// run only one snapshot at a time
LockGuard lg(mutexSnapshot);
//get Instance of the next opc
string sRequest_instance;
// get the OPC to check en bois
bool bMental = bInput.check("mental");
if (bMental)
{
// change the providers !!
}
if (isconnected2reasoning)
{
Bottle b2reasoning;
b2reasoning.addString("updateObjectLocation");
b2reasoning.addString(s_real_OPC.c_str());
abm2reasoning.write(b2reasoning);
}
sRequest_instance = "SELECT instance FROM main ORDER BY instance DESC LIMIT 1;";
Bottle bRequest, bTemp;
bRequest.addString("request");
bRequest.addString(sRequest_instance.c_str());
bRequest = request(bRequest);
int instance;
if (bRequest.toString() != "NULL") {
instance = atoi(bRequest.get(0).asList()->get(0).toString().c_str()) + 1;
}
else {
instance = 0;
}
OPCEARS.setInstance(instance);
currentInstance = instance;
// Filling table main:
Bottle bMain;
bMain.addString("request");
ostringstream osMain;
osMain << "INSERT INTO main(activityname, activitytype, time, instance, begin, opcname) VALUES ('";
string sName;
//for streaming image
bool isStreamActivity = false;
string fullSentence = "defaultLabel";
string activityType;
//Action
bool done = false;
for (int i = 1; i < bInput.size(); i++)
{
bTemp = *(bInput.get(i).asList());
if (bTemp.get(0) == "action" && !done)
{
osMain << bTemp.get(1).asString() << "' , '"; // activityname
sName = bTemp.get(1).asString();
imgLabel = bTemp.get(1).asString();
//used to name the single image
ostringstream labelImg;
labelImg << imgLabel << "_" << instance;
fullSentence = labelImg.str();
activityType = bTemp.get(2).asString();
//if activity is an action -> stream
if (activityType == "action") {
isStreamActivity = true;
}
osMain << bTemp.get(2).asString() << "' , '"; // activitytype
done = true;
}
}
if (!done) {
osMain << "unknown' , 'unknown', '";
}
// Time
string sTime = getCurrentTime();
osMain << sTime << "' , " << instance << " , "; // time + instance
//Begin
done = false;
bool bBegin = false;
//yDebug() << "bInput has a size of " << bInput.size() << " and is : " << bInput.toString().c_str();
for (int i = 1; i < bInput.size(); i++)
{
bTemp = *(bInput.get(i).asList());
if (bTemp.get(0) == "begin" && !done)
{
if (bTemp.get(1).asInt() == 1)
{
osMain << "TRUE ";
bBegin = true;
}
else
{
osMain << "FALSE ";
bBegin = false;
}
done = true;
}
}
if (!done) {
osMain << "FALSE";
}
if (isStreamActivity == true && !bBegin) { //just stop stream images stores when relevant activity
yDebug() << "[mutexChangeover] try locking in snapshot end";
mutexChangeover.lock();
yDebug() << "[mutexChangeover] locked in snapshot end";
streamStatus = "end"; //is done here (before the OPC snapshot), because the snapshot is slowing everything down
}
if(bMental) {
osMain << " , '" << s_mental_OPC << "' ) ; ";
} else {
osMain << " , '" << s_real_OPC << "' ) ; ";
}
bMain.addString(string(osMain.str()).c_str());
request(bMain);
OPCClient *opcWorld;
//Connection to the OPC
(bMental) ? opcWorld = opcWorldMental : opcWorld = opcWorldReal;
Bottle bOPCEearsResponse = OPCEARS.snapshot(bInput, opcWorld);
if(bOPCEearsResponse.get(0).asString() == "nack") {
return bOPCEearsResponse;
}
ostringstream osName;
osName << sName << instance;
sName += osName.str(); //I dont understand this Gregoire : you concatenate the name with nameInstance with itself, producing namenameinstance
//yDebug() << "OPCEARS: " << sName;
Bottle bSnapShot = OPCEARS.insertOPC(sName);
if(bSnapShot.get(0).asString() == "nack") {
return bSnapShot;
}
ostringstream osAllArg;
Bottle bRecogSemantic;
bool bShouldSend = true;
// Filling contentArg
if (opcWorld->isConnected()) {
for (int i = 1; i < bInput.size(); i++)
{
bTemp = *(bInput.get(i).asList());
if (bTemp.get(0) == "arguments" && bTemp.size() > 1)
{
for (int j = 1; j < bTemp.size(); j++)
{
ostringstream osArg;
string cArgArgument, cArgType, cArgSubtype, cArgRole;
//check if the argument is an entity in OPC
Entity* currentEntity = opcWorld->getEntity(bTemp.get(j).asList()->get(0).toString().c_str());
if (currentEntity == NULL) {
cArgArgument = bTemp.get(j).asList()->get(0).asString();
cArgType = "external";
cArgSubtype = "default";
}
else {
cArgArgument = currentEntity->name();
cArgType = "entity";
cArgSubtype = currentEntity->entity_type();
}
if (bTemp.get(j).asList()->size() > 1) {
cArgRole = bTemp.get(j).asList()->get(1).asString();
//add sentence for single img label
if (cArgRole == "sentence"){
fullSentence = cArgArgument;
}
if (cArgRole == "semantic"){
yInfo() << " " << bTemp.get(j).asList()->get(0).toString();
bRecogSemantic.fromString(bTemp.get(j).asList()->get(0).toString());
yInfo() << " " << bRecogSemantic.toString();
yInfo() << " bRecog size: " << bRecogSemantic.size();
bShouldSend = false;
}
}
else {
cArgRole = "unknown";
}
std::replace( cArgArgument.begin(), cArgArgument.end(), '\'', ' ');
if (bShouldSend){
osArg << "INSERT INTO contentarg(instance, argument, type, subtype, role) VALUES ( " << instance << ", '" << cArgArgument << "', " << "'" << cArgType << "', '" << cArgSubtype << "', '" << cArgRole << "') ; ";
}
else{
bShouldSend = true;
}
// one stringstream with all argments
osAllArg << osArg.str().c_str();
}
}
}
// add the snapshot of the OPC
osAllArg << bSnapShot.get(0).asString();
bRequest.clear();
if(osAllArg.str() != "") {
bRequest.addString("request");
bRequest.addString(osAllArg.str().c_str());
request(bRequest);
}
}
if ((!bBegin) && isconnected2reasoning)
{
Bottle b2reasoning;
b2reasoning.addString("addLastActivity");
b2reasoning.addString("action");
abm2reasoning.write(b2reasoning);
}
if (isStreamActivity == true) { //just launch stream images stores when relevant activity
if (bBegin) {
while(!mutexStreamRecord.tryLock()) {
yDebug() << "[mutexStreamRecord] tryLock";
yarp::os::Time::delay(0.1);
}
yDebug() << "[mutexStreamRecord] unlock";
mutexStreamRecord.unlock();
yDebug() << "[mutexChangeover] try locking in snapshot begin";
mutexChangeover.lock();
yDebug() << "[mutexChangeover] locked in snapshot begin";
streamStatus = "begin"; //streamStatus = "end" is done before the OPC snapshot, because the snapshot is slowing everything down
}
}
else
{ //just one image (sentence?)
while(streamStatus == "send") { // when it's sending, don't record new images!
yarp::os::Time::delay(0.1);
}
if(streamStatus != "record") { // only change instance number if we are not recording already
imgInstance = currentInstance;
}
string synchroTime = getCurrentTime();
frameNb = 0;
storeImagesAndData(synchroTime, true, fullSentence);
//if activity = say, we have to take one image/data + the sound that is coming from another port and catch in the update method of ABM (store in /tmp/sound/default.wav
if (activityType == "recog") {
string sndName;
//take the full sentence, replace space by _ to have the sound name
replace(fullSentence.begin(), fullSentence.end(), ' ', '_');
sndName = fullSentence + ".wav";
//read sound from file and put data in yarp::sig::Sound to store properly with semantic/instance that we are now aware of
yarp::sig::Sound s;
string defaultSoundFullPath = storingPath + "/" + storingTmpSuffix + "/sound/" + "default.wav";
yDebug() << "opening sound file " << defaultSoundFullPath.c_str();
if (yarp::sig::file::read(s, defaultSoundFullPath.c_str()) == false) {
yWarning() << "Cannot open the default sound file: check " << defaultSoundFullPath;
}
else {
yInfo() << "Default sound file loaded from " << defaultSoundFullPath;
stringstream sInstance;
sInstance << currentInstance;
//build the path and the name of the sound according to the instance and sentence said
string relativePath = sInstance.str() + "/" + sndName;
string fullPath = storingPath + "/" + relativePath;
if (yarp::sig::file::write(s, fullPath.c_str()) == false) {
yError() << "Cannot save the default sound file to " << fullPath;
}
else {
yInfo() << "Default sound file renamed and moved to " << fullPath;
database_mutex.lock();
//add the sound into the large_objects table of ABM
unsigned int snd_oid = ABMDataBase->lo_import(fullPath.c_str());
database_mutex.unlock();
Bottle bRequest;
ostringstream osArg;
//Populate the sounddata table with the infos
bRequest.addString("request");
osArg << "INSERT INTO sounddata(instance, relative_path, time, snd_provider_port, snd_oid) VALUES ('" << currentInstance << "', '" << relativePath << "', '" << synchroTime << "', '" << portSoundStreamInput.getName() << "', '" << snd_oid << "');";
bRequest.addString(osArg.str());
request(bRequest);
}
}
osInsertTemp.str("");
recogFromGrammarSemantic(bRecogSemantic, "", 1, currentInstance);
requestFromString(osInsertTemp.str());
}
}
bSnapShot.addInt(instance);
return bSnapShot;
}
int main(int argc, char *argv[]) {
Network yarp;
Port output;
string from_port;
string to_port;
string fn;
if (argc >=4) {
from_port = string(argv[1]);
to_port = string(argv[2]);
fn = string(argv[3]);
}
else {
if (argc < 2) {
cout<<"usage: port_playback /thisPort /destPort filename"<<endl;
return 0;
}
}
//parse input file
// ...
output.open(from_port.c_str());
yarp::os::Network::connect(from_port.c_str(),to_port.c_str());
std::clock_t start;
std::clock_t elapsed;
ifstream file(fn.c_str());
string line;
if (file.is_open()) {
start = std::clock();
while (cont) {
while (getline(file, line)) {
cout<<"read line: "<<line<<endl;
Bottle bot;
bot.fromString(line.c_str());
long t = bot.get(bot.size()-1).asInt(); //last value is timestamp
cout<<"t="<<t<<endl;
elapsed = std::clock() - start;
while (elapsed <= t) {
elapsed = std::clock() - start;
};
bot.pop(); //remove that timestamp!
//wait, until we get past time.
// of course, we should handle cases where there is nothing, in which case
// it will hang!
cout<<"sending! t="<<t<<endl;
output.write(bot);
}
cont = false;
}
}
output.close();
return 0;
}
bool XmlRpcCarrier::write(ConnectionState& proto, SizedWriter& writer) {
StringOutputStream sos;
StringInputStream sis;
writer.write(sos);
sis.reset(sos.toString());
ConstString header;
if (sender) {
header = sis.readLine();
}
ConstString body = sis.readLine();
Value v;
if (header.length()>0 && header[0]=='q') {
body = "yarp.quit";
// XMLRPC does not need a quit message, this should get stripped
return false;
}
Bottle *bot = v.asList();
bot->fromString(body.c_str());
ConstString methodName;
if (sender) {
methodName = bot->get(0).toString();
*bot = bot->tail();
}
XmlRpcValue args;
if (bot->size()==1) {
toXmlRpcValue(bot->get(0),args);
} else {
toXmlRpcValue(v,args);
}
std::string req;
if (sender) {
const Contact& addr = host.isValid()?host:proto.getStreams().getRemoteAddress();
XmlRpcClient c(addr.getHost().c_str(),(addr.getPort()>0)?addr.getPort():80);
c.generateRequest(methodName.c_str(),args);
req = c.getRequest();
} else {
XmlRpcServerConnection c(0,NULL);
c.generateResponse(args.toXml());
req = c.getResponse();
}
int start = 0;
if (sender) {
if (req.length()<8) {
fprintf(stderr, "XmlRpcCarrier fail, %s:%d\n", __FILE__, __LINE__);
return false;
}
for (int i=0; i<(int)req.length(); i++) {
if (req[i] == '\n') {
start++;
break;
}
start++;
}
if (!firstRound) {
Bytes b((char*)http.c_str(),http.length());
proto.os().write(b);
}
firstRound = false;
}
Bytes b((char*)req.c_str()+start,req.length()-start);
proto.os().write(b);
return proto.os().isOk();
}
