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;
}
void configure(Config& config, uint32_t level)
{
ROS_DEBUG("Reconfigure request received");
if (level >= (uint32_t)driver_base::SensorLevels::RECONFIGURE_STOP)
stop();
// Trigger mode
if (config.trigger_mode == "streaming") {
trigger_mode_ = prosilica::Freerun;
/// @todo Tighter bound than this minimal check
desired_freq_ = 1; // make sure we get _something_
}
else if (config.trigger_mode == "syncin1") {
trigger_mode_ = prosilica::SyncIn1;
desired_freq_ = config.trig_rate;
}
else if (config.trigger_mode == "syncin2") {
trigger_mode_ = prosilica::SyncIn2;
desired_freq_ = config.trig_rate;
}
//#if 0
else if (config.trigger_mode == "fixedrate") {
//ROS_DEBUG("Fixed rate not supported yet implementing software");
trigger_mode_ = prosilica::FixedRate;
// ROS_ERROR("Invalid trigger mode '%s' in reconfigure request", config.trigger_mode.c_str());
///@todo add the fixed rate implementation
cam_->setAttribute("FrameRate",(tPvFloat32)config.fixed_rate);//1.0;//config.fixed_rate;
}
//#endif
else if (config.trigger_mode == "polled") {
trigger_mode_ = prosilica::Software;
desired_freq_ = 0;
}
else {
ROS_ERROR("Invalid trigger '%s' in reconfigure request", config.trigger_mode.c_str());
}
trig_timestamp_topic_ = config.trig_timestamp_topic;
// Exposure
if (config.auto_exposure)
{
cam_->setExposure(0, prosilica::Auto);
if (cam_->hasAttribute("ExposureAutoMax"))
{
tPvUint32 us = config.exposure_auto_max*1000000. + 0.5;
cam_->setAttribute("ExposureAutoMax", us);
}
if (cam_->hasAttribute("ExposureAutoTarget"))
cam_->setAttribute("ExposureAutoTarget", (tPvUint32)config.exposure_auto_target);
}
else {
unsigned us = config.exposure*1000000. + 0.5;
cam_->setExposure(us, prosilica::Manual);
}
// Gain
if (config.auto_gain) {
if (cam_->hasAttribute("GainAutoMax"))
{
cam_->setGain(0, prosilica::Auto);
cam_->setAttribute("GainAutoMax", (tPvUint32)config.gain_auto_max);
cam_->setAttribute("GainAutoTarget", (tPvUint32)config.gain_auto_target);
}
else {
tPvUint32 major, minor;
cam_->getAttribute("FirmwareVerMajor", major);
cam_->getAttribute("FirmwareVerMinor", minor);
ROS_WARN("Auto gain not available for this camera. Auto gain is available "
"on firmware versions 1.36 and above. You are running version %u.%u.",
(unsigned)major, (unsigned)minor);
config.auto_gain = false;
}
}
else
cam_->setGain(config.gain, prosilica::Manual);
// White balance
if (config.auto_whitebalance) {
if (cam_->hasAttribute("WhitebalMode"))
cam_->setWhiteBalance(0, 0, prosilica::Auto);
else {
ROS_WARN("Auto white balance not available for this camera.");
config.auto_whitebalance = false;
}
}
else
cam_->setWhiteBalance(config.whitebalance_blue, config.whitebalance_red, prosilica::Manual);
// Binning configuration
if (cam_->hasAttribute("BinningX")) {
tPvUint32 max_binning_x, max_binning_y, dummy;
PvAttrRangeUint32(cam_->handle(), "BinningX", &dummy, &max_binning_x);
PvAttrRangeUint32(cam_->handle(), "BinningY", &dummy, &max_binning_y);
config.binning_x = std::min(config.binning_x, (int)max_binning_x);
config.binning_y = std::min(config.binning_y, (int)max_binning_y);
cam_->setBinning(config.binning_x, config.binning_y);
}
else if (config.binning_x > 1 || config.binning_y > 1)
{
ROS_WARN("Binning not available for this camera.");
config.binning_x = config.binning_y = 1;
}
// Region of interest configuration
// Make sure ROI fits in image
config.x_offset = std::min(config.x_offset, (int)sensor_width_ - 1);
config.y_offset = std::min(config.y_offset, (int)sensor_height_ - 1);
config.width = std::min(config.width, (int)sensor_width_ - config.x_offset);
config.height = std::min(config.height, (int)sensor_height_ - config.y_offset);
// If width or height is 0, set it as large as possible
int width = config.width ? config.width : sensor_width_ - config.x_offset;
int height = config.height ? config.height : sensor_height_ - config.y_offset;
// Adjust full-res ROI to binning ROI
/// @todo Replicating logic from polledCallback
int x_offset = config.x_offset / config.binning_x;
int y_offset = config.y_offset / config.binning_y;
unsigned int right_x = (config.x_offset + width + config.binning_x - 1) / config.binning_x;
unsigned int bottom_y = (config.y_offset + height + config.binning_y - 1) / config.binning_y;
// Rounding up is bad when at max resolution which is not divisible by the amount of binning
right_x = std::min(right_x, (unsigned)(sensor_width_ / config.binning_x));
bottom_y = std::min(bottom_y, (unsigned)(sensor_height_ / config.binning_y));
width = right_x - x_offset;
height = bottom_y - y_offset;
cam_->setRoi(x_offset, y_offset, width, height);
// TF frame
img_.header.frame_id = cam_info_.header.frame_id = config.frame_id;
// Normally the node adjusts the bandwidth used by the camera during diagnostics, to use as
// much as possible without dropping packets. But this can create interference if two
// cameras are on the same switch, e.g. for stereo. So we allow the user to set the bandwidth
// directly.
auto_adjust_stream_bytes_per_second_ = config.auto_adjust_stream_bytes_per_second;
if (!auto_adjust_stream_bytes_per_second_)
cam_->setAttribute("StreamBytesPerSecond", (tPvUint32)config.stream_bytes_per_second);
/// @todo If exception thrown due to bad settings, will fail to start camera
if (level >= (uint32_t)driver_base::SensorLevels::RECONFIGURE_STOP)
start();
}