C++ Timestamp::update方法代码示例

int
FFmpegTagger::IORead(void *opaque, uint8_t *buf, int buf_size)
{
    URLContext* pUrlContext = (URLContext*)opaque;

    std::istream* pInputStream = (std::istream*)pUrlContext->priv_data;
    if (!pInputStream) {
        LOGNS(Omm::AvStream, avstream, error, "IORead failed, std::istream not set");
        return -1;
    }

    pInputStream->read((char*)buf, buf_size);
    Poco::Timestamp::TimeDiff time = _timestamp.elapsed();
    _timestamp.update();
    Poco::Timestamp::TimeDiff startTime = _startTimestamp.elapsed();
    int bytes = pInputStream->gcount();
    _totalBytes += bytes;
    if (!pInputStream->good()) {
        LOGNS(Omm::AvStream, avstream, error, "IORead failed to read from std::istream");
        return -1;
    }
    LOGNS(Omm::AvStream, avstream, trace, "IORead() bytes read: " + Poco::NumberFormatter::format(bytes) + " in " + Poco::NumberFormatter::format(time/1000.0, 3) + " msec (" +  Poco::NumberFormatter::format(bytes*1000/time) + " kB/s), total : " +\
    Poco::NumberFormatter::format(_totalBytes/1000) + "kB in " + Poco::NumberFormatter::format(startTime/(Poco::Timestamp::TimeDiff)1000000) + " sec (" + Poco::NumberFormatter::format((Poco::Timestamp::TimeDiff)(_totalBytes*1000)/startTime) + "kB/s)");
    return bytes;
}

bool Debounce(void) {
    if (fpsTimeLast.isElapsed(debounceTimeout.totalMicroseconds())) {
        fpsTimeLast.update();
        return true;
    } else {
        return false;
    }
}

void Timer::run()
{
	Poco::Timestamp now;
	long interval(0);
	do
	{
		long sleep(0);
		do
		{
			now.update();
			sleep = static_cast<long>((_nextInvocation - now)/1000);
			if (sleep < 0)
			{
				if (interval == 0)
				{
					sleep = 0;
					break;
				}
				_nextInvocation += static_cast<Timestamp::TimeVal>(interval)*1000;
				++_skipped;
			}
		}
		while (sleep < 0);

		if (_wakeUp.tryWait(sleep))
		{
			Poco::FastMutex::ScopedLock lock(_mutex);
			_nextInvocation.update();
			interval = _periodicInterval;
		}
		else
		{
			try
			{
				_pCallback->invoke(*this);
			}
			catch (Poco::Exception& exc)
			{
				Poco::ErrorHandler::handle(exc);
			}
			catch (std::exception& exc)
			{
				Poco::ErrorHandler::handle(exc);
			}
			catch (...)
			{
				Poco::ErrorHandler::handle();
			}
			interval = _periodicInterval;
		}
		_nextInvocation += static_cast<Timestamp::TimeVal>(interval)*1000;
		_skipped = 0;
	}
	while (interval > 0);
	_done.set();
}

int main(int argc, char**argv) {
  dsosg::DSOSG *dsosg;
  Poco::Timestamp time;
  bool done;

  osg::ArgumentParser arguments(&argc, argv);
  arguments.getApplicationUsage()->setApplicationName(arguments.getApplicationName());
  arguments.getApplicationUsage()->setDescription("Manual display/camera calibration utility");
  arguments.getApplicationUsage()->addCommandLineOption("--config <filename>","Display server config JSON file");
  arguments.getApplicationUsage()->addCommandLineOption("--display-mode <name>","Display mode");

  osg::ApplicationUsage::Type help = arguments.readHelpType();
  if (help != osg::ApplicationUsage::NO_HELP) {
      arguments.getApplicationUsage()->write(std::cout);
      exit(0);
  }

  std::string config_filename = "~/flyvr-devel/flyvr/flyvr/config/config.json";
  while(arguments.read("--config", config_filename));

  std::string display_mode = "vr_display";
  while(arguments.read("--display-mode", display_mode));

  std::string flyvr_basepath = "/home/john/Programming/flyvr.git/flyvr/";
  float observer_radius = 0.01;
  bool two_pass = false;
  bool show_geom_coords = false;

  osg::Vec3 observer_position(0,0,0);
  osg::Quat observer_orientation(0,0,0,1);

  dsosg = new dsosg::DSOSG(
                        flyvr_basepath,
                        display_mode,
                        observer_radius,
                        config_filename,
                        two_pass,
                        show_geom_coords);

  dsosg->setup_viewer("display_server","{}");
  dsosg->set_stimulus_plugin("Stimulus3DDemo");

  done = false;
  while (!done) {
    float now;

    time.update();
    now = time.epochMicroseconds() * 1e6; /* microseconds to seconds */

    dsosg->update(now, observer_position, observer_orientation);
    dsosg->frame();
    done = dsosg->done();
  }

	return 0;
}

