void WorldRpcInterface::grabObject( const yarp::os::Bottle& command, yarp::os::Bottle& reply, int& n )
{
KinematicModel::CompositeObject* object = getObject(command, reply, n);
if (!object)
return;
QString robotName = command.get(n).asString().c_str(); n++;
KinematicModel::Robot *robot = model->getRobot(robotName);
if (!robot) {
reply.addString("Robot not found.");
return;
}
reply.addString("Robot found.");
QString markerName = command.get(n).asString().c_str(); n++;
int markerIndex = -1;
//KinematicModel::RobotObservation robotObs = robot->observe();
//for (int i = 0; i<robotObs.getNumMarkers(); i++) {
// if (robotObs.markerName(i).compare(markerName) == 0)
// markerIndex = i;
//}
if (markerIndex < 0) {
reply.addString("Marker not found, releasing object.");
// return;
} else
reply.addString("Marker found, grabbing object.");
model->grabObject(object, robot, markerIndex);
//printf("Grabbing object \"%s\" with marker \"%s\" on robot \"%s\"\n", object->getName().toStdString().c_str(), markerName.toStdString().c_str(), robotName.toStdString().c_str());
}
void WorldRpcInterface::setRot( const yarp::os::Bottle& command, yarp::os::Bottle& reply, int& n )
{
KinematicModel::CompositeObject* object = getObject( command, reply, n );
if ( object )
{
if ((command.size() - n) == 3) {
double x = command.get(n).asDouble()*M_PI/180.; n++; //std::cout << x << std::endl; // x position
double y = command.get(n).asDouble()*M_PI/180.; n++; //std::cout << y << std::endl; // y position
double z = command.get(n).asDouble()*M_PI/180.; n++; //std::cout << z << std::endl; // z position
object->setCartesianOrientation( QVector3D(x,y,z) );
reply.addString("Set rotation (about x,y,z in degrees).");
} else {
// replace rotation part of object's T matrix
QMatrix4x4 rt = object->getT();
for (int i = 0; i<3; i++) {
for (int j = 0; j<3; j++) {
rt(i, j) = command.get(n).asDouble(); n++;
}
}
object->setT(rt);
reply.addString("Set full rotation matrix.");
}
}
}
/**
* Parser for string commands. It is called by virtual bool respond().
* @param command the bottle containing the user command
* @param reply the bottle which will be returned to the RPC client
* @return true if the command was successfully parsed
*/
bool parse_respond_string(const yarp::os::Bottle& command, yarp::os::Bottle& reply)
{
if (command.get(0).isString() && command.get(0).asString() == "getLoc")
{
std::string s = std::string("Current Location is: ") + m_localization_data.toString();
reply.addString(s);
}
else if (command.get(0).isString() && command.get(0).asString() == "initLoc")
{
yarp::dev::Map2DLocation loc;
loc.map_id = command.get(1).asString();
loc.x = command.get(2).asDouble();
loc.y = command.get(3).asDouble();
loc.theta = command.get(4).asDouble();
initializeLocalization(loc);
std::string s = std::string("Localization initialized to: ") + loc.toString();
reply.addString(s);
}
else
{
reply.addString("Unknown command.");
}
return true;
}
bool GuiUpdaterModule::respond(const yarp::os::Bottle& command, yarp::os::Bottle& reply)
{
string helpMessage = string(getName().c_str()) +
" commands are: \n" +
"reset \n" +
"help \n" +
"quit \n" ;
reply.clear();
if (command.get(0).asString()=="quit") {
reply.addString("quitting");
return false;
}
else if (command.get(0).asString()=="help") {
cout << helpMessage;
reply.addString("ok");
}
else if (command.get(0).asString()=="reset") {
cout << "Reset"<<endl;
resetGUI();
reply.addString("ok");
}
return true;
}
void WorldRpcInterface::respClass( const yarp::os::Bottle& command, yarp::os::Bottle& reply, int& n )
{
KinematicModel::CompositeObject* object = getObject( command, reply, n );
if ( object )
{
//model->clearWorldObject(object);
model->removeWorldObject(object);
int type = command.get(n).asVocab();
QColor collidingColor,freeColor;
switch (type)
{
case VOCAB_OBSTACLE:
freeColor = Qt::blue;
freeColor = freeColor.lighter();
collidingColor = freeColor;
freeColor.setAlphaF(0.5);
collidingColor.setAlphaF(0.5);
object->setResponseClass(model->OBSTACLE());
object->setFreeColor( freeColor );
object->setCollidingColor( collidingColor );
reply.addString("Changed object type to 'obstacle'.");
break;
case VOCAB_TARGET:
freeColor = Qt::green;
freeColor = freeColor.lighter();
collidingColor = freeColor;
freeColor.setAlphaF(1.0);
collidingColor.setAlphaF(0.5);
object->setResponseClass(model->TARGET());
object->setFreeColor( freeColor );
object->setCollidingColor( collidingColor );
reply.addString("Changed object type to 'target'.");
break;
default:
reply.addString("Unknown definition, use 'obs' or 'tgt'.");
}
model->appendObject( object );
}
}
void WorldRpcInterface::startSimSyncer(const yarp::os::Bottle& command, yarp::os::Bottle& reply, int& n) {
if ((command.size() - n) != 1) {
reply.addString("Please provide the refresh period for the synchronization thread in seconds");
return;
}
double period = command.get(n).asDouble(); n++;
if (model->getSimSyncer().isRunning()) {
model->getSimSyncer().stop();
}
model->getSimSyncer().setRefreshPeriod(period);
model->getSimSyncer().start();
reply.addString("ok");
}
/**
* Parser for VOCAB commands. It is called by virtual bool respond().
* @param command the bottle containing the user command
* @param reply the bottle which will be returned to the RPC client
* @return true if the command was successfully parsed
*/
bool parse_respond_vocab(const yarp::os::Bottle& command, yarp::os::Bottle& reply)
{
int request = command.get(1).asVocab();
if (request == VOCAB_NAV_GET_CURRENT_POS)
{
//plannerThread->setNewAbsTarget(loc);
reply.addVocab(VOCAB_OK);
reply.addString(m_localization_data.map_id);
reply.addDouble(m_localization_data.x);
reply.addDouble(m_localization_data.y);
reply.addDouble(m_localization_data.theta);
}
else if (request == VOCAB_NAV_SET_INITIAL_POS)
{
yarp::dev::Map2DLocation loc;
loc.map_id = command.get(2).asString();
loc.x = command.get(3).asDouble();
loc.y = command.get(4).asDouble();
loc.theta = command.get(5).asDouble();
initializeLocalization(loc);
reply.addVocab(VOCAB_OK);
}
else
{
reply.addVocab(VOCAB_ERR);
}
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
ExtraArgumentDescriptor::addValueToBottle(yarp::os::Bottle & container)
{
ODL_ENTER(); //####
ODL_P1("container = ", &container); //####
container.addString(getProcessedValue());
ODL_EXIT(); //####
} // ExtraArgumentDescriptor::addValueToBottle
void WorldRpcInterface::removeObject( const yarp::os::Bottle& command, yarp::os::Bottle& reply, int& n )
{
KinematicModel::CompositeObject* object = getObject( command, reply, n );
if ( object )
{
object->kill();
reply.addString("Removed object");
}
}
void WorldRpcInterface::getList(yarp::os::Bottle& reply)
{
//printf("called getList\n");
QVector<QString>::iterator i;
QVector<QString> list = model->listWorldObjects();
for ( i=list.begin(); i!=list.end(); ++i )
{
reply.addString( (*i).toStdString().c_str() );
}
}
virtual bool respond(const yarp::os::Bottle &command, yarp::os::Bottle &reply)
{
reply.clear();
if (command.get(0).isString())
{
if (command.get(0).asString()=="help")
{
reply.addVocab(Vocab::encode("many"));
reply.addString("Available commands:");
reply.addString("currently nothing");
return true;
}
else if (command.get(0).asString()=="***")
{
return true;
}
}
reply.addString("Unknown command");
return true;
}
virtual bool cmdList(yarp::os::Bottle& cmd,
yarp::os::Bottle& reply,
yarp::os::Contact& remote) {
reply.addString("old");
for (map<string,Entry>::iterator it = names.begin();
it!=names.end();
it++) {
appendEntry(reply,it->second);
}
return true;
}
virtual bool cmdQuery(yarp::os::Bottle& cmd,
yarp::os::Bottle& reply,
yarp::os::Contact& remote) {
reply.addString("old");
ConstString name = cmd.get(1).asString();
Contact c = Network::queryName(name);
if (c.isValid()) {
appendEntry(reply,c);
}
return true;
}
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]);
}
/**
* Handles command line commands to interact with the motors and run procedures
* @return true unless quit is called
*/
bool SarsaLearner::respond(const yarp::os::Bottle& command, yarp::os::Bottle& reply)
{
string helpMessage = string(getName().c_str()) +
" commands are: \n" +
"help \n" +
"quit \n" ;
reply.clear();
if (command.get(0).asString()=="quit") {
reply.addString("quitting");
return false;
}
else if (command.get(0).asString()=="help") {
cout << helpMessage;
reply.addString("ok");
}
return true;
}
void FrameTransformServer::list_response(yarp::os::Bottle& out)
{
std::vector<Transforms_server_storage*> storages;
std::vector<string> storageDescription;
storages.push_back(m_ros_timed_transform_storage);
storageDescription.push_back("ros timed transforms");
storages.push_back(m_ros_static_transform_storage);
storageDescription.push_back("ros static transforms");
storages.push_back(m_yarp_timed_transform_storage);
storageDescription.push_back("yarp timed transforms");
storages.push_back(m_yarp_static_transform_storage);
storageDescription.push_back("yarp static transforms");
if (storages[0]->size() == 0 &&
storages[1]->size() == 0 &&
storages[2]->size() == 0 &&
storages[3]->size() == 0)
{
out.addString("no transforms found");
return;
}
for(size_t s = 0; s < storages.size(); s++ )
{
if(!storages[s])
{
continue;
}
yDebug() << storages[s]->size();
out.addString(storageDescription[s] + ": ");
for(size_t i = 0; i < storages[s]->size(); i++)
{
out.addString((*storages[s])[i].toString());
}
}
}
virtual bool apply(yarp::os::Bottle& cmd,
yarp::os::Bottle& reply,
yarp::os::Bottle& event,
yarp::os::Contact& remote) {
bool ok = false;
printf(" + %s\n", cmd.toString().c_str());
ConstString tag = cmd.get(0).asString();
if (tag=="register") {
ok = cmdRegister(cmd,reply,remote);
} else if (tag=="unregister") {
ok = cmdUnregister(cmd,reply,remote);
} else if (tag=="query") {
ok = cmdQuery(cmd,reply,remote);
} else if (tag=="list") {
ok = cmdList(cmd,reply,remote);
} else {
reply.addString("old");
reply.addString("I have no idea what you are talking about");
}
return ok;
}
/**
* Parser for user command received from the RPC port
* @param command the bottle containing the user command
* @param reply the bottle which will be returned to the RPC client
* @return true if the command was successfully parsed
*/
virtual bool respond(const yarp::os::Bottle& command,yarp::os::Bottle& reply)
{
yarp::os::LockGuard lock(m_mutex);
reply.clear();
//parser for VOCAB commands
if (command.get(0).isVocab())
{
if(command.get(0).asVocab() == VOCAB_INAVIGATION && command.get(1).isVocab())
{
parse_respond_vocab(command,reply);
}
else
{
yError() << "Invalid vocab received";
reply.addVocab(VOCAB_ERR);
}
}
//parser for string commands
else if (command.get(0).isString())
{
if (command.get(0).asString()=="help")
{
reply.addVocab(Vocab::encode("many"));
reply.addString("Available commands are:");
reply.addString("getLoc");
reply.addString("initLoc <map_name> <x> <y> <angle in degrees>");
}
else if (command.get(0).isString())
{
parse_respond_string(command, reply);
}
}
//unknown/invalid command received
else
{
yError() << "Invalid command type";
reply.addVocab(VOCAB_ERR);
}
return true;
}
void WorldRpcInterface::set( const yarp::os::Bottle& command, yarp::os::Bottle& reply, int& n )
{
KinematicModel::CompositeObject* object = getObject( command, reply, n );
if ( object )
{
double x = command.get(n).asDouble(); n++; //std::cout << x << std::endl; // x position
double y = command.get(n).asDouble(); n++; //std::cout << y << std::endl; // y position
double z = command.get(n).asDouble(); n++; //std::cout << z << std::endl; // z position
object->setPosition( QVector3D(x,y,z) );
reply.addString("Set Cartesian position of object.");
}
}
void WorldRpcInterface::setRTfromSim(const yarp::os::Bottle& command, yarp::os::Bottle& reply, int& n) {
KinematicModel::CompositeObject* object = getObject( command, reply, n );
if ( object ) {
QMatrix4x4 rt;
std::string name = object->getName().toStdString();
if(parseSimRTBottle(name, command, n, rt)) {
object->setT(rt);
} else {
reply.addString("parseSimRTBottle: invalid number of arguments supplied");
}
}
}
virtual bool cmdQuery(yarp::os::Bottle& cmd,
yarp::os::Bottle& reply,
yarp::os::Contact& remote) {
reply.addString("old");
string name = cmd.get(1).asString().c_str();
map<string,Entry>::iterator it = names.find(name);
if (it==names.end()) {
return true;
}
Entry& e = it->second;
appendEntry(reply,e);
return true;
}
/* Respond function */
bool babbler::respond(const yarp::os::Bottle& bCommand, yarp::os::Bottle& bReply)
{
std::string helpMessage = std::string(getName().c_str()) +
" commands are: \n" +
"help \n" +
"F + (int)Frequency\n" +
"RF \n" +
"quit \n";
bReply.clear();
std::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 == "F") {
if (bCommand.size() == 2)
{
if (bCommand.get(1).isInt())
{
setNewFrequency(bCommand.get(1).asInt());
bReply.addInt(f);
}
}
}
else if (keyWord == "RF") {
bReply.addInt(newRandomFrequency());
}
return true;
}
/* 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::listTopics(yarp::os::Bottle& topics) {
mutex.wait();
sqlite3_stmt *statement = NULL;
char *query = NULL;
query = sqlite3_mprintf("SELECT topic FROM topics");
if (verbose) {
printf("Query: %s\n", query);
}
int result = sqlite3_prepare_v2(SQLDB(implementation),query,-1,&statement,
NULL);
if (result!=SQLITE_OK) {
printf("Error in query\n");
}
while (result == SQLITE_OK && sqlite3_step(statement) == SQLITE_ROW) {
char *topic = (char *)sqlite3_column_text(statement,0);
topics.addString(topic);
}
sqlite3_finalize(statement);
sqlite3_free(query);
mutex.post();
return true;
}
virtual bool respond(const yarp::os::Bottle& command,
yarp::os::Bottle& reply) {
mutex.wait();
switch (command.get(0).asVocab()) {
case VOCAB3('a','d','d'):
{
string name = command.get(1).asString().c_str();
if (name!="") {
removeName(name.c_str());
Entry entry;
entry.name = name;
q.push_back(entry);
reply.clear();
reply.add(Value::makeVocab("add"));
reply.addString(name.c_str());
addQueue(reply);
}
}
break;
case VOCAB3('d','e','l'):
{
if (command.get(1).isInt()) {
int idx = command.get(1).asInt();
bool acted = removeName(idx);
if (acted) {
reply.clear();
reply.add(Value::makeVocab("del"));
reply.addInt(idx);
} else {
reply.clear();
reply.add(Value::makeVocab("no"));
reply.addInt(idx);
}
addQueue(reply);
} else {
string name = command.get(1).asString().c_str();
if (name!="") {
bool acted = removeName(name.c_str());
if (acted) {
reply.clear();
reply.add(Value::makeVocab("del"));
reply.addString(name.c_str());
} else {
reply.clear();
reply.add(Value::makeVocab("no"));
reply.addString(name.c_str());
}
addQueue(reply);
}
}
}
break;
case VOCAB4('l','i','s','t'):
{
reply.clear();
addQueue(reply);
}
break;
default:
updateHelp();
mutex.post();
return DeviceResponder::respond(command,reply);
}
mutex.post();
printf("%s\n", reply.toString().c_str());
return true;
}
bool OpcInspector::respond(const yarp::os::Bottle& command, yarp::os::Bottle& reply)
{
string key1 = command.get(0).asString().c_str();
if( key1 == "relation")
{
string key2 = command.get(1).asString().c_str();
if( key2 == "add")
{
string subject = command.get(2).asString().c_str();
string verb = command.get(3).asString().c_str();
string object = command.get(4).asString().c_str();
string ccplace = "none";
string cctime = "none";
string ccmanner = "none";
if (command.size()>5)
ccplace = command.get(5).asString().c_str();
if (command.size()>6)
cctime = command.get(6).asString().c_str();
if (command.size()>7)
ccmanner = command.get(7).asString().c_str();
if(opc->getEntity(subject) == NULL)
{
reply.addString("Warning: " + subject + "not present in OPC. Created as an adjective");
opc->addAdjective(subject);
}
if(opc->getEntity(verb) == NULL)
{
reply.addString("Warning: " + verb + "not present in OPC. Created as an action");
opc->addAction(verb);
}
if(opc->getEntity(object) == NULL)
{
reply.addString("Warning: " + object + "not present in OPC. Created as an adjective");
opc->addAdjective(object);
}
if(ccplace!= "none" && opc->getEntity(ccplace) == NULL)
{
reply.addString("Warning: " + ccplace + "not present in OPC. Created as an adjective");
opc->addAdjective(ccplace);
}
if(cctime!= "none" && opc->getEntity(cctime) == NULL)
{
reply.addString("Warning: " + cctime + "not present in OPC. Created as an adjective");
opc->addAdjective(cctime);
}
if(ccmanner!= "none" && opc->getEntity(ccmanner) == NULL)
{
reply.addString("Warning: " + ccmanner + "not present in OPC. Created as an adjective");
opc->addAdjective(ccmanner);
}
Relation r(subject,verb,object,ccplace,cctime,ccmanner);
opc->addRelation(r);
reply.addString("Relation addded");
}
if( key2 == "rem")
{
string subject = command.get(2).asString().c_str();
string verb = command.get(3).asString().c_str();
string object = command.get(4).asString().c_str();
string ccplace = "none";
string cctime = "none";
string ccmanner = "none";
if (command.size()>5)
ccplace = command.get(5).asString().c_str();
if (command.size()>6)
cctime = command.get(6).asString().c_str();
if (command.size()>7)
ccmanner = command.get(7).asString().c_str();
if(opc->getEntity(subject) == NULL)
{
reply.addString("Warning: " + subject + "not present in OPC. Created as an adjective");
opc->addAdjective(subject);
}
if(opc->getEntity(verb) == NULL)
{
reply.addString("Warning: " + verb + "not present in OPC. Created as an action");
opc->addAction(verb);
}
if(opc->getEntity(object) == NULL)
{
reply.addString("Warning: " + object + "not present in OPC. Created as an adjective");
opc->addAdjective(object);
}
if(ccplace!= "none" && opc->getEntity(ccplace) == NULL)
{
reply.addString("Warning: " + ccplace + "not present in OPC. Created as an adjective");
opc->addAdjective(ccplace);
}
if(cctime!= "none" && opc->getEntity(cctime) == NULL)
{
reply.addString("Warning: " + cctime + "not present in OPC. Created as an adjective");
opc->addAdjective(cctime);
}
if(ccmanner!= "none" && opc->getEntity(ccmanner) == NULL)
{
reply.addString("Warning: " + ccmanner + "not present in OPC. Created as an adjective");
opc->addAdjective(ccmanner);
}
Relation r(subject,verb,object,ccplace,cctime,ccmanner);
opc->removeRelation(r);
reply.addString("Relation removed");
}
if( key2 == "check")
{
string subject = command.get(2).asString().c_str();
string verb = command.get(3).asString().c_str();
string object = command.get(4).asString().c_str();
string ccplace = "any";
string cctime = "any";
string ccmanner = "any";
if (command.size()>5)
ccplace = command.get(5).asString().c_str();
if (command.size()>6)
cctime = command.get(6).asString().c_str();
if (command.size()>7)
ccmanner = command.get(7).asString().c_str();
if(opc->getEntity(subject) == NULL)
{
reply.addString("Warning: " + subject + "not present in OPC. Created as an adjective");
opc->addAdjective(subject);
}
if(opc->getEntity(verb) == NULL)
{
reply.addString("Warning: " + verb + "not present in OPC. Created as an action");
opc->addAction(verb);
}
if(opc->getEntity(object) == NULL)
{
reply.addString("Warning: " + object + "not present in OPC. Created as an adjective");
opc->addAdjective(object);
}
if(ccplace!= "any" && opc->getEntity(ccplace) == NULL)
{
reply.addString("Warning: " + ccplace + "not present in OPC. Created as an adjective");
opc->addAdjective(ccplace);
}
if(cctime!= "any" && opc->getEntity(cctime) == NULL)
{
reply.addString("Warning: " + cctime + "not present in OPC. Created as an adjective");
opc->addAdjective(cctime);
}
if(ccmanner!= "any" && opc->getEntity(ccmanner) == NULL)
{
reply.addString("Warning: " + ccmanner + "not present in OPC. Created as an adjective");
opc->addAdjective(ccmanner);
}
Relation r(subject,verb,object,ccplace,cctime,ccmanner);
bool result = opc->containsRelation(r);
if (result)
reply.addString("OPC contains " + r.toString());
else
reply.addString("OPC does not contain " + r.toString());
}
}
if( key1 == "object")
{
string key2 = command.get(1).asString().c_str();
string objectName = command.get(2).asString().c_str();
Object* objPtr = (Object* )opc->getEntity(objectName);
if( objPtr == NULL)
{
reply.addString("Warning: " + objectName + "not present in OPC. Created as a generic object");
objPtr = opc->addObject(objectName);
}
if( key2 == "display")
{
reply.addString(objPtr->toString());
}
if( key2 == "show")
{
objPtr->m_present = true;
opc->commit(objPtr);
}
if( key2 == "hide")
{
objPtr->m_present = false;
opc->commit(objPtr);
}
}
return true;
}
/*! @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
virtual bool respond(const yarp::os::Bottle &command, yarp::os::Bottle &reply)
{
reply.clear();
yDebug("receiving command from port\n");
int index = 0;
int cmdSize = command.size();
while(cmdSize>0)
{
switch(command.get(index).asVocab())
{
case VOCAB4('s','u','s','p'):
reply.addVocab(Vocab::encode("ack"));
vc->halt();
cmdSize--;
index++;
break;
case VOCAB3('r','u','n'):
reply.addVocab(Vocab::encode("ack"));
vc->go();
cmdSize--;
index++;
break;
//this set current position reference
case VOCAB3('s','e','t'):
if (command.size()>=3)
{
int i=command.get(index+1).asInt();
double pos=command.get(index+2).asDouble();
vc->setRef(i, pos);
index +=3;
cmdSize-=3;
}
else
{
cmdSize--;
index++;
yError("Invalid set message, ignoring\n");
}
reply.addVocab(Vocab::encode("ack"));
break;
//this set maximum velocity (limiter)
case VOCAB4('s','v','e','l'):
if(command.size()>=3)
{
int i=command.get(index+1).asInt();
double vel = command.get(index+2).asDouble();
vc->setVel(i,vel);
index += 3;
cmdSize-=3;;
reply.addVocab(Vocab::encode("ack"));
}
else
{
cmdSize--;
index++;
yError("Invalid set vel message, ignoring\n");
reply.addVocab(Vocab::encode("fail"));
}
break;
case VOCAB4('g','a','i','n'):
if(command.size()>=3)
{
int i=command.get(index+1).asInt();
double gain = command.get(index+2).asDouble();
vc->setGain(i,gain);
index+=3;
cmdSize-=3;
reply.addVocab(Vocab::encode("ack"));
}
else
{
cmdSize--;
index++;
yError("Invalid set gain message, ignoring\n");
reply.addVocab(Vocab::encode("fail"));
}
break;
case VOCAB4('h','e','l','p'):
fprintf(stdout,"VelocityControl module, valid commands are:\n");
fprintf(stdout,"- [susp] suspend the controller (command zero velocity)\n");
fprintf(stdout,"- [run] start (and resume after being suspended) the controller\n");
fprintf(stdout,"- [quit] quit the module (exit)\n");
fprintf(stdout,"- [set] <j> <p> move joint j to p (degrees)\n");
fprintf(stdout,"- [svel] <j> <v> set maximum speed for joint j to v (deg/sec)\n");
fprintf(stdout,"- [gain] <j> <k> set P gain for joint j to k\n");
fprintf(stdout,"- [help] to get this help\n");
fprintf(stdout,"\n typical commands:\n gain 0 10\n svel 0 10\n run\n set 0 x\n\n");
reply.addVocab(Vocab::encode("many"));
reply.addVocab(Vocab::encode("ack"));
reply.addString("VelocityControl module, valid commands are:");
reply.addString("- [susp] suspend the controller (command zero velocity)");
reply.addString("- [run] start (and resume after being suspended) the controller");
reply.addString("- [quit] quit the module (exit)");
reply.addString("- [set] <j> <p> move joint j to p (degrees)");
reply.addString("- [svel] <j> <v> set maximum speed for joint j to v (deg/sec)");
reply.addString("- [gain] <j> <k> set P gain for joint j to k");
reply.addString("- [help] to get this help");
reply.addString("\n typical commands:\n gain 0 10\n svel 0 10\n run\n set 0 x\n\n");
break;
default:
yError("Invalid command, ignoring\n");
reply.addVocab(Vocab::encode("fail"));
//cmdSize--;
//index++;
//return respond(command, reply); // call default
break;
}
return true;
}
return false;
}
bool addAppName(const char *appName) {
apps.addString(appName);
return true;
}