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;
}
