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
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; }
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; }
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
/* * 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; }
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; } }
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
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); }
// 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; }
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
/* 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
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; }
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; }