C++ iceutil::Time类代码示例

std::string 
ReplayConductor::toString( const IceUtil::Time &t )
{
    stringstream ss;
    ss << t.toSeconds() << ":" << t.toMicroSeconds()-t.toSeconds()*1e6;
    return ss.str();
}

bool
CPatchDlg::patchProgress(Ice::Long, Ice::Long, Ice::Long totalProgress, Ice::Long totalSize)
{
    IceUtil::Time elapsed = IceUtil::Time::now(IceUtil::Time::Monotonic) - _startTime;
    if(elapsed.toSeconds() > 0)
    {
        CString speed;
        speed.Format(L" %s/s", convertSize(totalProgress / elapsed.toSeconds()));
        _speed->SetWindowText(speed);
    }

    int pcnt = 100;
    if(totalSize > 0)
    {
        pcnt = static_cast<int>(totalProgress * 100 / totalSize);
    }
    CString percent;
    percent.Format(L"%d%%", pcnt);
    _percent->SetWindowText(percent);

    CString total;
    total.Format(L" %s / %s", convertSize(totalProgress), convertSize(totalSize));
    _total->SetWindowText(total);

    _progress->SetPos(pcnt);

    processMessages();
    return !_isCancel;
}

void
Driver::read( hydrointerfaces::LaserScanner2d::Data &data )
{
    // tmp data storage
    hokuyo_aist::HokuyoData hokuyoData;

    try {
        laser_->GetRanges( &hokuyoData );
        // set the time stamp right away
        IceUtil::Time t = IceUtil::Time::now();
        data.timeStampSec = (int)t.toSeconds();
        data.timeStampUsec = (int)t.toMicroSeconds() - data.timeStampSec*1000000;

        //TODO: Michael 23/Sept/08
        //this timeStamp is actually at the _end_ of the scan; also
        //scan-time is _not_ negligable for the hokuyos (10-30Hz --> 100-33ms)
        //
        //--> need to keep track of delta time between scans, and use fieldOfView
        //to calculate a backcorrection, assuming 1 rotation per scan (true for
        //URG and UHG, unknown for TopURG). Also should put something like
        //durationOfScan into the interface.
        //
        //This would allow to interpolate senor pose per individual range-reading.
        //Yes, I really think this is neccessary.
    }
    catch (hokuyo_aist::HokuyoError &e)
    {
        std::stringstream ss;
        ss << "Read on urg_nz failed. Errorcode: "
            << e.Code()
            << ", Errordescription: "
            << e.what()
            << " continuing";
        //context_.tracer().error( ss.str() ,2 );
        throw gbxutilacfr::HardwareException( ERROR_INFO, ss.str() );
    }

    const uint32_t* ranges = hokuyoData.Ranges();
    //const uint32_t* intensities = hokuyoData.Intensities();
    size_t length = hokuyoData.Length();
    assert(config_.numberOfSamples == (int)length); // make sure the receiving end has enough space
    double minRangeHokuyo = config_.minRange*1000.0; // convert to mm
    for (unsigned int i=0; i<length; ++i )
    {
        if(ranges[i] < minRangeHokuyo){
            //these guys are error-codes (per reading); setting to maxRange to get behavior similar to SICK
            data.ranges[i]  = config_.maxRange;
        }else{
            data.ranges[i]  = ranges[i] / 1000.0;
        }
    }
}

void pointcloudClient::run(){

	IceUtil::Time last;

	last=IceUtil::Time::now();

	while (!(_done)){
		if (pauseStatus){
			IceUtil::Mutex::Lock sync(this->controlMutex);
			this->sem.wait(sync);
		}

		try{
			jderobot::pointCloudDataPtr localCloud=this->prx->getCloudData();
			this->controlMutex.lock();
	        this->newData=true;
			this->data.resize(localCloud->p.size());
			std::copy( localCloud->p.begin(), localCloud->p.end(), this->data.begin() );
			this->controlMutex.unlock();
			this->semBlock.broadcast();
		}
		catch(...){
			jderobot::Logger::getInstance()->warning(prefix +"error during request (connection error)");
			usleep(50000);

		}




		int process = this->cycle - (IceUtil::Time::now().toMicroSeconds() - last.toMicroSeconds());

		if (process > (int)cycle ){
			jderobot::Logger::getInstance()->warning(prefix + ": pointCloud adquisition timeout-");
		}
		else{
			int delay = (int)cycle - process;
			if (delay <1 || delay > (int)cycle)
				delay = 1;

			usleep(delay);
		}


		this->refreshRate=(int)(1000000/(IceUtil::Time::now().toMicroSeconds() - last.toMicroSeconds()));
		last=IceUtil::Time::now();
		usleep(100);
	}
	this->data.clear();

}

