bool CameraV4LPublisher::getImage(cv::Mat& img, bool emptyBuffer)
{
bool ok = false;
if ( emptyBuffer )
{
for (int i = 0; i < 4; ++i) //v4l stores a buffer of 4 images
_capture->grab();
}
if ( _capture->grab() )
if ( _capture->retrieve(img) )
ok = true;
return ok;
}
void pollCallback(polled_camera::GetPolledImage::Request& req,
polled_camera::GetPolledImage::Response& rsp,
sensor_msgs::Image& image, sensor_msgs::CameraInfo& info)
{
if (trigger_mode_ != prosilica::Software) {
rsp.success = false;
rsp.status_message = "Camera is not in software triggered mode";
return;
}
tPvFrame* frame = NULL;
try {
cam_->setBinning(req.binning_x, req.binning_y);
if (req.roi.x_offset || req.roi.y_offset || req.roi.width || req.roi.height) {
// GigE cameras use ROI in binned coordinates, so scale the values down.
// The ROI is expanded if necessary to land on binned coordinates.
unsigned int left_x = req.roi.x_offset / req.binning_x;
unsigned int top_y = req.roi.y_offset / req.binning_y;
unsigned int right_x = (req.roi.x_offset + req.roi.width + req.binning_x - 1) / req.binning_x;
unsigned int bottom_y = (req.roi.y_offset + req.roi.height + req.binning_y - 1) / req.binning_y;
unsigned int width = right_x - left_x;
unsigned int height = bottom_y - top_y;
cam_->setRoi(left_x, top_y, width, height);
} else {
cam_->setRoiToWholeFrame();
}
// Zero duration means "no timeout"
unsigned long timeout = req.timeout.isZero() ? PVINFINITE : req.timeout.toNSec() / 1e6;
frame = cam_->grab(timeout);
}
catch (prosilica::ProsilicaException &e) {
if (e.error_code == ePvErrBadSequence)
throw; // not easily recoverable
rsp.success = false;
rsp.status_message = (boost::format("Prosilica exception: %s") % e.what()).str();
return;
}
if (!frame) {
/// @todo Would be nice if grab() gave more info
rsp.success = false;
rsp.status_message = "Failed to capture frame, may have timed out";
return;
}
info = cam_info_;
image.header.frame_id = img_.header.frame_id;
if (!processFrame(frame, image, info)) {
rsp.success = false;
rsp.status_message = "Captured frame but failed to process it";
return;
}
info.roi.do_rectify = req.roi.do_rectify; // do_rectify should be preserved from request
rsp.success = true;
}
// Try to capture an image.
void imageTest(diagnostic_updater::DiagnosticStatusWrapper& status)
{
status.name = "Image Capture Test";
try {
if (trigger_mode_ != prosilica::Software) {
tPvUint32 start_completed, current_completed;
cam_->getAttribute("StatFramesCompleted", start_completed);
for (int i = 0; i < 6; ++i) {
boost::this_thread::sleep(boost::posix_time::millisec(500));
cam_->getAttribute("StatFramesCompleted", current_completed);
if (current_completed > start_completed) {
status.summary(0, "Captured a frame, see diagnostics for detailed stats");
return;
}
}
// Give up after 3s
status.summary(2, "No frames captured over 3s in freerun mode");
return;
}
// In software triggered mode, try to grab a frame
cam_->setRoiToWholeFrame();
tPvFrame* frame = cam_->grab(500);
if (frame)
{
status.summary(0, "Successfully triggered a frame capture");
}
else
{
status.summary(2, "Attempted to grab a frame, but received NULL");
}
}
catch (prosilica::ProsilicaException &e) {
status.summary(2, e.what());
}
}
