Beispiel #1
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;
}
Beispiel #2
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;
}
Beispiel #3
0
 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());
 }
Beispiel #4
0
 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());    
 }
Beispiel #5
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]);
}
Beispiel #6
0
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;
	
	
}
Beispiel #7
0
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;
}
Beispiel #10
0
/* 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;
}
Beispiel #11
0
/*
* 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;
}