ByteIOContext*
FFmpegTagger::initIo(std::istream& istr, bool isSeekable, unsigned char* pIoBuffer)
{
    static char streamName[] = "std::istream";

    // FIXME: deallocate URLContext after stream has finished.
    URLContext* pUrlContext = new URLContext;
    pUrlContext->is_streamed = 1;
//     pUrlContext->priv_data = 0;
    pUrlContext->priv_data = &istr;
    pUrlContext->filename = streamName;
    pUrlContext->prot = new URLProtocol;
    pUrlContext->prot->name = "OMM avio wrapper for std::iostream";
    pUrlContext->prot->next = 0;
    pUrlContext->prot->url_open = 0;
//     pUrlContext->prot->url_open = (int (*)(URLContext *, const char *, int))IOOpen;
    pUrlContext->prot->url_read = (int (*) (URLContext *, unsigned char *, int))FFmpegTagger::IORead;
    pUrlContext->prot->url_write = 0;
    if (isSeekable) {
        pUrlContext->prot->url_seek = (int64_t (*) (URLContext *, int64_t, int))FFmpegTagger::IOSeek;
    }
    else {
        pUrlContext->prot->url_seek = 0;
    }
    pUrlContext->prot->url_close = 0;
//     pUrlContext->prot->url_close = (int (*)(URLContext *))IOClose;

    LOG(ffmpeg, trace, "ffmpeg::av_alloc_put_byte() ...");
    ByteIOContext* pIoContext;
    if (isSeekable) {
        pIoContext = av_alloc_put_byte(pIoBuffer, _IoBufferSize, 0, pUrlContext, FFmpegTagger::IORead, 0, FFmpegTagger::IOSeek);
    }
    else {
        pIoContext = av_alloc_put_byte(pIoBuffer, _IoBufferSize, 0, pUrlContext, FFmpegTagger::IORead, 0, 0);
        pIoContext->is_streamed = 1;
    }
    pIoContext->max_packet_size = _IoBufferSize;

    _timestamp.update();
    _startTimestamp.update();

    return pIoContext;
}

void XplDevice::run ( void )
{
    if ( !m_bInitialised )
    {
        assert ( 0 );
        return;
    }

    while ( !m_bExitThread )
    {
        // Deal with heartbeats
        int64_t currentTime;
        Poco::Timestamp tst;
        tst.update();
        currentTime = tst.epochMicroseconds();
        //GetSystemTimeAsFileTime( (FILETIME*)&currentTime );
        
        if ( m_nextHeartbeat <= currentTime )
        {
            poco_debug( devLog, "Sending heartbeat" );
            // It is time to send a heartbeat
            if ( m_bConfigRequired )
            {
                // Send a config heartbeat, then calculate the time of the next one
                m_pComms->SendConfigHeartbeat ( m_completeId, m_heartbeatInterval, m_version );
            }
            else
            {
                // Send a heartbeat, then calculate the time of the next one
                m_pComms->SendHeartbeat ( m_completeId, m_heartbeatInterval, m_version );
            }

            SetNextHeartbeatTime();
        }
        // Calculate the time (in milliseconds) until the next heartbeat
        int32 heartbeatTimeout = ( int32 ) ( ( m_nextHeartbeat - currentTime ) ); // Divide by 10000 to convert 100 nanosecond intervals to milliseconds.
        poco_debug ( devLog, "Sleeping " + NumberFormatter::format ( heartbeatTimeout/1000 ) + " seconds till next hbeat" );
        m_hRxInterrupt->tryWait ( heartbeatTimeout );
        //Thread::sleep();
        poco_debug ( devLog, "Woken up for hbeat or interrupt" );

    }
// 	cout << "exiting dev thread (ret)\n";
    return;
}