jderobot::ImageDataPtr CameraUtils::convert(const cv::Mat &image) {

    jderobot::ImageDataPtr reply=jderobot::ImageDataPtr(new jderobot::ImageData());
    reply->description = jderobot::ImageDescriptionPtr(new jderobot::ImageDescription());
    IceUtil::Time t = IceUtil::Time::now();
    reply->timeStamp.seconds = (long)t.toSeconds();
    reply->timeStamp.useconds = (long)t.toMicroSeconds() - reply->timeStamp.seconds*1000000;
    reply->description->format = colorspaces::ImageRGB8::FORMAT_RGB8.get()->name;
    reply->description->width=image.size().width;
    reply->description->height=image.size().height;
    reply->pixelData.resize(image.rows*image.cols * image.channels());
    memcpy(&(reply->pixelData[0]),(unsigned char *) image.data, image.rows*image.cols * image.channels());
    return reply;
}

void
Driver::read( hydrointerfaces::Image::Data &data )
{
    context_.tracer().debug( "Copying image file data..." );

    //Qt uses a uint32_t array not a byte aligned array so we must cut off the end of each scanline.
    uint32_t bytesPerLine = static_cast<uint32_t>(image_.width()) * 3;
    for(uint32_t i = 0; i < static_cast<uint32_t>(image_.height()); i++)
    {
        memcpy(data.pixelData+i*bytesPerLine, image_.scanLine(i), bytesPerLine);
    }

    IceUtil::Time t = IceUtil::Time::now();
    data.timeStampSec = (int)t.toSeconds();
    data.timeStampUsec = (int)t.toMicroSeconds() - data.timeStampSec*1000000;
}

void Pose3dIceClient::run(){
	JdeRobotTypes::Pose3d pose3d;

	IceUtil::Time last;

	last=IceUtil::Time::now();
	while (this->on){
		if (pauseStatus){
			IceUtil::Mutex::Lock sync(this->controlMutex);
			this->sem.wait(sync);
		}

		try{
			jderobot::Pose3DDataPtr pose3ddata = this->prx->getPose3DData();

			pose3d.x = pose3ddata->x;
			pose3d.y = pose3ddata->y;
			pose3d.z = pose3ddata->z;
			pose3d.q[0] = pose3ddata->q0;
			pose3d.q[1] = pose3ddata->q1;
			pose3d.q[2] = pose3ddata->q2;
			pose3d.q[3] = pose3ddata->q3;
			pose3d.yaw = this->quat2Yaw(pose3d.q);
			pose3d.pitch = this->quat2Pitch(pose3d.q);
			pose3d.roll = this->quat2Roll(pose3d.q);



			this->controlMutex.lock();
			this->pose = pose3d;
			this->controlMutex.unlock();
		}
		catch(...){
			std::cerr << prefix +"error during request (connection error)" << std::endl;
			usleep(5000);

		}


		if ((IceUtil::Time::now().toMicroSeconds() - last.toMicroSeconds()) <= this->cycle ){
			usleep(this->cycle - (IceUtil::Time::now().toMicroSeconds() - last.toMicroSeconds()));
		}
		last=IceUtil::Time::now();
	}
}

 void ReplyTask::run() {
   jderobot::ImageDataPtr reply(new jderobot::ImageData);
   ::jderobot::Time t;
   while (1) {
     IceUtil::Time t = IceUtil::Time::now();
     reply->timeStamp.seconds = (long) t.toSeconds();
     reply->timeStamp.useconds = (long) t.toMicroSeconds()
         - reply->timeStamp.seconds * 1000000;
     {  //critical region start
       IceUtil::Mutex::Lock sync(requestsMutex);
       while (!requests.empty()) {
         jderobot::AMD_ImageProvider_getImageDataPtr cb = requests.front();
         requests.pop_front();
         cb->ice_response(reply);
       }
     }
   }
 }

void
ReplayConductor::handleRewind( const IceUtil::Time &deltaT )
{
    bool wasPlaying = isPlaying_;
    if ( wasPlaying )
        handlePause();

    int sec, usec;
    masterFileReader_.getCursorTime( sec, usec );

    IceUtil::Time tNew = orcalog::iceUtilTime( sec, usec );
    tNew -= deltaT;
    masterFileReader_.placeCursorAtOrAfterTime( tNew.toSeconds(),
                                                (int)(tNew.toMicroSeconds()-tNew.toSeconds()*1e6) );

    if ( wasPlaying )
        handleStart();
}

bool
IceUtilInternal::Semaphore::timedWait(const IceUtil::Time& timeout) const
{
    long msec = (long)timeout.toMilliSeconds();

    int rc = WaitForSingleObject(_sem, msec);
    if(rc != WAIT_TIMEOUT && rc != WAIT_OBJECT_0)
    {
        throw IceUtil::ThreadSyscallException(__FILE__, __LINE__, GetLastError());
    }
    return rc != WAIT_TIMEOUT;
}

