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 ();
  }
Beispiel #2
0
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));
  }