Example #1
0
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());
}
Example #2
0
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.");
		}

	}
}
Example #3
0
    /**
    * 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;
    }
Example #4
0
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;
}
Example #5
0
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 );
	}
}
Example #6
0
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");
}
Example #7
0
 /**
 * 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;
 }
Example #8
0
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
Example #10
0
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");
	}
}
Example #11
0
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() );
    }
}
Example #12
0
 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;
 }
Example #13
0
  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;
  }
Example #14
0
 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;
 }
Example #15
0
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]);
}
Example #16
0
/**
 * 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());
        }

    }
}
Example #18
0
 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;
 }
Example #19
0
 /**
 * 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;
 }
Example #20
0
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.");
	}
}
Example #21
0
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");
		}

	}
}
Example #22
0
  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;
  }
Example #23
0
/* 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;
}
Example #24
0
/* 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;
}
Example #25
0
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;
}
Example #26
0
 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;
 }
Example #27
0
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
Example #29
0
    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;
    }
Example #30
0
 bool addAppName(const char *appName) {
     apps.addString(appName);
     return true;
 }