void configure (pgr_camera_driver::PGRCameraConfig & config, uint32_t level)
{
ROS_INFO ("Reconfigure request received");
if (level >= (uint32_t) driver_base::SensorLevels::RECONFIGURE_STOP)
stop ();
loadIntrinsics (config.intrinsics_ini, config.camera_name);
/*
* If Shutter and Gain are both set to auto, then auto exposure
* will tune each one to maintain a constant exposure at each pixel.
*
* If only one of Shutter/Gain is set to auto, then that value
* will be tuned to enforce the constant exposure value at each
* pixel, while the other value is not changed.
*
* If both Shutter and Gain are set to manual, then auto exposure
* has no effect.
*
*/
// Exposure
if (config.auto_exposure)
cam_->SetExposure (true, true);
else
cam_->SetExposure (false, true);
// Shutter
if (config.auto_shutter)
cam_->SetShutter (true);
else
cam_->SetShutter (false, (float)config.shutter);
// Gain
if(config.auto_gain)
cam_->SetGain(true);
else
cam_->SetGain(false, (float)config.gain);
// video mode / framerate
cam_->SetVideoModeAndFramerate (config.width, config.height, config.format, config.frame_rate);
width_ = config.width;
height_ = config.height;
// TF frame
frame_id_ = config.frame_id;
if (level >= (uint32_t) driver_base::SensorLevels::RECONFIGURE_STOP)
start ();
}
void StereoNode::reconfig(stereoConfig &config, uint32_t level)
{
force_streaming_ = config.force_streaming;
handlePath(config.config_path);
const FlashMode flash_active_mode = config.flash_polarity ? FLASH_FREERUN_ACTIVE_HI : FLASH_FREERUN_ACTIVE_LO;
if (trigger_mode_ != config.trigger) {
stopCamera();
TriggerMode l_trigger, r_trigger;
FlashMode l_flash = FLASH_OFF;
FlashMode r_flash = FLASH_OFF;
switch (config.trigger) {
case stereo_TGR_SOFTWARE:
case stereo_TGR_HARDWARE_RISING:
l_trigger = r_trigger = TRIGGER_LO_HI;
break;
case stereo_TGR_HARDWARE_FALLING:
l_trigger = r_trigger = TRIGGER_HI_LO;
break;
case stereo_TGR_L_MASTER_R_RISING:
l_trigger = TRIGGER_OFF;
r_trigger = TRIGGER_LO_HI;
l_flash = flash_active_mode;
break;
case stereo_TGR_L_MASTER_R_FALLING:
l_trigger = TRIGGER_OFF;
r_trigger = TRIGGER_HI_LO;
l_flash = flash_active_mode;
break;
case stereo_TGR_R_MASTER_L_RISING:
l_trigger = TRIGGER_LO_HI;
r_trigger = TRIGGER_OFF;
r_flash = flash_active_mode;
break;
case stereo_TGR_R_MASTER_L_FALLING:
l_trigger = TRIGGER_HI_LO;
r_trigger = TRIGGER_OFF;
r_flash = flash_active_mode;
break;
case stereo_TGR_AUTO:
default:
config.trigger = stereo_TGR_AUTO;
l_trigger = r_trigger = TRIGGER_OFF;
break;
}
if (!(l_cam_.setTriggerMode(l_trigger) && r_cam_.setTriggerMode(r_trigger))) {
ROS_ERROR("Failed to configure triggers");
l_cam_.setTriggerMode(TRIGGER_OFF);
r_cam_.setTriggerMode(TRIGGER_OFF);
config.trigger = stereo_TGR_AUTO;
l_cam_.setFlash(FLASH_OFF);
r_cam_.setFlash(FLASH_OFF);
} else {
l_cam_.setFlash(l_flash, config.flash_delay, config.flash_duration);
r_cam_.setFlash(r_flash, config.flash_delay, config.flash_duration);
}
}
// Latch Auto Parameters
if (auto_gain_ && !config.auto_gain) {
config.gain = l_cam_.getHardwareGain();
}
auto_gain_ = config.auto_gain;
if (auto_exposure_ && !config.auto_exposure) {
config.exposure_time = l_cam_.getExposure();
}
auto_exposure_ = config.auto_exposure;
// Pixel Clock
if (l_cam_.getPixelClock() != config.l_pixel_clock) {
l_cam_.setPixelClock(&config.l_pixel_clock);
}
if (r_cam_.getPixelClock() != config.r_pixel_clock) {
r_cam_.setPixelClock(&config.r_pixel_clock);
}
reconfigCam(config, level, l_cam_);
reconfigCam(config, level, r_cam_);
trigger_mode_ = config.trigger;
if (trigger_mode_ == stereo_TGR_SOFTWARE) {
timer_force_trigger_.setPeriod(ros::Duration(1 / config.frame_rate));
}
if (zoom_ != config.zoom) {
zoom_ = config.zoom;
loadIntrinsics(l_cam_, l_msg_camera_info_);
loadIntrinsics(r_cam_, r_msg_camera_info_);
}
l_msg_camera_info_.header.frame_id = config.l_frame_id;
r_msg_camera_info_.header.frame_id = config.r_frame_id;
configured_ = true;
}
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));
}
