bool setCameraInfo(sensor_msgs::SetCameraInfo::Request& req,
sensor_msgs::SetCameraInfo::Response& rsp)
{
ROS_INFO("New camera info received");
sensor_msgs::CameraInfo &info = req.camera_info;
// Sanity check: the image dimensions should match the max resolution of the sensor.
tPvUint32 width, height, dummy;
PvAttrRangeUint32(cam_->handle(), "Width", &dummy, &width);
PvAttrRangeUint32(cam_->handle(), "Height", &dummy, &height);
if (info.width != width || info.height != height) {
rsp.success = false;
rsp.status_message = (boost::format("Camera_info resolution %ix%i does not match current video "
"setting, camera running at resolution %ix%i.")
% info.width % info.height % width % height).str();
ROS_ERROR("%s", rsp.status_message.c_str());
return true;
}
stop();
std::string cam_name = "prosilica";
cam_name += hw_id_;
std::stringstream ini_stream;
if (!camera_calibration_parsers::writeCalibrationIni(ini_stream, cam_name, info)) {
rsp.status_message = "Error formatting camera_info for storage.";
rsp.success = false;
}
else {
std::string ini = ini_stream.str();
if (ini.size() > prosilica::Camera::USER_MEMORY_SIZE) {
rsp.success = false;
rsp.status_message = "Unable to write camera_info to camera memory, exceeded storage capacity.";
}
else {
try {
cam_->writeUserMemory(ini.c_str(), ini.size());
cam_info_ = info;
rsp.success = true;
}
catch (prosilica::ProsilicaException &e) {
rsp.success = false;
rsp.status_message = e.what();
}
}
}
if (!rsp.success)
ROS_ERROR("%s", rsp.status_message.c_str());
start();
return true;
}
unsigned long Camera::getMaxDataRate()
{
tPvUint32 min_data_rate, max_data_rate;
CHECK_ERR( PvAttrRangeUint32(handle_, "StreamBytesPerSecond", &min_data_rate, &max_data_rate),
"Couldn't get range of attribute StreamBytesPerSecond" );
return max_data_rate;
}
void Camera::setRoiToWholeFrame()
{
tPvUint32 min_val, max_val;
CHECK_ERR( PvAttrUint32Set(handle_, "RegionX", 0),
"Couldn't set region x (left edge)" );
CHECK_ERR( PvAttrUint32Set(handle_, "RegionY", 0),
"Couldn't set region y (top edge)" );
CHECK_ERR( PvAttrRangeUint32(handle_, "Width", &min_val, &max_val),
"Couldn't get range of Width attribute" );
CHECK_ERR( PvAttrUint32Set(handle_, "Width", max_val),
"Couldn't set region width" );
CHECK_ERR( PvAttrRangeUint32(handle_, "Height", &min_val, &max_val),
"Couldn't get range of Height attribute" );
CHECK_ERR( PvAttrUint32Set(handle_, "Height", max_val),
"Couldn't set region height" );
}
//
// idlPvAttrUint32Range
//
// Get the range of values of a Uint32 attribute
//
// command line arguments
// argv[0]: IN/FLAG debug
// argv[1]: IN camera index
// argv[2]: IN attribute name
// argv[3]: OUT attribute range min
// argv[4]: OUT attribute range max
int idlPvAttrUint32Range (int argc, char *argv[])
{
unsigned long n;
unsigned long err;
IDL_STRING *name;
unsigned long *vmin;
unsigned long *vmax;
debug = *(IDL_INT *) argv[0];
n = *(unsigned long *) argv[1];
name = (IDL_STRING *) argv[2];
vmin = (unsigned long *) argv[3];
vmax = (unsigned long *) argv[4];
CHECKINDEX(n);
err = PvAttrRangeUint32(camera[n],
(const char *) IDL_STRING_STR(name),
(tPvUint32 *) vmin, (tPvUint32 *) vmax);
return idlPvErrCode(err);
}
// set the value of a given attribute from a value encoded in a string
bool String2Value(tPvHandle aCamera,const char* aLabel,tPvDatatype aType,char* aValue)
{
switch(aType)
{
case ePvDatatypeString:
{
if(!PvAttrStringSet(aCamera,aLabel,aValue))
return true;
else
return false;
}
case ePvDatatypeEnum:
{
if(!PvAttrEnumSet(aCamera,aLabel,aValue))
return true;
else
return false;
}
case ePvDatatypeUint32:
{
tPvUint32 lValue = atol(aValue);
tPvUint32 lMin,lMax;
if(!PvAttrRangeUint32(aCamera,aLabel,&lMin,&lMax))
{
if(lMin > lValue)
lValue = lMin;
else
if(lMax < lValue)
lValue = lMax;
if(!PvAttrUint32Set(aCamera,aLabel,lValue))
return true;
else
return false;
}
else
return false;
}
case ePvDatatypeFloat32:
{
tPvFloat32 lValue = (tPvFloat32)atof(aValue);
tPvFloat32 lMin,lMax;
if(!PvAttrRangeFloat32(aCamera,aLabel,&lMin,&lMax))
{
if(lMin > lValue)
lValue = lMin;
else
if(lMax < lValue)
lValue = lMax;
if(!PvAttrFloat32Set(aCamera,aLabel,lValue))
return true;
else
return false;
}
else
return false;
}
default:
return false;
}
}
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();
}
ProsilicaNode(const ros::NodeHandle& node_handle)
: nh_(node_handle),
it_(nh_),
cam_(NULL), running_(false), auto_adjust_stream_bytes_per_second_(false),
count_(0),
frames_dropped_total_(0), frames_completed_total_(0),
frames_dropped_acc_(WINDOW_SIZE),
frames_completed_acc_(WINDOW_SIZE),
packets_missed_total_(0), packets_received_total_(0),
packets_missed_acc_(WINDOW_SIZE),
packets_received_acc_(WINDOW_SIZE)
{
// Two-stage initialization: in the constructor we open the requested camera. Most
// parameters controlling capture are set and streaming started in configure(), the
// callback to dynamic_reconfig.
prosilica::init();
if (prosilica::numCameras() == 0)
ROS_WARN("Found no cameras on local subnet");
// Determine which camera to use. Opening by IP address is preferred, then guid. If both
// parameters are set we open by IP and verify the guid. If neither are set we default
// to opening the first available camera.
ros::NodeHandle local_nh("~");
unsigned long guid = 0;
std::string guid_str;
if (local_nh.getParam("guid", guid_str) && !guid_str.empty())
guid = strtol(guid_str.c_str(), NULL, 0);
std::string ip_str;
if (local_nh.getParam("ip_address", ip_str) && !ip_str.empty()) {
cam_.reset( new prosilica::Camera(ip_str.c_str()) );
// Verify guid is the one expected
unsigned long cam_guid = cam_->guid();
if (guid != 0 && guid != cam_guid)
throw prosilica::ProsilicaException(ePvErrBadParameter,
"guid does not match expected");
guid = cam_guid;
}
else {
if (guid == 0) guid = prosilica::getGuid(0);
cam_.reset( new prosilica::Camera(guid) );
}
hw_id_ = boost::lexical_cast<std::string>(guid);
ROS_INFO("Found camera, guid = %s", hw_id_.c_str());
diagnostic_.setHardwareID(hw_id_);
// Record some attributes of the camera
tPvUint32 dummy;
PvAttrRangeUint32(cam_->handle(), "Width", &dummy, &sensor_width_);
PvAttrRangeUint32(cam_->handle(), "Height", &dummy, &sensor_height_);
// Try to load intrinsics from on-camera memory.
loadIntrinsics();
// Set up self tests and diagnostics.
// NB: Need to wait until here to construct self_test_, otherwise an exception
// above from failing to find the camera gives bizarre backtraces
// (http://answers.ros.org/question/430/trouble-with-prosilica_camera-pvapi).
self_test_.reset(new self_test::TestRunner);
self_test_->add( "Info Test", this, &ProsilicaNode::infoTest );
self_test_->add( "Attribute Test", this, &ProsilicaNode::attributeTest );
self_test_->add( "Image Test", this, &ProsilicaNode::imageTest );
diagnostic_.add( "Frequency Status", this, &ProsilicaNode::freqStatus );
diagnostic_.add( "Frame Statistics", this, &ProsilicaNode::frameStatistics );
diagnostic_.add( "Packet Statistics", this, &ProsilicaNode::packetStatistics );
diagnostic_.add( "Packet Error Status", this, &ProsilicaNode::packetErrorStatus );
diagnostic_timer_ = nh_.createTimer(ros::Duration(0.1), boost::bind(&ProsilicaNode::runDiagnostics, this));
// Service call for setting calibration.
set_camera_info_srv_ = nh_.advertiseService("set_camera_info", &ProsilicaNode::setCameraInfo, this);
// Start dynamic_reconfigure
reconfigure_server_.setCallback(boost::bind(&ProsilicaNode::configure, this, _1, _2));
}
bool CameraGigE::onInit() {
LOG(LTRACE) << "CameraGigE::initialize\n";
h_onTrigger.setup(this, &CameraGigE::onTrigger);
registerHandler("onTrigger", &h_onTrigger);
newImage = registerEvent("newImage");
endOfSequence = registerEvent("endOfSequence");
registerStream("out_img", &out_img);
if (!props.address.empty()) {
unsigned long ip = inet_addr(props.address.c_str());
if (PvCameraOpenByAddr(ip, ePvAccessMaster, &cHandle) != ePvErrSuccess) {
LOG(LERROR) << "Unable to open camera on adress "
<< props.address << " \n";
return false;
}
} else if (props.uid != 0) {
if (PvCameraOpen(props.uid, ePvAccessMaster, &cHandle) != ePvErrSuccess) {
LOG(LERROR) << "Unable to open camera with uid " << props.uid
<< " \n";
return false;
}
} else {
return false;
}
// Set parameters
tPvErr err;
/// Exposure
if (!props.exposureMode.empty()) {
if ((err = PvAttrEnumSet(cHandle, "ExposureMode",
props.exposureMode.c_str())) == ePvErrSuccess) {
if (props.exposureMode == "Manual") {
if ((err = PvAttrUint32Set(cHandle, "ExposureValue",
props.exposureValue / 1000000.0)) != ePvErrSuccess) {
if (err == ePvErrOutOfRange) {
tPvUint32 min, max;
PvAttrRangeUint32(cHandle, "ExposureValue", &min, &max);
LOG(LWARNING) << "ExposureValue : "
<< props.exposureValue
<< " is out of range, valid range [ "
<< (double) min / 1000000.0 << " , "
<< (double) max / 1000000.0 << " ]\n";
}
}
}
} else {
LOG(LWARNING) << "Unable to set ExposureMode \n";
}
}
/// Gain
if (!props.gainMode.empty()) {
if ((err = PvAttrEnumSet(cHandle, "GainMode", props.gainMode.c_str()))
== ePvErrSuccess) {
if (props.gainMode == "Manual") {
if ((err = PvAttrUint32Set(cHandle, "gainValue",
props.gainValue)) != ePvErrSuccess) {
if (err == ePvErrOutOfRange) {
tPvUint32 min, max;
PvAttrRangeUint32(cHandle, "GainValue", &min, &max);
LOG(LWARNING) << "GainValue : " << props.gainValue
<< " is out of range, valid range [ "
<< (double) min << " , " << (double) max
<< " ]\n";
}
}
}
} else {
LOG(LWARNING) << "Unable to set GainMode \n";
}
}
/// White Balance
if (!props.whitebalMode.empty()) {
if ((err = PvAttrEnumSet(cHandle, "WhitebalMode",
props.gainMode.c_str())) == ePvErrSuccess) {
if (props.whitebalMode == "Manual") {
if ((err = PvAttrUint32Set(cHandle, "WhitebalValueRed",
props.whitebalValueRed)) != ePvErrSuccess) {
if (err == ePvErrOutOfRange) {
tPvUint32 min, max;
PvAttrRangeUint32(cHandle, "WhitebalValueRed", &min,
&max);
LOG(LWARNING) << "WhitebalValueRed : "
<< props.whitebalValueRed
<< " is out of range, valid range [ "
<< (double) min << " , " << (double) max
<< " ]\n";
}
}
if ((err = PvAttrUint32Set(cHandle, "WhitebalValueBlue",
props.whitebalValueBlue)) != ePvErrSuccess) {
if (err == ePvErrOutOfRange) {
tPvUint32 min, max;
PvAttrRangeUint32(cHandle, "WhitebalValueBlue", &min,
&max);
LOG(LWARNING) << "WhitebalValueBlue : "
<< props.whitebalValueBlue
<< " is out of range, valid range [ "
<< (double) min << " , " << (double) max
<< " ]\n";
}
}
}
} else {
LOG(LWARNING) << "Unable to set WhitebalMode" << err << "\n";
}
}
if ((err = PvAttrEnumSet(cHandle, "MirrorX", props.mirrorX ? "On" : "Off"))
!= ePvErrSuccess) {
}
if ((err = PvAttrEnumSet(cHandle, "PixelFormat", props.pixelFormat.c_str()))
!= ePvErrSuccess) {
LOG(LERROR) << "Unable to set pixelformat " << err;
}
if ((err = PvAttrUint32Set(cHandle, "Height", props.height))
!= ePvErrSuccess) {
if (err == ePvErrOutOfRange) {
tPvUint32 min, max;
PvAttrRangeUint32(cHandle, "Height", &min, &max);
LOG(LWARNING) << "Height : " << props.height
<< " is out of range, valid range [ " << (double) min
<< " , " << (double) max << " ]";
}
}
if ((err = PvAttrUint32Set(cHandle, "Width", props.width)) != ePvErrSuccess) {
if (err == ePvErrOutOfRange) {
tPvUint32 min, max;
PvAttrRangeUint32(cHandle, "Width", &min, &max);
LOG(LWARNING) << "Width : " << props.width
<< " is out of range, valid range [ " << (double) min
<< " , " << (double) max << " ]\n";
}
}
if ((err = PvAttrUint32Set(cHandle, "RegionX", props.regionX))
!= ePvErrSuccess) {
if (err == ePvErrOutOfRange) {
tPvUint32 min, max;
PvAttrRangeUint32(cHandle, "RegionX", &min, &max);
LOG(LWARNING) << "RegionX : " << props.regionX
<< " is out of range, valid range [ " << (double) min
<< " , " << (double) max << " ]\n";
}
}
if ((err = PvAttrUint32Set(cHandle, "RegionY", props.regionY))
!= ePvErrSuccess) {
if (err == ePvErrOutOfRange) {
tPvUint32 min, max;
PvAttrRangeUint32(cHandle, "RegionY", &min, &max);
LOG(LWARNING) << "RegionY : " << props.regionY
<< " is out of range, valid range [ " << (double) min
<< " , " << (double) max << " ]\n";
}
}
if ((err = PvAttrUint32Set(cHandle, "BinningX", props.binningX))
!= ePvErrSuccess) {
if (err == ePvErrOutOfRange) {
tPvUint32 min, max;
PvAttrRangeUint32(cHandle, "BinningX", &min, &max);
LOG(LWARNING) << "BinningX : " << props.binningX
<< " is out of range, valid range [ " << (double) min
<< " , " << (double) max << " ]\n";
}
}
if ((err = PvAttrUint32Set(cHandle, "BinningY", props.binningY))
!= ePvErrSuccess) {
if (err == ePvErrOutOfRange) {
tPvUint32 min, max;
PvAttrRangeUint32(cHandle, "BinningY", &min, &max);
LOG(LWARNING) << "BinningY : " << props.binningY
<< " is out of range, valid range [ " << (double) min
<< " , " << (double) max << " ]\n";
}
}
// ----------------
PvAttrEnumSet(cHandle, "FrameStartTriggerMode", "Freerun");
unsigned long frameSize = 0;
if (PvAttrUint32Get(cHandle, "TotalBytesPerFrame", &frameSize)
!= ePvErrSuccess) {
return false;
}
frame.ImageBuffer = new char[frameSize];
frame.ImageBufferSize = frameSize;
return true;
}
