bool
StartSumRequestHandler::processRequest(const YarpString &           request,
                                       const yarp::os::Bottle &     restOfInput,
                                       const YarpString &           senderChannel,
                                       yarp::os::ConnectionWriter * replyMechanism)
{
#if (! defined(ODL_ENABLE_LOGGING_))
# if MAC_OR_LINUX_
#  pragma unused(request,restOfInput)
# endif // MAC_OR_LINUX_
#endif // ! defined(ODL_ENABLE_LOGGING_)
    ODL_OBJENTER(); //####
    ODL_S3s("request = ", request, "restOfInput = ", restOfInput.toString(), //####
            "senderChannel = ", senderChannel); //####
    ODL_P1("replyMechanism = ", replyMechanism); //####
    bool result = true;

    try
    {
        static_cast<RunningSumService &>(_service).startSum(senderChannel);
        sendOKResponse(replyMechanism);
    }
    catch (...)
    {
        ODL_LOG("Exception caught"); //####
        throw;
    }
    ODL_OBJEXIT_B(result); //####
    return result;
} // StartSumRequestHandler::processRequest
bool
JavaScriptFilterInputHandler::handleInput(const yarp::os::Bottle &     input,
                                          const YarpString &           senderChannel,
                                          yarp::os::ConnectionWriter * replyMechanism,
                                          const size_t                 numBytes)
{
#if (! defined(ODL_ENABLE_LOGGING_))
# if MAC_OR_LINUX_
#  pragma unused(senderChannel,replyMechanism,numBytes)
# endif // MAC_OR_LINUX_
#endif // ! defined(ODL_ENABLE_LOGGING_)
    ODL_OBJENTER(); //####
    ODL_S2s("senderChannel = ", senderChannel, "got ", input.toString()); //####
    ODL_P1("replyMechanism = ", replyMechanism); //####
    ODL_I1("numBytes = ", numBytes); //####
    bool result = true;

    try
    {
        if (_active && _owner)
        {
            _received = input;
            _owner->stallUntilIdle(_slotNumber);
            _owner->signalRunFunction();
        }
    }
    catch (...)
    {
        ODL_LOG("Exception caught"); //####
        throw;
    }
    ODL_OBJEXIT_B(result); //####
    return result;
} // JavaScriptFilterInputHandler::handleInput
bool
RandomRequestHandler::processRequest(const YarpString &           request,
                                     const yarp::os::Bottle &     restOfInput,
                                     const YarpString &           senderChannel,
                                     yarp::os::ConnectionWriter * replyMechanism)
{
#if (! defined(ODL_ENABLE_LOGGING_))
# if MAC_OR_LINUX_
#  pragma unused(request,senderChannel)
# endif // MAC_OR_LINUX_
#endif // ! defined(ODL_ENABLE_LOGGING_)
    ODL_OBJENTER(); //####
    ODL_S3s("request = ", request, "restOfInput = ", restOfInput.toString(), //####
            "senderChannel = ", senderChannel); //####
    ODL_P1("replyMechanism = ", replyMechanism); //####
    bool result = true;

    try
    {
        int count;

        _response.clear();
        if (0 < restOfInput.size())
        {
            yarp::os::Value number(restOfInput.get(0));

            if (number.isInt())
            {
                count = number.asInt();
            }
            else
            {
                count = -1;
            }
        }
        else
        {
            count = 1;
        }
        if (count > 0)
        {
            for (int ii = 0; ii < count; ++ii)
            {
                _response.addDouble(yarp::os::Random::uniform());
            }
        }
        else
        {
            ODL_LOG("! (count > 0)"); //####
        }
        sendResponse(replyMechanism);
    }
    catch (...)
    {
        ODL_LOG("Exception caught"); //####
        throw;
    }
    ODL_OBJEXIT_B(result); //####
    return result;
} // RandomRequestHandler::processRequest
bool
MetricsRequestHandler::processRequest(const YarpString &           request,
                                      const yarp::os::Bottle &     restOfInput,
                                      const YarpString &           senderChannel,
                                      yarp::os::ConnectionWriter * replyMechanism)
{
#if (! defined(ODL_ENABLE_LOGGING_))
# if MAC_OR_LINUX_
#  pragma unused(request,restOfInput,senderChannel)
# endif // MAC_OR_LINUX_
#endif // ! defined(ODL_ENABLE_LOGGING_)
    ODL_OBJENTER(); //####
    ODL_S3s("request = ", request, "restOfInput = ", restOfInput.toString(), //####
            "senderChannel = ", senderChannel); //####
    ODL_P1("replyMechanism = ", replyMechanism); //####
    bool result = true;

    try
    {
        _response.clear();
        _service.gatherMetrics(_response);
        sendResponse(replyMechanism);
    }
    catch (...)
    {
        ODL_LOG("Exception caught"); //####
        throw;
    }
    ODL_OBJEXIT_B(result); //####
    return result;
} // MetricsRequestHandler::processRequest
bool
SendToMQOutputInputHandler::handleInput(const yarp::os::Bottle &     input,
                                        const YarpString &           senderChannel,
                                        yarp::os::ConnectionWriter * replyMechanism,
                                        const size_t                 numBytes)
{
#if (! defined(ODL_ENABLE_LOGGING_))
# if MAC_OR_LINUX_
#  pragma unused(senderChannel,replyMechanism,numBytes)
# endif // MAC_OR_LINUX_
#endif // ! defined(ODL_ENABLE_LOGGING_)
    ODL_OBJENTER(); //####
    ODL_S2s("senderChannel = ", senderChannel, "got ", input.toString()); //####
    ODL_P1("replyMechanism = ", replyMechanism); //####
    ODL_I1("numBytes = ", numBytes); //####
    bool result = true;

    try
    {
        if (_owner.isActive())
        {
            ODL_LOG("(_owner.isActive())"); //####
#if (! defined(MpM_UseCustomStringBuffer))
            std::stringstream outBuffer;
#endif // ! defined(MpM_UseCustomStringBuffer)

#if defined(MpM_UseCustomStringBuffer)
            Utilities::ConvertMessageToJSON(_outBuffer, input);
#else // ! defined(MpM_UseCustomStringBuffer)
            Utilities::ConvertMessageToJSON(outBuffer, input);
#endif // ! defined(MpM_UseCustomStringBuffer)
            size_t      outLength;
            std::string buffAsString;

#if defined(MpM_UseCustomStringBuffer)
            buffAsString = _outBuffer.getString(outLength);
#else // ! defined(MpM_UseCustomStringBuffer)
            buffAsString = outBuffer.str();
            outLength = buffAsString.length();
#endif // ! defined(MpM_UseCustomStringBuffer)
            if (buffAsString.length())
            {
                ODL_LOG("(buffAsString.length())"); //####
                SendReceiveCounters toBeAdded(0, 0, outLength, 1);

                _owner.sendMessage(buffAsString, outLength);
                _owner.incrementAuxiliaryCounters(toBeAdded);
            }
        }
    }
    catch (...)
    {
        ODL_LOG("Exception caught"); //####
        throw;
    }
    ODL_OBJEXIT_B(result); //####
    return result;
} // SendToMQOutputInputHandler::handleInput
bool
TruncateFloatFilterInputHandler::handleInput(const yarp::os::Bottle &     input,
                                             const YarpString &           senderChannel,
                                             yarp::os::ConnectionWriter * replyMechanism,
                                             const size_t                 numBytes)
{
#if (! defined(ODL_ENABLE_LOGGING_))
# if MAC_OR_LINUX_
#  pragma unused(senderChannel,replyMechanism,numBytes)
# endif // MAC_OR_LINUX_
#endif // ! defined(ODL_ENABLE_LOGGING_)
    ODL_OBJENTER(); //####
    ODL_S2s("senderChannel = ", senderChannel, "got ", input.toString()); //####
    ODL_P1("replyMechanism = ", replyMechanism); //####
    ODL_I1("numBytes = ", numBytes); //####
    bool result = true;

    try
    {
        yarp::os::Bottle outBottle;

        for (int ii = 0, mm = input.size(); mm > ii; ++ii)
        {
            yarp::os::Value aValue(input.get(ii));

            if (aValue.isInt())
            {
                outBottle.addInt(aValue.asInt());
            }
            else if (aValue.isDouble())
            {
                outBottle.addInt(static_cast<int>(aValue.asDouble()));
            }
        }
        if ((0 < outBottle.size()) && _outChannel)
        {
            if (! _outChannel->write(outBottle))
            {
                ODL_LOG("(! _outChannel->write(message))"); //####
#if defined(MpM_StallOnSendProblem)
                Stall();
#endif // defined(MpM_StallOnSendProblem)
            }
        }
    }
    catch (...)
    {
        ODL_LOG("Exception caught"); //####
        throw;
    }
    ODL_OBJEXIT_B(result); //####
    return result;
} // TruncateFloatFilterInputHandler::handleInput
Example #7
0
bool NetworkProfiler::getPortmonitorParams(std::string portName, yarp::os::Bottle& param) {
    //e.g.,  get in /portname"
    yarp::os::Bottle cmd;
    cmd.addString("get");
    cmd.addString("in");
    cmd.addString(portName.c_str());
    Contact srcCon = Contact::fromString(portName);
    bool ret = yarp::os::NetworkBase::write(srcCon, cmd, param, true, true, 2.0);
    if(!ret) {
        yError()<<"Cannot write to"<<portName;
        return false;
    }
    if(param.size() > 1) {
        if(param.get(0).isString() && param.get(0).asString() == "fail") {
            yError()<<param.toString();
            return false;
        }
        else if(param.get(0).isInt() && param.get(0).asInt() == -1) {
            yError()<<param.toString();
            return false;
        }
    }
    return true;
}
Example #8
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;
	
	
}
Example #9
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;
 }