void XplDevice::SetNextHeartbeatTime()
{
    // Set the new heartbeat time
    int64_t currentTime;
    Poco::Timestamp tst;
    tst.update();
    currentTime = tst.epochMicroseconds();
    //GetSystemTimeAsFileTime( (FILETIME*)&currentTime );

    // If we're waiting for a hub, we have to send at more
    // rapid intervals - every 3 seconds for the first two
    // minutes, then once every 30 seconds after that.
    if ( m_bWaitingForHub )
    {
        if ( m_rapidHeartbeatCounter )
        {
            // This counter starts at 40 for 2 minutes of
            // heartbeats at 3 second intervals.
            --m_rapidHeartbeatCounter;


            m_nextHeartbeat = currentTime + ( ( int64_t ) c_rapidHeartbeatFastInterval * 1000l );
        }
        else
        {
            // one second
            m_nextHeartbeat = currentTime + ( ( int64_t ) c_rapidHeartbeatSlowInterval * 1000l );
        }
    }
    else
    {
        // It is time to send a heartbeat
        if ( m_bConfigRequired )
        {
            // one minute
            m_nextHeartbeat = currentTime + 60*1000l;
        }
        else
        {
            // 60000000 is one minute in the 100 nanosecond intervals
            // that the system time is measured in.
            m_nextHeartbeat = currentTime + ( ( int64_t ) m_heartbeatInterval * 60*1000l );
        }
    }
}

	void run()
	{
		Poco::Timestamp lastScan;
		ItemInfoMap entries;
		scan(entries);

		while (!_stopped)
		{
			struct timespec timeout;
			timeout.tv_sec = 0;
			timeout.tv_nsec = 200000000;
			unsigned eventFilter = NOTE_WRITE;
			struct kevent event;
			struct kevent eventData;
			EV_SET(&event, _dirFD, EVFILT_VNODE, EV_ADD | EV_CLEAR, eventFilter, 0, 0);
			int nEvents = kevent(_queueFD, &event, 1, &eventData, 1, &timeout);
			if (nEvents < 0 || eventData.flags == EV_ERROR)
			{
				try
				{
					FileImpl::handleLastErrorImpl(owner().directory().path());
				}
				catch (Poco::Exception& exc)
				{
					owner().scanError(&owner(), exc);
				}
			}
			else if (nEvents > 0 || ((owner().eventMask() & DirectoryWatcher::DW_ITEM_MODIFIED) && lastScan.isElapsed(owner().scanInterval()*1000000)))
			{
				ItemInfoMap newEntries;
				scan(newEntries);
				compare(entries, newEntries);
				std::swap(entries, newEntries);
				lastScan.update();
			}
		}
	}

void RealSenseSensorImpl::faceDetect(bool depthMask)
{
	Poco::Int64 maskTime = 0;
	Poco::Int64 detectTime = 0;

	Poco::Timestamp now;
	ImageGrayscale mask(grayscalePreview.clone());
	if (depthMask) {
		PXCCapture::Sample *sample = senseManager->QuerySample();
		mask = RealSenseProcessor::depthMask(sample->depth, sample->color, projection, grayscalePreview);
		//cv::imshow("Preview mask", mask);
	}
	maskTime = now.elapsed();

	now.update();
	cv::resize(mask, resizedPreview, cv::Size(320, 240), 0, 0, CV_INTER_NN);
	roiPreview = resizedPreview(detectRoi);
	tracker->detect(roiPreview);
	detectTime = now.elapsed();

	if (depthMask && tracker->isLastDetect()) {
		cv::Rect faceRoi = toRealSizeRoi(tracker->getLastRegion());
		mask = RealSenseProcessor::fillHoles(faceRoi, mask, grayscalePreview, 0.0);
		//cv::imshow("grayscalePreview mask 2", mask);
	}

	//std::cout << std::dec << "Mask time: " << maskTime << "; Face detect time: " << detectTime << std::endl;

	/*
	if (Face::Settings::instance().debug()) {
		if (tracker->isLastDetect()) {
			cv::rectangle(colorPreview, toRealSizeRoi(tracker->getLastRegion()), 255);
		}
		//cv::imshow("detection", roiPreview);
	}*/
}

