Printing to the DriverStation LCD

[Ross3098]

Last year we used the command:

DriverStationLCD *m_LCD;

m_LCD->GetInstance();

//Code.............................

m_LCD->Printf(DriverStationLCD::kUser_Line1, 1, "Message");
m_LCD->UpdateLCD();

In C++ to print information to the Driver Station LCD user messages display. This year we have been using the same lines, with no success. We really need to get feedback so we can test our mechanisms.

Any Ideas?
P.S Also trying to setup a target console results in the error:

"******* is not licensed" or something along those lines...(can't remember exactly as we have switched to LabVIEW temporarily).

[inkspell4]

Try DriverStationLCD m_ds = DriverStationLCD::GetInstance();
We got it to work for our team and also there is an example demonstrating this

[mikets]

Originally Posted by Ross3098

Last year we used the command:

DriverStationLCD *m_LCD;
 
m_LCD->GetInstance();
 
//Code.............................
 
m_LCD->Printf(DriverStationLCD::kUser_Line1, 1, "Message");
m_LCD->UpdateLCD();

No, your code is declaring a DriverStationLCD pointer m_LCD but it is uninitialized. Your code will fault when you call m_LCD->GetInstance(). Instead, your code should look something like this:

DriverStationLCD *m_LCD;
 
m_LCD = DriverStationLCD::GetInstance();

[JHebes]

Also, this:

m_LCD->Printf(DriverStationLCD::kUser_Line1, 1, "Message");

should look like this:

m_LCD->PrintfLine((DriverStationLCD::Line) 0, "Message");

[Ross3098]

Originally Posted by mikets

No, your code is declaring a DriverStationLCD pointer m_LCD but it is uninitialized. Your code will fault when you call m_LCD->GetInstance().

Whoops sorry I was typing it by memory and the code is in fact
DriverStationLCD::GetInstance();

As with all the issues we have been having lately it seems that the fault of this mishap was in fact the camera code we had written. That little camera takes up 100% of the cpu on our cRIO so no wonder nothing was printing, nothing was getting done!

[mikets]

The vision processing code is really slow. We timed it and from camera->GetImage() to finally getting ParticleAnalysisReport took between 1.5 to 2.5 seconds. Because of this, we spawned a separate thread (task) to just doing vision processing so it doesn't block our main robot task. But it is still disappointing to know vision processing is so slow. Did anybody have similar experience or did we do something wrong that caused this "slowness". One of my suspicions is that we did not set up the lighting condition right and it took longer to process the image. Without the source code to nivision, I can't tell why it took so long.

[Greg McKaskle]

What resolution is the camera? You should be able to get five fps easily. Post the code if you cannot discover why the image processing is taking over a second.

Greg McKaskle

[mikets]

The code is pretty much based on the VisionSample2012 code with more error checking and generalized so that the caller can provide parameters such as color space (RGB vs HSL), minimum particle size and particle filter criteria etc. Notice the time stamp code in there. That's how we found out how long it takes to process one frame.

void ProcessImage(void)
{
    if (IsEnabled() && m_camera->IsFreshImage())
    {
        int err = ERR_SUCCESS;
        BinaryImage *image;
        BinaryImage *thresholdImage;
        BinaryImage *bigObjImage;
        BinaryImage *convexHullImage;
        BinaryImage *filteredImage;
        UINT32 startTime = GetMsecTime();
        m_camera->GetImage(&m_cameraImage);
        //
        // Filter the image by color.
        //
        switch (m_imageType)
        {
        case IMAQ_IMAGE_RGB:
            thresholdImage = m_cameraImage.ThresholdRGB(*m_colorThresholds);
            break;
        case IMAQ_IMAGE_HSL:
            thresholdImage = m_cameraImage.ThresholdHSL(*m_colorThresholds);
            break;
        default:
            TErr(("Unsupported image type (type=%d).", m_imageType));
            break;
        }
        image = thresholdImage;
        if (thresholdImage == NULL)
        {
            err = imaqGetLastError();
            TErr(("Failed to filter image with thresholds (err=%d).",
                  err));
        }
        else if (m_sizeThreshold > 0)
        {
            //
            // Remove small objects
            //
            bigObjImage = image->RemoveSmallObjects(false, m_sizeThreshold);
            image = bigObjImage;
            if (bigObjImage == NULL)
            {
                err = imaqGetLastError();
                TErr(("Failed to filter image with size (err=%d).", err));
            }
        }
        if (err == ERR_SUCCESS)
        {
            convexHullImage = image->ConvexHull(false);
            image = convexHullImage;
            if (convexHullImage == NULL)
            {
                err = imaqGetLastError();
                TErr(("Failed to generate Convex Hull image (err=%d).",
                      err));
            }
        }
        if ((err == ERR_SUCCESS) && (m_filterCriteria != NULL))
        {
            filteredImage = image->ParticleFilter(m_filterCriteria,
                                                  m_numCriteria);
            image = filteredImage;
            if (filteredImage == NULL)
            {
                err = imaqGetLastError();
                TErr(("Failed to filter image based on criteria (err=%d).",
                      err));
            }
        }
        if (err == ERR_SUCCESS)
        {
            vector<ParticleAnalysisReport> *reports =
                    image->GetOrderedParticleAnalysisReports();
            if (reports == NULL)
            {
                err = imaqGetLastError();
                TErr(("Failed to get particle analysis reports (err=%d).",
                      err));
            }
            else
            {
                for (unsigned i = 0; i < reports->size(); i++)
                {
                    ParticleAnalysisReport *p = &(reports->at(i));
                    TInfo(("%d: (%d,%d) [%d,%d/%d,%d]: Q=%5.2f,Per=%5.2f",
                           i,
                           p->center_mass_x,
                           p->center_mass_y,
                           p->boundingRect.left,
                           p->boundingRect.top,
                           p->boundingRect.width,
                           p->boundingRect.height,
                           p->particleQuality,
                           p->particleToImagePercent));
                }
                Synchronized sync(m_semaphore);
                SAFE_DELETE(m_targets);
                m_targets = reports;
                reports = NULL;
            }
        }
        SAFE_DELETE(filteredImage);
        SAFE_DELETE(convexHullImage);
        SAFE_DELETE(bigObjImage);
        SAFE_DELETE(thresholdImage);
        TInfo(("Elapsed time = %d", GetMsecTime() - startTime));
    }
}   //ProcessImage

[mikets]

Nevermind, after playing with some camera parameters (e.g. crank the brightness all the way down etc.), I got the code to run with a more reasonable performance. Here is the current debug output. BTW, it seems even though our code explicitly set the resolution to 320x240, it seems I have to go to the camera settings to force it to 320x240 or I will get a jpg dump that seems to be 640x480.

Vision.ProcessImage_Info: AcquireImageTime = 43
Vision.ProcessImage_Info: ColorThresholdTime = 7
Vision.ProcessImage_Info: BigObjFilterTime = 39
Vision.ProcessImage_Info: ConvexHullTime = 19
Vision.ProcessImage_Info: ParticleFilterTime = 56
Vision.ProcessImage_Info: GetReportTime = 4
Vision.ProcessImage_Info: NumParticles = 3
Vision.ProcessImage_Info: 0: (189,188) [162,167/57,42]: AR= 1.36,Q=100.00,Per=2.96
Vision.ProcessImage_Info: 1: (251,120) [228,100/50,41]: AR= 1.22,Q=99.95,Per=2.38
Vision.ProcessImage_Info: 2: (193,45) [169,29/50,35]: AR= 1.43,Q=100.00,Per=2.00
Vision.ProcessImage_Info: Total Elapsed Time = 168

[Greg McKaskle]

That is more reasonable. I thought that the image was likely the big one. The other thing you may look into is whether or not the small particle filter actually buys you much. I'm also not able to see what the filter after the convexHull does, but those two passes are pretty expensive. Are they needed to make more robust processing? If you can leave them out, it looks like the framerate will be quite a bit faster. I suspect that when you get rid of the small Particle filter, you may find that convexHull time rises, but the sum of the smallParticleFilter and ConvexHull will probably fall.

Greg McKaskle

[mikets]

I would love to see the result of each of the filtered steps so I can judge if each step buys anything. But I was unable to write each of the images out. If I set the color mode to HSL, binImage->Write will fault. If I stick with RGB, I was able to write out the images but only the original camera image showed something. All other images are total blackness. However, the filtering must have done something right because I did get back a list of reasonable targets. May be it's just a problem with writing the images.

Since you mentioned it, it seems the "Small Particle Removal" step and the "Particle Filter" step are doing similar things. The Particle Filter criteria is to make sure the particles have its width and height in a certain range. So if you have really small particles, the criteria will filter them out anyway.

I also played with an additional criteria IMAQ_MT_RATIO_OF_EQUIVALENT_RECT_SIDES, knowing that the target rectangle has an aspect ratio of 1.33 approx, I set the RATIO to between 1.0 to 2.0. But this filter doesn't seem working because I got a false particle with an aspect ratio way small even the inverse of it would be larger than 2.0 (forgot the exact number now). It would have been filtered out unless I misundertand what this filter does. (e.g. It doesn't say if the RECT is the BOUNDING_RECT) Too bad that this filter implements LongSide/ShortSide instead of Width/Height because it means a rectangle with the right aspect ratio but just turning 90-degree would pass the filter. Do you know if I interpreted this filter correctly?

[Greg McKaskle]

My suggestion would be to implement the same or similar steps in Vision Assistant and use that to measure the value of the various filters. It also helps with discovering what the various advanced functions do, since sometimes a picture is worth ...

As for the advanced filter about rectangle ratios, the example code for LV just does that in its own loop on up to ten particles using the bounding box. It doesn't make another call into IMAQ.

The example code is very similar to yours. It thresholds using HSL or L, then it does the convexHull, then it requests the particleAnalysis but only for area and bounding box. Then it sorts the particles by area and analyses them further. Using the original image, it requests the row and column averages for each of the major particles to measure if it was hollow. The position, aspect ratio, and coverage tests are done on those measurements.

Greg McKaskle