bool
Test11EchoRequestHandler::processRequest(const YarpString &           request,
                                         const yarp::os::Bottle &     restOfInput,
                                         const YarpString &           senderChannel,
                                         yarp::os::ConnectionWriter * replyMechanism)
{
#if (! defined(ODL_ENABLE_LOGGING_))
# if MAC_OR_LINUX_
#  pragma unused(request,senderChannel)
# endif // MAC_OR_LINUX_
#endif // ! defined(ODL_ENABLE_LOGGING_)
    ODL_OBJENTER(); //####
    ODL_S3s("request = ", request, "restOfInput = ", restOfInput.toString(), //####
            "senderChannel = ", senderChannel); //####
    ODL_P1("replyMechanism = ", replyMechanism); //####
    bool result = true;

    _response = restOfInput;
    sendResponse(replyMechanism);
    ODL_OBJEXIT_B(result); //####
    return result;
} // Test11EchoRequestHandler::processRequest
bool
WhereRequestHandler::processRequest(const YarpString &           request,
                                    const yarp::os::Bottle &     restOfInput,
                                    const YarpString &           senderChannel,
                                    yarp::os::ConnectionWriter * replyMechanism)
{
#if (! defined(ODL_ENABLE_LOGGING_))
# if MAC_OR_LINUX_
#  pragma unused(request,restOfInput,senderChannel)
# endif // MAC_OR_LINUX_
#endif // ! defined(ODL_ENABLE_LOGGING_)
    ODL_OBJENTER(); //####
    ODL_S3s("request = ", request, "restOfInput = ", restOfInput.toString(), //####
            "senderChannel = ", senderChannel); //####
    ODL_P1("replyMechanism = ", replyMechanism); //####
    bool result = true;

    try
    {
        YarpString address;
        int        port;

        static_cast<TunnelService &>(_service).getAddress(address, port);
        _response.clear();
        _response.addString(address);
        _response.addInt(port);
        sendResponse(replyMechanism);
    }
    catch (...)
    {
        ODL_LOG("Exception caught"); //####
        throw;
    }
    ODL_OBJEXIT_B(result); //####
    return result;
} // WhereRequestHandler::processRequest
Example #12
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;
}
Example #13
0
void handleInteractionModeMsg(IInteractionMode *iInteract, const yarp::os::Bottle& cmd,
                          yarp::os::Bottle& response, bool *rec, bool *ok)
{
    fprintf(stderr, "Handling IInteractionMode message %s\n", cmd.toString().c_str());
    if (!iInteract)
    {
        fprintf(stderr, "Error I do not have a valid interface\n");
        *ok=false;
        return;
    }

    int code = cmd.get(1).asVocab();
    *ok=true;

    switch(code)
    {
        case VOCAB_SET:
        {
            int axis = cmd.get(3).asInt();
            yarp::dev::InteractionModeEnum mode = (yarp::dev::InteractionModeEnum) cmd.get(2).asVocab();
            *ok = iInteract->setInteractionMode(axis, mode);
            *rec=true; //or false
        }
        break;

        case VOCAB_GET:
        {
            int which = cmd.get(2).asVocab();
            switch(which)
            {
                case VOCAB_INTERACTION_MODE:
                {
                    int axis = cmd.get(3).asInt();
                    yarp::dev::InteractionModeEnum mode;
                    *ok = iInteract->getInteractionMode(axis, &mode);
                    // create response
                    if(*ok)
                    {
                        response.addVocab(VOCAB_IS);
                        response.addInt(axis);
                        response.addVocab(VOCAB_INTERACTION_MODE);
                        response.addVocab(mode);
                        *rec=true;
                    }
                    else
                    {
                        response.addVocab(VOCAB_FAILED);
                        *rec = false;
                    }
                }
                break;

                case VOCAB_INTERACTION_MODES:
                {
                    int axis = cmd.get(3).asInt();
                    yarp::dev::InteractionModeEnum *modes;
                    modes = new yarp::dev::InteractionModeEnum[jnts];

                    *ok = iInteract->getInteractionMode(axis, modes);
                    // create response
                    if(*ok)
                    {
                        response.addVocab(VOCAB_IS);
                        response.addVocab(VOCAB_INTERACTION_MODES);
                        for(int i=0; i<jnts; i++)
                            response.addVocab(modes[i]);
                        *rec=true;
                    }
                    else
                    {
                        response.addVocab(VOCAB_FAILED);
                        *rec = false;
                    }
                }
                break;

                default:
                {
                    fprintf(stderr, "get command not understood");
                    *rec=false;
                    break;
                }
                break;
            }
        }
        break;
        default:
        {
            fprintf(stderr, "type of command not understood");
            *rec=false;
        }
        break;
    }
}
Example #14
0
void handleControlModeMsg(IControlMode2 *iMode, const yarp::os::Bottle& cmd,
                          yarp::os::Bottle& response, bool *rec, bool *ok)
{
    fprintf(stderr, "Handling IControlMode message %s\n", cmd.toString().c_str());
    if (!iMode)
        {
            fprintf(stderr, "Error I do not have a valid interface\n");
            *ok=false;
            return;
        }

    int code = cmd.get(1).asVocab();
    *ok=true;

    switch(code)
        {
        case VOCAB_SET:
            {
				int axis = cmd.get(3).asInt();
                int mode=cmd.get(2).asVocab();
				switch (mode)
					{
                    case VOCAB_CM_POSITION:
                        *ok = iMode->setPositionMode(axis);
						break;
                    case VOCAB_CM_POSITION_DIRECT:
                        *ok = iMode->setControlMode(axis, VOCAB_CM_POSITION_DIRECT);
						break;
                    case VOCAB_CM_VELOCITY:
                        *ok = iMode->setVelocityMode(axis);
						break;
                    case VOCAB_CM_MIXED:
                        *ok = iMode->setControlMode(axis, VOCAB_CM_MIXED);
                        break;
                    case VOCAB_CM_TORQUE:
                        *ok = iMode->setTorqueMode(axis);
                        break;
                    case VOCAB_CM_OPENLOOP:
                        *ok = iMode->setOpenLoopMode(axis);
                        break;
                    case VOCAB_CM_IDLE:
                        *ok = iMode->setControlMode(axis, VOCAB_CM_IDLE);
                        break;
                    case VOCAB_CM_FORCE_IDLE:
                        *ok = iMode->setControlMode(axis, VOCAB_CM_FORCE_IDLE);
                        break;
                    case VOCAB_CM_IMPEDANCE_POS:
                        *ok = iMode->setControlMode(axis, VOCAB_CM_IMPEDANCE_POS);
                        break;
                    case VOCAB_CM_IMPEDANCE_VEL:
                        *ok = iMode->setControlMode(axis, VOCAB_CM_IMPEDANCE_VEL);
                        break;
                    default:
                        *ok = false;
						break;
					}
                *rec=true; //or false
            }
            break;
        case VOCAB_GET:
            {
                if (cmd.get(2).asVocab()==VOCAB_CM_CONTROL_MODE)
                    {
                        int p=-1;
                        int axis = cmd.get(3).asInt();
                        fprintf(stderr, "Calling getControlMode\n");
                        *ok = iMode->getControlMode(axis, &p);

                        response.addVocab(VOCAB_IS);
                        response.addInt(axis);
                        response.addVocab(VOCAB_CM_CONTROL_MODE);       
                        response.addVocab(p);
			
                        //fprintf(stderr, "Returning %d\n", p);
                        *rec=true;
                    }
            }
            break;
        default:
            {
                *rec=false;
            }
            break;
        }
}
bool
PingRequestHandler::processRequest(const YarpString &           request,
                                   const yarp::os::Bottle &     restOfInput,
                                   const YarpString &           senderChannel,
                                   yarp::os::ConnectionWriter * replyMechanism)
{
#if (! defined(ODL_ENABLE_LOGGING_))
# if MAC_OR_LINUX_
#  pragma unused(request,senderChannel)
# endif // MAC_OR_LINUX_
#endif // ! defined(ODL_ENABLE_LOGGING_)
    ODL_OBJENTER(); //####
    ODL_S3s("request = ", request, "restOfInput = ", restOfInput.toString(), //####
            "senderChannel = ", senderChannel); //####
    ODL_P1("replyMechanism = ", replyMechanism); //####
    bool result = true;

    try
    {
        // Validate the name as a channel name
        _response.clear();
        if (1 == restOfInput.size())
        {
            yarp::os::Value argument(restOfInput.get(0));

            if (argument.isString())
            {
                YarpString argAsString(argument.toString());

                if (Endpoint::CheckEndpointName(argAsString))
                {
                    RegistryService & theService = static_cast<RegistryService &>(_service);

                    theService.reportStatusChange(argAsString,
                                                  RegistryService::kRegistryPingFromService);
                    if (theService.checkForExistingService(argAsString))
                    {
                        // This service is already known, so just update the last-checked time.
                        theService.updateCheckedTimeForChannel(argAsString);
                    }
                    else if (theService.checkForExistingService(argAsString))
                    {
                        // Second try - something happened with the first call.
                        // This service is already known, so just update the last-checked time.
                        theService.updateCheckedTimeForChannel(argAsString);
                    }
                    else
                    {
                        // Send a 'name' request to the channel
                        YarpString      aName = GetRandomChannelName(HIDDEN_CHANNEL_PREFIX_
                                                                     BUILD_NAME_("ping_",
                                                                          DEFAULT_CHANNEL_ROOT_));
                        ClientChannel * outChannel = new ClientChannel;

                        if (outChannel)
                        {
                            if (outChannel->openWithRetries(aName, STANDARD_WAIT_TIME_))
                            {
                                if (outChannel->addOutputWithRetries(argAsString,
                                                                     STANDARD_WAIT_TIME_))
                                {
                                    yarp::os::Bottle message1(MpM_NAME_REQUEST_);
                                    yarp::os::Bottle reply;

                                    if (outChannel->writeBottle(message1, reply))
                                    {
                                        if (theService.processNameResponse(argAsString,
                                                                           ServiceResponse(reply)))
                                        {
                                            yarp::os::Bottle message2(MpM_LIST_REQUEST_);

                                            if (outChannel->writeBottle(message2, reply))
                                            {
                                                if (theService.processListResponse(argAsString,
                                                                           ServiceResponse(reply)))
                                                {
                                                    // Remember the response
                                                    _response.addString(MpM_OK_RESPONSE_);
                                                theService.updateCheckedTimeForChannel(argAsString);
                                                }
                                                else
                                                {
                                                    ODL_LOG("! (theService.processList" //####
                                                            "Response(argAsString, reply))"); //####
                                                    _response.addString(MpM_FAILED_RESPONSE_);
                                                    _response.addString("Invalid response to "
                                                                        "'list' request");
                                                }
                                            }
                                            else
                                            {
                                                ODL_LOG("! (outChannel->" //####
                                                        "writeBottle(message2, reply))"); //####
                                                _response.addString(MpM_FAILED_RESPONSE_);
                                                _response.addString("Could not write to channel");
#if defined(MpM_StallOnSendProblem)
                                                Stall();
#endif // defined(MpM_StallOnSendProblem)
                                            }
                                        }
                                        else
                                        {
                                            ODL_LOG("! (theService.processNameResponse(" //####
                                                    "argAsString, reply))"); //####
                                            _response.addString(MpM_FAILED_RESPONSE_);
                                            _response.addString("Invalid response to 'name' "
                                                                "request");
                                        }
                                    }
                                    else
                                    {
                                        ODL_LOG("! (outChannel->writeBottle(message1, " //####
                                                "reply))"); //####
                                        _response.addString(MpM_FAILED_RESPONSE_);
                                        _response.addString("Could not write to channel");
#if defined(MpM_StallOnSendProblem)
                                        Stall();
#endif // defined(MpM_StallOnSendProblem)
                                    }
#if defined(MpM_DoExplicitDisconnect)
                                if (! Utilities::NetworkDisconnectWithRetries(outChannel->name(),
                                                                              argAsString,
                                                                              STANDARD_WAIT_TIME_))
                                    {
                                        ODL_LOG("(! Utilities::NetworkDisconnectWith" //####
                                                "Retries(outChannel->name(), " //####
                                                "argAsString, STANDARD_WAIT_TIME_))"); //####
                                    }
#endif // defined(MpM_DoExplicitDisconnect)
                                }
                                else
                                {
                                    ODL_LOG("! (outChannel->addOutputWithRetries(" //####
                                            "argAsString, STANDARD_WAIT_TIME_))"); //####
                                    _response.addString(MpM_FAILED_RESPONSE_);
                                    _response.addString("Could not connect to channel");
                                    _response.addString(argAsString);
                                }
#if defined(MpM_DoExplicitClose)
                                outChannel->close();
#endif // defined(MpM_DoExplicitClose)
                            }
                            else
                            {
                                ODL_LOG("! (outChannel->openWithRetries(aName, " //####
                                        "STANDARD_WAIT_TIME_))"); //####
                                _response.addString(MpM_FAILED_RESPONSE_);
                                _response.addString("Channel could not be opened");
                            }
                            BaseChannel::RelinquishChannel(outChannel);
                        }
                        else
                        {
                            ODL_LOG("! (outChannel)");
                        }
                    }
                }
                else
                {
                    ODL_LOG("! (Endpoint::CheckEndpointName(argAsString))"); //####
                    _response.addString(MpM_FAILED_RESPONSE_);
                    _response.addString("Invalid channel name");
                }
            }
            else
            {
                ODL_LOG("! (argument.isString())"); //####
                _response.addString(MpM_FAILED_RESPONSE_);
                _response.addString("Invalid channel name");
            }
        }
        else
        {
            ODL_LOG("! (1 == restOfInput.size())"); //####
            _response.addString(MpM_FAILED_RESPONSE_);
            _response.addString("Missing channel name or extra arguments to request");
        }
        sendResponse(replyMechanism);
    }
    catch (...)
    {
        ODL_LOG("Exception caught"); //####
        throw;
    }
    ODL_OBJEXIT_B(result); //####
    return result;
} // PingRequestHandler::processRequest
bool
UnregisterRequestHandler::processRequest(const YarpString &           request,
                                         const yarp::os::Bottle &     restOfInput,
                                         const YarpString &           senderChannel,
                                         yarp::os::ConnectionWriter * replyMechanism)
{
#if (! defined(ODL_ENABLE_LOGGING_))
# if MAC_OR_LINUX_
#  pragma unused(request,senderChannel)
# endif // MAC_OR_LINUX_
#endif // ! defined(ODL_ENABLE_LOGGING_)
    ODL_OBJENTER(); //####
    ODL_S3s("request = ", request, "restOfInput = ", restOfInput.toString(), //####
            "senderChannel = ", senderChannel); //####
    ODL_P1("replyMechanism = ", replyMechanism); //####
    bool result = true;

    try
    {
        // Validate the name as a channel name
        _response.clear();
        if (1 == restOfInput.size())
        {
            yarp::os::Value argument(restOfInput.get(0));

            if (argument.isString())
            {
                YarpString argAsString(argument.toString());

                if (Endpoint::CheckEndpointName(argAsString))
                {
                    RegistryService & theService = static_cast<RegistryService &>(_service);

                    theService.reportStatusChange(argAsString,
                                                  RegistryService::kRegistryUnregisterService);
                    // Forget the information associated with the channel name
                    if (theService.removeServiceRecord(argAsString))
                    {
                        _response.addString(MpM_OK_RESPONSE_);
                        if (argAsString != MpM_REGISTRY_ENDPOINT_NAME_)
                        {
                            theService.removeCheckedTimeForChannel(argAsString);
                        }
                    }
                    else
                    {
                        ODL_LOG("! (theService.removeServiceRecord(argAsString))"); //####
                        _response.addString(MpM_FAILED_RESPONSE_);
                        _response.addString("Could not remove service");
                    }
                }
                else
                {
                    ODL_LOG("! (Endpoint::CheckEndpointName(argAsString))"); //####
                    _response.addString(MpM_FAILED_RESPONSE_);
                    _response.addString("Invalid channel name");
                }
            }
            else
            {
                ODL_LOG("! (argument.isString())"); //####
                _response.addString(MpM_FAILED_RESPONSE_);
                _response.addString("Invalid channel name");
            }
        }
        else
        {
            ODL_LOG("! (1 == restOfInput.size())"); //####
            _response.addString(MpM_FAILED_RESPONSE_);
            _response.addString("Missing channel name or extra arguments to request");
        }
        sendResponse(replyMechanism);
    }
    catch (...)
    {
        ODL_LOG("Exception caught"); //####
        throw;
    }
    ODL_OBJEXIT_B(result); //####
    return result;
} // UnregisterRequestHandler::processRequest
Example #17
0
bool ServerFrameGrabber::respond(const yarp::os::Bottle& cmd,
                                 yarp::os::Bottle& response) {
    int code = cmd.get(0).asVocab();

    IFrameGrabberControlsDC1394* fgCtrlDC1394=dynamic_cast<IFrameGrabberControlsDC1394*>(fgCtrl);

    yInfo() << "cmd is "<< cmd.toString();
    switch (code)
    {
    // first check if requests are coming from new iFrameGrabberControl2 interface and process them
    case VOCAB_FRAMEGRABBER_CONTROL2:
    {
        return ifgCtrl2_Parser.respond(cmd, response);    // I don't like all those returns everywhere!!! :-(
    } break;

    case VOCAB_SET:
        yDebug("set command received\n");

        switch(cmd.get(1).asVocab())
        {
        case VOCAB_BRIGHTNESS:
            response.addInt(int(setBrightness(cmd.get(2).asDouble())));
            return true;
        case VOCAB_EXPOSURE:
            response.addInt(int(setExposure(cmd.get(2).asDouble())));
            return true;
        case VOCAB_SHARPNESS:
            response.addInt(int(setSharpness(cmd.get(2).asDouble())));
            return true;
        case VOCAB_WHITE:
            response.addInt(int(setWhiteBalance(cmd.get(2).asDouble(),cmd.get(3).asDouble())));
            return true;
        case VOCAB_HUE:
            response.addInt(int(setHue(cmd.get(2).asDouble())));
            return true;
        case VOCAB_SATURATION:
            response.addInt(int(setSaturation(cmd.get(2).asDouble())));
            return true;
        case VOCAB_GAMMA:
            response.addInt(int(setGamma(cmd.get(2).asDouble())));
            return true;
        case VOCAB_SHUTTER:
            response.addInt(int(setShutter(cmd.get(2).asDouble())));
            return true;
        case VOCAB_GAIN:
            response.addInt(int(setGain(cmd.get(2).asDouble())));
            return true;
        case VOCAB_IRIS:
            response.addInt(int(setIris(cmd.get(2).asDouble())));
            return true;
        /*
        case VOCAB_TEMPERATURE:
            response.addInt(int(setTemperature(cmd.get(2).asDouble())));
            return true;
        case VOCAB_WHITE_SHADING:
            response.addInt(int(setWhiteShading(cmd.get(2).asDouble(),cmd.get(3).asDouble(),cmd.get(4).asDouble())));
            return true;
        case VOCAB_OPTICAL_FILTER:
            response.addInt(int(setOpticalFilter(cmd.get(2).asDouble())));
            return true;
        case VOCAB_CAPTURE_QUALITY:
            response.addInt(int(setCaptureQuality(cmd.get(2).asDouble())));
            return true;
        */
        }

        return DeviceResponder::respond(cmd,response);

    case VOCAB_GET:
        yDebug("get command received\n");

        response.addVocab(VOCAB_IS);
        response.add(cmd.get(1));

        switch(cmd.get(1).asVocab())
        {
        case VOCAB_BRIGHTNESS:
            response.addDouble(getBrightness());
            return true;
        case VOCAB_EXPOSURE:
            response.addDouble(getExposure());
            return true;
        case VOCAB_SHARPNESS:
            response.addDouble(getSharpness());
            return true;
        case VOCAB_WHITE:
            {
                double b=0.0;
                double r=0.0;

                getWhiteBalance(b,r);
                response.addDouble(b);
                response.addDouble(r);
            }
            return true;
        case VOCAB_HUE:
            response.addDouble(getHue());
            return true;
        case VOCAB_SATURATION:
            response.addDouble(getSaturation());
            return true;
        case VOCAB_GAMMA:
            response.addDouble(getGamma());
            return true;
        case VOCAB_SHUTTER:
            response.addDouble(getShutter());
            return true;
        case VOCAB_GAIN:
            response.addDouble(getGain());
            return true;
        case VOCAB_IRIS:
            response.addDouble(getIris());
            return true;
        /*
        case VOCAB_CAPTURE_QUALITY:
            response.addDouble(getCaptureQuality());
            return true;
        case VOCAB_OPTICAL_FILTER:
            response.addDouble(getOpticalFilter());
            return true;
        */
        case VOCAB_WIDTH:
            // normally, this would come from stream information
            response.addInt(width());
            return true;
        case VOCAB_HEIGHT:
            // normally, this would come from stream information
            response.addInt(height());
            return true;
        }

        return DeviceResponder::respond(cmd,response);

        //////////////////
        // DC1394 COMMANDS
        //////////////////
    default:
        if (fgCtrlDC1394) switch(code)
        {
            case VOCAB_DRHASFEA: // VOCAB_DRHASFEA 00
                response.addInt(int(fgCtrlDC1394->hasFeatureDC1394(cmd.get(1).asInt())));
                return true;
            case VOCAB_DRSETVAL: // VOCAB_DRSETVAL 01
                response.addInt(int(fgCtrlDC1394->setFeatureDC1394(cmd.get(1).asInt(),cmd.get(2).asDouble())));
                return true;
            case VOCAB_DRGETVAL: // VOCAB_DRGETVAL 02
                response.addDouble(fgCtrlDC1394->getFeatureDC1394(cmd.get(1).asInt()));
                return true;

            case VOCAB_DRHASACT: // VOCAB_DRHASACT 03
                response.addInt(int(fgCtrlDC1394->hasOnOffDC1394(cmd.get(1).asInt())));
                return true;
            case VOCAB_DRSETACT: // VOCAB_DRSETACT 04
                response.addInt(int(fgCtrlDC1394->setActiveDC1394(cmd.get(1).asInt(),(cmd.get(2).asInt()!=0))));
                return true;
            case VOCAB_DRGETACT: // VOCAB_DRGETACT 05
                response.addInt(int(fgCtrlDC1394->getActiveDC1394(cmd.get(1).asInt())));
                return true;

            case VOCAB_DRHASMAN: // VOCAB_DRHASMAN 06
                response.addInt(int(fgCtrlDC1394->hasManualDC1394(cmd.get(1).asInt())));
                return true;
            case VOCAB_DRHASAUT: // VOCAB_DRHASAUT 07
                response.addInt(int(fgCtrlDC1394->hasAutoDC1394(cmd.get(1).asInt())));
                return true;
            case VOCAB_DRHASONP: // VOCAB_DRHASONP 08
                response.addInt(int(fgCtrlDC1394->hasOnePushDC1394(cmd.get(1).asInt())));
                return true;
            case VOCAB_DRSETMOD: // VOCAB_DRSETMOD 09
                response.addInt(int(fgCtrlDC1394->setModeDC1394(cmd.get(1).asInt(),(cmd.get(2).asInt()!=0))));
                return true;
            case VOCAB_DRGETMOD: // VOCAB_DRGETMOD 10
                response.addInt(int(fgCtrlDC1394->getModeDC1394(cmd.get(1).asInt())));
                return true;
            case VOCAB_DRSETONP: // VOCAB_DRSETONP 11
                response.addInt(int(fgCtrlDC1394->setOnePushDC1394(cmd.get(1).asInt())));
                return true;
            case VOCAB_DRGETMSK: // VOCAB_DRGETMSK 12
                response.addInt(int(fgCtrlDC1394->getVideoModeMaskDC1394()));
                return true;
            case VOCAB_DRGETVMD: // VOCAB_DRGETVMD 13
                response.addInt(int(fgCtrlDC1394->getVideoModeDC1394()));
                return true;
            case VOCAB_DRSETVMD: // VOCAB_DRSETVMD 14
                response.addInt(int(fgCtrlDC1394->setVideoModeDC1394(cmd.get(1).asInt())));
                return true;
            case VOCAB_DRGETFPM: // VOCAB_DRGETFPM 15
                response.addInt(int(fgCtrlDC1394->getFPSMaskDC1394()));
                return true;
            case VOCAB_DRGETFPS: // VOCAB_DRGETFPS 16
                response.addInt(int(fgCtrlDC1394->getFPSDC1394()));
                return true;
            case VOCAB_DRSETFPS: // VOCAB_DRSETFPS 17
                response.addInt(int(fgCtrlDC1394->setFPSDC1394(cmd.get(1).asInt())));
                return true;

            case VOCAB_DRGETISO: // VOCAB_DRGETISO 18
                response.addInt(int(fgCtrlDC1394->getISOSpeedDC1394()));
                return true;
            case VOCAB_DRSETISO: // VOCAB_DRSETISO 19
                response.addInt(int(fgCtrlDC1394->setISOSpeedDC1394(cmd.get(1).asInt())));
                return true;

            case VOCAB_DRGETCCM: // VOCAB_DRGETCCM 20
                response.addInt(int(fgCtrlDC1394->getColorCodingMaskDC1394(cmd.get(1).asInt())));
                return true;
            case VOCAB_DRGETCOD: // VOCAB_DRGETCOD 21
                response.addInt(int(fgCtrlDC1394->getColorCodingDC1394()));
                return true;
            case VOCAB_DRSETCOD: // VOCAB_DRSETCOD 22
                response.addInt(int(fgCtrlDC1394->setColorCodingDC1394(cmd.get(1).asInt())));
                return true;

            case VOCAB_DRSETWHB: // VOCAB_DRSETWHB 23
                response.addInt(int(fgCtrlDC1394->setWhiteBalanceDC1394(cmd.get(1).asDouble(),cmd.get(2).asDouble())));
                return true;
            case VOCAB_DRGETWHB: // VOCAB_DRGETWHB 24
                {
                    double b,r;
                    fgCtrlDC1394->getWhiteBalanceDC1394(b,r);
                    response.addDouble(b);
                    response.addDouble(r);
                }
                return true;

            case VOCAB_DRGETF7M: // VOCAB_DRGETF7M 25
                {
                    unsigned int xstep,ystep,xdim,ydim,xoffstep,yoffstep;
                    fgCtrlDC1394->getFormat7MaxWindowDC1394(xdim,ydim,xstep,ystep,xoffstep,yoffstep);
                    response.addInt(xdim);
                    response.addInt(ydim);
                    response.addInt(xstep);
                    response.addInt(ystep);
                    response.addInt(xoffstep);
                    response.addInt(yoffstep);
                }
                return true;
            case VOCAB_DRGETWF7: // VOCAB_DRGETWF7 26
                {
                    unsigned int xdim,ydim;
                    int x0,y0;
                    fgCtrlDC1394->getFormat7WindowDC1394(xdim,ydim,x0,y0);
                    response.addInt(xdim);
                    response.addInt(ydim);
                    response.addInt(x0);
                    response.addInt(y0);
                }
                return true;
            case VOCAB_DRSETWF7: // VOCAB_DRSETWF7 27
                response.addInt(int(fgCtrlDC1394->setFormat7WindowDC1394(cmd.get(1).asInt(),cmd.get(2).asInt(),cmd.get(3).asInt(),cmd.get(4).asInt())));
                return true;
            case VOCAB_DRSETOPM: // VOCAB_DRSETOPM 28
                response.addInt(int(fgCtrlDC1394->setOperationModeDC1394(cmd.get(1).asInt()!=0)));
                return true;
            case VOCAB_DRGETOPM: // VOCAB_DRGETOPM 29
                response.addInt(fgCtrlDC1394->getOperationModeDC1394());
                return true;

            case VOCAB_DRSETTXM: // VOCAB_DRSETTXM 30
                response.addInt(int(fgCtrlDC1394->setTransmissionDC1394(cmd.get(1).asInt()!=0)));
                return true;
            case VOCAB_DRGETTXM: // VOCAB_DRGETTXM 31
                response.addInt(fgCtrlDC1394->getTransmissionDC1394());
                return true;
            /*
            case VOCAB_DRSETBAY: // VOCAB_DRSETBAY 32
                response.addInt(int(fgCtrlDC1394->setBayerDC1394(bool(cmd.get(1).asInt()))));
                return true;
            case VOCAB_DRGETBAY: // VOCAB_DRGETBAY 33
                response.addInt(fgCtrlDC1394->getBayerDC1394());
                return true;
            */
            case VOCAB_DRSETBCS: // VOCAB_DRSETBCS 34
                response.addInt(int(fgCtrlDC1394->setBroadcastDC1394(cmd.get(1).asInt()!=0)));
                return true;
            case VOCAB_DRSETDEF: // VOCAB_DRSETDEF 35
                response.addInt(int(fgCtrlDC1394->setDefaultsDC1394()));
                return true;
            case VOCAB_DRSETRST: // VOCAB_DRSETRST 36
                response.addInt(int(fgCtrlDC1394->setResetDC1394()));
                return true;
            case VOCAB_DRSETPWR: // VOCAB_DRSETPWR 37
                response.addInt(int(fgCtrlDC1394->setPowerDC1394(cmd.get(1).asInt()!=0)));
                return true;
            case VOCAB_DRSETCAP: // VOCAB_DRSETCAP 38
                response.addInt(int(fgCtrlDC1394->setCaptureDC1394(cmd.get(1).asInt()!=0)));
                return true;
            case VOCAB_DRSETBPP: // VOCAB_DRSETCAP 39
                response.addInt(int(fgCtrlDC1394->setBytesPerPacketDC1394(cmd.get(1).asInt())));
                return true;
            case VOCAB_DRGETBPP: // VOCAB_DRGETTXM 40
                response.addInt(fgCtrlDC1394->getBytesPerPacketDC1394());
                return true;
        }
    }

    return DeviceResponder::respond(cmd,response);
}
Example #18
0
// Callback handler for RPC commands (?)
bool RpcMsgHandler::respond(const yarp::os::Bottle& cmd, yarp::os::Bottle& reply)
{
    bool ok = true;
    bool commandUnderstood = true; // is the command recognized?

    if (caller->verbose())
        printf("command received: %s\n", cmd.toString().c_str());

    int code = cmd.get(0).asVocab();

    switch (code)
    {
        case VOCAB_GET:
        {
            double dtmp  = 0.0;
            double dtmp2 = 0.0;
            reply.addVocab(VOCAB_IS);
            reply.add(cmd.get(1));

            switch(cmd.get(1).asVocab())
            {
                case VOCAB_AXES:
                {
                    int axes;
                    caller->getAxes(axes);
                    reply.addInt(axes);
                }
                break;

                case VOCAB_DEBUG_PARAMETER:
                {
                    int j     = cmd.get(2).asInt();
                    int index = cmd.get(3).asInt();
                    ok = caller->getDebugParameter(j, index, &dtmp);
                    reply.addInt(j);
                    reply.addInt(index);
                    reply.addDouble(dtmp);
                }
                break;

                case VOCAB_GENERIC_PARAMETER:
                {
                    int j     = cmd.get(2).asInt();
                    int param = cmd.get(3).asInt();
                    ok = caller->getParameter(j, param, &dtmp);
                    reply.addInt(j);
                    reply.addInt(param);
                    reply.addDouble(dtmp);
                }
                break;

                default:
                {
                    commandUnderstood = false;
                    std::cout << "Debug Interface 1: command not understood! received " << cmd.toString().c_str() << std::endl;
                }
                break;
            }
        }
        break;      // case VOCAB_GET

        case VOCAB_SET:
        {
            switch(cmd.get(1).asVocab())
            {
                case VOCAB_GENERIC_PARAMETER:
                {
                    int j     = cmd.get(2).asInt();
                    int param = cmd.get(3).asInt();
                    double val   = cmd.get(4).asDouble();
                    ok = caller->setParameter(j, param, val);
                }
                break;

                case VOCAB_DEBUG_PARAMETER:
                {
                    int j     = cmd.get(2).asInt();
                    int index = cmd.get(3).asInt();
                    double val   = cmd.get(4).asDouble();
                    ok = caller->setDebugParameter(j, index, val);
                }
                break;

                default:
                {
                    commandUnderstood = false;
                    std::cout << "Debug Interface 2: command not understood! received " << cmd.toString().c_str() << std::endl;
                }
                break;
            }
        }
        break;      // case VOCAB_SET

        default:
        {
            commandUnderstood = false;
            std::cout << "Debug Interface 3: command not understood! received " << cmd.toString().c_str() << std::endl;
        }
        break;

    } //switch code

    if (!commandUnderstood)
    {
        ok = DeviceResponder::respond(cmd,reply);
    }


    if (!ok)
    {
        // failed thus send only a VOCAB back.
        reply.clear();
        reply.addVocab(VOCAB_FAILED);
    }
    else
        reply.addVocab(VOCAB_OK);

    return ok;
}
Example #19
0
bool Implement_DepthVisualParams_Parser::respond(const yarp::os::Bottle& cmd, yarp::os::Bottle& response)
{
    bool ret = false;
    response.clear();
    if(!iDepthVisual)
    {
        yError() << "Depth Visual parameter Parser has not been correctly configured. IDepthVisualParams interface is not valid";
        response.addVocab(VOCAB_FAILED);
        return false;
    }

    int code = cmd.get(0).asVocab();
    if(code != VOCAB_DEPTH_VISUAL_PARAMS)
    {
        yError() << "Depth Visual Params Parser received a command not belonging to this interface. Required interface was " << yarp::os::Vocab::decode(code);
        response.addVocab(VOCAB_FAILED);
        return false;
    }

    switch (cmd.get(1).asVocab())
    {
        case VOCAB_GET:
        {
            switch(cmd.get(2).asVocab())
            {
                case VOCAB_HEIGHT:
                {
                    response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
                    response.addVocab(VOCAB_HEIGHT);
                    response.addVocab(VOCAB_IS);
                    response.addInt(iDepthVisual->getDepthHeight());
                }
                break;

                case VOCAB_WIDTH:
                {
                    response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
                    response.addVocab(VOCAB_WIDTH);
                    response.addVocab(VOCAB_IS);
                    response.addInt(iDepthVisual->getDepthWidth());
                }
                break;

                case VOCAB_FOV:
                {
                    double hFov, vFov;
                    ret = iDepthVisual->getDepthFOV(hFov, vFov);
                    if(ret)
                    {
                        response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
                        response.addVocab(VOCAB_FOV);
                        response.addVocab(VOCAB_IS);
                        response.addDouble(hFov);
                        response.addDouble(vFov);
                    }
                    else
                        response.addVocab(VOCAB_FAILED);
                }
                break;

                case VOCAB_INTRINSIC_PARAM:
                {
                    yarp::os::Property params;
                    ret = iDepthVisual->getDepthIntrinsicParam(params);
                    if(ret)
                    {
                        yarp::os::Bottle params_b;
                        response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
                        response.addVocab(VOCAB_INTRINSIC_PARAM);
                        response.addVocab(VOCAB_IS);
                        Property::copyPortable(params, params_b);  // will it really work??
                        response.append(params_b);
                    }
                    else
                    {
                        response.addVocab(VOCAB_FAILED);
                    }
                }
                break;

                case VOCAB_ACCURACY:
                {
                    response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
                    response.addVocab(VOCAB_ACCURACY);
                    response.addVocab(VOCAB_IS);
                    response.addDouble(iDepthVisual->getDepthAccuracy());
                }
                break;

                case VOCAB_CLIP_PLANES:
                {
                    double nearPlane, farPlane;
                    iDepthVisual->getDepthClipPlanes(nearPlane, farPlane);
                    response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
                    response.addVocab(VOCAB_CLIP_PLANES);
                    response.addVocab(VOCAB_IS);
                    response.addDouble(nearPlane);
                    response.addDouble(farPlane);
                }
                break;

                case VOCAB_MIRROR:
                {
                    bool mirror;
                    ret = iDepthVisual->getDepthMirroring(mirror);
                    if(ret)
                    {
                        response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
                        response.addVocab(VOCAB_MIRROR);
                        response.addVocab(VOCAB_IS);
                        response.addInt(mirror);
                    }
                    else
                        response.addVocab(VOCAB_FAILED);
                }
                break;

                default:
                {
                    yError() << "Depth Visual Parameter interface parser received am unknown GET command. Command is " << cmd.toString();
                    response.addVocab(VOCAB_FAILED);
                    ret = false;
                }
                break;
            }
        }
        break;

        case VOCAB_SET:
        {
            switch(cmd.get(2).asVocab())
            {
                case VOCAB_RESOLUTION:
                {
                    ret = iDepthVisual->setDepthResolution(cmd.get(3).asInt(), cmd.get(4).asInt());
                    response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
                    response.addVocab(VOCAB_SET);
                    response.addInt(ret);
                }
                break;

                case VOCAB_FOV:
                {
                    ret = iDepthVisual->setDepthFOV(cmd.get(3).asDouble(), cmd.get(4).asDouble());
                    response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
                    response.addVocab(VOCAB_SET);
                    response.addInt(ret);
                }
                break;

                case VOCAB_ACCURACY:
                {
                    ret = iDepthVisual->setDepthAccuracy(cmd.get(3).asDouble());
                    response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
                    response.addVocab(VOCAB_SET);
                    response.addInt(ret);
                }
                break;

                case VOCAB_CLIP_PLANES:
                {
                    ret = iDepthVisual->setDepthClipPlanes(cmd.get(3).asDouble(), cmd.get(4).asDouble());
                    response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
                    response.addVocab(VOCAB_SET);
                    response.addInt(ret);
                }
                break;

                case VOCAB_INTRINSIC_PARAM:
                {
                    yarp::os::Property params;
                    ret = iDepthVisual->getDepthIntrinsicParam(params);
                    if(ret)
                    {
                        yarp::os::Bottle params_b;
                        response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
                        response.addVocab(VOCAB_INTRINSIC_PARAM);
                        response.addVocab(VOCAB_IS);
                        Property::copyPortable(params, params_b);  // will it really work??
                        response.append(params_b);
                    }
                    else
                        response.addVocab(VOCAB_FAILED);
                }
                break;

                case VOCAB_MIRROR:
                {
                    ret = iDepthVisual->setDepthMirroring(cmd.get(3).asBool());
                    response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
                    response.addVocab(VOCAB_SET);
                    response.addInt(ret);
                }
                break;

                default:
                {
                    yError() << "Rgb Visual Parameter interface parser received am unknown SET command. Command is " << cmd.toString();
                    response.addVocab(VOCAB_FAILED);
                    ret = false;
                }
                break;
            }
        }
        break;

        default:
        {
            yError() << "Rgb Visual parameter interface Parser received a malformed request. Command should either be 'set' or 'get', received " << cmd.toString();
            response.addVocab(VOCAB_FAILED);
            ret = false;
        }
        break;
    }
    return ret;
}
bool FrameGrabberControls2_Parser::respond(const yarp::os::Bottle& cmd, yarp::os::Bottle& response)
{
    bool ok = false;
    int action = cmd.get(1).asVocab();
    int param  = cmd.get(2).asVocab();

//     yTrace() << "cmd received\n\t" << cmd.toString().c_str();


    if(!fgCtrl2)
    {
        yError() << " Selected camera device has no IFrameGrabberControl2 interface";
        return false;
    }

    response.clear();

    switch (action)
    {
        case VOCAB_HAS:
        {
            response.addVocab(VOCAB_FRAMEGRABBER_CONTROL2);
            response.addVocab(VOCAB_HAS);
            response.addVocab(VOCAB_FEATURE);
            response.addInt(param);

            switch (param)
            {
                case VOCAB_FEATURE:
                {
                    bool _hasFeat;
                    ok = fgCtrl2->hasFeature(cmd.get(3).asInt(), &_hasFeat);
                    response.addInt(_hasFeat);
                } break;

                case VOCAB_ONOFF:
                {
                    bool _hasOnOff;
                    ok = fgCtrl2->hasOnOff(cmd.get(3).asInt(), &_hasOnOff);
                    response.addInt(_hasOnOff);
                } break;

                case VOCAB_AUTO:
                {
                    bool _hasAuto;
                    ok = fgCtrl2->hasAuto(cmd.get(3).asInt(), &_hasAuto);
                    response.addInt(_hasAuto);
                } break;

                case VOCAB_MANUAL:
                {
                    bool _hasManual;
                    ok = fgCtrl2->hasManual(cmd.get(3).asInt(), &_hasManual);
                    response.addInt(_hasManual);
                } break;

                case VOCAB_ONEPUSH:
                {
                    bool _hasOnePush;
                    ok = fgCtrl2->hasOnePush(cmd.get(3).asInt(), &_hasOnePush);
                    response.addInt(_hasOnePush);
                } break;

                default:
                {
                    yError() << "Unknown command 'HAS " << Vocab::decode(param) << "' received on IFrameGrabber2 interface";
                    response.clear();
                    ok = false;
                } break;
            } break; // end switch (param)

        } break; // end VOCAB_HAS

        case VOCAB_SET:
        {
            switch (param)
            {
                case VOCAB_FEATURE:
                {
                    ok = fgCtrl2->setFeature(cmd.get(3).asInt(), cmd.get(4).asDouble());
                } break;

                case VOCAB_FEATURE2:
                {
                    ok = fgCtrl2->setFeature(cmd.get(3).asInt(), cmd.get(4).asDouble(), cmd.get(5).asDouble());
                } break;

                case VOCAB_ACTIVE:
                {
                    ok = fgCtrl2->setActive(cmd.get(3).asInt(), cmd.get(4).asInt());
                } break;

                case VOCAB_MODE:
                {
                    ok = fgCtrl2->setMode(cmd.get(3).asInt(), (FeatureMode) cmd.get(4).asInt());
                } break;

                case VOCAB_ONEPUSH:
                {
                    ok = fgCtrl2->setOnePush(cmd.get(3).asInt());
                } break;

                default:
                {
                    yError() << "Unknown command 'SET " << Vocab::decode(param) << "' received on IFrameGrabber2 interface";
                    response.clear();
                    ok = false;
                }
            } break; // end switch (param)

        } break; // end VOCAB_SET

        case VOCAB_GET:
        {
            response.addVocab(VOCAB_FRAMEGRABBER_CONTROL2);
            response.addVocab(param);
            response.addVocab(VOCAB_IS);
            switch (param)
            {
                case VOCAB_CAMERA_DESCRIPTION:
                {
                    CameraDescriptor camera;
                    ok = fgCtrl2->getCameraDescription(&camera);
                    response.addInt(camera.busType);
                    response.addString(camera.deviceDescription);
                    yDebug() << "Response is " << response.toString();
                } break;

                case VOCAB_FEATURE:
                {
                    double value;
                    ok = fgCtrl2->getFeature(cmd.get(3).asInt(), &value);
                    response.addDouble(value);
                } break;

                case VOCAB_FEATURE2:
                {
                    double value1, value2;
                    ok = fgCtrl2->getFeature(cmd.get(3).asInt(), &value1, &value2);
                    response.addDouble(value1);
                    response.addDouble(value2);
                } break;

                case VOCAB_ACTIVE:
                {
                    bool _isActive;
                    ok = fgCtrl2->getActive(cmd.get(3).asInt(), &_isActive);
                    response.addInt(_isActive);
                } break;

                case VOCAB_MODE:
                {
                    FeatureMode _mode;
                    ok = fgCtrl2->getMode(cmd.get(3).asInt(), &_mode);
                    response.addInt(_mode);
                } break;

                default:
                {
                    yError() << "Unknown command 'GET " << Vocab::decode(param) << "' received on IFrameGrabber2 interface";
                    response.clear();
                    ok = false;
                }

            } break; // end switch (param)

        } break; // end VOCAB_GET
    }
//     yTrace() << "response is\n\t" << response.toString().c_str();
    return ok;
}
bool
UnrealOutputViconInputHandler::handleInput(const yarp::os::Bottle &     input,
                                           const YarpString &           senderChannel,
                                           yarp::os::ConnectionWriter * replyMechanism,
                                           const size_t                 numBytes)
{
#if (! defined(OD_ENABLE_LOGGING_))
# if MAC_OR_LINUX_
#  pragma unused(senderChannel,replyMechanism)
# endif // MAC_OR_LINUX_
#endif // ! defined(OD_ENABLE_LOGGING_)
    ODL_OBJENTER(); //####
    ODL_S2s("senderChannel = ", senderChannel, "got ", input.toString()); //####
    ODL_P1("replyMechanism = ", replyMechanism); //####
    ODL_I1("numBytes = ", numBytes); //####
    bool result = true;

    try
    {
        if (_owner.isActive())
        {
            if (INVALID_SOCKET == _outSocket)
            {
                cerr << "invalid socket" << endl; //!!!!
            }
            else
            {
                int numSubjects = input.size();

                if (0 < numSubjects)
                {
                    bool              okSoFar = true;
#if (! defined(MpM_UseCustomStringBuffer))
                    std::stringstream outBuffer;
#endif // ! defined(MpM_UseCustomStringBuffer)

//                    cerr << "# subjects = " << numSubjects << endl; //!!!!
#if defined(MpM_UseCustomStringBuffer)
                    _outBuffer.reset().addLong(numSubjects).addString(LINE_END_);
#else // ! defined(MpM_UseCustomStringBuffer)
                    outBuffer << numSubjects << LINE_END_;
#endif // ! defined(MpM_UseCustomStringBuffer)
                    for (int ii = 0; okSoFar && (numSubjects > ii); ++ii)
                    {
                        yarp::os::Value & aValue = input.get(ii);

                        if (aValue.isList())
                        {
                            yarp::os::Bottle * asBottle = aValue.asList();

                            if (asBottle)
                            {
                                if (2 == asBottle->size())
                                {
                                    yarp::os::Value & firstValue = asBottle->get(0);

                                    if (firstValue.isString())
                                    {
                                        YarpString        subjName = firstValue.asString();
                                        yarp::os::Value & secondValue = asBottle->get(1);

                                        if (secondValue.isDict())
                                        {
                                            yarp::os::Property * segments = secondValue.asDict();

//                                            cerr << subjName.c_str() << endl; //!!!!
                                            if (segments)
                                            {
                                                YarpString       segmentsAsString =
                                                                            segments->toString();
//                                                cerr << ":" << segments->toString() << //!!!!
//                                                        ":" << endl; //!!!!
                                                yarp::os::Bottle segmentsAsBottle =
                                                                                segmentsAsString;

                                                cerr << ":" << segmentsAsBottle.size() << ":" <<
                                                        segmentsAsBottle.toString() << ":" <<
                                                        endl; //!!!!
#if defined(MpM_UseCustomStringBuffer)
                                                _outBuffer.addString(subjName).addTab();
                                                _outBuffer.addLong(segmentsAsBottle.size()).
                                                    addString("\t0" LINE_END_);
                                                okSoFar = dumpSegments(_outBuffer, segmentsAsBottle,
                                                                       _scale);
#else // ! defined(MpM_UseCustomStringBuffer)
                                                outBuffer << subjName.c_str() << "\t" <<
                                                            segmentsAsBottle.size() <<
                                                            "\t0" LINE_END_;
                                                okSoFar = dumpSegments(outBuffer, segmentsAsBottle,
                                                                       _scale);
#endif // ! defined(MpM_UseCustomStringBuffer)
                                            }
                                            else
                                            {
                                                cerr << "bad segments pointer" << endl; //!!!!
                                                okSoFar = false;
                                            }
                                        }
                                        else if (secondValue.isList())
                                        {
                                            yarp::os::Bottle * asList = secondValue.asList();

                                            if (asList)
                                            {
                                                yarp::os::Property segments;

                                                if (ListIsReallyDictionary(*asList, segments))
                                                {
                                                    YarpString       segmentsAsString =
                                                                                segments.toString();
//                                                    cerr << ":" << segments->toString() << ":" <<
//                                                            endl; //!!!!
                                                    yarp::os::Bottle segmentsAsBottle =
                                                                                segmentsAsString;

                                                    cerr << ":" << segmentsAsBottle.size() << ":" <<
                                                            segmentsAsBottle.toString() << ":" <<
                                                            endl; //!!!!
#if defined(MpM_UseCustomStringBuffer)
                                                    _outBuffer.addString(subjName).addTab();
                                                    _outBuffer.addLong(segmentsAsBottle.size()).
                                                        addString("\t0" LINE_END_);
                                                    okSoFar = dumpSegments(_outBuffer,
                                                                           segmentsAsBottle,
                                                                           _scale);
#else // ! defined(MpM_UseCustomStringBuffer)
                                                    outBuffer << subjName.c_str() << "\t" <<
                                                                segmentsAsBottle.size() <<
                                                                "\t0" LINE_END_;
                                                    okSoFar = dumpSegments(outBuffer,
                                                                           segmentsAsBottle,
                                                                           _scale);
#endif // ! defined(MpM_UseCustomStringBuffer)
                                                }
                                                else
                                                {
                                                    cerr << "not a dictionary" << endl; //!!!!
                                                    okSoFar = false;
                                                }
                                            }
                                            else
                                            {
                                                cerr << "bad segments pointer" << endl; //!!!!
                                                okSoFar = false;
                                            }
                                        }
                                        else
                                        {
                                            cerr << "not a dictionary" << endl; //!!!!
                                            okSoFar = false;
                                        }
                                    }
                                    else
                                    {
                                        cerr << "not a string" << endl; //!!!!
                                        okSoFar = false;
                                    }
                                }
                                else
                                {
                                    cerr << "not 2 pieces in list" << endl; //!!!!
                                    okSoFar = false;
                                }
                            }
                            else
                            {
                                cerr << "bad subject pointer" << endl; //!!!!
                                okSoFar = false;
                            }
                        }
                        else
                        {
                            cerr << "subject is not a list" << endl; //!!!!
                            okSoFar = false;
                        }
                    }
                    if (okSoFar)
                    {
#if defined(MpM_UseCustomStringBuffer)
                        _outBuffer.addString("END" LINE_END_);
#else // ! defined(MpM_UseCustomStringBuffer)
                        outBuffer << "END" LINE_END_;
#endif // ! defined(MpM_UseCustomStringBuffer)
                        const char * outString;
                        size_t       outLength;
#if (! defined(MpM_UseCustomStringBuffer))
                        std::string  buffAsString(outBuffer.str());
#endif // ! defined(MpM_UseCustomStringBuffer)

#if defined(MpM_UseCustomStringBuffer)
                        outString = _outBuffer.getString(outLength);
#else // ! defined(MpM_UseCustomStringBuffer)
                        outString = buffAsString.c_str();
                        outLength = buffAsString.length();
#endif // ! defined(MpM_UseCustomStringBuffer)
                        if (outString && outLength)
                        {
                            int retVal = send(_outSocket, outString,
                                              static_cast<int>(outLength), 0);

                            cerr << "send--> " << retVal << endl; //!!!!
                            if (0 > retVal)
                            {
                                _owner.deactivateConnection();
                            }
                            else
                            {
                                SendReceiveCounters toBeAdded(0, 0, retVal, 1);

                                _owner.incrementAuxiliaryCounters(toBeAdded);
                            }
                        }
                    }
                }
                else
                {
                    cerr << "no subjects" << endl; //!!!!
                }
            }
        }
    }
    catch (...)
    {
        ODL_LOG("Exception caught"); //####
        throw;
    }
    ODL_OBJEXIT_B(result); //####
    return result;
} // UnrealOutputViconInputHandler::handleInput
bool
MatchRequestHandler::processRequest(const YarpString &           request,
                                    const yarp::os::Bottle &     restOfInput,
                                    const YarpString &           senderChannel,
                                    yarp::os::ConnectionWriter * replyMechanism)
{
#if (! defined(ODL_ENABLE_LOGGING_))
# if MAC_OR_LINUX_
#  pragma unused(request,senderChannel)
# endif // MAC_OR_LINUX_
#endif // ! defined(ODL_ENABLE_LOGGING_)
    ODL_OBJENTER(); //####
    ODL_S3s("request = ", request, "restOfInput = ", restOfInput.toString(), //####
            "senderChannel = ", senderChannel); //####
    ODL_P1("replyMechanism = ", replyMechanism); //####
    bool result = true;

    try
    {
        // We are expecting an integer and a string as the parameter
        _response.clear();
        if (2 == restOfInput.size())
        {
            yarp::os::Value condition(restOfInput.get(0));
            yarp::os::Value argument(restOfInput.get(1));

            if (condition.isInt() && argument.isString())
            {
                int        conditionAsInt = condition.asInt();
                YarpString argAsString(argument.toString());

                ODL_S1s("argAsString <- ", argAsString); //####
                size_t                    endPos;
                Parser::MatchExpression * matcher =
                                        Parser::MatchExpression::CreateMatcher(argAsString,
                                                                               argAsString.length(),
                                                                               0, endPos,
                                                                               _validator);

                if (matcher)
                {
                    ODL_LOG("(matcher)"); //####
                    // Hand off the processing to the Registry Service. First, put the 'OK' response
                    // in the output buffer, as we have successfully parsed the request.
                    _response.addString(MpM_OK_RESPONSE_);
                    if (! static_cast<RegistryService &>(_service).processMatchRequest(matcher,
                                                                               0 != conditionAsInt,
                                                                                       _response))
                    {
                        ODL_LOG("(! static_cast<RegistryService &>(_service)." //####
                                "processMatchRequest(matcher, 0 != conditionAsInt, " //####
                                "_response))"); //####
                        _response.clear();
                        _response.addString(MpM_FAILED_RESPONSE_);
                        _response.addString("Invalid criteria");
                    }
                    delete matcher;
                }
                else
                {
                    ODL_LOG("! (matcher)"); //####
                    _response.addString(MpM_FAILED_RESPONSE_);
                    _response.addString("Invalid criteria");
                }
            }
            else
            {
                ODL_LOG("! (argument.isString())"); //####
                _response.addString(MpM_FAILED_RESPONSE_);
                _response.addString("Invalid criteria");
            }
        }
        else
        {
            ODL_LOG("! (1 == restOfInput.size())"); //####
            _response.addString(MpM_FAILED_RESPONSE_);
            _response.addString("Missing criteria or extra arguments to request");
        }
        sendResponse(replyMechanism);
    }
    catch (...)
    {
        ODL_LOG("Exception caught"); //####
        throw;
    }
    ODL_OBJEXIT_B(result); //####
    return result;
} // MatchRequestHandler::processRequest
Example #23
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;
}
bool GazeboYarpControlBoardDriver::setRemoteVariable(yarp::os::ConstString key, const yarp::os::Bottle& val)
{
    std::string s1 = val.toString();
    yarp::os::Bottle* bval = val.get(0).asList();
    if (bval == 0)
    {
        yWarning("setRemoteVariable(): Protocol error %s", s1.c_str());
        return false;
    }

    std::string s2 = bval->toString();

    if (key == "hardwareDamping")
    {
        for (size_t i = 0; i < m_numberOfJoints; i++)
        {
            double value = bval->get(i).asDouble();
            m_jointPointers[i]->SetDamping(0,value);
        }
        return true;
    }
    if (key == "hardwareFriction")
    {
        for (size_t i = 0; i < m_numberOfJoints; i++)
        {
            double value = bval->get(i).asDouble();
            m_jointPointers[i]->SetParam("friction",0,value);
        }
        return true;
    }
    if (key == "hardwareEffortLimit")
    {
        for (size_t i = 0; i < m_numberOfJoints; i++)
        {
            double value = bval->get(i).asDouble();
            m_jointPointers[i]->SetEffortLimit(0,value);
        }
        return true;
    }
    if (key == "hardwareVelocityLimit")
    {
        for (size_t i = 0; i < m_numberOfJoints; i++)
        {
            double value = bval->get(i).asDouble();
            m_jointPointers[i]->SetVelocityLimit(0,value);
        }
        return true;
    }
    if (key == "hardwareHiStop")
    {
        for (size_t i = 0; i < m_numberOfJoints; i++)
        {
            double value = bval->get(i).asDouble();
            m_jointPointers[i]->SetUpperLimit(0,convertUserToGazebo(i,value));
        }
        return true;
    }
    if (key == "hardwareLowStop")
    {
        for (size_t i = 0; i < m_numberOfJoints; i++)
        {
            double value = bval->get(i).asDouble();
            m_jointPointers[i]->SetLowerLimit(0,convertUserToGazebo(i,value));
        }
        return true;
    }
    if (key == "yarp_jntMaxVel")
    {
        for (size_t i = 0; i < m_numberOfJoints; i++)
        {
            double value = bval->get(i).asDouble();
            setVelLimits(i,0,value);
        }
        return true;
    }
    if (key == "yarp_jntMaxPos")
    {
        for (size_t i = 0; i < m_numberOfJoints; i++)
        {
            double value = bval->get(i).asDouble();
            double t_max=0, t_min=0;
            getLimits(i,&t_min,&t_max);
            setLimits(i,t_min,value);
        }
        return true;
    }
    if (key == "yarp_jntMinPos")
    {
        for (size_t i = 0; i < m_numberOfJoints; i++)
        {
            double value = bval->get(i).asDouble();
            double t_max=0, t_min=0;
            getLimits(i,&t_min,&t_max);
            setLimits(i,value,t_max);
        }
        return true;
    }
    if (key == "yarp_kPWM")
    {
        for (size_t i = 0; i < m_numberOfJoints; i++)
        {
            double value = bval->get(i).asDouble();
            m_kPWM[i]=value;
        }
        return true;
    }
    if (key == "SHORTCUT_all_pos_kp")
    {
        for (size_t i = 0; i < m_numberOfJoints; i++)
        {
            yarp::dev::Pid tmp_pid;
            getPid(VOCAB_PIDTYPE_POSITION, i,&tmp_pid);
            tmp_pid.kp = bval->get(i).asDouble();
            setPid(VOCAB_PIDTYPE_POSITION, i,tmp_pid);
        }
        return true;
    }
    if (key == "SHORTCUT_all_pos_kd")
    {
        for (size_t i = 0; i < m_numberOfJoints; i++)
        {
            yarp::dev::Pid tmp_pid;
            getPid(VOCAB_PIDTYPE_POSITION, i,&tmp_pid);
            tmp_pid.kd = bval->get(i).asDouble();
            setPid(VOCAB_PIDTYPE_POSITION, i,tmp_pid);
        }
        return true;
    }
    if (key == "SHORTCUT_all_pos_ki")
    {
        for (size_t i = 0; i < m_numberOfJoints; i++)
        {
            yarp::dev::Pid tmp_pid;
            getPid(VOCAB_PIDTYPE_POSITION, i,&tmp_pid);
            tmp_pid.ki = bval->get(i).asDouble();
            setPid(VOCAB_PIDTYPE_POSITION, i,tmp_pid);
        }
        return true;
    }
    if (key == "VelocityTimeout")
    {
        for (size_t i = 0; i < m_numberOfJoints; i++)
        {
            double value = bval->get(i).asDouble();
            m_velocity_watchdog[i]->modifyDuration(value);
        }
        return true;
    }

    yWarning("setRemoteVariable(): Unknown variable %s", key.c_str());
    return false;
}
bool
RandomNumberInputHandler::handleInput(const yarp::os::Bottle &     input,
                                      const YarpString &           senderChannel,
                                      yarp::os::ConnectionWriter * replyMechanism,
                                      const size_t                 numBytes)
{
#if (! defined(ODL_ENABLE_LOGGING_))
# if MAC_OR_LINUX_
#  pragma unused(senderChannel,replyMechanism,numBytes)
# endif // MAC_OR_LINUX_
#endif // ! defined(ODL_ENABLE_LOGGING_)
    ODL_OBJENTER(); //####
    ODL_S2s("senderChannel = ", senderChannel, "got ", input.toString()); //####
    ODL_P1("replyMechanism = ", replyMechanism); //####
    ODL_I1("numBytes = ", numBytes); //####
    bool result = true;

    try
    {
        if (0 < input.size())
        {
            BaseChannel *        theOutput = _shared.getOutput();
            RandomNumberClient * theClient = (RandomNumberClient *) _shared.getClient();

            if (theClient && theOutput)
            {
                int             count;
                yarp::os::Value argValue(input.get(0));

                if (argValue.isInt())
                {
                    count = argValue.asInt();
                }
                else if (argValue.isDouble())
                {
                    count = static_cast<int>(argValue.asDouble());
                }
                else
                {
                    count = 1;
                }
                if (0 > count)
                {
                    count = 1;
                }
                if (1 < count)
                {
                    DoubleVector randResult;

                    if (theClient->getRandomNumbers(count, randResult))
                    {
                        yarp::os::Bottle message;

                        if (0 < randResult.size())
                        {
                            for (DoubleVector::const_iterator it(randResult.begin());
                                 randResult.end() != it; ++it)
                            {
                                message.addDouble(*it);
                            }
                        }
                        _shared.lock();
                        if (! theOutput->write(message))
                        {
                            ODL_LOG("(! theOutput->write(message))"); //####
#if defined(MpM_StallOnSendProblem)
                            Stall();
#endif // defined(MpM_StallOnSendProblem)
                        }
                        _shared.unlock();
                    }
                    else
                    {
                        ODL_LOG("! (theClient->getRandomNumbers(count, randResult))"); //####
                    }
                }
                else if (0 < count)
                {
                    double randResult;

                    if (theClient->getOneRandomNumber(randResult))
                    {
                        yarp::os::Bottle message;

                        message.addDouble(randResult);
                        _shared.lock();
                        if (! theOutput->write(message))
                        {
                            ODL_LOG("(! theOutput->write(message))"); //####
#if defined(MpM_StallOnSendProblem)
                            Stall();
#endif // defined(MpM_StallOnSendProblem)
                        }
                        _shared.unlock();
                    }
                    else
                    {
                        ODL_LOG("! (theClient->getOneRandomNumber(randResult))"); //####
                    }
                }
                else
                {
                    _shared.deactivate();
                }
            }
        }
    }
    catch (...)
    {
        ODL_LOG("Exception caught"); //####
        throw;
    }
    ODL_OBJEXIT_B(result); //####
    return result;
} // RandomNumberInputHandler::handleInput
Example #26
0
bool NameServiceOnTriples::apply(yarp::os::Bottle& cmd,
                                 yarp::os::Bottle& reply,
                                 yarp::os::Bottle& event,
                                 const yarp::os::Contact& remote) {
    ConstString key = cmd.get(0).toString();
    ConstString prefix = " * ";

    access.wait();
    if (key=="register") {
        lastRegister = cmd.get(1).asString().c_str();
    } else if (key=="set") {
        if (cmd.get(1).asString()==lastRegister.c_str()) {
            prefix = "   + ";
        }
    } else {
        lastRegister = "";
    }
    if (!silent) {
        printf("%s%s\n",
               prefix.c_str(),
               cmd.toString().c_str());
    }
    access.post();

    TripleSource& mem = *db;
    //mem.begin();
    mem.reset();
    reply.clear();
    NameTripleState act(cmd,reply,event,remote,mem);

    if (cmd.check("format")) {
        if (cmd.find("format")=="json") {
            act.bottleMode = true;
        }
    }

    if (key == "NAME_SERVER") {
        cmd = cmd.tail();
        key = cmd.get(0).asString();
    }
    if (key == "bot") {
        act.bottleMode = true;
        cmd = cmd.tail();
        key = cmd.get(0).asString();
    }

    if (key=="register") {
        return cmdRegister(act);
    } else if (key=="unregister") {
        return cmdUnregister(act);
    } else if (key=="query") {
        return cmdQuery(act);
    } else if (key=="list") {
        return cmdList(act);
    } else if (key=="set") {
        return cmdSet(act);
    } else if (key=="get") {
        return cmdGet(act);
    } else if (key=="check") {
        return cmdCheck(act);
    } else if (key=="route") {
        return cmdRoute(act);
    } else if (key=="gc") {
        return cmdGc(act);
    } else if (key=="help") {
        return cmdHelp(act);
    } else {
        // not understood
        act.reply.addString("old");
    }
    //mem.end();

    return true;
}
Example #27
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;
 }