//.........这里部分代码省略.........
                    pI::ShortMatrix depth (getImageAndDepth_out[1]);
                    double sumDistance = 0.;
                    int sumCount = 0;

                    for (int j = 230; j <= 250; ++j) {
                        for (int i = 310; i <= 330; ++i) {
                            int dep = depth.GetData (j, i);

                            if (dep < 2047) {
                                sumDistance += RawDepthToMeters (dep);
                                ++sumCount;
                            }
                        }
                    }

                    if (sumCount > 0) {
                        avgDistance = sumDistance / (double) sumCount;
                    }

                    char str[32];
                    sprintf (str, "%.2f m", avgDistance);
                    puttext_text.SetData (str);

                    puttext->Execute (puttext_in, puttext_out);
                }

                // ... and display both the image and the depth map.
                displayImage->Execute (displayImage_in, displayImage_out);
                displayDepth->Execute (displayDepth_in, displayDepth_out);

                std::cout << "Tilt " << (int) newTilt.GetData();
                std::cout << "; " << (1000000. / now.elapsed()) << " frames/sec          \r";

                now.update();
            }

            int ch = getch();

            switch (ch) {
            case 'q':
            case 'Q':
            case 27 /* ESC */:
                // Terminates
                newTilt.SetData (0); // Tilt angle 0 degree
                newLED.SetData (0); // LED status off

                setTiltAndLED->Execute (setTiltAndLED_in, setTiltAndLED_out);

                doLoop = false;
                break;

            case 'm':
            case 'M': // Measurement mode
                showDistance = !showDistance;
                break;

            case 's':
            case 'S': { // Takes snapshot

                std::string dateTime = Poco::DateTimeFormatter::format (Poco::LocalDateTime(), "%Y-%m-%d_%H-%M-%S");

                std::string name = std::string ("KinectImage_") + dateTime + std::string (".png");
                fileName.SetData (const_cast<char*> (name.c_str()));

                saveImage->Execute (saveImage_in);

 void UnGhost() { 
     unghost = true; 
     unghost_request_time.update(); 
 }

void RealSenseSensorImpl::singleCapture(bool calcUvMap, bool calcLandmarks, bool releaseFrame)
{
	Poco::Int64 acquireTime = 0;
	Poco::Int64 getBuffersTime = 0;
	
	Poco::Timestamp now;
	sts = senseManager->AcquireFrame(true);

	if (sts != PXC_STATUS_NO_ERROR) {
		senseManager->ReleaseFrame();
		std::string msg("Can't acquire frame from camera (" + std::to_string(sts) + ")");
		std::cerr << msg << std::endl;
		throw FACELIB_EXCEPTION(msg);
	}
	PXCCapture::Sample *sample = senseManager->QuerySample();
	acquireTime = now.elapsed();
	
	now.update();
	getBuffers(sample->depth, sample->color, calcUvMap, calcLandmarks);
	getBuffersTime = now.elapsed();

	try {
		brightnessToSet = 0;
		Poco::Int64 detectFaceRoiTime = 0;
		Poco::Int64 meanCalcTime = 0;
		Poco::Int64 queryColorBrightnessTime = 0;
		Poco::Int64 queryColorBrightnessInfoTime = 0;
		Poco::Int64 setColorBrightnessTime = 0;

		now.update();
		cv::Rect croi = realsenseDepthFaceTracker->detectFaceRoi(depthMatrix, sample->depth, RealSenseDepthFaceTracker::Domain::Color, false /*debug*/);
		detectFaceRoiTime = now.elapsed();

		// brightness control
		if ((settings.brightnessControl == static_cast<int>(RealSenseSensor::BrightnessControlType::HeadRoiBased) && croi.area() && 
			(state == RealSenseSensor::STATE_POSITIONING || state == RealSenseSensor::STATE_IDLE))) {
			now.update();
			cv::Mat mask = cv::Mat::zeros(grayscalePreview.size(), CV_8U);
			cv::rectangle(mask, croi, cv::Scalar(255), CV_FILLED);
			int mean = cv::mean(grayscalePreview, mask)[0];
			meanCalcTime = now.elapsed();

//			if (Face::Settings::instance().debug()) {
//				cv::Mat maskDrawing = grayscalePreview.clone();
//				cv::rectangle(maskDrawing, croi, cv::Scalar(255));
//				cv::imshow("Depth roi for gain projected to color", maskDrawing);
//			}

			now.update();
			pxcI32 brightness = device->QueryColorBrightness();
			queryColorBrightnessTime = now.elapsed();
			diff = targetColorMean - mean;

			auto newValue = [=](const PXCCapture::Device::PropertyInfo& info, pxcI32 val, pxcI32 newVal) -> pxcI32 {
				pxcI32 newV = std::max(info.range.min, std::min(static_cast<pxcF32>(newVal), info.range.max));
				if (Face::Settings::instance().debug()) {
					std::stringstream sstr;
					sstr << "Val: " << val << "; adjusted val: " << newV << "; mean: " << mean << "/" << targetColorMean;
					shadowText(grayscalePreview, sstr.str(), cv::Point(150, 30), 0.5);
				}
				return newV;
			};

			int _diff = diff == 0 ? 0 : diff / abs(diff);
			pxcI32 newBrightness = newValue(device->QueryColorBrightnessInfo(), brightness, brightness + _diff);
			if (abs(diff) > 15) {
				brightnessToSet = newBrightness;
			}
		}
	} catch (std::exception& e) {
		std::cout << " --- Brightness control exception: " << e.what() << std::endl;
	}

	//std::cout << std::dec << "Acquire time: " << acquireTime << "; getBuffers time: " << getBuffersTime << std::endl;

	if (releaseFrame) {
		senseManager->ReleaseFrame();
	}
}

void RealSenseSensorImpl::doPositioning()
{
	Poco::Int64 singleCaptureTime = 0;
	Poco::Int64 faceDetectTime = 0;
	Poco::Int64 pose1Time = 0;
	Poco::Int64 pose2Time = 0;
	Poco::Int64 frameReleaseTime = 0;
	Poco::Int64 frameTime = 0;

	static Poco::Timestamp frameTimer;

	RealSenseSensor::PositioningOutput prevPosOutput = positioningOutput;
	positioningOutput = RealSenseSensor::POS_NONE;

	Poco::Timestamp now;
	singleCapture(false, false, false);
	singleCaptureTime = now.elapsed();
	
	now.update();
	faceDetect(settings.hasPosFeature(RealSenseSensor::PositioningFeature::DepthMask));
	faceDetectTime = now.elapsed();

	frameTime = frameTimer.elapsed();
	frameTimer.update();

	calcMinZ();

	if (tracker->getConsecutiveNonDetects() >= settings.consecutiveNonDetectsToStopPositioning ||
			(tracker->isLastDetect() && nearestZpoint > settings.maximalPositioningDistance)) {
		setState(RealSenseSensor::STATE_IDLE);
		senseManager->ReleaseFrame();
		return;
	}

	if (tracker->getConsecutiveNonDetects() > 10) {
		positioningOutput = RealSenseSensor::POS_NONE;
		senseManager->ReleaseFrame();
		return;
	}

	if (!tracker->isLastDetect()) {
		senseManager->ReleaseFrame();
		return;
	}

	cv::Rect reg = toRealSizeRoi(tracker->getLastRegion());
	double distance = calcDistance(reg);

	now.update();

	FacePoseEstimator::Pose pose = FacePoseEstimator::Pose::UnknownPose();

	if (settings.hasPosFeature(RealSenseSensor::PositioningFeature::PoseEstimation) && tracker->isLastDetect()) {
		landmarks = lmDetector.getLandmarks(grayscalePreview, toRealSizeRoi4LMDetector(tracker->getLastRegion()), false);
		std::map<std::string, std::vector<cv::Point2d> > selectedLMPoints = lmDetector.lmGroupPoints(landmarks);
		pose1Time = now.elapsed();
		
		now.update();
		pose = facePoseEstimator->facePose(selectedLMPoints, realsenseProjection);
		pose2Time = now.elapsed();

		if (Face::Settings::instance().debug() && pose != FacePoseEstimator::Pose::UnknownPose()) {
			cv::Scalar red(0, 0, 255);
			cv::Scalar green(0, 255, 0);
			cv::Scalar white(255, 255, 255);
			cv::Scalar black(0, 0, 0);

			{
				positioningFrameStats.update();
				std::stringstream sstr;
				sstr.precision(1);
				sstr.setf(std::ios::fixed, std::ios::floatfield);
				sstr << "Pitch: " << pose.pitch - pose.relativePitch << "/" << facePoseEstimator->getSettings().pitchZeroAngle <<
					", Yaw: " << pose.yaw - pose.relativeYaw << "/" << facePoseEstimator->getSettings().yawZeroAngle <<
					", FPS: " << positioningFrameStats.fps();
				sstr.flush();
				shadowText(grayscalePreview, sstr.str(), cv::Point(150, 15), 0.5);

				{
					std::stringstream sstrDebug;
					sstrDebug << std::dec << "Capture time: " << singleCaptureTime / 1000 << "; FaceDetect time: " << faceDetectTime / 1000;
					shadowText(grayscalePreview, sstrDebug.str(), cv::Point(150, 45), 0.5);
				}

				{
					std::stringstream sstrDebug;
					sstrDebug << std::dec << "pose time: " << pose1Time / 1000 << "/" << pose2Time / 1000 << "; frame time: " << frameTime / 1000;
					shadowText(grayscalePreview, sstrDebug.str(), cv::Point(150, 60), 0.5);
				}

				auto mirror = [](const cv::Mat& img, const cv::Point& p) -> cv::Point {
					return cv::Point(img.cols - p.x, p.y);
				};

				cv::line(grayscalePreview, mirror(grayscalePreview, pose.imageAxisX.from), mirror(grayscalePreview, pose.imageAxisX.to), black);
				cv::line(grayscalePreview, mirror(grayscalePreview, pose.imageAxisY.from), mirror(grayscalePreview, pose.imageAxisY.to), black);
			}

			cv::rectangle(grayscalePreview, toRealSizeRoi4LMDetector(tracker->getLastRegion()), 255);

//.........这里部分代码省略.........

int main(int argc, char** argv) {

	int maxManSpeed = 30000;

	try {

		TBS::initLogs("cvclient", 8);

		std::cout << "HAL Client Starts" << std::endl;
		Mat image(200, 200, CV_8UC3);
		Mat imageRadar(50, 200, CV_8UC3);

		cv::line(image, cv::Point(0, 100), cv::Point(200, 100), cv::Scalar(255, 0, 0), 1);
		cv::line(image, cv::Point(100, 0), cv::Point(100, 200), cv::Scalar(255, 0, 0), 1);

		namedWindow("movement", 0);
		setMouseCallback("movement", onMouse, 0);

		namedWindow("bioradar", 0);
		setMouseCallback("bioradar", onMouse2, 0);

		TBS::Services::JsonClientParams bp("localhost", HAL::API::Communication::BioRadarPort, TBS::Services::JsonClientParams::JsonHttp);
		bioRadarClient = new HAL::API::BioRadar::Json::Client(bp);

		TBS::Services::JsonClientParams cp("localhost", HAL::API::Communication::CameraPort, TBS::Services::JsonClientParams::JsonHttp);
		cameraClient = new HAL::API::Camera::Json::Client(cp);

		TBS::Services::JsonClientParams mc("localhost", HAL::API::Communication::ManipulatorPort, TBS::Services::JsonClientParams::JsonHttp);
		manipulatorClient = new HAL::API::Manipulator::Json::Client(mc);

		client = new HAL::API::MovementClient();
		client->Movement().StatusChanged += Poco::delegate(&updateSpeed);
		for (;;) {

			cv::Mat cpy = image.clone();
			drawSpeed(cpy);
			imshow("movement", cpy);
			imshow("bioradar", cpy);

			int c = waitKey(100);
			if ((c & 255) == 27) {
				cout << "Exiting ...\n";
				break;
			}

			try {
				static int speed = 0;
				static Poco::Timestamp tst;
				static int lastInRow = 0;
				if ((c & 255) == 'q') {
					lastInRow++;
					speed = 50;
					bioRadarClient->BioRadar().GoRelBase(speed);
				} else if ((c & 255) == 'a') {
					speed = -50;
					lastInRow++;
					bioRadarClient->BioRadar().GoRelBase(speed);
				} else {
					speed = 0;
					if (lastInRow > 1) {
						bioRadarClient->BioRadar().GoRelBase(speed);
					}
					lastInRow = 0;
				}
				//std::cout << "tst " << tst.elapsed() / 1000 << "ms - speed: " << speed << std::endl;
				tst.update();
			} catch (Poco::Exception & e) {

			}

			try {
				static int speed = 0;
				static Poco::Timestamp tst;
				static int lastInRow = 0;
				if ((c & 255) == 'w') {
					lastInRow++;
					speed = 20;
					bioRadarClient->BioRadar().GoRelAntenna(speed);
				} else if ((c & 255) == 's') {
					speed = -20;
					lastInRow++;
					bioRadarClient->BioRadar().GoRelAntenna(speed);
				} else {
					speed = 0;
					if (lastInRow > 1) {
						bioRadarClient->BioRadar().GoRelAntenna(speed);
					}
					lastInRow = 0;
				}
				//std::cout << "tst " << tst.elapsed() / 1000 << "ms - speed: " << speed << std::endl;
				tst.update();
			} catch (Poco::Exception & e) {

			}

			try {
				if ((c & 255) == 'e') {
					bioRadarClient->BioRadar().Enable();
				} else if ((c & 255) == 'd') {
					bioRadarClient->BioRadar().Disable();
//.........这里部分代码省略.........