void Eye2world::WorkerThread::run() { Vector head6d(6); Vector torso3d(3); double prevZOffset = 0.0; #ifndef DEBUG_OFFLINE bool positionUpdated = false; bool transformationAvailable = false; BufferedPort < Bottle > *in = (BufferedPort<Bottle>*) dataPorts[id.Input_Coordinates2d]; BufferedPort < Bottle > *head = (BufferedPort<Bottle>*) dataPorts[id.Input_HeadPosition]; BufferedPort < Bottle > *torso = (BufferedPort<Bottle>*) dataPorts[id.Input_TorsoPosition]; #endif BufferedPort < Bottle > *out = (BufferedPort<Bottle>*) dataPorts[id.Output_Coordinates3d]; while (!isStopping()) { string camera = "left"; Vector object2d(2); double zOffset = 0; #ifdef DEBUG_OFFLINE yarp::os::Time::delay(1); #else inputCoordinates = in->read(true); if (isStopping() || inputCoordinates == NULL) { break; } unsigned int coordinatesPos = 0; if (inputCoordinates->get(0).isString()) { camera = inputCoordinates->get(0).asString().c_str(); // optional! default: 0.0 coordinatesPos = 1; } else { // Assume (x,y) pair! // This is for testing with YARP viewer click port. } double motor2eye = moduleOptions["motor2eye"].getValue().asDouble(); for (unsigned int i = 0; i < 2; i++) { object2d[i] = inputCoordinates->get(coordinatesPos + i).asDouble(); } zOffset = moduleOptions["heightOffset"].getValue().asDouble(); zOffset += motor2eye; zOffset += inputCoordinates->get(coordinatesPos + 2).asDouble(); positionUpdated = zOffset != prevZOffset; Bottle* headPosition = head->read(!transformationAvailable); if (headPosition != NULL && headPosition->size() >= 6) { //get data and convert from degrees to radiant for (unsigned int i = 0; i < 6; i++) { head6d[i] = headPosition->get(i).asDouble() * M_PI / 180.0; } positionUpdated = true; } if (isStopping()) { break; } Bottle* torsoPosition = torso->read(!transformationAvailable); if (torsoPosition != NULL && torsoPosition->size() >= 3) { //get data and convert from degrees to radiant for (unsigned int i = 0; i < 3; i++) { torso3d[i] = torsoPosition->get(i).asDouble() * M_PI / 180.0; } positionUpdated = true; } if (isStopping()) { break; } if (positionUpdated || !transformationAvailable) { // The 2nd one is just to be sure ;) projections[camera]->setHeightOffset(zOffset, false); projections[camera]->setBaseTransformation(torso3d, head6d); transformationAvailable = true; } #endif Vector object3d(3); projections[camera]->project(object2d, object3d); // Add scalor to the result double scale = moduleOptions["scale"].getValue().asDouble(); object3d[0] = object3d[0] * scale; object3d[1] = object3d[1] * scale; object3d[2] = object3d[2] - motor2eye; Bottle &outputCoordinates = out->prepare(); outputCoordinates.clear(); for (unsigned int i = 0; i < 3; i++) { outputCoordinates.addDouble(object3d[i]); } cout << "out = " << outputCoordinates.toString() << endl; out->write(); } }
void run(void) { Frame* frame = NULL; while (!isStopping()) { if (!Poll::poll(m_socket, 1.0)) continue; size_t rv = m_socket.read(m_buffer, m_buffer_capacity); if (rv == c_header_size) { m_count = 0; frame = dequeueClean(); if (frame == NULL) { m_task->err(DTR("buffer overrun")); break; } frame->setTimeStamp(); } else if (rv == c_footer_size) { enqueueDirty(frame); frame = NULL; } else { if (frame != NULL) { uint16_t packet_number = 0; ByteCopy::fromBE(packet_number, m_buffer + 6); m_count += frame->writePacket(packet_number, m_buffer + 8, rv - 8); } else { m_task->err(DTR("null frame")); } } } }
void DriverThread::work() { while ( !isStopping() ) { try { setMaxHeartbeatInterval( 5.0 ); enableHardware(); setMaxHeartbeatInterval( 1.0 ); operateHardware(); } catch ( ... ) { string problem = hydroiceutil::catchExceptionsWithStatusAndSleep( "work loop", health(), gbxutilacfr::SubsystemFault, 1000 ); stateMachine_.setFault( problem ); } } }
//-------------------------------------------------------------------------- // // Runs the next appropriate waiting Job. // // Pre-conditions: // A RunnableJob must exist in the JobSet // // Post-conditions: // The chosen RunnableJob will have Job::doJob() called. // // Invariants: // <none> // void processTask () { Job job; { ScopedLock lock (m_mutex); getNextJob (job, lock); ++m_processCount; } JobTypeData& data (getJobTypeData (job.getType ())); // Skip the job if we are stopping and the // skipOnStop flag is set for the job type // if (!isStopping() || !data.info.skip ()) { beast::Thread::setCurrentThreadName (data.name ()); m_journal.trace << "Doing " << data.name () << " job"; Job::clock_type::time_point const start_time ( Job::clock_type::now()); on_dequeue (job.getType (), start_time - job.queue_time ()); job.doJob (); on_execute (job.getType (), Job::clock_type::now() - start_time); } else { m_journal.trace << "Skipping processTask ('" << data.name () << "')"; } { ScopedLock lock (m_mutex); finishJob (job, lock); --m_processCount; checkStopped (lock); } // Note that when Job::~Job is called, the last reference // to the associated LoadEvent object (in the Job) may be destroyed. }
void NDirWatcherThread::parseSharedDirs() { //logDebug("NDirWatcherThread", "parseSharedDirs start"); for(int i = 0; i < m_sharedDirectories.count(); i++) { const NDir & dir = m_sharedDirectories[i]; if (isStopping()) break; if (!dir.shared()) continue; if (!dir.dir().exists()) continue; parseDir(dir.dir().absolutePath(), dir.recursive(), dir.dir().absolutePath()); } //logDebug("NDirWatcherThread", "parseSharedDirs stop"); }
bool GiGraphics::drawArc(const GiContext* ctx, const Point2d& center, float rx, float ry, float startAngle, float sweepAngle, bool modelUnit) { if (rx < _MGZERO || fabsf(sweepAngle) < 1e-5f || isStopping()) return false; if (ry < _MGZERO) ry = rx; if (!DRAW_RECT(m_impl, modelUnit).isIntersect(Box2d(center, 2 * rx, 2 * ry))) return false; Point2d points[16]; int count = mgcurv::arcToBezier(points, center, rx, ry, startAngle, sweepAngle); S2D(xf(), modelUnit).transformPoints(count, points); return count > 3 && rawBeziers(ctx, points, count); }
void DriverThread::enableHardware() { while ( !isStopping() ) { stringstream exceptionSS; try { segwayRmp_.enable(); tracer_.info( "DriverThread: Enable succeeded." ); stateMachine_.setOK(); callback_.hardwareInitialised(); return; } catch ( ... ) { string problem = hydroiceutil::catchExceptionsWithStatusAndSleep( "enabling", health(), gbxutilacfr::SubsystemFault, 2000 ); stateMachine_.setFault( problem ); } } cout<<"TRACE(driverthread.cpp): dropping out of enableHardware. isStopping(): " << isStopping() << endl; }
bool sceneFlowModule::updateModule() { if(isStopping()) return false; // Whole Flow Access int u=160; int v=120; Mat flow3D; sceneFlow->getSceneFlow(flow3D); Point3f Pflow3D; if(!flow3D.empty()) { Pflow3D.x=flow3D.ptr<float>(v)[2*u]; // Flow DeltaX Pflow3D.y=flow3D.ptr<float>(v)[2*u+1]; // Flow DeltaY Pflow3D.z=flow3D.ptr<float>(v)[2*u+2]; // Flow DeltaZ fprintf(stdout,"3D Motion of Pixel (%i,%i): (%f, %f, %f) \n",u,v,Pflow3D.x,Pflow3D.y,Pflow3D.z); // Compute and show motion field and flow module IplImage* module=cvCreateImage(cvSize(sceneFlow->getImgWidth(),sceneFlow->getImgHeight()),8,3); IplImage* flowField=sceneFlow->draw2DMotionField(); sceneFlow->drawFlowModule(module); if(flowField!=NULL) cvShowImage("flowField", flowField); cvShowImage("Module", module); cvWaitKey(5); cvReleaseImage(&flowField); cvReleaseImage(&module); } return true; }
std::shared_ptr <SHAMap> getSet ( uint256 const& hash, bool acquire) override { TransactionAcquire::pointer ta; { ScopedLockType sl (mLock); auto it = m_map.find (hash); if (it != m_map.end ()) { if (acquire) { it->second.mSeq = m_seq; if (it->second.mAcquire) { it->second.mAcquire->stillNeed (); } } return it->second.mSet; } if (!acquire || isStopping ()) return std::shared_ptr <SHAMap> (); ta = std::make_shared <TransactionAcquire> (hash, m_clock); auto &obj = m_map[hash]; obj.mAcquire = ta; obj.mSeq = m_seq; } ta->init (startPeers); return {}; }
void OverlayImpl::connect (beast::IP::Endpoint const& remote_endpoint) { if (isStopping()) { m_journal.debug << "Skipping " << remote_endpoint << " connect on stop"; return; } PeerFinder::Slot::ptr const slot ( m_peerFinder->new_outbound_slot (remote_endpoint)); if (slot == nullptr) return; MultiSocket::Flag const flags ( MultiSocket::Flag::client_role | MultiSocket::Flag::ssl); PeerImp::ptr const peer (std::make_shared <PeerImp> ( remote_endpoint, m_io_service, *this, m_resourceManager, *m_peerFinder, slot, m_ssl_context, flags)); { std::lock_guard <decltype(m_mutex)> lock (m_mutex); { std::pair <PeersBySlot::iterator, bool> const result ( m_peers.emplace (slot, peer)); assert (result.second); } ++m_child_count; // This has to happen while holding the lock, // otherwise the socket might not be canceled during a stop. peer->start (); } }
void xdPort::run() { while (!isStopping() && !closing) { syncEvent.reset(); syncEvent.wait(); Vector theta=static_cast<Solver*>(slv)->neckTargetRotAngles(xd); double timeDelay=0.0; if (norm(theta)<NECKSOLVER_RESTORINGANGLE*CTRL_DEG2RAD) timeDelay=NECKSOLVER_ACTIVATIONDELAY; Time::delay(timeDelay); mutex_1.wait(); xdDelayed=xd; isNewDelayed=true; mutex_1.post(); } }
bool GiGraphics::drawPolygon(const GiContext* ctx, int count, const Point2d* points, bool modelUnit) { if (count < 2 || !points || isStopping()) return false; count = count > 0x2000 ? 0x2000 : count; ctx = ctx ? ctx : &(m_impl->ctx); bool ret = false; const Box2d extent (count, points); // 模型坐标范围 if (!DRAW_RECT(m_impl, modelUnit).isIntersect(extent)) // 全部在显示区域外 return false; if (DRAW_MAXR(m_impl, modelUnit).contains(extent)) { // 全部在显示区域内 ret = _drawPolygon(ctx, count, points, true, true, true, modelUnit); } else { // 部分在显示区域内 PolygonClip clip (m_impl->rectDraw); if (!clip.clip(count, points, &S2D(xf(), modelUnit))) // 多边形剪裁 return false; count = clip.getCount(); points = clip.getPoints(); ret = _drawPolygon(ctx, count, points, false, true, false, modelUnit); int ienter = findInvisibleEdge(clip); if (ienter == count) { ret = _drawPolygon(ctx, count, points, false, false, true, modelUnit) || ret; } else { ret = drawPolygonEdge(PolylineAux(this, ctx), count, clip, ienter) || ret; } } return ret; }
bool GiGraphics::drawPie(const GiContext* ctx, const Point2d& center, float rx, float ry, float startAngle, float sweepAngle, bool modelUnit) { if (rx < _MGZERO || fabsf(sweepAngle) < 1e-5f || isStopping()) return false; if (ry < _MGZERO) ry = rx; const Box2d extent (center, rx*2.f, ry*2.f); // 模型坐标范围 if (!DRAW_RECT(m_impl, modelUnit).isIntersect(extent)) // 全部在显示区域外 return false; Point2d pxs[16]; int count = mgcurv::arcToBezier(pxs, center, rx, ry, startAngle, sweepAngle); if (count < 4) return false; S2D(xf(), modelUnit).transformPoints(count, pxs); Point2d cen(center * S2D(xf(), modelUnit)); bool ret = rawBeginPath(); if (ret) { rawMoveTo(cen.x, cen.y); rawLineTo(pxs[0].x, pxs[0].y); for (int i = 1; i + 2 < count; i += 3) { rawBezierTo(pxs[i].x, pxs[i].y, pxs[i+1].x, pxs[i+1].y, pxs[i+2].x, pxs[i+2].y); } rawClosePath(); ret = rawEndPath(ctx, true); } return ret; }
bool GiGraphics::setClipBox(const RECT_2D& rc) { if (!isDrawing() || isStopping()) return false; bool ret = false; Box2d rect; if (!rect.intersectWith(Box2d(rc), Box2d(m_impl->clipBox0)).isEmpty()) { if (rect != Box2d(m_impl->clipBox)) { rect.get(m_impl->clipBox); m_impl->rectDraw.set(Box2d(rc)); m_impl->rectDraw.inflate(GiGraphicsImpl::CLIP_INFLATE); m_impl->rectDrawM = m_impl->rectDraw * xf().displayToModel(); m_impl->rectDrawW = m_impl->rectDrawM * xf().modelToWorld(); SafeCall(m_impl->canvas, clipRect(m_impl->clipBox.left, m_impl->clipBox.top, m_impl->clipBox.width(), m_impl->clipBox.height())); } ret = true; } return ret; }
// Caller must hold the mutex void OverlayImpl::checkStopped () { if (isStopping() && areChildrenStopped () && list_.empty()) stopped(); }
void stereoCalibThread::monoCalibRun() { while(imagePortInLeft.getInputCount()==0 && imagePortInRight.getInputCount()==0) { fprintf(stdout, "Connect one camera.. \n"); Time::delay(1.0); if(isStopping()) return; } bool left= imagePortInLeft.getInputCount()>0?true:false; string cameraName; if(left) cameraName="LEFT"; else cameraName="RIGHT"; fprintf(stdout, "CALIBRATING %s CAMERA \n",cameraName.c_str()); int count=1; Size boardSize, imageSize; boardSize.width=this->boardWidth; boardSize.height=this->boardHeight; while (!isStopping()) { if(left) imageL = imagePortInLeft.read(false); else imageL = imagePortInRight.read(false); if(imageL!=NULL){ bool foundL=false; mutex->wait(); if(startCalibration>0) { string pathImg=imageDir; preparePath(pathImg.c_str(), pathL,pathR,count); string iml(pathL); imgL= (IplImage*) imageL->getIplImage(); Mat Left(imgL); std::vector<Point2f> pointbufL; if(boardType == "CIRCLES_GRID") { foundL = findCirclesGridDefault(Left, boardSize, pointbufL, 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); } else { foundL = findChessboardCorners(Left, boardSize, pointbufL, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE); } if(foundL) { cvCvtColor(imgL,imgL,CV_RGB2BGR); saveImage(pathImg.c_str(),imgL,count); imageListL.push_back(iml); Mat cL(pointbufL); drawChessboardCorners(Left, boardSize, cL, foundL); count++; } if(count>numOfPairs) { fprintf(stdout," Running %s Camera Calibration... \n", cameraName.c_str()); monoCalibration(imageListL,this->boardWidth,this->boardHeight,this->Kleft,this->DistL); fprintf(stdout," Saving Calibration Results... \n"); if(left) 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"); else 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_RIGHT"); fprintf(stdout, "Calibration Results Saved in %s \n", camCalibFile.c_str()); startCalibration=0; count=1; imageListL.clear(); } } mutex->post(); ImageOf<PixelRgb>& outimL=outPortLeft.prepare(); outimL=*imageL; outPortLeft.write(); ImageOf<PixelRgb>& outimR=outPortRight.prepare(); outimR=*imageL; outPortRight.write(); if(foundL && startCalibration==1) Time::delay(2.0); cout.flush(); } } }
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"); fprintf(stdout, "Calibration Results Saved in %s \n", camCalibFile.c_str()); startCalibration=0; count=1; imageListR.clear(); imageListL.clear(); imageListLR.clear(); } } mutex->post(); ImageOf<PixelRgb>& outimL=outPortLeft.prepare(); outimL=*imageL; outPortLeft.write(); ImageOf<PixelRgb>& outimR=outPortRight.prepare(); outimR=*imageR; outPortRight.write(); if(foundL && foundR && startCalibration==1) Time::delay(2.0); initL=initR=false; cout.flush(); } } delete imageL; delete imageR; }
void EndogenousSalienceThread::run(){ /* * Identify the modal hue and saturation values in the logpolar image, * segment the cartesian image accordingly, * identify the largest blob, extract the coordinates of the centroid, * package everything with the encoder values & the focal length values in a bottle, * and send it out (typically to the egosphere module) * send out the segmented image out for viewing */ unsigned char pixel_value; if (debug) { printf("endogenousSalienceThread: parameters are %d %d %d %d\n\n", *hueTolerance, *saturationTolerance, *hueBins, *saturationBins); } /* create the histogram */ if (hsHistogram == NULL) { hsHistogram = new DVhs_histogram(*hueBins, *saturationBins); } while (isStopping() != true) { // the thread continues to run until isStopping() returns true /* * Step 1: grab cartesian and logpolar images and copy images to local format * ========================================================================== */ if (debug) cout << "endogenousSalienceThread: grabbing images " << endl; do { cartesianImage = cartesianImagePortIn->read(true); } while ((cartesianImage == NULL) && (isStopping() != true)); if (isStopping()) break; // abort this loop to avoid make sure we don't continue and possibly use NULL images do { logpolarImage = logpolarImagePortIn->read(true); } while ((logpolarImage == NULL)&& (isStopping() != true)); if (isStopping()) break; // abort this loop to avoid make sure we don't continue and possibly use NULL images width = cartesianImage->width(); height = cartesianImage->height(); depth = 3; if (debug) printf("endogenousSalienceThread: width = %d, height = %d, depth = %d\n",width, height, depth); if (cartesianInput == NULL) { cartesianInput = new DVimage(width, height, depth); } if (segmentedOutput == NULL) { segmentedOutput = new DVimage(width, height, depth); } if (tempImageA == NULL) { tempImageA = new DVimage(width, height, depth); } if (tempImageB == NULL) { tempImageB = new DVimage(width, height, depth); } width = logpolarImage->width(); height = logpolarImage->height(); depth = 3; if (debug) printf("endogenousSalienceThread: width = %d, height = %d, depth = %d\n",width, height, depth); if (logpolarInput == NULL) { logpolarInput = new DVimage(width, height, depth); } width = cartesianImage->width(); height = cartesianImage->height(); for (x=0; x<width; x++) { for (y=0; y<height; y++) { rgbPixel = cartesianImage->safePixel(x,y); cartesianInput->put_pixel(x, y, rgbPixel.r, 0); cartesianInput->put_pixel(x, y, rgbPixel.g, 1); cartesianInput->put_pixel(x, y, rgbPixel.b, 2); } } width = logpolarImage->width(); height = logpolarImage->height(); for (x=0; x<width; x++) { for (y=0; y<height/2; y++) { // ***************************** only use the top half, i.e. the fovea rgbPixel = logpolarImage->safePixel(x,y); logpolarInput->put_pixel(x, y, rgbPixel.r, 0); logpolarInput->put_pixel(x, y, rgbPixel.g, 1); logpolarInput->put_pixel(x, y, rgbPixel.b, 2); } } /* * Step 2: Identify the modal hue and saturation values in the logpolar image * ========================================================================== * */ if (debug) cout << "endogenousSalienceThread: generating histogram " << endl; colour_histogram(logpolarInput, hsHistogram); if (true && debug) cout << "endogenousSalienceThread: identifying histogram mode " << endl; hsHistogram->hsMode(&hueMode, &saturationMode); /* * Step 3: segment the cartesian image * ===================================== */ if (debug) cout << "endogenousSalienceThread: performing segmentation " << endl; hueRange = (*hueTolerance * (float) 360) / (float) (100); // convert percentage to real values saturationRange = (*saturationTolerance) / (float) (100); colour_segmentation(cartesianInput, hueMode, saturationMode, hueRange, saturationRange, segmentedOutput); // Step 4: filter the colour_segmentation by performing an morphological opening // ============================================================================= if (debug) cout << "endogenousSalienceThread: post-segmentation filtering - morphological opening " << endl; if (*filterRadius > 0) { erosion (segmentedOutput, *filterRadius, tempImageA); dilation(tempImageA, *filterRadius, tempImageB); } // Step 5: filter the colour segmentation by performing a medial distance transform // ============================================================================= if (debug) cout << "endogenousSalienceThread: post-segmentation filtering - medial distance transform" << endl; distance_from_background_transform(tempImageB, segmentedOutput); segmentedOutput->contrast_stretch(); /* * Step 6: copy the segmented image to the salience image in YARP format and write it out * ====================================================================================== */ if (debug) cout << "endogenousSalienceThread: sending images " << endl; width = cartesianImage->width(); height = cartesianImage->height(); ImageOf<PixelRgb> &salienceImage = salienceImagePortOut->prepare(); salienceImage.resize(width,height); for (x=0; x < width; x++) { for (y=0; y < height; y++) { segmentedOutput->get_pixel(x, y, &pixel_value, 0); rgbPixel.r=pixel_value; segmentedOutput->get_pixel(x, y, &pixel_value, 1); rgbPixel.g=pixel_value; segmentedOutput->get_pixel(x, y, &pixel_value, 2); rgbPixel.b=pixel_value; salienceImage(x,y) = rgbPixel; } } if (isStopping()) break; // abort this loop to avoid make sure we don't continue and possibly use NULL images salienceImagePortOut->write(); } }
void ReplayConductor::work() { int id, index; int seconds, useconds; IceUtil::Time nextLogTime; IceUtil::Time realTimeTillNextItem; // should this be a configurable parameter? can't think why it would be useful. IceUtil::Time replayTimeTolerance = IceUtil::Time::microSeconds(10); while( !isStopping() ) { handleEvents(); // Check if we're playing if ( !isPlaying() ) { // Slow the loop down a bit... usleep( (int)(0.1 * 1e6) ); continue; } // // If we get to here we're playing. // // read a line and act appropriately if ( masterFileReader_.getData( seconds, useconds, id, index ) ) { // end of file context_.tracer().info( "ReplayConductor: End of File." ); pausePlaying(); continue; } nextLogTime = orcalog::iceUtilTime( seconds, useconds ); if ( id > (int)replayers_.size() ) { stringstream ss; ss << "ReplayConductor: Reference to subfile number " << id << ", when only " << replayers_.size() << " exist."; context_.tracer().error( ss.str() ); exit(1); } do { // Work out how long till we should send realTimeTillNextItem = clock_.realTimeTillNextItem( nextLogTime ); if ( realTimeTillNextItem > replayTimeTolerance ) { if ( realTimeTillNextItem > orcalog::iceUtilTime(1,0) ) { context_.tracer().warning( "ReplayConductor: long time between replays, could screw with interactivity." ); } IceUtil::ThreadControl::sleep(realTimeTillNextItem); } } while ( !isStopping() && realTimeTillNextItem > replayTimeTolerance ); // // Now send it out // // cout<<"TRACE(replayconductor.cpp): replayData("<<index<<", t="<<toString(nextLogTime)<<")" << endl; replayData( id, index ); } // end of main loop }
void RpcServerThread::run() { uint32_t eventNum = 0; uint32_t idx = 0; IoEvent* ev = nullptr; eIoAction act = eIoAction::None; try { while (!isStopping()) { _now = std::time(nullptr); RSConnMgr::_timeoutMgr->checkTimeout(_now); RSConnMgr::addConnToNotifier(); eventNum = _notifier->waitIoEvents(5000); // Here handle events. for (idx = 0; idx != eventNum; ++idx) { // We are sure that the IoEvnet is WsServerConn, // so we can use static_cast. ev = _notifier->getIoEvent(idx); act = ev->handleIoEvent(); ev->setNextAction(act); switch (act) { case eIoAction::Read: case eIoAction::ReadWrite: { if (ev->isConnection()) { RSConnMgr::updateTimeout( static_cast<RpcServerConn*>( ev->getDerivedPtr()), _now); } } // No break; case eIoAction::Write: { _notifier->updateEventAction(ev); } break; case eIoAction::Remove: { RSConnMgr::removeConn( static_cast<RpcServerConn*>(ev->getDerivedPtr())); } break; default: { PARROT_ASSERT(false); } break; } // switch } // for // Append packet which needs to be sent to connections. handleJobs(); } // while } catch (const std::system_error& e) { LOG_ERROR("RpcServerThread::run: Errno is " << e.code().message() << ". Meaning " << e.what()); // There's nothing we can do here ... PARROT_ASSERT(false); } }
void NodeStoreScheduler::doTask (NodeStore::Task& task) { task.performScheduledTask (); if ((--m_taskCount == 0) && isStopping()) stopped(); }
void RectificationThread::run(){ /* * rectify the two images */ unsigned char pixel_value; FILE *fp_out; // encoder values log file for testing ... this code is not executed during normal operation if (debug) { printf("rectificationThread: parameters are\n%4.1f\n%4.1f\n%4.1f\n%4.1f\n%4.1f\n%4.1f\n%4.1f\n%4.1f\n\n", *fxLeft,*fyLeft,*cxLeft,*cyLeft,*fxRight,*fyRight,*cxRight,*cyRight); } /* log encoder values during tests ... this code is not executed during normal operation */ if (log) { if ((fp_out = fopen("rectification.log","w")) == 0) { printf("rectificationThread: can't open output rectification.log\n"); } } while (isStopping() != true) { // the thread continues to run until isStopping() returns true /* * Step 1: determine the the camera angles which cause the epipolar distortion to be rectified * =========================================================================================== * * version is the average camera azimuth angle: vs ~ (L+R)/2 * vergence is the relative camera azimuth angle: vg = L-R * * hence: * * L = vs + vg/2 * R = vs - vg/2 * * where L and R are the angles specifying the rotation of the camera about the camera Y axis * i.e. the absolute camera azimuth angle. * * See http://wiki.icub.org/wiki/Vergence%2C_Version_and_Disparity * * However, we wish to rectify relative to, not the absolute camera azimuth angle, * but relative to the gaze angle given by the version angle. * Thus, the angles we use are L'and R', viz * * L' = L - vs = +vg/2 * R' = R - vs = -vg/2 * */ do { encoderPositions = robotPort->read(true); } while ((encoderPositions == NULL) && (isStopping() != true)); // exit loop if shutting down if (isStopping()) break; // abort this loop to avoid make sure we don't continue and possibly use NULL images vergence = (float) encoderPositions->data()[5]; // get the vergence angle if (debug) { cout << "rectificationThread: vergence angle is " << vergence << endl; } if (log) { if (fp_out != NULL) fprintf(fp_out,"Vergence angle is %f\n",vergence); } leftCameraAngle = vergence / 2; rightCameraAngle = -vergence / 2; /* * Step 2: grab left and right images and copy images to local format * ================================================================== */ if (debug) cout << "rectificationThread: grabbing images " << endl; do { leftImage = leftImagePortIn->read(true); } while ((leftImage == NULL) && (isStopping() != true)); // exit loop if shutting down do { rightImage = rightImagePortIn->read(true); } while ((rightImage == NULL) && (isStopping() != true)); // exit loop if shutting down if (isStopping()) break; // abort this loop to avoid make sure we don't continue and possibly use NULL images for (x=0; x<width; x++) { for (y=0; y<height; y++) { rgbPixel = leftImage->safePixel(x,y); leftInput->put_pixel(x, y, rgbPixel.r, 0); leftInput->put_pixel(x, y, rgbPixel.g, 1); leftInput->put_pixel(x, y, rgbPixel.b, 2); } } for (x=0; x<width; x++) { for (y=0; y<height; y++) { rgbPixel = rightImage->safePixel(x,y); rightInput->put_pixel(x, y, rgbPixel.r, 0); rightInput->put_pixel(x, y, rgbPixel.g, 1); rightInput->put_pixel(x, y, rgbPixel.b, 2); } } /* * Step 3: rectify left and right images * ===================================== */ if (debug) cout << "rectificationThread: performing rectification " << endl; rectify(leftInput, rightInput, *fxLeft, *fyLeft, *cxLeft, *cyLeft, leftCameraAngle, *fxRight,*fyRight,*cxRight,*cyRight,rightCameraAngle, leftRectified, rightRectified); /* * Step 4: copy images back to YARP format and write them out * ========================================================== */ if (debug) cout << "rectificationThread: sending images " << endl; ImageOf<PixelRgb> &rectifiedLeftImage = leftImagePortOut->prepare(); ImageOf<PixelRgb> &rectifiedRightImage = rightImagePortOut->prepare(); rectifiedLeftImage.resize(width,height); rectifiedRightImage.resize(width,height); for (x=0; x < width; x++) { for (y=0; y < height; y++) { leftRectified->get_pixel(x, y, &pixel_value, 0); rgbPixel.r=pixel_value; leftRectified->get_pixel(x, y, &pixel_value, 1); rgbPixel.g=pixel_value; leftRectified->get_pixel(x, y, &pixel_value, 2); rgbPixel.b=pixel_value; rectifiedLeftImage(x,y) = rgbPixel; rightRectified->get_pixel(x, y, &pixel_value, 0); rgbPixel.r=pixel_value; rightRectified->get_pixel(x, y, &pixel_value, 1); rgbPixel.g=pixel_value; rightRectified->get_pixel(x, y, &pixel_value, 2); rgbPixel.b=pixel_value; rectifiedRightImage(x,y) = rgbPixel; } } leftImagePortOut->write(); rightImagePortOut->write(); } if (log) { if (fp_out != NULL) fclose(fp_out); } }
void on_ledger_closed (RippleLedgerHash const& ledgerHash) { if (! isStopping()) m_queue.dispatch (m_context.wrap (std::bind ( &Logic::ledgerClosed, &m_logic, ledgerHash))); }
void LoadManager::run () { beast::Thread::setCurrentThreadName ("LoadManager"); using clock_type = std::chrono::steady_clock; // Initialize the clock to the current time. auto t = clock_type::now(); bool stop = false; while (! (stop || isStopping ())) { { // VFALCO NOTE I think this is to reduce calls to the operating // system for retrieving the current time. // // TODO Instead of incrementing can't we just retrieve the // current time again? // // Manually update the timer. UptimeTimer::getInstance ().incrementElapsedTime (); // Copy out shared data under a lock. Use copies outside lock. std::unique_lock<std::mutex> sl (mutex_); auto const deadLock = deadLock_; auto const armed = armed_; stop = stop_; sl.unlock(); // Measure the amount of time we have been deadlocked, in seconds. // // VFALCO NOTE deadLock_ is a canary for detecting the condition. int const timeSpentDeadlocked = UptimeTimer::getInstance ().getElapsedSeconds () - deadLock; // VFALCO NOTE I think that "armed" refers to the deadlock detector. // int const reportingIntervalSeconds = 10; if (armed && (timeSpentDeadlocked >= reportingIntervalSeconds)) { // Report the deadlocked condition every 10 seconds if ((timeSpentDeadlocked % reportingIntervalSeconds) == 0) { JLOG(journal_.warning) << "Server stalled for " << timeSpentDeadlocked << " seconds."; } // If we go over 500 seconds spent deadlocked, it means that // the deadlock resolution code has failed, which qualifies // as undefined behavior. // assert (timeSpentDeadlocked < 500); } } bool change = false; if (app_.getJobQueue ().isOverloaded ()) { JLOG(journal_.info) << app_.getJobQueue ().getJson (0); change = app_.getFeeTrack ().raiseLocalFee (); } else { change = app_.getFeeTrack ().lowerLocalFee (); } if (change) { // VFALCO TODO replace this with a Listener / observer and // subscribe in NetworkOPs or Application. app_.getOPs ().reportFeeChange (); } t += std::chrono::seconds (1); auto const duration = t - clock_type::now(); if ((duration < std::chrono::seconds (0)) || (duration > std::chrono::seconds (1))) { JLOG(journal_.warning) << "time jump"; t = clock_type::now(); } else { alertable_sleep_for(duration); } } stopped (); }
void MainThread::initialise() { setMaxHeartbeatInterval( 10.0 ); // // INITIAL CONFIGURATION // std::string prefix = context_.tag() + ".Config."; hydroutil::Properties prop = context_.toHydroContext(prefix).properties(); // // Create hardware driver factory // std::string driverLibName = prop.getPropertyWithDefault( "DriverLib", "libHydroSegwayRmpAcfrCan.so" ); hydroDriverLib_.reset( new hydrodll::DynamicallyLoadedLibrary(driverLibName) ); std::auto_ptr<hydrointerfaces::SegwayRmpFactory> driverFactory( hydrodll::dynamicallyLoadClass<hydrointerfaces::SegwayRmpFactory,SegwayRmpDriverFactoryMakerFunc> ( *hydroDriverLib_, "createSegwayRmpDriverFactory" ) ); // // Create powerbase managers // std::vector<std::string> powerbaseNames = hydroutil::toStringSeq( prop.getPropertyWithDefault( "PowerbaseNames", "" ), ' ' ); if ( powerbaseNames.size() == 0 ) powerbaseNames.push_back( "Main" ); for ( size_t i=0; i < powerbaseNames.size(); i++ ) { std::string stripPrefix = prefix; if ( powerbaseNames.size() > 1 ) { stripPrefix += powerbaseNames[i]+"."; } std::auto_ptr<hydrointerfaces::SegwayRmp> hydroDriver( driverFactory->createDriver( powerbaseNames[i], context_.toHydroContext(stripPrefix) ) ); powerbaseManagers_.push_back( hydrormputil::PowerbaseManagerPtr( new hydrormputil::PowerbaseManager( powerbaseNames[i], i, *this, hydroDriver, context_.toHydroContext(stripPrefix)) ) ); } // // Read (user-configured) vehicle description // orca::VehicleDescription descr; orcaobjutil::readVehicleDescription( context_.properties(), context_.tag()+".Config.", descr ); stringstream ss; ss<<"TRACE(component.cpp): Read vehicle description from configuration: " << endl << orcaobj::toString(descr) << endl; context_.tracer().info( ss.str() ); // // Check vehicle desciption sanity // orca::VehicleControlVelocityDifferentialDescription *controlDescr = dynamic_cast<orca::VehicleControlVelocityDifferentialDescription*>(&(*(descr.control))); if ( controlDescr == NULL ) throw gbxutilacfr::Exception( ERROR_INFO, "Can only deal with differential drive vehicles." ); if ( controlDescr->maxForwardSpeed != controlDescr->maxReverseSpeed ) throw gbxutilacfr::Exception( ERROR_INFO, "Can't handle max forward speed != max reverse speed." ); // // Set constraints // // convert the user-configured description into capabilities convert( *controlDescr, capabilities_ ); // constrain based on hardware capabilities hydrointerfaces::constrain( capabilities_, powerbaseManagers_.back()->capabilities() ); // convert back to orca format update( capabilities_, *controlDescr ); // // Initialise the powerbase managers // hydrormputil::DriverThread::Config cfg; cfg.driveInReverse = (bool)prop.getPropertyAsIntWithDefault( "DriveInReverse", 0 ); cfg.isMotionEnabled = (bool)prop.getPropertyAsIntWithDefault( "EnableMotion", 0 ); cfg.maxForwardAcceleration = controlDescr->maxForwardAcceleration; cfg.maxReverseAcceleration = controlDescr->maxReverseAcceleration; std::string stallPrefix = "StallSensor."; cfg.stallSensorConfig.torqueThreshold = prop.getPropertyAsDoubleWithDefault( stallPrefix+"TorqueThreshold", 3.0 ); cfg.stallSensorConfig.speedThreshold = prop.getPropertyAsDoubleWithDefault( stallPrefix+"speedThreshold", 0.5 ); cfg.stallSensorConfig.timeThreshold = prop.getPropertyAsDoubleWithDefault( stallPrefix+"TimeThreshold", 0.5 ); for ( size_t i=0; i < powerbaseManagers_.size(); i++ ) { try { powerbaseManagers_[i]->init( cfg ); } catch ( std::exception &e ) { stringstream ss; ss << "Failed to init powerbase '"<<powerbaseManagers_[i]->name()<<": " << e.what(); throw gbxutilacfr::Exception( ERROR_INFO, ss.str() ); } } // // EXTERNAL PROVIDED INTERFACES // // Initialise external interfaces odometry2dI_ = new orcaifaceimpl::Odometry2dImpl( descr, "Odometry2d", context_ ); odometry2dI_->initInterface( this, subsysName() ); odometry3dI_ = new orcaifaceimpl::Odometry3dImpl( descr, "Odometry3d", context_ ); odometry3dI_->initInterface( this, subsysName() ); powerI_ = new orcaifaceimpl::PowerImpl( "Power", context_ ); powerI_->initInterface( this, subsysName() ); velocityControl2dI_ = new orcaifaceimpl::VelocityControl2dImpl( *this, descr, "VelocityControl2d", context_ ); velocityControl2dI_->initInterface( this, subsysName() ); // Set up the publisher publisherThread_ = new PublisherThread( prop.getPropertyAsDoubleWithDefault( "Odometry2dPublishInterval", 0.1 ), prop.getPropertyAsDoubleWithDefault( "Odometry3dPublishInterval", 0.1 ), prop.getPropertyAsDoubleWithDefault( "PowerPublishInterval", 20.0 ), odometry2dI_, odometry3dI_, powerI_, context_.tracer(), context_.status(), context_.history() ); publisherThreadPtr_ = publisherThread_; publisherThread_->start(); // // (optionally) required e-stop interface // const bool isEStopEnabled = (bool)prop.getPropertyAsIntWithDefault( "EnableEStopInterface", 0 ); if ( isEStopEnabled ) { while ( !isStopping() ) { try { orca::EStopPrx eStopPrx; orcaice::connectToInterfaceWithTag( context_, eStopPrx, "EStop" ); orca::EStopDescription descr = eStopPrx->getDescription(); eStopMonitor_.reset( new orcaestoputil::EStopMonitor(descr) ); eStopConsumerI_ = new orcaifaceimpl::NotifyingEStopConsumerImpl(context_); eStopConsumerI_->setNotifyHandler( this ); eStopConsumerI_->subscribeWithTag( "EStop", this, subsysName() ); break; } catch ( ... ) { orcaice::catchExceptionsWithStatusAndSleep( "connecting to e-stop", health() ); } } } // // Finally, start the powerbase threads rolling // for ( size_t i=0; i < powerbaseManagers_.size(); i++ ) { powerbaseManagers_[i]->startThread(); } }
void DocumentLoader::responseReceived(CachedResource* resource, const ResourceResponse& response) { ASSERT_UNUSED(resource, m_mainResource == resource); RefPtr<DocumentLoader> protect(this); m_applicationCacheHost->didReceiveResponseForMainResource(response); // The memory cache doesn't understand the application cache or its caching rules. So if a main resource is served // from the application cache, ensure we don't save the result for future use. All responses loaded // from appcache will have a non-zero appCacheID(). if (response.appCacheID()) memoryCache()->remove(m_mainResource.get()); DEFINE_STATIC_LOCAL(AtomicString, xFrameOptionHeader, ("x-frame-options", AtomicString::ConstructFromLiteral)); HTTPHeaderMap::const_iterator it = response.httpHeaderFields().find(xFrameOptionHeader); if (it != response.httpHeaderFields().end()) { String content = it->value; ASSERT(m_mainResource); unsigned long identifier = mainResourceIdentifier(); ASSERT(identifier); if (frameLoader()->shouldInterruptLoadForXFrameOptions(content, response.url(), identifier)) { InspectorInstrumentation::continueAfterXFrameOptionsDenied(m_frame, this, identifier, response); String message = "Refused to display '" + response.url().elidedString() + "' in a frame because it set 'X-Frame-Options' to '" + content + "'."; frame()->document()->addConsoleMessage(SecurityMessageSource, ErrorMessageLevel, message, identifier); frame()->document()->enforceSandboxFlags(SandboxOrigin); if (HTMLFrameOwnerElement* ownerElement = frame()->ownerElement()) ownerElement->dispatchEvent(Event::create(eventNames().loadEvent, false, false)); // The load event might have detached this frame. In that case, the load will already have been cancelled during detach. if (frameLoader()) cancelMainResourceLoad(frameLoader()->cancelledError(m_request)); return; } } ASSERT(!mainResourceLoader() || !mainResourceLoader()->defersLoading()); m_response = response; if (isArchiveMIMEType(m_response.mimeType()) && m_mainResource->dataBufferingPolicy() != BufferData) m_mainResource->setDataBufferingPolicy(BufferData); if (m_identifierForLoadWithoutResourceLoader) frameLoader()->notifier()->dispatchDidReceiveResponse(this, m_identifierForLoadWithoutResourceLoader, m_response, 0); if (!shouldContinueForResponse()) { InspectorInstrumentation::continueWithPolicyIgnore(m_frame, this, m_mainResource->identifier(), m_response); stopLoadingForPolicyChange(); return; } if (m_response.isHTTP()) { int status = m_response.httpStatusCode(); if (status < 200 || status >= 300) { bool hostedByObject = frameLoader()->isHostedByObjectElement(); frameLoader()->handleFallbackContent(); // object elements are no longer rendered after we fallback, so don't // keep trying to process data from their load if (hostedByObject) cancelMainResourceLoad(frameLoader()->cancelledError(m_request)); } } if (!isStopping() && m_substituteData.isValid()) { if (m_substituteData.content()->size()) dataReceived(0, m_substituteData.content()->data(), m_substituteData.content()->size()); if (isLoadingMainResource()) finishedLoading(0); } }
void on_receive_validation (ReceivedValidation const& rv) { if (! isStopping()) m_queue.dispatch (m_context.wrap (std::bind ( &Logic::receiveValidation, &m_logic, rv))); }
void AttentionNetworkTestThread::run(){ debug = true; if (debug) { cout << "AttentionNetworkTestThread::run: database name " << *databaseNameValue << endl; cout << "AttentionNetworkTestThread::run: path " << *pathValue << endl; cout << "AttentionNetworkTestThread::run: d1 " << *d1Value << endl; cout << "AttentionNetworkTestThread::run: d2 " << *d2Value << endl; cout << "AttentionNetworkTestThread::run: d3 " << *d3Value << endl; cout << "AttentionNetworkTestThread::run: d4 " << *d4Value << endl; cout << "AttentionNetworkTestThread::run: d5 " << *d5Value << endl; } /* ANT images ANT00 fixation ANT01 cue centre ANT02 cue top and bottom ANT03 cue top ANT04 cue bottom ANT05 target neutral top right ANT06 target neutral bottom right ANT07 target neutral top left ANT08 target neutral bottom left ANT09 target congurent top right ANT10 target congurent bottom right ANT11 target congurent top left ANT12 target congurent bottom left ANT13 target incongurent top right ANT14 target incongurent bottom right ANT15 target incongurent top left ANT16 target incongurent bottom left */ while (isStopping() != true) { // the thread continues to run until isStopping() returns true std::vector<ImageOf<PixelRgb> >& images = data.images(); for (cueCondition = 0; cueCondition < 4; cueCondition++) { for (location = 0; location <2 ; location++) { for (direction = 0; direction < 2; direction++) { for (flankerCondition = 0; flankerCondition <3; flankerCondition++) { /* fixation */ imageId = 0; it = images.begin() + imageId; // set the iterator ANTimage = *it; // retrieve the image ANTimageOutPort->prepare() = ANTimage; ANTimageOutPort->write(); // write image pause(*d1Value); /* cue */ if (cueCondition < 3) { // not spatial imageId = cueCondition; } else { // spatial ... depends on location imageId = cueCondition+location; } it = images.begin() + imageId; // set the iterator ANTimage = *it; // retrieve the image ANTimageOutPort->prepare() = ANTimage; ANTimageOutPort->write(); // write image pause(*d2Value); /* fixation */ imageId = 0; it = images.begin() + imageId; // set the iterator ANTimage = *it; // retrieve the image ANTimageOutPort->prepare() = ANTimage; ANTimageOutPort->write(); // write image pause(*d3Value); /* target */ imageId = 5 + 4*flankerCondition + location + 2*direction; it = images.begin() + imageId; // set the iterator ANTimage = *it; // retrieve the image ANTimageOutPort->prepare() = ANTimage; ANTimageOutPort->write(); // write image pause(*d4Value); /* fixation */ imageId = 0; it = images.begin() + imageId; // set the iterator ANTimage = *it; // retrieve the image ANTimageOutPort->prepare() = ANTimage; ANTimageOutPort->write(); // write image pause(*d5Value); } } } } } }
void DriverThread::operateHardware() { // temp data structures. hydrointerfaces::SegwayRmp::Data data; hydrointerfaces::SegwayRmp::Command command(0,0); // monitor the speed set-point, don't allow it to change faster than the acceleration limits // (initialises to zero speed) SpeedSetPoint speedSetPoint( config_.maxForwardAcceleration, config_.maxReverseAcceleration ); // initialise stall sensor StallSensor stallSensor( config_.stallSensorConfig ); StallType stallType; // clear any pending commands commandStore_.set( command ); gbxiceutilacfr::Timer newCommandTimer; // // The big loop (we exit immediately on detecting a fault) // while ( !isStopping() ) { speedSetPoint.evaluateDt(); if ( stateMachine_.isFault() ) { // Can't operate in a fault condition. break; } // // Read from the hardware // try { segwayRmp_.read( data ); if ( config_.driveInReverse ) reverseDirection( data ); // Let the higher-ups know stallType = stallSensor.isStalled(data); callback_.receiveData( data, stallType ); // Check for warnings/faults if ( data.hasFaults || (stallType != NoStall) ) { stringstream ssFault; if ( data.hasFaults ) { ssFault << data.warnFaultReason << "\n"; } if ( stallType != NoStall ) { ssFault << stallString( data, stallType ); } health().fault( ssFault.str() ); if ( data.hasFaults ) { stateMachine_.setFault( ssFault.str() ); break; } } else if ( data.hasWarnings ) { stateMachine_.setWarning( data.warnFaultReason ); health().warning( data.warnFaultReason ); } else { stateMachine_.setOK(); health().ok(); } } catch ( ... ) { string problem = hydroiceutil::catchExceptionsWithStatus( "reading from hardware", health() ); stateMachine_.setFault( problem ); break; } // // Write pending commands to the hardware // // Have we got a new command? bool gotNewCommand = false; if ( commandStore_.isNewData() ) { gotNewCommand = true; commandStore_.get( command ); speedSetPoint.set( command.vx ); newCommandTimer.restart(); } // Are we still trying to hit our set-point? bool setPointAlreadyReached = false; command.vx = speedSetPoint.currentCmdSpeed( setPointAlreadyReached ); // Write if we're supposed to if ( gotNewCommand || !setPointAlreadyReached ) { if ( config_.driveInReverse ) reverseDirection( command ); try { // Probably better not to have this (mutex-locking) // statement in the middle of this very tight loop... // //stringstream ss; //ss << "DriverThread::"<<__func__<<"(): writing command: " << command.toString(); //tracer_.debug( ss.str(), 2 ); segwayRmp_.write( command ); } catch ( ... ) { string problem = hydroiceutil::catchExceptionsWithStatus( "writing to hardware", health() ); stateMachine_.setFault( problem ); break; } } // Check for timeouts in receiving commands if ( newCommandTimer.elapsedSec() > 0.2 ) { if ( command.vx != 0 || command.w != 0 ) { tracer_.info( "newCommandTimer timed out, resetting desired speed to zero" ); command = hydrointerfaces::SegwayRmp::Command(0,0); commandStore_.set(command); newCommandTimer.restart(); } } } std::string faultReason; if ( stateMachine_.isFault(faultReason) ) { health().fault( faultReason ); // pause in case of persistent fault sleep(1); } }
bool GiGraphics::drawBeziers(const GiContext* ctx, int count, const Point2d* knot, const Vector2d* knotvs, bool closed, bool modelUnit) { if (count < 2 || !knot || !knotvs || isStopping()) return false; if (count > 0x1000) count = 0x1000; bool ret = false; vector<Point2d> pxpoints; vector<Point2d> pointBuf; int i, j, n, si, ei; Point2d * pxs; Matrix2d matD(S2D(xf(), modelUnit)); const Box2d extent (count, knot); // 模型坐标范围 if (!DRAW_RECT(m_impl, modelUnit).isIntersect(extent)) // 全部在显示区域外 return false; pointBuf.resize(1 + (count - 1) * 3); pxs = &pointBuf.front(); if (closed) { pxs[0] = knot[0] * matD; for (i = 0, j = 1; i + 1 < count; i++) { pxs[j++] = (knot[i] + knotvs[i]) * matD; pxs[j++] = (knot[i+1] - knotvs[i+1]) * matD; pxs[j++] = knot[i+1] * matD; } ret = rawBeziers(ctx, pxs, j, closed); } else if (DRAW_MAXR(m_impl, modelUnit).contains(extent)) { // 全部在显示区域内 pxs[0] = knot[0] * matD; for (i = 0, j = 1; i + 1 < count; i++) { pxs[j++] = (knot[i] + knotvs[i]) * matD; pxs[j++] = (knot[i+1] - knotvs[i+1]) * matD; pxs[j++] = knot[i+1] * matD; } ret = rawBeziers(ctx, pxs, j); } else { pxs[0] = knot[0] * matD; for (i = 0, j = 1; i + 1 < count; i++) { pxs[j++] = (knot[i] + knotvs[i]) * matD; pxs[j++] = (knot[i+1] - knotvs[i+1]) * matD; pxs[j++] = knot[i+1] * matD; } Point2d* pts = pxs; count = 1 + (count - 1) * 3; for (i = 0; i + 3 < count;) { for (; i + 3 < count && !m_impl->rectDraw.isIntersect(Box2d(4, &pts[i])); i += 3) ; si = ei = i; for (; i + 3 < count && m_impl->rectDraw.isIntersect(Box2d(4, &pts[i])); i += 3) ei = i + 3; if (ei > si) { n = ei - si + 1; pxpoints.resize(n); pxs = &pxpoints.front(); for (j=0; j<n; j++) pxs[j] = pts[si + j]; ret = rawBeziers(ctx, pxs, n); } } } return ret; }