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;
}