bool processFrame(tPvFrame* frame, sensor_msgs::Image &img, sensor_msgs::CameraInfo &cam_info)
{
/// @todo Better trigger timestamp matching
if ((trigger_mode_ == prosilica::SyncIn1 || trigger_mode_ == prosilica::SyncIn2) && !trig_time_.isZero()) {
img.header.stamp = cam_info.header.stamp = trig_time_;
trig_time_ = ros::Time(); // Zero
}
else {
/// @todo Match time stamp from frame to ROS time?
img.header.stamp = cam_info.header.stamp = ros::Time::now();
}
/// @todo Binning values retrieved here may differ from the ones used to actually
/// capture the frame! Maybe need to clear queue when changing binning and/or
/// stuff binning values into context?
tPvUint32 binning_x = 1, binning_y = 1;
if (cam_->hasAttribute("BinningX")) {
cam_->getAttribute("BinningX", binning_x);
cam_->getAttribute("BinningY", binning_y);
}
// Binning averages bayer samples, so just call it mono8 in that case
if (frame->Format == ePvFmtBayer8 && (binning_x > 1 || binning_y > 1))
frame->Format = ePvFmtMono8;
if (!frameToImage(frame, img))
return false;
// Set the operational parameters in CameraInfo (binning, ROI)
cam_info.binning_x = binning_x;
cam_info.binning_y = binning_y;
// ROI in CameraInfo is in unbinned coordinates, need to scale up
cam_info.roi.x_offset = frame->RegionX * binning_x;
cam_info.roi.y_offset = frame->RegionY * binning_y;
cam_info.roi.height = frame->Height * binning_y;
cam_info.roi.width = frame->Width * binning_x;
cam_info.roi.do_rectify = (frame->Height != sensor_height_ / binning_y) ||
(frame->Width != sensor_width_ / binning_x);
count_++;
return true;
}
bool processFrame (FlyCapture2::Image * frame, sensor_msgs::Image & img, sensor_msgs::CameraInfoPtr & cam_info)
{
// cam_->saveImageToFile(frame); // Just a test: comment this out if not needed
if (!frameToImage (frame, img))
return false;
/// @todo Use time from frame?
img.header.stamp = cam_info->header.stamp = ros::Time::now ();
// Throw out any CamInfo that's not calibrated to this camera mode
if (cam_info->K[0] != 0.0 && (img.width != cam_info->width || img.height != cam_info->height))
{
cam_info.reset(new sensor_msgs::CameraInfo());
}
// If we don't have a calibration, set the image dimensions
if (cam_info->K[0] == 0.0)
{
cam_info->width = img.width = width_;
cam_info->height = img.height = height_;
}
// TF frame
img.header.frame_id = cam_info->header.frame_id = frame_id_;
//frame->GetDimensions(&cam_info.height, &cam_info.width);
//cam_info.width = frame->GetCols();
//
// cam_info.roi.x_offset = frame->RegionX;
// cam_info.roi.y_offset = frame->RegionY;
// cam_info.roi.height = frame->Height;
// cam_info.roi.width = frame->Width;
count_++;
//ROS_INFO("count = %d", count_);
return true;
}
bool FFmpegDecoder::handleVideoPacket(
const AVPacket& packet,
double& videoClock,
VideoParseContext& context)
{
enum { MAX_SKIPPED = 4 };
const double MAX_DELAY = 0.2;
const int ret = avcodec_send_packet(m_videoCodecContext, &packet);
if (ret < 0)
return false;
AVFramePtr videoFrame(av_frame_alloc());
while (avcodec_receive_frame(m_videoCodecContext, videoFrame.get()) == 0)
{
const int64_t duration_stamp =
videoFrame->best_effort_timestamp; //av_frame_get_best_effort_timestamp(m_videoFrame);
// compute the exact PTS for the picture if it is omitted in the stream
// pts1 is the dts of the pkt / pts of the frame
if (duration_stamp != AV_NOPTS_VALUE)
{
videoClock = duration_stamp * av_q2d(m_videoStream->time_base);
}
const double pts = videoClock;
// update video clock for next frame
// for MPEG2, the frame can be repeated, so we update the clock accordingly
const double frameDelay = av_q2d(m_videoCodecContext->time_base) *
(1. + videoFrame->repeat_pict * 0.5);
videoClock += frameDelay;
boost::posix_time::time_duration td(boost::posix_time::pos_infin);
bool inNextFrame = false;
const bool haveVideoPackets = !m_videoPacketsQueue.empty();
{
boost::lock_guard<boost::mutex> locker(m_isPausedMutex);
inNextFrame = m_isPaused && m_isVideoSeekingWhilePaused;
if (!context.initialized || inNextFrame)
{
m_videoStartClock = (m_isPaused ? m_pauseTimer : GetHiResTime()) - pts;
}
// Skipping frames
if (context.initialized && !inNextFrame && haveVideoPackets)
{
const double curTime = GetHiResTime();
if (m_videoStartClock + pts <= curTime)
{
if (m_videoStartClock + pts < curTime - MAX_DELAY)
{
InterlockedAdd(m_videoStartClock, MAX_DELAY);
}
if (++context.numSkipped > MAX_SKIPPED)
{
context.numSkipped = 0;
}
else
{
CHANNEL_LOG(ffmpeg_sync) << "Hard skip frame";
// pause
if (m_isPaused && !m_isVideoSeekingWhilePaused)
{
break;
}
continue;
}
}
else
{
int speedNumerator, speedDenominator;
std::tie(speedNumerator, speedDenominator) = static_cast<const std::pair<int, int>&>(m_speedRational);
context.numSkipped = 0;
td = boost::posix_time::milliseconds(
int((m_videoStartClock + pts - curTime) * 1000. * speedDenominator / speedNumerator) + 1);
}
}
}
context.initialized = true;
{
boost::unique_lock<boost::mutex> locker(m_videoFramesMutex);
if (!m_videoFramesCV.timed_wait(locker, td, [this]
{
return m_isPaused && !m_isVideoSeekingWhilePaused ||
m_videoFramesQueue.canPush();
}))
{
continue;
}
}
{
boost::lock_guard<boost::mutex> locker(m_isPausedMutex);
if (m_isPaused && !m_isVideoSeekingWhilePaused)
{
break;
}
m_isVideoSeekingWhilePaused = false;
}
if (inNextFrame)
{
m_isPausedCV.notify_all();
}
VideoFrame& current_frame = m_videoFramesQueue.back();
handleDirect3dData(videoFrame.get());
if (!frameToImage(current_frame, videoFrame, m_imageCovertContext, m_pixelFormat))
{
continue;
}
current_frame.m_pts = pts;
current_frame.m_duration = duration_stamp;
{
boost::lock_guard<boost::mutex> locker(m_videoFramesMutex);
m_videoFramesQueue.pushBack();
}
m_videoFramesCV.notify_all();
}
return true;
}
