예제 #1
0
  // Try to capture an image.
  void imageTest(diagnostic_updater::DiagnosticStatusWrapper& status)
  {
    status.name = "Image Capture Test";

    try {
      if (trigger_mode_ != prosilica::Software) {
        tPvUint32 start_completed, current_completed;
        cam_->getAttribute("StatFramesCompleted", start_completed);
        for (int i = 0; i < 6; ++i) {
          boost::this_thread::sleep(boost::posix_time::millisec(500));
          cam_->getAttribute("StatFramesCompleted", current_completed);
          if (current_completed > start_completed) {
            status.summary(0, "Captured a frame, see diagnostics for detailed stats");
            return;
          }
        }

        // Give up after 3s
        status.summary(2, "No frames captured over 3s in freerun mode");
        return;
      }

      // In software triggered mode, try to grab a frame
      cam_->setRoiToWholeFrame();
      tPvFrame* frame = cam_->grab(500);
      if (frame)
	{
	  status.summary(0, "Successfully triggered a frame capture");
	}
      else
	{
	  status.summary(2, "Attempted to grab a frame, but received NULL");
	}

    }
    catch (prosilica::ProsilicaException &e) {
      status.summary(2, e.what());
    }
  }
예제 #2
0
  void packetErrorStatus(diagnostic_updater::DiagnosticStatusWrapper& status)
  {
    unsigned long erroneous;
    cam_->getAttribute("StatPacketsErroneous", erroneous);

    if (erroneous == 0) {
      status.summary(0, "No erroneous packets");
    } else {
      status.summary(2, "Possible camera hardware failure");
    }

    status.add("Erroneous Packets", erroneous);
  }
예제 #3
0
  void normalizeCallback(tPvFrame* frame)
  {
    unsigned long exposure;
    cam_->getAttribute("ExposureValue", exposure);
    //ROS_WARN("Exposure value = %u", exposure);

    if (exposure == last_exposure_value_)
      consecutive_stable_exposures_++;
    else {
      last_exposure_value_ = exposure;
      consecutive_stable_exposures_ = 0;
    }
  }
예제 #4
0
  void frameStatistics(diagnostic_updater::DiagnosticStatusWrapper& status)
  {
    // Get stats from camera driver
    float frame_rate;
    unsigned long completed, dropped;
    cam_->getAttribute("StatFrameRate", frame_rate);
    cam_->getAttribute("StatFramesCompleted", completed);
    cam_->getAttribute("StatFramesDropped", dropped);

    // Compute rolling totals, percentages
    frames_completed_acc_.add(completed - frames_completed_total_);
    frames_completed_total_ = completed;
    unsigned long completed_recent = frames_completed_acc_.sum();

    frames_dropped_acc_.add(dropped - frames_dropped_total_);
    frames_dropped_total_ = dropped;
    unsigned long dropped_recent = frames_dropped_acc_.sum();

    float recent_ratio = float(completed_recent) / (completed_recent + dropped_recent);
    float total_ratio = float(completed) / (completed + dropped);

    // Set level based on recent % completed frames
    if (dropped_recent == 0) {
      status.summary(0, "No dropped frames");
    }
    else if (recent_ratio > 0.8f) {
      status.summary(1, "Some dropped frames");
    }
    else {
      status.summary(2, "Excessive proportion of dropped frames");
    }

    status.add("Camera Frame Rate", frame_rate);
    status.add("Recent % Frames Completed", recent_ratio * 100.0f);
    status.add("Overall % Frames Completed", total_ratio * 100.0f);
    status.add("Frames Completed", completed);
    status.add("Frames Dropped", dropped);
  }
예제 #5
0
  // Try to load camera name, etc. Should catch gross communication failure.
  void infoTest(diagnostic_updater::DiagnosticStatusWrapper& status)
  {
    status.name = "Info Test";

    self_test_->setID(hw_id_);

    std::string camera_name;
    try {
      cam_->getAttribute("CameraName", camera_name);
      status.summary(0, "Connected to Camera");
      status.add("Camera Name", camera_name);
    }
    catch (prosilica::ProsilicaException& e) {
      status.summary(2, e.what());
    }
  }
예제 #6
0
  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;
  }
예제 #7
0
  void packetStatistics(diagnostic_updater::DiagnosticStatusWrapper& status)
  {
    // Get stats from camera driver
    unsigned long received, missed, requested, resent;
    cam_->getAttribute("StatPacketsReceived", received);
    cam_->getAttribute("StatPacketsMissed", missed);
    cam_->getAttribute("StatPacketsRequested", requested);
    cam_->getAttribute("StatPacketsResent", resent);

    // Compute rolling totals, percentages
    packets_received_acc_.add(received - packets_received_total_);
    packets_received_total_ = received;
    unsigned long received_recent = packets_received_acc_.sum();

    packets_missed_acc_.add(missed - packets_missed_total_);
    packets_missed_total_ = missed;
    unsigned long missed_recent = packets_missed_acc_.sum();

    float recent_ratio = float(received_recent) / (received_recent + missed_recent);
    float total_ratio = float(received) / (received + missed);

    if (missed_recent == 0) {
      status.summary(0, "No missed packets");
    }
    else if (recent_ratio > 0.99f) {
      status.summary(1, "Some missed packets");
    }
    else {
      status.summary(2, "Excessive proportion of missed packets");
    }

    // Adjust data rate
    unsigned long data_rate = 0;
    cam_->getAttribute("StreamBytesPerSecond", data_rate);
    unsigned long max_data_rate = cam_->getMaxDataRate();

    if (auto_adjust_stream_bytes_per_second_)
    {
      if (max_data_rate < prosilica::Camera::GIGE_MAX_DATA_RATE)
        status.mergeSummary(1, "Max data rate is lower than expected for a GigE port");

      try
      {
        /// @todo Something that doesn't oscillate
        float multiplier = 1.0f;
        if (recent_ratio == 1.0f)
        {
          multiplier = 1.1f;
	    }
        else if (recent_ratio < 0.99f)
        {
          multiplier = 0.9f;
        }
        if (multiplier != 1.0f)
        {
          unsigned long new_data_rate = std::min((unsigned long)(multiplier * data_rate + 0.5), max_data_rate);
          new_data_rate = std::max(new_data_rate, max_data_rate/1000);
          if (data_rate != new_data_rate)
          {
            data_rate = new_data_rate;
            cam_->setAttribute("StreamBytesPerSecond", data_rate);
            ROS_DEBUG("Changed data rate to %lu bytes per second", data_rate);
          }
        }
      }
      catch (prosilica::ProsilicaException &e)
      {
        if (e.error_code == ePvErrUnplugged)
          throw;
        ROS_ERROR("Exception occurred: '%s'\n"
                  "Possible network issue. Attempting to reset data rate to the current maximum.",
                  e.what());
        data_rate = max_data_rate;
        cam_->setAttribute("StreamBytesPerSecond", data_rate);
      }
    }

    status.add("Recent % Packets Received", recent_ratio * 100.0f);
    status.add("Overall % Packets Received", total_ratio * 100.0f);
    status.add("Received Packets", received);
    status.add("Missed Packets", missed);
    status.add("Requested Packets", requested);
    status.add("Resent Packets", resent);
    status.add("Data Rate (bytes/s)", data_rate);
    status.add("Max Data Rate (bytes/s)", max_data_rate);
  }
예제 #8
0
  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();
  }