chrono::system_clock::time_point
#else
IceUtil::Time
#endif
getX509Date(SecCertificateRef cert, CFTypeRef key)
{
    assert(key == kSecOIDX509V1ValidityNotAfter || key == kSecOIDX509V1ValidityNotBefore);
    UniqueRef<CFDictionaryRef> property(getCertificateProperty(cert, key));
    CFAbsoluteTime seconds = 0;
    if(property)
    {
        CFNumberRef date = static_cast<CFNumberRef>(CFDictionaryGetValue(property.get(), kSecPropertyKeyValue));
        CFNumberGetValue(date, kCFNumberDoubleType, &seconds);
    }

    IceUtil::Time time = IceUtil::Time::secondsDouble(kCFAbsoluteTimeIntervalSince1970 + seconds);

#ifdef ICE_CPP11_MAPPING
    return chrono::system_clock::time_point(chrono::microseconds(time.toMicroSeconds()));
#else
    return time;
#endif
}

void laserClient::run(){

	IceUtil::Time last;

	last=IceUtil::Time::now();
	while (!(_done)){
		if (pauseStatus){
			IceUtil::Mutex::Lock sync(this->controlMutex);
			this->sem.wait(sync);
		}

		try{
			jderobot::LaserDataPtr localLaser=this->prx->getLaserData();

			this->controlMutex.lock();
			this->data.resize(localLaser->distanceData.size());
			std::copy( localLaser->distanceData.begin(), localLaser->distanceData.end(), this->data.begin() );

			this->controlMutex.unlock();
		}
		catch(...){
			jderobot::Logger::getInstance()->warning(prefix +"error during request (connection error)");
			usleep(5000);

		}


		if ((IceUtil::Time::now().toMicroSeconds() - last.toMicroSeconds()) > this->cycle ){
			jderobot::Logger::getInstance()->warning(prefix + ": pointCloud adquisition timeout-");
		}
		else{
			usleep(this->cycle - (IceUtil::Time::now().toMicroSeconds() - last.toMicroSeconds()));
		}
		this->refreshRate=(int)(1000000/(IceUtil::Time::now().toMicroSeconds() - last.toMicroSeconds()));
		last=IceUtil::Time::now();
	}
}

void
Driver::read( hydrointerfaces::MultiImage::Data& data )
{

    context_.tracer().debug( "Grabbing frame(s) from camera(s)..." );
   
    for( unsigned int i=0; i<cameras_.size(); ++i )
    {
        // This performs the grab and the retrieve with one cv function call
        frames_.at(i) = cvQueryFrame( cameras_.at(i) );
        if( frames_.at(i) == NULL )
        {
            stringstream ss;
            ss << "Failed to retrieve frame from Camera " << i;
            throw gbxutilacfr::Exception( ERROR_INFO, ss.str() );
        }
    
        memcpy( data.at(i).pixelData, frames_.at(i)->imageData, frames_.at(i)->imageSize );
        
        IceUtil::Time t = IceUtil::Time::now();
        data.at(i).timeStampSec = (int)t.toSeconds();
        data.at(i).timeStampUsec = (int)t.toMicroSeconds() - data.at(i).timeStampSec*1000000;
    }
}

bool
IceUtilInternal::Semaphore::timedWait(const IceUtil::Time& timeout) const
{
    IceUtil::Int64 msTimeout = timeout.toMilliSeconds();
    if(msTimeout < 0 || msTimeout > 0x7FFFFFFF)
    {
        throw IceUtil::InvalidTimeoutException(__FILE__, __LINE__, timeout);
    } 

    DWORD rc = WaitForSingleObject(_sem, static_cast<DWORD>(msTimeout));
    if(rc != WAIT_TIMEOUT && rc != WAIT_OBJECT_0)
    {
        throw IceUtil::ThreadSyscallException(__FILE__, __LINE__, GetLastError());
    }
    return rc != WAIT_TIMEOUT;
}

void checkedSleep( gbxutilacfr::Stoppable* activity, const IceUtil::Time& duration, int checkIntervalMs  )
{
    assert( activity && "Null activity pointer" );

    // Handle durations less than the check interval
    if ( duration.toMilliSeconds() < checkIntervalMs )
    {
        IceUtil::ThreadControl::sleep( duration );
        return;
    }

    IceUtil::Time wakeupTime = IceUtil::Time::now() + duration;
    IceUtil::Time checkInterval = IceUtil::Time::milliSeconds( checkIntervalMs );

    while ( !activity->isStopping() && IceUtil::Time::now() < wakeupTime )
    {
        IceUtil::ThreadControl::sleep( checkInterval );
    }
}