bool JoypadCtrlParser::respond(const yarp::os::Bottle& cmd, yarp::os::Bottle& response)
{
bool ret;
ret = false;
if(cmd.get(0).asVocab() != VOCAB_IJOYPADCTRL || !cmd.get(1).isVocab() || !cmd.get(2).isVocab() || !cmd.get(3).isVocab())
{
response.addVocab(VOCAB_FAILED);
return ret;
}
if(cmd.get(1).asVocab() == VOCAB_GET)
{
int toGet;
toGet = cmd.get(2).asVocab();
if(cmd.get(3).asVocab() == VOCAB_COUNT)
{
if(countGetters.find(toGet) != countGetters.end())
{
unsigned int count;
getcountmethod getter;
getter = countGetters[toGet];
if((device->*getter)(count))
{
response.addVocab(VOCAB_OK);
response.addInt32(count);
ret = true;
}
}
else if (toGet == VOCAB_STICKDOF && cmd.get(4).isInt32())
{
unsigned int count;
if (device->getStickDoF(cmd.get(4).asInt32(), count))
{
response.addVocab(VOCAB_OK);
response.addInt32(count);
ret = true;
}
else
{
response.addVocab(VOCAB_FAILED);
ret = false;
}
}
else
{
response.addVocab(VOCAB_FAILED);
ret = false;
}
}
else if(cmd.get(3).asVocab() == VOCAB_VALUE)
{
switch (cmd.get(2).asVocab()) {
case VOCAB_BUTTON:
{
float value;
if(cmd.get(4).isInt32() && device->getButton(cmd.get(4).asInt32(), value))
{
response.addVocab(VOCAB_OK);
response.addFloat64(value);
ret = true;
}
break;
}
case VOCAB_AXIS:
{
double value;
if(cmd.get(4).isInt32() && device->getAxis(cmd.get(4).asInt32(), value))
{
response.addVocab(VOCAB_OK);
response.addFloat64(value);
ret = true;
}
break;
}
case VOCAB_STICK:
{
if(cmd.get(4).isVocab())
{
yarp::sig::Vector frame;
auto mode = cmd.get(4).asVocab() == VOCAB_CARTESIAN ? yarp::dev::IJoypadController::JypCtrlcoord_CARTESIAN : yarp::dev::IJoypadController::JypCtrlcoord_POLAR;
if(cmd.get(5).isInt32() && device->getStick(cmd.get(5).asInt32(), frame, mode))
{
response.addVocab(VOCAB_OK);
for(size_t i = 0; i < frame.size(); ++i)
{
response.addFloat64(frame[i]);
}
ret = true;
}
}
break;
}
case VOCAB_STICKDOF:
{
unsigned int dofCount;
if(cmd.get(5).isInt32() && device->getStickDoF(cmd.get(5).asInt32(), dofCount))
{
response.addVocab(VOCAB_OK);
response.addInt32(dofCount);
ret = true;
}
break;
}
case VOCAB_TOUCH:
{
yarp::sig::Vector pos;
unsigned int id;
id = cmd.get(4).asInt32();
if(cmd.get(4).isInt32() && device->getTouch(id, pos))
{
response.addVocab(VOCAB_OK);
for(size_t i = 0; i < pos.size(); ++i)
{
response.addFloat64(pos[i]);
}
ret = true;
}
break;
}
case VOCAB_TRACKBALL:
{
yarp::sig::Vector axes;
unsigned int id;
id = cmd.get(4).asInt32();
if(cmd.get(4).isInt32() && device->getTrackball(id, axes))
{
response.addVocab(VOCAB_OK);
for(size_t i = 0; i < axes.size(); ++i)
{
response.addFloat64(axes[i]);
}
ret = true;
}
break;
}
case VOCAB_HAT:
{
unsigned char value;
if(cmd.get(4).isInt32() && device->getHat(cmd.get(4).asInt32(), value))
{
response.addVocab(VOCAB_OK);
response.addInt32(value);
ret = true;
}
break;
}
default:
break;
}
}
}
return ret;
}
