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)); }