本文整理汇总了C++中Stamp::getTime方法的典型用法代码示例。如果您正苦于以下问题:C++ Stamp::getTime方法的具体用法?C++ Stamp::getTime怎么用?C++ Stamp::getTime使用的例子?那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Stamp
的用法示例。
在下文中一共展示了Stamp::getTime方法的10个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: onRead
void onRead(T &obj) override
{
if (++cnt==dwnsample)
{
if (firstIncomingData)
{
yInfo() << "Incoming data detected";
firstIncomingData=false;
}
DumpItem item;
Stamp info;
BufferedPort<T>::getEnvelope(info);
item.seqNumber=info.getCount();
if (txTime || (info.isValid() && !rxTime))
item.timeStamp.setTxStamp(info.getTime());
if (rxTime || !info.isValid())
item.timeStamp.setRxStamp(Time::now());
item.obj=factory(obj);
buf.lock();
buf.push_back(item);
buf.unlock();
cnt=0;
}
}
示例2: getEncoderPositions
void ThreeDModule::getEncoderPositions(double *headjnt_pos, double *torsojnt_pos, Stamp stamp) {
mutex.wait();
if(stamp.getTime() == 0) {
std::cout << "WARNING: No timestamp found in this bottle!! check your implementation!! " << std::endl;
stamp.update();
}
// store the current (start) indices in the circular buffer
int tIdx = torsoIdx;
int hIdx = headIdx;
// helpers
int idxH = -1, idxT = -1;
int hH, hT;
double smallestDiffH = 10000;
double smallestDiffT = 10000;
double tDiff;
// the bottles to be copied out of the circ buffer
Bottle head, torso;
// going through the circular buffer
idxH = 1;
idxT = 1;
// for(int k = 0; k < LIST_LENGTH; k++) {
// hT = (LIST_LENGTH + tIdx - k) % LIST_LENGTH;
// hH = (LIST_LENGTH + hIdx - k) % LIST_LENGTH;
//
// tDiff = fabs(stamp.getTime() - torsoStamp[hT].getTime());
// std::cout << stamp.getTime() << " idx: " << hT << " getTime: " << torsoStamp[hT].getTime() << ", diffT: "<< tDiff << std::endl;
// if( tDiff < smallestDiffT ) {
// idxT = hT;
// smallestDiffT = tDiff;
// }
//
// tDiff = fabs(stamp.getTime() - torsoStamp[hH].getTime());
// if( tDiff < smallestDiffH ) {
// idxH = hH;
// smallestDiffH = tDiff;
// }
// }
if( idxH != -1 ) head = headState[idxH];
if( idxT != -1 ) torso = torsoState[idxT];
std::cout << "idxT: " << idxT << "/" << idxT-tIdx << " val: " << torso.toString() << std::endl;
std::cout << "idxH: " << idxH << "/" << idxH-hIdx << " val: " << head.toString() << std::endl;
for(int i = 0; i < 6; i++) {
headjnt_pos[i] = head.get(i).asDouble();
if(i < 3) torsojnt_pos[i] = torso.get(i).asDouble();
}
mutex.post();
}
示例3: getEnvelope
void Rangefinder2DInputPortProcessor::onRead(yarp::os::Bottle &b)
{
now=SystemClock::nowSystem();
mutex.wait();
if (count>0)
{
double tmpDT=now-prev;
deltaT+=tmpDT;
if (tmpDT>deltaTMax)
deltaTMax=tmpDT;
if (tmpDT<deltaTMin)
deltaTMin=tmpDT;
//compare network time
if (tmpDT*1000<LASER_TIMEOUT)
{
state = b.get(1).asInt();
}
else
{
state = IRangefinder2D::DEVICE_TIMEOUT;
}
}
prev=now;
count++;
lastBottle=b;
Stamp newStamp;
getEnvelope(newStamp);
//initialialization (first received data)
if (lastStamp.isValid()==false)
{
lastStamp = newStamp;
}
//now compare timestamps
if ((1000*(newStamp.getTime()-lastStamp.getTime()))<LASER_TIMEOUT)
{
state = b.get(1).asInt();
}
else
{
state = IRangefinder2D::DEVICE_TIMEOUT;
}
lastStamp = newStamp;
mutex.post();
}
示例4: onRead
void InputPortProcessor::onRead(yarp::sig::Vector &v)
{
now=Time::now();
mutex.wait();
if (count>0)
{
double tmpDT=now-prev;
deltaT+=tmpDT;
if (tmpDT>deltaTMax)
deltaTMax=tmpDT;
if (tmpDT<deltaTMin)
deltaTMin=tmpDT;
//compare network time
if (tmpDT*1000<ANALOG_TIMEOUT)
{
state=IAnalogSensor::AS_OK;
}
else
{
state=IAnalogSensor::AS_TIMEOUT;
}
}
prev=now;
count++;
lastVector=v;
Stamp newStamp;
getEnvelope(newStamp);
//initialialization (first received data)
if (lastStamp.isValid()==false)
{
lastStamp = newStamp;
}
//now compare timestamps
if ((1000*(newStamp.getTime()-lastStamp.getTime()))<ANALOG_TIMEOUT)
{
state=IAnalogSensor::AS_OK;
}
else
{
state=IAnalogSensor::AS_TIMEOUT;
}
lastStamp = newStamp;
mutex.post();
}
示例5: updateModule
bool updateModule()
{
Vector *imuData=iPort.read();
if (imuData==NULL)
return false;
Stamp stamp;
iPort.getEnvelope(stamp);
double t0=Time::now();
Vector gyro=imuData->subVector(6,8);
Vector gyro_filt=gyroFilt.filt(gyro);
gyro-=gyroBias;
gyro_filt-=gyroBias;
Vector mag_filt=magFilt.filt(imuData->subVector(9,11));
double magVel=norm(velEst.estimate(AWPolyElement(mag_filt,stamp.getTime())));
adaptGyroBias=adaptGyroBias?(magVel<mag_vel_thres_up):(magVel<mag_vel_thres_down);
gyroBias=biasInt.integrate(adaptGyroBias?gyro_filt:Vector(3,0.0));
double dt=Time::now()-t0;
if (oPort.getOutputCount()>0)
{
Vector &outData=oPort.prepare();
outData=*imuData;
outData.setSubvector(6,gyro);
oPort.setEnvelope(stamp);
oPort.write();
}
if (bPort.getOutputCount()>0)
{
bPort.prepare()=gyroBias;
bPort.setEnvelope(stamp);
bPort.write();
}
if (verbose)
{
yInfo("magVel = %g => [%s]",magVel,adaptGyroBias?"adapt-gyroBias":"no-adaption");
yInfo("gyro = %s",gyro.toString(3,3).c_str());
yInfo("gyroBias = %s",gyroBias.toString(3,3).c_str());
yInfo("dt = %.0f [us]",dt*1e6);
yInfo("\n");
}
return true;
}
示例6: stereoCalibRun
void stereoCalibThread::stereoCalibRun()
{
imageL=new ImageOf<PixelRgb>;
imageR=new ImageOf<PixelRgb>;
Stamp TSLeft;
Stamp TSRight;
bool initL=false;
bool initR=false;
int count=1;
Size boardSize, imageSize;
boardSize.width=this->boardWidth;
boardSize.height=this->boardHeight;
while (!isStopping()) {
ImageOf<PixelRgb> *tmpL = imagePortInLeft.read(false);
ImageOf<PixelRgb> *tmpR = imagePortInRight.read(false);
if(tmpL!=NULL)
{
*imageL=*tmpL;
imagePortInLeft.getEnvelope(TSLeft);
initL=true;
}
if(tmpR!=NULL)
{
*imageR=*tmpR;
imagePortInRight.getEnvelope(TSRight);
initR=true;
}
if(initL && initR && checkTS(TSLeft.getTime(),TSRight.getTime())){
bool foundL=false;
bool foundR=false;
mutex->wait();
if(startCalibration>0) {
string pathImg=imageDir;
preparePath(pathImg.c_str(), pathL,pathR,count);
string iml(pathL);
string imr(pathR);
imgL= (IplImage*) imageL->getIplImage();
imgR= (IplImage*) imageR->getIplImage();
Mat Left(imgL);
Mat Right(imgR);
std::vector<Point2f> pointbufL;
std::vector<Point2f> pointbufR;
if(boardType == "CIRCLES_GRID") {
foundL = findCirclesGridDefault(Left, boardSize, pointbufL, CALIB_CB_SYMMETRIC_GRID | CALIB_CB_CLUSTERING);
foundR = findCirclesGridDefault(Right, boardSize, pointbufR, CALIB_CB_SYMMETRIC_GRID | CALIB_CB_CLUSTERING);
} else if(boardType == "ASYMMETRIC_CIRCLES_GRID") {
foundL = findCirclesGridDefault(Left, boardSize, pointbufL, CALIB_CB_ASYMMETRIC_GRID | CALIB_CB_CLUSTERING);
foundR = findCirclesGridDefault(Right, boardSize, pointbufR, CALIB_CB_ASYMMETRIC_GRID | CALIB_CB_CLUSTERING);
} else {
foundL = findChessboardCorners( Left, boardSize, pointbufL, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);
foundR = findChessboardCorners( Right, boardSize, pointbufR, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);
}
if(foundL && foundR) {
cvCvtColor(imgL,imgL,CV_RGB2BGR);
cvCvtColor(imgR,imgR, CV_RGB2BGR);
saveStereoImage(pathImg.c_str(),imgL,imgR,count);
imageListR.push_back(imr);
imageListL.push_back(iml);
imageListLR.push_back(iml);
imageListLR.push_back(imr);
Mat cL(pointbufL);
Mat cR(pointbufR);
drawChessboardCorners(Left, boardSize, cL, foundL);
drawChessboardCorners(Right, boardSize, cR, foundR);
count++;
}
if(count>numOfPairs) {
fprintf(stdout," Running Left Camera Calibration... \n");
monoCalibration(imageListL,this->boardWidth,this->boardHeight,this->Kleft,this->DistL);
fprintf(stdout," Running Right Camera Calibration... \n");
monoCalibration(imageListR,this->boardWidth,this->boardHeight,this->Kright,this->DistR);
stereoCalibration(imageListLR, this->boardWidth,this->boardHeight,this->squareSize);
fprintf(stdout," Saving Calibration Results... \n");
updateIntrinsics(imgL->width,imgL->height,Kright.at<double>(0,0),Kright.at<double>(1,1),Kright.at<double>(0,2),Kright.at<double>(1,2),DistR.at<double>(0,0),DistR.at<double>(0,1),DistR.at<double>(0,2),DistR.at<double>(0,3),"CAMERA_CALIBRATION_RIGHT");
updateIntrinsics(imgL->width,imgL->height,Kleft.at<double>(0,0),Kleft.at<double>(1,1),Kleft.at<double>(0,2),Kleft.at<double>(1,2),DistL.at<double>(0,0),DistL.at<double>(0,1),DistL.at<double>(0,2),DistL.at<double>(0,3),"CAMERA_CALIBRATION_LEFT");
Mat Rot=Mat::eye(3,3,CV_64FC1);
Mat Tr=Mat::zeros(3,1,CV_64FC1);
updateExtrinsics(this->R,this->T,"STEREO_DISPARITY");
//.........这里部分代码省略.........
示例7: updateModule
bool GBSegmModule::updateModule()
{
ImageOf<PixelRgb> *yrpImgIn;
static int cycles = 0;
yrpImgIn = _imgPort.read();
if (yrpImgIn == NULL) // this is the case if module is requested to quit while waiting for image
return true;
bool use_private_stamp;
Stamp s;
if(!_imgPort.getEnvelope(s))
{
cout << "No stamp found in input image. Will use private stamp" << endl;
use_private_stamp = true;
}
else
{
cout << "Received image #" << s.getCount() << " generated at time " << s.getTime() << endl;
use_private_stamp = false;
}
if(cycles == 0)
_timestart = yarp::os::Time::now();
cycles++;
//IplImage *iplimg = (IplImage*)yrpImgIn->getIplImage();
cout << "converting image of size " << yrpImgIn->width() << yrpImgIn->height() <<" to size" << input->width() << input->height() << endl;
YarpImageToRGBImage(input, yrpImgIn);
cout << "converted" << endl;
segMutex.wait();
if(seg)
delete seg;
seg=segment_image(input, sigma, k, min_size, &num_components);
segMutex.post();
cout << "processed" << endl;
//prepare timestamps
if(use_private_stamp)
{
_stamp.update();
_viewPort.setEnvelope(_stamp);
}
else
{
_viewPort.setEnvelope(s);
}
ImageOf<PixelRgb> &yrpImgView = _viewPort.prepare();
//Rescale image if required
yrpImgView.resize(seg->width(), seg->height());
RGBImageToYarpImage(seg, &yrpImgView);
_viewPort.write();
//report the frame rate
if(cycles % 100 == 0)
{
double cps = ((double)cycles)/(yarp::os::Time::now() - _timestart);
printf("fps: %02.2f\n", cps);
}
return true;
}
示例8: timerEvent
// Generate new values
void DataPlot::timerEvent(QTimerEvent *)
{
for (int k = 0; k < numberOfInputPorts; k++)
{
int timeout = 0;
int maxTimeout = 2;
Bottle *b = NULL;
while(b==NULL && timeout < maxTimeout)
{
b = inputPorts[k].read(false);
Time::delay(0.005);
timeout++;
}
if (timeout==maxTimeout)
{
if(VERBOSE) fprintf(stderr, "MESSAGE: Couldn't receive data. Going to put a zero! \n");
for(int i = 0; i < numberOfPlots[k]; i++)
{
d_y[k][i][PLOT_SIZE - 1] = 0;
for ( int j = 0; j < PLOT_SIZE - 1; j++ )
{
d_y[k][i][j] = d_y[k][i][j+1];
}
}
}
else
{
for(int i = 0; i < numberOfPlots[k]; i++)
{
Stamp stmp;
inputPorts[k].getEnvelope(stmp);
#ifdef ENABLE_REALTIME
if (stmp.isValid())
d_x_real_time[PLOT_SIZE - 1] = (stmp.getTime() - initialTime);
for ( int j = 0; j < PLOT_SIZE - 1; j++ )
{
if(realTime)
d_x_real_time[j] = d_x_real_time[j+1];
}
#endif
if (b==NULL)
d_y[k][i][PLOT_SIZE - 1] = 0;
else
if (b->size()-1 >= index[k][i])
{
d_y[k][i][PLOT_SIZE - 1] = b->get(index[k][i]).asDouble();
//if(VERBOSE) fprintf(stderr, "MESSAGE: Getting from port %d the index %d\n", k, index[k][i]);
}
// y moves from left to right:
// Shift y array right and assign new value to y[0].
for ( int j = 0; j < PLOT_SIZE - 1; j++ )
{
d_y[k][i][j] = d_y[k][i][j+1];
}
// update the display
//setAxisScale(QwtPlot::yLeft, min, max);
#ifdef ENABLE_REALTIME
if(numberAcquiredData==PLOT_SIZE && realTime)
{
if(VERBOSE) fprintf(stderr, "MESSAGE: switching to real time\n");
QwtPlotCurve *timeBasedCurve = new QwtPlotCurve("Data");
timeBasedCurve->attach(this);
timeBasedCurve->setRawData(d_x_real_time, d_y[k][i], PLOT_SIZE);
timeBasedCurve->setPen(coloredPens[k%NUMBER_OF_LIN][i%NUMBER_OF_COL]);
nonTimeBasedCurve[k][i].attach(NULL);
//Set title
char cTitle[256];
sprintf(cTitle, "Data(%d)", index[k][i]);
QwtText curveTitle(cTitle, QwtText::PlainText);
curveTitle.setFont(plotFont);
timeBasedCurve->setTitle(curveTitle);
}
#endif
}
}
}
if (acquire)
replot();
numberAcquiredData++;
//if(VERBOSE) fprintf(stderr, "Number of acquired data is %d\n", numberAcquiredData);
//QSize plotSize= this->sizeHint();
//if(VERBOSE) fprintf(stderr, "Hint is: hInt=%d, vInt=%d\n", plotSize.height(), plotSize.width());
static double before = Time::now();
double now = Time::now();
static double meanEffectiveTime = 0;
double currentEffectiveTime = (now - before)*1000.0;
if (numberAcquiredData >= 2)
meanEffectiveTime = (meanEffectiveTime*(numberAcquiredData-2) + currentEffectiveTime)/(numberAcquiredData-1);
//if(VERBOSE) fprintf(stderr, "Iteration %d: Current time is %f and mean is %f\n", numberAcquiredData, currentEffectiveTime, meanEffectiveTime);
//.........这里部分代码省略.........
示例9: initSignalDimensions
void DataPlot::initSignalDimensions()
{
if(VERBOSE) fprintf(stderr, "MESSAGE: Will now initialize the signal dimensions\n");
//getting info on the connected port
nonTimeBasedCurve = new QwtPlotCurve*[numberOfInputPorts];
for (int k = 0; k < numberOfInputPorts; k++)
{
Bottle *b = NULL;
int timeout = 0;
const int maxTimeout = 1000;
while(b==NULL && timeout < maxTimeout)
{
b = inputPorts[k].read(false);
Time::delay(0.005);
timeout++;
}
if (timeout==maxTimeout)
{
if(VERBOSE) fprintf(stderr, "MESSAGE: Couldn't receive data. Going to put a zero! \n");
realTime = false;
// Initialize data
for (int j = 0; j< numberOfPlots[k]; j++)
{
for (int i = 0; i < PLOT_SIZE; i++)
{
d_x[i] = i; // time axis
//if(VERBOSE) fprintf(stderr, "MESSAGE: (%d, %d)\n", j, i);
d_y[k][j][i] = 0;
}
}
}
else
{
if(VERBOSE) fprintf(stderr, "MESSAGE: Will now try real time!\n");
inputVectorSize = b->size();
Stamp stmp;
inputPorts[k].getEnvelope(stmp);
if (stmp.isValid())
{
if(VERBOSE) fprintf(stderr, "MESSAGE: will use real time!\n");
realTime = true;
initialTime = stmp.getTime();
}
// Initialize data
for (int j = 0; j< numberOfPlots[k]; j++)
{
if (b->size()-1 < index[k][j])
if(VERBOSE) fprintf(stderr, "WARNING: will plot some zeros since the accessed index exceed the input vector dimensions!\n");
for (int i = 0; i < PLOT_SIZE; i++)
{
d_x[i] = i; // time axis
#ifdef ENABLE_REALTIME
if (realTime)
d_x_real_time[i] = i;
#endif
//if(VERBOSE) fprintf(stderr, "MESSAGE: (%d, %d)\n", j, i);
d_y[k][j][i] = 0;
}
}
}
//if(VERBOSE) fprintf(stderr, "MESSAGE: initializing plot datas!\n");
// Assign a title
insertLegend(new QwtLegend(), QwtPlot::BottomLegend);
nonTimeBasedCurve[k] = new QwtPlotCurve[numberOfPlots[k]];
for(int i=0; i < numberOfPlots[k]; i++)
{
//Set title
char cTitle[256];
sprintf(cTitle, "Data(%d)", index[k][i]);
QwtText curveTitle(cTitle, QwtText::PlainText);
curveTitle.setFont(plotFont);
nonTimeBasedCurve[k][i].setTitle(curveTitle);
//if(VERBOSE) fprintf(stderr, "MESSAGE: Will now initialize the plot %d\n", index[i]);
// Insert new curves
nonTimeBasedCurve[k][i].attach(this);
// Set curve styles
nonTimeBasedCurve[k][i].setPen(coloredPens[k%NUMBER_OF_LIN][i%NUMBER_OF_COL]);
// Attach (don't copy) data. Both curves use the same x array.
nonTimeBasedCurve[k][i].setRawData(d_x, d_y[k][i], PLOT_SIZE);
}
// Axis
QwtText axisTitle("Time/seconds");
axisTitle.setFont(plotFont);
setAxisTitle(QwtPlot::xBottom, axisTitle);
setAxisScale(QwtPlot::xBottom, 0, 100);
setAxisFont(QwtPlot::xBottom, plotFont);
setAxisTitle(QwtPlot::yLeft, "Values");
//setAxisScale(QwtPlot::yLeft, -1.5, 1.5);
setAxisAutoScale(QwtPlot::yLeft);
setAxisAutoScale(QwtPlot::xBottom);
setAxisFont(QwtPlot::yLeft, plotFont);
//.........这里部分代码省略.........
示例10: updateModule
bool EdisonSegmModule::updateModule()
{
ImageOf<PixelRgb> *yrpImgIn;
static int cycles = 0;
yrpImgIn = _imgPort.read();
if (yrpImgIn == NULL) // this is the case if module is requested to quit while waiting for image
return true;
bool use_private_stamp;
Stamp s;
if(!_imgPort.getEnvelope(s))
{
cout << "No stamp found in input image. Will use private stamp" << endl;
use_private_stamp = true;
}
else
{
cout << "Received image #" << s.getCount() << " generated at time " << s.getTime() << endl;
use_private_stamp = false;
}
if(cycles == 0)
_timestart = yarp::os::Time::now();
cycles++;
IplImage *iplimg = (IplImage*)yrpImgIn->getIplImage();
//computing the ROI to crop the image
/*struct _IplROI roi;
roi.coi = 0; // all channels are selected
roi.height = height_;
roi.width = width_;
roi.xOffset = ( orig_width_ - width_ ) / 2;
roi.yOffset = ( orig_height_ - height_ ) / 2;*/
//copying roi data to buffer
/*iplimg->roi = &roi;
cvCopy( iplimg, inputImage.getIplImage());*/
//Rescale image if required
if( (width_ != orig_width_) || (height_ != orig_height_ ) )
cvResize(iplimg, inputImage.getIplImage(), CV_INTER_NN);
else
cvCopy( iplimg, inputImage.getIplImage());
double edgetime = yarp::os::Time::now();
//compute gradient and confidence maps
BgEdgeDetect edgeDetector(gradWindRad);
BgImage bgImage;
bgImage.SetImage(inputImage_, width_, height_, true);
edgeDetector.ComputeEdgeInfo(&bgImage, confMap_, gradMap_);
//compute the weigth map
for(int i = 0; i < width_*height_; i++) {
if(gradMap_[i] > 0.02) {
weightMap_[i] = mixture*gradMap_[i] + (1 - mixture)*confMap_[i];
} else {
weightMap_[i] = 0;
}
}
///////////////////////////// This block can be parallelized
cout << "Edge computation Time (ms): " << (yarp::os::Time::now() - edgetime)*1000.0 << endl;
msImageProcessor iProc;
if( dim_ == 3 )
iProc.DefineImage(inputImage_, COLOR, height_, width_);
else
{
cvCvtColor(inputImage.getIplImage(), inputHsv.getIplImage(), CV_RGB2HSV);
cvSplit(inputHsv.getIplImage(), inputHue.getIplImage(), 0, 0, 0);
iProc.DefineImage(inputHue_, GRAYSCALE, height_, width_);
}
if(iProc.ErrorStatus) {
cout << "MeanShift Error" << endl;
return false;
}
iProc.SetWeightMap(weightMap_, threshold);
if(iProc.ErrorStatus) {
cout << "MeanShift Error" << endl;
return false;
}
double filtertime = yarp::os::Time::now();
iProc.Filter(sigmaS, sigmaR, speedup);
if(iProc.ErrorStatus) {
cout << "MeanShift Error" << endl;
return false;
}
cout << "Mean Shift Filter Computation Time (ms): " << (yarp::os::Time::now() - filtertime)*1000.0 << endl;
//obtain the filtered image
iProc.GetResults(filtImage_);
if(iProc.ErrorStatus) {
cout << "MeanShift Error" << endl;
return false;
}
//fuse regions
//.........这里部分代码省略.........