/* Respond function */
bool opcManager::respond(const yarp::os::Bottle& bCommand, yarp::os::Bottle& bReply)
{
string helpMessage = string(getName().c_str()) +
" commands are: \n" +
"help \n" +
"connect + name\n" +
"quit \n";
bReply.clear();
string keyWord = bCommand.get(0).asString().c_str();
if (keyWord == "quit") {
bReply.addString("quitting");
return false;
}
else if (keyWord == "help") {
cout << helpMessage;
bReply.addString("ok");
}
else if (keyWord == "connect") {
bReply = connect(bCommand);
}
else if (keyWord == "updateBeliefs") {
bReply.addString("nack");
if (bCommand.size() == 2)
{
if (bCommand.get(1).isString())
{
bReply.addList() = updateBelief(bCommand.get(1).toString().c_str());
}
}
}
else if (keyWord == "synchronise")
{
bReply.addString("ack");
bReply.addList() = synchoniseOPCs();
}
else if (keyWord == "executeActivity")
{
bReply.addString("ack");
bReply.addList() = simulateActivity(bCommand);
}
else if (keyWord == "diffOPC")
{
bReply.addString("ack");
bReply.addList() = diffOPC();
}
return true;
}
bool SubscriberOnSql::listSubscriptions(const ConstString& port,
yarp::os::Bottle& reply) {
mutex.wait();
sqlite3_stmt *statement = NULL;
char *query = NULL;
if (ConstString(port)!="") {
query = sqlite3_mprintf("SELECT s.srcFull, s.DestFull, EXISTS(SELECT topic FROM topics WHERE topic = s.src), EXISTS(SELECT topic FROM topics WHERE topic = s.dest), s.mode FROM subscriptions s WHERE s.src = %Q OR s.dest= %Q ORDER BY s.src, s.dest",port.c_str(),port.c_str());
} else {
query = sqlite3_mprintf("SELECT s.srcFull, s.destFull, EXISTS(SELECT topic FROM topics WHERE topic = s.src), EXISTS(SELECT topic FROM topics WHERE topic = s.dest), s.mode FROM subscriptions s ORDER BY s.src, s.dest");
}
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);
}
}
reply.addString("subscriptions");
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);
int srcTopic = sqlite3_column_int(statement,2);
int destTopic = sqlite3_column_int(statement,3);
char *mode = (char *)sqlite3_column_text(statement,4);
Bottle& b = reply.addList();
b.addString("subscription");
Bottle bsrc;
bsrc.addString("src");
bsrc.addString(src);
Bottle bdest;
bdest.addString("dest");
bdest.addString(dest);
b.addList() = bsrc;
b.addList() = bdest;
if (mode!=NULL) {
if (mode[0]!='\0') {
Bottle bmode;
bmode.addString("mode");
bmode.addString(mode);
b.addList() = bmode;
}
}
if (srcTopic||destTopic) {
Bottle btopic;
btopic.addString("topic");
btopic.addInt(srcTopic);
btopic.addInt(destTopic);
b.addList() = btopic;
}
}
sqlite3_finalize(statement);
sqlite3_free(query);
mutex.post();
return true;
}
void appendEntry(yarp::os::Bottle& reply, const Contact& c) {
Bottle& info = reply.addList();
info.addString("registration");
info.addString("name");
info.addString(c.getName().c_str());
info.addString("ip");
info.addString(c.getHost().c_str());
info.addString("port");
info.addInt(c.getPort());
info.addString("type");
info.addString(c.getCarrier().c_str());
}
void appendEntry(yarp::os::Bottle& reply, const Entry& e) {
Bottle& info = reply.addList();
info.addString("registration");
info.addString("name");
info.addString(e.name.c_str());
info.addString("ip");
info.addString(e.machine.c_str());
info.addString("port");
info.addInt(e.portNumber);
info.addString("type");
info.addString(e.carrier.c_str());
}
void HandIKModule::prepareData(yarp::os::Bottle &data)
{
data.addString(tag.c_str());
Bottle &handB=data.addList();
handB.addString("hand");
handB.addString(hand.c_str());
Bottle &cost=data.addList();
cost.addString("cost");
cost.addDouble(bestObjValue);
rot_tran=zeros(4,4);
rot_tran(1,0)=1.0;
rot_tran(0,1)=-1.0;
rot_tran(2,2)=1.0;
rot_tran(3,3)=1.0;
rot_tran(0,3)=center[0];
rot_tran(1,3)=center[1];
rot_tran(2,3)=center[2];
yarp::sig::Vector ee_ob=bestSolution.xyz_ee;
ee_ob.push_back(1.0);
yarp::sig::Vector ee_root=rot_tran*ee_ob;
yarp::sig::Matrix tmp=rpy2dcm(bestSolution.rpy_ee);
yarp::sig::Matrix tmp2=rot_tran*tmp;
yarp::sig::Vector or_root=dcm2axis(tmp2);
Bottle &ee=data.addList();
ee.addString("ee");
Bottle &ee_coord=ee.addList();
ee_coord.addDouble(ee_root[0]);
ee_coord.addDouble(ee_root[1]);
ee_coord.addDouble(ee_root[2]);
Bottle &orientation=data.addList();
orientation.addString("or");
Bottle &or_coord=orientation.addList();
yarp::sig::Vector or_axisangle=dcm2axis(rpy2dcm(bestSolution.rpy_ee));
or_coord.addDouble(or_root[0]);
or_coord.addDouble(or_root[1]);
or_coord.addDouble(or_root[2]);
or_coord.addDouble(or_root[3]);
Bottle &j=data.addList();
j.addString("joints");
Bottle &joints=j.addList();
for (unsigned int i=0; i<bestSolution.joints.size(); i++)
joints.addDouble(bestSolution.joints[i]);
Bottle &order=data.addList();
order.addString("combination");
Bottle &combination=order.addList();
combination.addInt((int)combinations.at(winnerIndex)[0]);
combination.addInt((int)combinations.at(winnerIndex)[1]);
combination.addInt((int)combinations.at(winnerIndex)[2]);
}
bool ThreeDModule::respond(const yarp::os::Bottle& in, yarp::os::Bottle& out, yarp::os::Stamp stamp) {
//? Stamp stamp;
std::cout << "responding: " << in.toString() << std::endl;
//TODO sanity check
//...
out.clear();
// process data "in", prepare "out"
out.append(in);
out.addList() = calculatePosition(in, stamp);
// reply
return true;
}
void handleTorqueMsg(ITorqueControl *torque, const yarp::os::Bottle& cmd,
yarp::os::Bottle& response, bool *rec, bool *ok)
{
fprintf(stderr, "Handling ITorque messages\n");
if (!torque)
{
fprintf(stderr, "Error, I do not have a valid ITorque interface\n");
*ok=false;
return;
}
int controlledJoints;
torque->getAxes(&controlledJoints);
int code = cmd.get(1).asVocab();
switch (code)
{
case VOCAB_SET:
{
*rec = true;
switch(cmd.get(2).asVocab())
{
case VOCAB_REF:
{
*ok = torque->setRefTorque(cmd.get(3).asInt(), cmd.get(4).asDouble());
}
break;
case VOCAB_REFS:
{
Bottle& b = *(cmd.get(3).asList());
int i;
const int njs = b.size();
if (njs==controlledJoints)
{
double *p = new double[njs]; // LATER: optimize to avoid allocation.
for (i = 0; i < njs; i++)
p[i] = b.get(i).asDouble();
*ok = torque->setRefTorques (p);
delete[] p;
}
}
break;
case VOCAB_LIM:
{
*ok = torque->setTorqueErrorLimit (cmd.get(3).asInt(), cmd.get(4).asDouble());
}
break;
case VOCAB_LIMS:
{
Bottle& b = *(cmd.get(3).asList());
int i;
const int njs = b.size();
if (njs==controlledJoints)
{
double *p = new double[njs]; // LATER: optimize to avoid allocation.
for (i = 0; i < njs; i++)
p[i] = b.get(i).asDouble();
*ok = torque->setTorqueErrorLimits (p);
delete[] p;
}
}
break;
case VOCAB_PID:
{
Pid p;
int j = cmd.get(3).asInt();
Bottle& b = *(cmd.get(4).asList());
p.kp = b.get(0).asDouble();
p.kd = b.get(1).asDouble();
p.ki = b.get(2).asDouble();
p.max_int = b.get(3).asDouble();
p.max_output = b.get(4).asDouble();
p.offset = b.get(5).asDouble();
p.scale = b.get(6).asDouble();
*ok = torque->setTorquePid(j, p);
}
break;
case VOCAB_PIDS:
{
Bottle& b = *(cmd.get(3).asList());
int i;
const int njs = b.size();
if (njs==controlledJoints)
{
Pid *p = new Pid[njs];
for (i = 0; i < njs; i++)
{
Bottle& c = *(b.get(i).asList());
p[i].kp = c.get(0).asDouble();
p[i].kd = c.get(1).asDouble();
p[i].ki = c.get(2).asDouble();
p[i].max_int = c.get(3).asDouble();
p[i].max_output = c.get(4).asDouble();
p[i].offset = c.get(5).asDouble();
p[i].scale = c.get(6).asDouble();
}
*ok = torque->setTorquePids(p);
delete[] p;
}
}
break;
case VOCAB_RESET:
{
*ok = torque->resetTorquePid (cmd.get(3).asInt());
}
break;
case VOCAB_DISABLE:
{
*ok = torque->disableTorquePid (cmd.get(3).asInt());
}
break;
case VOCAB_ENABLE:
{
*ok = torque->enableTorquePid (cmd.get(3).asInt());
}
break;
case VOCAB_TORQUE_MODE:
{
*ok = torque->setTorqueMode();
}
break;
}
}
break;
case VOCAB_GET:
{
*rec = true;
int tmp = 0;
double dtmp = 0.0;
response.addVocab(VOCAB_IS);
response.add(cmd.get(1));
switch(cmd.get(2).asVocab())
{
case VOCAB_AXES:
{
int tmp;
*ok = torque->getAxes(&tmp);
response.addInt(tmp);
}
break;
case VOCAB_TRQ:
{
*ok = torque->getTorque(cmd.get(3).asInt(), &dtmp);
response.addDouble(dtmp);
}
break;
case VOCAB_TRQS:
{
double *p = new double[controlledJoints];
*ok = torque->getTorques(p);
Bottle& b = response.addList();
int i;
for (i = 0; i < controlledJoints; i++)
b.addDouble(p[i]);
delete[] p;
}
break;
case VOCAB_ERR:
{
*ok = torque->getTorqueError(cmd.get(3).asInt(), &dtmp);
response.addDouble(dtmp);
}
break;
case VOCAB_ERRS:
{
double *p = new double[controlledJoints];
*ok = torque->getTorqueErrors(p);
Bottle& b = response.addList();
int i;
for (i = 0; i < controlledJoints; i++)
b.addDouble(p[i]);
delete[] p;
}
break;
case VOCAB_OUTPUT:
{
*ok = torque->getTorquePidOutput(cmd.get(3).asInt(), &dtmp);
response.addDouble(dtmp);
}
break;
case VOCAB_OUTPUTS:
{
double *p = new double[controlledJoints];
*ok = torque->getTorquePidOutputs(p);
Bottle& b = response.addList();
int i;
for (i = 0; i < controlledJoints; i++)
b.addDouble(p[i]);
delete[] p;
}
break;
case VOCAB_PID:
{
Pid p;
*ok = torque->getTorquePid(cmd.get(3).asInt(), &p);
Bottle& b = response.addList();
b.addDouble(p.kp);
b.addDouble(p.kd);
b.addDouble(p.ki);
b.addDouble(p.max_int);
b.addDouble(p.max_output);
b.addDouble(p.offset);
b.addDouble(p.scale);
}
break;
case VOCAB_PIDS:
{
Pid *p = new Pid[controlledJoints];
*ok = torque->getTorquePids(p);
Bottle& b = response.addList();
int i;
for (i = 0; i < controlledJoints; 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);
}
delete[] p;
}
break;
case VOCAB_REFERENCE:
{
*ok = torque->getRefTorque(cmd.get(3).asInt(), &dtmp);
response.addDouble(dtmp);
}
break;
case VOCAB_REFERENCES:
{
double *p = new double[controlledJoints];
*ok = torque->getRefTorques(p);
Bottle& b = response.addList();
int i;
for (i = 0; i < controlledJoints; i++)
b.addDouble(p[i]);
delete[] p;
}
break;
case VOCAB_LIM:
{
*ok = torque->getTorqueErrorLimit(cmd.get(3).asInt(), &dtmp);
response.addDouble(dtmp);
}
break;
case VOCAB_LIMS:
{
double *p = new double[controlledJoints];
*ok = torque->getTorqueErrorLimits(p);
Bottle& b = response.addList();
int i;
for (i = 0; i < controlledJoints; i++)
b.addDouble(p[i]);
delete[] p;
}
break;
}
}
break;
}
//rec --> true se il comando e' riconosciuto
//ok --> contiene il return value della chiamata all'interfaccia
// ...*ok=torque->setPid();
//torque->
}
/*! @brief Fill a bottle with the contents of an object.
@param[in] jct The %JavaScript engine context.
@param[in,out] aBottle The bottle to be filled.
@param[in] theData The value to be sent.
@param[in] topLevel @c true if this is the outermost list of an object. */
static void
fillBottleFromValue(JSContext * jct,
yarp::os::Bottle & aBottle,
JS::Value theData,
const bool topLevel)
{
ODL_ENTER(); //####
ODL_P2("jct = ", jct, "aBottle = ", &aBottle); //####
ODL_B1("topLevel = ", topLevel); //####
JS::RootedValue asRootedValue(jct);
asRootedValue = theData;
if (theData.isBoolean())
{
aBottle.addInt(JS::ToBoolean(asRootedValue) ? 1 : 0);
}
else if (theData.isDouble())
{
double aValue;
if (JS::ToNumber(jct, asRootedValue, &aValue))
{
aBottle.addDouble(aValue);
}
}
else if (theData.isInt32())
{
int32_t aValue;
if (JS::ToInt32(jct, asRootedValue, &aValue))
{
aBottle.addInt(aValue);
}
}
else if (theData.isString())
{
JSString * asString = theData.toString();
char * asChars = JS_EncodeString(jct, asString);
aBottle.addString(asChars);
JS_free(jct, asChars);
}
else if (theData.isObject())
{
JS::RootedObject asObject(jct);
if (JS_ValueToObject(jct, asRootedValue, &asObject))
{
bool processed = false;
#if (47 <= MOZJS_MAJOR_VERSION)
bool isArray;
#endif // 47 <= MOZJS_MAJOR_VERSION
#if (47 <= MOZJS_MAJOR_VERSION)
if (JS_IsArrayObject(jct, asObject, &isArray))
#else // 47 > MOZJS_MAJOR_VERSION
if (JS_IsArrayObject(jct, asObject))
#endif // 47 > MOZJS_MAJOR_VERSION
{
uint32_t arrayLength;
if (JS_GetArrayLength(jct, asObject, &arrayLength))
{
// Treat as a list
if (topLevel)
{
for (uint32_t ii = 0; arrayLength > ii; ++ii)
{
JS::RootedValue anElement(jct);
if (JS_GetElement(jct, asObject, ii, &anElement))
{
fillBottleFromValue(jct, aBottle, anElement, false);
}
}
}
else
{
yarp::os::Bottle & innerList(aBottle.addList());
for (uint32_t ii = 0; arrayLength > ii; ++ii)
{
JS::RootedValue anElement(jct);
if (JS_GetElement(jct, asObject, ii, &anElement))
{
fillBottleFromValue(jct, innerList, anElement, false);
}
}
}
processed = true;
}
}
if (! processed)
{
// Treat as a dictionary
yarp::os::Property & innerDict(aBottle.addDict());
#if (47 <= MOZJS_MAJOR_VERSION)
JS::Rooted<JS::IdVector> ids(jct, JS::IdVector(jct));
#else // 47 > MOZJS_MAJOR_VERSION
JS::AutoIdArray ids(jct, JS_Enumerate(jct, asObject));
#endif // 47 > MOZJS_MAJOR_VERSION
#if (47 <= MOZJS_MAJOR_VERSION)
if (JS_Enumerate(jct, asObject, &ids))
#else // 47 > MOZJS_MAJOR_VERSION
// Note that only operator! is defined, so we need to do a 'double-negative'.
if (!! ids)
#endif // 47 > MOZJS_MAJOR_VERSION
{
for (size_t ii = 0, len = ids.length(); len > ii; ++ii)
{
JS::RootedValue key(jct);
if (JS_IdToValue(jct, ids[ii], &key))
{
JS::RootedValue key(jct);
JS::RootedValue result(jct);
JS::RootedId aRootedId(jct);
aRootedId = ids[ii];
if (JS_IdToValue(jct, ids[ii], &key) &&
JS_GetPropertyById(jct, asObject, aRootedId, &result))
{
JSString * keyString = key.toString();
char * keyAsChars = JS_EncodeString(jct, keyString);
YarpString keyToUse(keyAsChars);
yarp::os::Bottle convertedResult;
JS_free(jct, keyAsChars);
fillBottleFromValue(jct, convertedResult, result, false);
if (1 == convertedResult.size())
{
yarp::os::Value anElement(convertedResult.get(0));
if (anElement.isInt())
{
innerDict.put(keyToUse, anElement.asInt());
}
else if (anElement.isDouble())
{
innerDict.put(keyToUse, anElement.asDouble());
}
else if (anElement.isString())
{
innerDict.put(keyToUse, anElement.asString());
}
else
{
innerDict.put(keyToUse, anElement);
}
}
}
}
}
}
}
}
}
ODL_EXIT(); //####
} // fillBottleFromValue
bool GazeboYarpControlBoardDriver::getRemoteVariable(yarp::os::ConstString key, yarp::os::Bottle& val)
{
val.clear();
if (key == "hardwareDamping")
{
yarp::os::Bottle& r = val.addList(); for (size_t i = 0; i< m_numberOfJoints; i++) { double tmp = m_jointPointers[i]->GetDamping(0); r.addDouble(tmp); }
return true;
}
if (key == "hardwareFriction")
{
yarp::os::Bottle& r = val.addList(); for (size_t i = 0; i< m_numberOfJoints; i++) { double tmp = m_jointPointers[i]->GetParam(std::string("friction"),0); r.addDouble(tmp); }
return true;
}
if (key == "hardwareHiStop")
{
yarp::os::Bottle& r = val.addList();
for (size_t i = 0; i< m_numberOfJoints; i++)
{
#if GAZEBO_MAJOR_VERSION >= 8
double upperLimit = m_jointPointers[i]->UpperLimit(0);
#else
double upperLimit = m_jointPointers[i]->GetUpperLimit(0).Radian();
#endif
double tmp = convertGazeboToUser(i, upperLimit);
r.addDouble(tmp);
}
return true;
}
if (key == "hardwareLowStop")
{
yarp::os::Bottle& r = val.addList();
for (size_t i = 0; i< m_numberOfJoints; i++) {
#if GAZEBO_MAJOR_VERSION >= 8
double lowerLimit = m_jointPointers[i]->LowerLimit(0);
#else
double lowerLimit = m_jointPointers[i]->GetLowerLimit(0).Radian();
#endif
double tmp = convertGazeboToUser(i, lowerLimit);
r.addDouble(tmp);
}
return true;
}
if (key == "hardwareEffortLimit")
{
yarp::os::Bottle& r = val.addList(); for (size_t i = 0; i< m_numberOfJoints; i++) { double tmp = m_jointPointers[i]->GetEffortLimit(0); r.addDouble(tmp); }
return true;
}
if (key == "hardwareVelocityLimit")
{
yarp::os::Bottle& r = val.addList(); for (size_t i = 0; i< m_numberOfJoints; i++) { double tmp = m_jointPointers[i]->GetVelocityLimit(0); r.addDouble(tmp); }
return true;
}
if (key == "yarp_jntMaxVel")
{
yarp::os::Bottle& r = val.addList(); for (size_t i = 0; i< m_numberOfJoints; i++) { double tmp_min,tmp_max; getVelLimits(i,&tmp_min,&tmp_max); r.addDouble(tmp_max); }
return true;
}
if (key == "yarp_jntMaxPos")
{
yarp::os::Bottle& r = val.addList(); for (size_t i = 0; i< m_numberOfJoints; i++) { double tmp_min,tmp_max; getLimits(i,&tmp_min,&tmp_max); r.addDouble(tmp_max); }
return true;
}
if (key == "yarp_kPWM")
{
yarp::os::Bottle& r = val.addList(); for (size_t i = 0; i< m_numberOfJoints; i++) { r.addDouble(m_kPWM[i]); }
return true;
}
if (key == "yarp_jntMinPos")
{
yarp::os::Bottle& r = val.addList(); for (size_t i = 0; i< m_numberOfJoints; i++) { double tmp_min,tmp_max; getLimits(i,&tmp_min,&tmp_max); r.addDouble(tmp_min); }
return true;
}
if (key == "SHORTCUT_all_pos_kp")
{
yarp::os::Bottle& r = val.addList(); for (size_t i = 0; i< m_numberOfJoints; i++) { yarp::dev::Pid tmp_pid; getPid(VOCAB_PIDTYPE_POSITION, i,&tmp_pid); r.addDouble(tmp_pid.kp); }
return true;
}
if (key == "SHORTCUT_all_pos_kd")
{
yarp::os::Bottle& r = val.addList(); for (size_t i = 0; i< m_numberOfJoints; i++) { yarp::dev::Pid tmp_pid; getPid(VOCAB_PIDTYPE_POSITION, i,&tmp_pid); r.addDouble(tmp_pid.kd); }
return true;
}
if (key == "SHORTCUT_all_pos_ki")
{
yarp::os::Bottle& r = val.addList(); for (size_t i = 0; i< m_numberOfJoints; i++) { yarp::dev::Pid tmp_pid; getPid(VOCAB_PIDTYPE_POSITION, i,&tmp_pid); r.addDouble(tmp_pid.ki); }
return true;
}
if (key == "VelocityTimeout")
{
yarp::os::Bottle& r = val.addList(); for (size_t i = 0; i< m_numberOfJoints; i++) { r.addDouble(m_velocity_watchdog[i]->getDuration()); }
return true;
}
yWarning("getRemoteVariable(): Unknown variable %s", key.c_str());
return false;
}
/* Respond function */
bool abmReasoning::respond(const yarp::os::Bottle& bCommand, yarp::os::Bottle& bReply)
{
bReply.clear();
if (!bReady)
{
bReply.addString("module not initialised yet, please wait.");
yInfo() << "\t" << "reply : " << bReply.toString().c_str() << "\n";
handlerPort.reply(bReply);
return true;
}
if (bCommand.get(0).asString() == "quit") {
bReply.addString("ack");
bReply.addString("quitting");
return false;
}
else if (bCommand.get(0).asString() == "sqlQueryTest") {
bReply.addString("ack");
// bReply.addList() = sqlQueryTest();
}
// FIND ACTIVITY
else if (bCommand.get(0).asString() == "findActivity") {
yInfo() << "\t" << "bCommand.size() = " << bCommand.size();
if (bCommand.size() <= 2) {
bReply.addString("nack");
bReply.addString("Usage : findActivity <actionName> 'begin'|'end'|'both' [columns,to,select]");
}
else if (bCommand.get(2).asString() != "begin" && bCommand.get(2).asString() != "end" && bCommand.get(2).asString() != "both") {
bReply.addString("nack");
bReply.addString("Usage : findOPC <actionName> 'begin'|'end'|'both' [<colums,to,select>] => check your begin/end/both");
}
else {
bReply.addString("ack");
string actionName = bCommand.get(1).asString().c_str();
string beginOrEnd = bCommand.get(2).asString().c_str();
if (bCommand.size() == 3) {
bReply.addList() = findActivity(actionName, beginOrEnd);
}
else {
string select = bCommand.get(3).asString().c_str();
bReply.addList() = findActivity(actionName, beginOrEnd, select);
}
}
}
// FIND OPC
else if (bCommand.get(0).asString() == "findOPC") {
if (bCommand.size() <= 2) {
bReply.addString("nack");
bReply.addString("Usage : findOPC <actionName> 'begin'|'end'|'both' [<colums,to,select>]");
}
else if (bCommand.get(2).asString() != "begin" && bCommand.get(2).asString() != "end" && bCommand.get(2).asString() != "both") {
bReply.addString("nack");
bReply.addString("Usage : findOPC <actionName> 'begin'|'end'|'both' [<colums,to,select>] => check your begin/end/both");
}
else {
bReply.addString("ack");
string actionName = bCommand.get(1).asString().c_str();
string beginOrEnd = bCommand.get(2).asString().c_str();
// bReply.addList() = findOPC(actionName, beginOrEnd);
}
}
// GET CONSEQUENCES DRIVES
else if (bCommand.get(0).asString() == "getActionConsequenceDrives") {
if (bCommand.size() <= 2) {
bReply.addString("nack");
bReply.addString("Usage : getActionConsequenceDrives 'actionName' 'arg'");
}
else {
bReply.addString("ack");
pair<string, string> pAction(bCommand.get(1).asString().c_str(), bCommand.get(2).asString().c_str());
bReply.addList() = getActionConsequenceDrives(pAction);
}
}
// PRINT PDDL DOMAIN
else if (bCommand.get(0).asString() == "printPDDLContextualKnowledgeDomain") {
if (bCommand.size() >= 2) {
bReply.addString("nack");
bReply.addString("Usage : printPDDLContextualKnowledge");
}
else {
bReply.addString("ack");
bReply.addList() = printPDDLContextualKnowledgeDomain();
}
}
// PRINT PDDL PROBLEM
else if (bCommand.get(0).asString() == "printPDDLContextualKnowledgeProblem") {
if (bCommand.size() != 2) {
bReply.addString("nack");
bReply.addString("Usage : printPDDLContextualKnowledgeProblem ( (condition1) (condition2) )");
}
else {
if (!bCommand.get(1).isList()) {
bReply.addString("nack");
bReply.addString("Usage : printPDDLContextualKnowledgeProblem ( (condition1) (condition2) )");
}
bReply.addString("ack");
Bottle bGoal;
bGoal = *bCommand.get(1).asList();
bReply.addList() = printPDDLContextualKnowledgeProblem(bGoal);
}
}
// HELP
else if (bCommand.get(0).asString() == "help") {
bReply.addString("ack");
bReply.addString("commands are: findActivity <actionName> <begin|end|both> [<colums,to,select>]| findSharedPlan <sharedPlanName> <begin|end|both> [<colums,to,select>] | help | sqlQueryTest | quit ");
}
// ASK LAST ACTION
else if (bCommand.get(0).asString() == "askLastAction") {
bReply.addString("ack");
bReply.addList() = askLastAction();
}
// ASK LAST ACTIVITY
else if (bCommand.get(0).asString() == "askLastActivity") {
bReply.addString("ack");
bReply.addList() = askLastActivity(bCommand);
}
// ADD LAST ACTIVITY
else if (bCommand.get(0).asString() == "addLastActivity") {
bReply.addString("ack");
bReply.addList() = addLastActivity(bCommand);
}
//TEST : to be removed when done
else if (bCommand.get(0).asString() == "findPossibleSharedPlan") {
bReply.addString("ack");
bReply.addList() = findPossibleSharedPlan(bCommand.get(1).asInt(), bCommand.get(2).asInt());
}
//TEST : to be removed when done
else if (bCommand.get(0).asString() == "availableSharedPlan") {
bReply.addString("ack");
bReply.addList() = availableSharedPlan(bCommand.get(1).asInt(), bCommand.get(2).asInt());
}
//TEST : to be removed when done
else if (bCommand.get(0).asString() == "findAllSentence") {
bReply.addString("ack");
bReply.addList() = findAllSentence();
}
else if (bCommand.get(0).asString() == "ago") {
bReply.addString("ack");
getIDfromTime(bCommand);
bReply.addList() = bCommand;
}
// DISCRIMINATE ACTION
else if (bCommand.get(0).asString() == "discriminateAction") {
bReply.addString("ack");
bReply.addList() = discriminateLastAction();
}
// IMAGINE INSTANCE
else if (bCommand.get(0).asString() == "imagine") {
bReply.addString("ack");
bReply.addList() = imagineOPC(atoi(bCommand.get(1).toString().c_str()));
}
// GET ACTION
else if (bCommand.get(0).asString() == "getAction") {
bReply.addString("ack");
bReply.addList() = askActionFromId(atoi(bCommand.get(1).toString().c_str()));
}
// QUERY BEHAVIOR
else if (bCommand.get(0).asString() == "queryBehavior") {
bReply.addString("ack");
bReply.addList() = queryBehavior(bCommand);
}
// SAVE KNOWLEDGE
else if (bCommand.get(0).asString() == "saveKnowledge") {
bReply.addString("ack");
bReply.addList() = saveKnowledge();
}
// RESET KNOWLEDGE
else if (bCommand.get(0).asString() == "resetKnowledge") {
bReply.addString("ack");
if (bCommand.size() == 2)
{
bReply.addList() = resetKnowledge(atoi(bCommand.get(1).toString().c_str()));
//bReply.addList() = retroReasoning(atoi(bCommand.get(1).toString().c_str()));
}
else
{
bReply.addList() = resetKnowledge();
// bReply.addList() = retroReasoning();
}
}
// RETRO REASONING
else if (bCommand.get(0).asString() == "retroReasoning") {
bReply.addString("ack");
if (bCommand.size() == 2)
{
bReply.addList() = retroReasoning(atoi(bCommand.get(1).toString().c_str()));
}
else
{
bReply.addList() = retroReasoning();
}
}
else if (bCommand.get(0).asString() == "level3") {
bReply.addString("ack");
if (bCommand.size() == 2)
{
bReply.addList() = level3Reasoning(atoi(bCommand.get(1).toString().c_str()));
}
else
{
bReply.addList() = level3Reasoning();
}
}
// DISCRIMINATE UNKNOWN ACTIONS
else if (bCommand.get(0).asString() == "discriminateUnknownActions") {
bReply.addString("ack");
bReply.addList() = discriminateUnknownActions();
}
// CONNECT OPC
else if (bCommand.get(0).asString() == "connectOPC") {
bReply.addString("ack");
bReply.addList() = connectOPC(bCommand);
}
// EXECUTE ACTION
else if (bCommand.get(0).asString() == "executeAction") {
bReply.addString("ack");
bReply.addList() = executeAction(bCommand);
}
else if (bCommand.get(0).asString() == "executeActionFromAdv") {
bReply.addString("ack");
bReply.addList() = executeActionFromAdv(bCommand);
}
// EXECUTE ACTIVITY
else if (bCommand.get(0).asString() == "executeActivity") {
bReply.addString("ack");
bReply.addList() = executeActivity(bCommand);
}
// EXECUTE ACTIVITY
else if (bCommand.get(0).asString() == "whatIs") {
bReply.addString("ack");
if (bCommand.size() == 2)
{
bReply.addList() = whatIs(bCommand.get(1).asString());
}
else
{
yWarning("Error in abmReasoning::respond wrong size of input | (whatIs input)");
bReply.addString("Error in abmReasoning::respond wrong size of input | (whatIs input)");
}
}
else if (bCommand.get(0).asString() == "howTo") {
bReply.addString("ack");
if (bCommand.size() == 4)
{
bReply.addList() = howTo(bCommand.get(1).asString(), bCommand.get(2).asString(), bCommand.get(3).asString());
}
else
{
bReply.addString("Error in abmReasoning::howTo wrong size of input | (howTo sAgent sPredicate sObject)");
yWarning("Error in abmReasoning::howTo wrong size of input | (howTo sAgent sPredicate sObject)");
}
}
// EXECUTE SHARED PLAN
else if (bCommand.get(0).asString() == "executeSharedPlan") {
bReply.addString("ack");
bReply.addList() = executeSharedPlan(bCommand);
}
// GRAMMAR
else if (bCommand.get(0).asString() == "askGrammar") {
bReply.addString("ack");
bReply.addList() = askGrammar(bCommand);
}
// WORD KNOWLEDGE
else if (bCommand.get(0).asString() == "askWordKnowledge") {
bReply.addString("ack");
// Get Context
bReply.addList() = askWordKnowledge(bCommand);
}
// UPDATE LOCATION
else if (bCommand.get(0).asString() == "updateLocation") {
bReply.addString("ack");
bReply.addList() = updateKnownLocations();
}
else if (bCommand.get(0).asString() == "displayContextual") {
bReply.addString("ack");
if (bCommand.size() == 4)
{
displayContextual(bCommand.get(1).toString(), bCommand.get(2).toString(), bCommand.get(3).toString());
}
else
{
yInfo() << " displaying " << listContextualKnowledge.size() << " contextual knowledge";
vector<string> pastCK;
for (vector<contextualKnowledge>::iterator itCK = listContextualKnowledge.begin();
itCK != listContextualKnowledge.end();
itCK++)
{
bool passed = false;
for (vector<string>::iterator itVS = pastCK.begin();
itVS != pastCK.end();
itVS++){
if (itCK->sName == *itVS) passed = true;
}
if (!passed){
whatIs(itCK->sName);
pastCK.push_back(itCK->sName);
}
// displayContextual(itCK->sName, itCK->sArgument, itCK->sType);
}
}
}
// UPDATE OBJECT LOCATION
else if (bCommand.get(0).asString() == "updateObjectLocation") {
bReply.addString("ack");
bReply.addList() = updateOpcObjectLocation(bCommand.get(1).toString().c_str());
}
//no command recognize
else {
bReply.addString("nack");
bReply.addString("wrong commands. Available are : findActivity <actionName> <begin|end|both> [<colums,to,select>] | findSharedPlan <sharedPlanName> <begin|end|both> [<colums,to,select>] | help | sqlQueryTest | quit ");
}
yInfo() << "\t" << "reply : " << bReply.toString().c_str() << "\n";
handlerPort.reply(bReply);
return true;
}
/*
* Message handler. RPC and from the terminal
*/
bool CoreModule::respond(const yarp::os::Bottle& command, yarp::os::Bottle& reply)
{
printDebug("Message Received: (echo is on)");
// debug
printf("GOT MESSAGE: %s\n", command.toString().c_str());
// QUIT
if( command.get(0).asString() == "quit" ||
command.get(0).asString() == "exit" ||
command.get(0).asString() == "stop" )
{
isRunning = false;
return false;
}
reply.clear();
reply.addString("The command is not valid! Try: quit|list|add|del");
// nothing there
if( command.size() < 1 ) return true;
if( command.size() == 1 && command.get(0).asString() == "") return true;
// LIST
if( command.get(0).asString() == "list" || command.get(0).asString() == "ls" ){
reply.clear();
reply.addString("list");
std::vector<ModuleInfo>::iterator itr;
for ( itr = listOfModules.begin(); itr != listOfModules.end(); ++itr ) {
std::cout << (*itr).toStdString() << std::endl;
Bottle b;
(*itr).toBottle(b);
reply.addList() = b;
}
}
// ADDING A MODULE
if( command.get(0).asString() == "add" || command.get(0).asString() == "launch" ){
reply.clear();
if( command.size() < 2 ) {
reply.addString("ERROR: The syntax should be:");
reply.addString("add <name>");
return true;
}
int thisModuleID = nextModuleID++;
ModuleInfo i;
i.set(thisModuleID, command);
listOfModules.push_back(i);
reply.addString("OK");
reply.addInt(thisModuleID);
}
// DELETING A MODULE
if(command.get(0).asString() == "del" ||
command.get(0).asString() == "rm" ||
command.get(0).asString() == "delete") {
reply.clear();
if( command.size() < 2 ) {
reply.addString("ERROR: The syntax should be:");
reply.addString("del <moduleID>");
return true;
}
if( command.get(1).isInt() ) {
int thisModuleID = command.get(1).asInt();
reply.addString("OK");
// delete from vector
std::vector<ModuleInfo>::iterator itr;
for ( itr = listOfModules.begin(); itr != listOfModules.end(); ++itr ) {
if(thisModuleID == (*itr).ID) {
listOfModules.erase(itr);
break;
}
}
} else {
reply.addString("ERROR: Could not parse integer! the syntax should be: del <moduleID as integer>");
}
}
// if( command.get(0).asString() == "set"){
//
// if( command.get(1).asString() == "tgt" || command.get(1).asString() == "target" ) {
// userSetTargetName = command.get(2).asString();
// reply.clear();
// if( userSetTargetName == "" ) {
// reply.addString("using latest object in the world as target!");
// } else {
// reply.addString(userSetTargetName.c_str());
// reply.addString(" is now the target!");
// }
//
//
// } else
//
// if( command.get(1).asString() == "offset" ) {
// if(reach) {
// if( command.get(2).isDouble() ) {
// reach->setOffset( command.get(2).asDouble() );
// reply.clear();
// reply.addString("OK");
// } else {
// reply.clear();
// reply.addString("ERROR: Could not parse double! the syntax should be: set offset <double>");
// }
// } else {
// reply.clear();
// reply.addString("ReachingWorker not ready yet!");
// }
// } else
//
// if( command.get(1).asString() == "policy" ) {
// if(reach) {
//
// if( command.get(2).asString() == "above" ) {
// reach->setPolicy( ReachingWorker::FROM_ABOVE | ReachingWorker::STRAIGHT );
// reply.clear();
// reply.addString("OK");
//
// } else if( command.get(2).asString() == "below" ) {
// reach->setPolicy( ReachingWorker::FROM_BELOW | ReachingWorker::STRAIGHT );
// reply.clear();
// reply.addString("OK");
//
// } else if( command.get(2).asString() == "left" ) {
// reach->setPolicy( ReachingWorker::FROM_LEFT | ReachingWorker::STRAIGHT );
// reply.clear();
// reply.addString("OK");
//
// } else if( command.get(2).asString() == "right" ) {
// reach->setPolicy( ReachingWorker::FROM_RIGHT | ReachingWorker::STRAIGHT );
// reply.clear();
// reply.addString("OK");
//
// } else {
// reply.clear();
// reply.addString("ERROR: Could not parse policy! the syntax should be: set policy <above|below|left|right>");
// }
// } else {
// reply.clear();
// reply.addString("ReachingWorker not ready yet!");
// }
// } else {
//
// // not a correct command?!
// reply.clear();
// reply.addString("Could not parse command try set [tgt|policy|offset]!");
// }
return true;
}