void UResPoly::gotNewData()
{ //
char * dataType = (char*)"poly";
double v = 0.0;
//
if (varCallDispOnNewData->getBool())
{ // tell display system of new image
callGlobal("view.newData", "sd", &dataType, &v, &v, NULL, NULL);
callGlobal("disp.newData", "sd", &dataType, &v, &v, NULL, NULL);
}
varUpdTime->setTimeNow();
}
bool deinitialize_(void) {
FFMixed result=callGlobal(FF_DEINITIALISE);
m_plugin=NULL;
m_name=m_id=m_description=m_about="";
return (FF_SUCCESS==result.UIntValue);
}
void UResLaserIfObst::newDataAvailable()
{
char * dataType = (char*) "laser";
double v;
bool isOK;
//
//setLocalVar(varScan, scan->getSerial());
if (callDispOnNewData)
{
// tell display system of new image
v = varSerial->getValued();
isOK = callGlobal("view.newData", "sd", &dataType, &v, &v, NULL, NULL);
isOK = callGlobal("disp.newData", "sd", &dataType, &v, &v, NULL, NULL);
//printf("UResLaserIfObst::newDataAvailable: now %d obstacle group lines\n",
// groupsCnt);
callDispOnNewData = isOK;
}
}
PluginExtendedInfoStruct*getExtendedInfo_(void) {
FFMixed result=callGlobal(FF_GETEXTENDEDINFO);
m_description=m_about="";
m_majorVersion = m_minorVersion = 0;
if(FF_FAIL==result.UIntValue)return NULL;
PluginExtendedInfoStruct*pis=reinterpret_cast<PluginExtendedInfoStruct*>(result.PointerValue);
m_description=pis->Description;
m_about = pis->About;
m_majorVersion = pis -> PluginMajorVersion;
m_minorVersion = pis -> PluginMinorVersion;
return pis;
}
bool getInfo_(void) {
FFMixed result=callGlobal(FF_GETINFO);
if(FF_FAIL==result.UIntValue) {
std::cout << "getInfo failed" << std::endl;
return false;
}
PluginInfoStruct*pis=reinterpret_cast<PluginInfoStruct*>(result.PointerValue);
#ifdef __GNUC__
# warning check whether the API is supported by us
#endif
m_name = nchar2str(pis->PluginName, 16);
m_id = nchar2str(pis->PluginUniqueID, 4);
m_type = pis->PluginType;
std::cout << "FF-API: "<<pis->APIMajorVersion<<"."<<pis->APIMinorVersion<<std::endl;
return true;
}
UImage * UResCamIfImg::getImageBuffer(int poolNumber, int height, int width, int channels, int depth)
{ // could be overwritten to get a image buffer associated with this pool number
UImage * result = NULL;
bool resize = false;
double imgNum = poolNumber;
double v;
//
if (imgPool != NULL)
{
result = imgPool->getImage(poolNumber, false);
if (result)
// test for image size change
resize = ((int)result->width() != width or (int)result->height() != height);
if (resize)
{ // remove old image first on resize
callGlobal("disp.resize", "d", NULL, &imgNum, &v, NULL, NULL);
}
if (resize or result == NULL)
result = imgPool->getImage(poolNumber, true, height, width, channels, depth);
}
return result;
}
void UResCamIfImg::gotNewImage(UImage * img, int poolNum, USmlTag * tag)
{
double imgNum = poolNum;
char * dataType = (char*) "img";
double v;
int dev;
const int MRL = 500;
char req[MRL];
int n;
//
n = varImgCnt->getInt();
varImgCnt->setInt(n + 1);
varImgPoolNum->setInt(poolNum);
varImgTime->setTime(img->imgTime);
/* printf("Got new image of size h=%d w=%d (from pool number %d)\n",
img->height(), img->width(), poolNum);*/
// tell display system of new image
img->used = false;
callGlobal("disp.newData", "sd", &dataType, &imgNum, &v, NULL, NULL);
// request camera data for this device too
dev = img->camDevice;
snprintf(req, MRL, "camget device=\"%d\" pos rot focal size fps name\n", dev);
tag->outputData(req);
}
FFUInt32 getPluginCaps_(FFUInt32 PluginCapsIndex) {
FFMixed result=callGlobal(FF_GETPLUGINCAPS, PluginCapsIndex);
return result.UIntValue;
}
FFUInt32 getParameterType_(FFUInt32 ParameterNumber) {
FFMixed result=callGlobal(FF_GETPARAMETERTYPE, ParameterNumber);
return result.UIntValue;
}
FFMixed getParameterDefault_(FFUInt32 ParameterNumber) {
FFMixed result=callGlobal(FF_GETPARAMETERDEFAULT, ParameterNumber);
return result;
}
std::string getParameterName_(FFUInt32 ParameterNumber) {
FFMixed result=callGlobal(FF_GETPARAMETERNAME, ParameterNumber);
std::string name = nchar2str(result.PointerValue, 16);
return name;
}
FFUInt32 getNumParameters_(void) {
FFMixed result=callGlobal(FF_GETNUMPARAMETERS);
if(FF_FAIL == result.UIntValue)
return 0;
return result.UIntValue;
}
bool initialize_(void) {
FFMixed result=callGlobal(FF_INITIALISE);
return (FF_SUCCESS==result.UIntValue);
}
FFMixed callGlobal(FFUInt32 funcode, void* value) {
FFMixed mixed;
mixed.PointerValue=value;
return callGlobal(funcode, mixed);
}
FFMixed callGlobal(FFUInt32 funcode, FFFloat32 value) {
FFMixed mixed;
mixed.FloatValue=value;
return callGlobal(funcode, mixed);
}
void UResCamIfCam::gotNewData(int device)
{
double dev = device;
char * dataType = (char*)"cam";
double v;
const int MDSL = 20;
char ds[MDSL];
UVarPool * vp;
UClientCamData * cam;
UTime t;
UPosRot pr;
//
//n = roundi(getLocalValue(varUpdCnt));
varUpdCnt->add(1.0, 0);
varCamDevNum->setInt(device);
cam = getCams()->getCam(device);
if (cam != NULL)
// camera structure not yet available
varUpdTime->setTime(cam->updTime);
else
{
t.now();
varUpdTime->setTime(t);
}
// set also global vars
snprintf(ds, MDSL, "dev%d", device);
vp = getVarPool()->getStruct(ds);
if (vp == NULL)
{ // no struct available - so create one
vp = getVarPool()->addStructLocal(ds, "Camera device details", false);
if (vp != NULL)
{
varPosX = vp->addVar("posX", 0.0, "3d", "(r) Position of camera on robot");
varPosY = vp->addVar("posY", 0.0, "3d", "");
varPosZ = vp->addVar("posZ", 0.0, "3d", "");
varPosO = vp->addVar("rotO", 0.0, "rot", "(r) orientation of camera");
varPosP = vp->addVar("rotP", 0.0, "rot", "");
varPosK = vp->addVar("rotK", 0.0, "rot", "");
varFocal = vp->addVar("focal", 500.0, "d", "(r) Focal length in pixels (at current resolution)");
varWidth = vp->addVar("width", 0.0, "d", "(r) Current image width (resolution)");
varHeight = vp->addVar("posY", 0.0, "d", "(r) Current image height");
}
}
if (vp != NULL)
{
cam = getCams()->getCamData(device, false);
pr.set(cam->pos, cam->rot);
varPosX->set6D(&pr);
varPosY->setDouble(cam->pos.y, 0);
varPosZ->setDouble(cam->pos.z, 0);
varPosO->setDouble(cam->rot.Omega, 0);
varPosP->setDouble(cam->rot.Phi, 0);
varPosK->setDouble(cam->rot.Kappa, 0);
// this focal length is in current resolution (full resolution in cam-structure)
varFocal->setDouble(cam->getFocalLength(), 0);
varWidth->setInt(cam->width, 0);
varHeight->setInt(cam->height, 0);
}
// tell display system of new image
if (varCallDisp->getBool())
{
callGlobal("view.newData", "sd", &dataType, &dev, &v, NULL, NULL);
callGlobal("disp.newData", "sd", &dataType, &dev, &v, NULL, NULL);
}
//
}
