00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #include <ros/ros.h>
00037 #include <sensor_msgs/Image.h>
00038 #include <sensor_msgs/CameraInfo.h>
00039 #include <sensor_msgs/fill_image.h>
00040 #include <sensor_msgs/SetCameraInfo.h>
00041 #include <std_msgs/Header.h>
00042 #include <image_transport/image_transport.h>
00043 #include <camera_calibration_parsers/parse_ini.h>
00044 #include <polled_camera/publication_server.h>
00045
00046
00047 #include <diagnostic_updater/diagnostic_updater.h>
00048 #include <diagnostic_updater/DiagnosticStatusWrapper.h>
00049 #include <self_test/self_test.h>
00050
00051
00052 #include <dynamic_reconfigure/server.h>
00053 #include <driver_base/SensorLevels.h>
00054 #include "prosilica_camera/ProsilicaCameraConfig.h"
00055
00056
00057 #include <boost/scoped_ptr.hpp>
00058 #include <boost/bind.hpp>
00059 #include <boost/lexical_cast.hpp>
00060 #include <boost/format.hpp>
00061 #include <sstream>
00062
00063 #include "prosilica/prosilica.h"
00064 #include "prosilica/rolling_sum.h"
00065
00067 class ProsilicaNode
00068 {
00069 private:
00070 ros::NodeHandle nh_;
00071 image_transport::ImageTransport it_;
00072 image_transport::CameraPublisher streaming_pub_;
00073 polled_camera::PublicationServer poll_srv_;
00074 ros::ServiceServer set_camera_info_srv_;
00075 ros::Subscriber trigger_sub_;
00076
00077
00078 boost::scoped_ptr<prosilica::Camera> cam_;
00079 prosilica::FrameStartTriggerMode trigger_mode_;
00080 bool running_;
00081 unsigned long max_data_rate_;
00082 tPvUint32 sensor_width_, sensor_height_;
00083 bool auto_adjust_stream_bytes_per_second_;
00084
00085
00086 std::string trig_timestamp_topic_;
00087 ros::Time trig_time_;
00088
00089
00090 sensor_msgs::Image img_;
00091 sensor_msgs::CameraInfo cam_info_;
00092
00093
00094 typedef prosilica_camera::ProsilicaCameraConfig Config;
00095 typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
00096 ReconfigureServer reconfigure_server_;
00097
00098
00099 ros::Timer diagnostic_timer_;
00100 boost::shared_ptr<self_test::TestRunner> self_test_;
00101 diagnostic_updater::Updater diagnostic_;
00102 std::string hw_id_;
00103 int count_;
00104 double desired_freq_;
00105 static const int WINDOW_SIZE = 5;
00106 unsigned long frames_dropped_total_, frames_completed_total_;
00107 RollingSum<unsigned long> frames_dropped_acc_, frames_completed_acc_;
00108 unsigned long packets_missed_total_, packets_received_total_;
00109 RollingSum<unsigned long> packets_missed_acc_, packets_received_acc_;
00110
00111
00112 unsigned long last_exposure_value_;
00113 int consecutive_stable_exposures_;
00114
00115 public:
00116 ProsilicaNode(const ros::NodeHandle& node_handle)
00117 : nh_(node_handle),
00118 it_(nh_),
00119 cam_(NULL), running_(false), auto_adjust_stream_bytes_per_second_(false),
00120 count_(0),
00121 frames_dropped_total_(0), frames_completed_total_(0),
00122 frames_dropped_acc_(WINDOW_SIZE),
00123 frames_completed_acc_(WINDOW_SIZE),
00124 packets_missed_total_(0), packets_received_total_(0),
00125 packets_missed_acc_(WINDOW_SIZE),
00126 packets_received_acc_(WINDOW_SIZE)
00127 {
00128
00129
00130
00131 prosilica::init();
00132
00133 if (prosilica::numCameras() == 0)
00134 ROS_WARN("Found no cameras on local subnet");
00135
00136
00137
00138
00139 ros::NodeHandle local_nh("~");
00140 unsigned long guid = 0;
00141 std::string guid_str;
00142 if (local_nh.getParam("guid", guid_str) && !guid_str.empty())
00143 guid = strtol(guid_str.c_str(), NULL, 0);
00144
00145 std::string ip_str;
00146 if (local_nh.getParam("ip_address", ip_str) && !ip_str.empty()) {
00147 cam_.reset( new prosilica::Camera(ip_str.c_str()) );
00148
00149
00150 unsigned long cam_guid = cam_->guid();
00151 if (guid != 0 && guid != cam_guid)
00152 throw prosilica::ProsilicaException(ePvErrBadParameter,
00153 "guid does not match expected");
00154 guid = cam_guid;
00155 }
00156 else {
00157 if (guid == 0) guid = prosilica::getGuid(0);
00158 cam_.reset( new prosilica::Camera(guid) );
00159 }
00160 hw_id_ = boost::lexical_cast<std::string>(guid);
00161 ROS_INFO("Found camera, guid = %s", hw_id_.c_str());
00162 diagnostic_.setHardwareID(hw_id_);
00163
00164
00165 tPvUint32 dummy;
00166 PvAttrRangeUint32(cam_->handle(), "Width", &dummy, &sensor_width_);
00167 PvAttrRangeUint32(cam_->handle(), "Height", &dummy, &sensor_height_);
00168
00169
00170 loadIntrinsics();
00171
00172
00173
00174
00175
00176 self_test_.reset(new self_test::TestRunner);
00177 self_test_->add( "Info Test", this, &ProsilicaNode::infoTest );
00178 self_test_->add( "Attribute Test", this, &ProsilicaNode::attributeTest );
00179 self_test_->add( "Image Test", this, &ProsilicaNode::imageTest );
00180
00181 diagnostic_.add( "Frequency Status", this, &ProsilicaNode::freqStatus );
00182 diagnostic_.add( "Frame Statistics", this, &ProsilicaNode::frameStatistics );
00183 diagnostic_.add( "Packet Statistics", this, &ProsilicaNode::packetStatistics );
00184 diagnostic_.add( "Packet Error Status", this, &ProsilicaNode::packetErrorStatus );
00185
00186 diagnostic_timer_ = nh_.createTimer(ros::Duration(0.1), boost::bind(&ProsilicaNode::runDiagnostics, this));
00187
00188
00189 set_camera_info_srv_ = nh_.advertiseService("set_camera_info", &ProsilicaNode::setCameraInfo, this);
00190
00191
00192 reconfigure_server_.setCallback(boost::bind(&ProsilicaNode::configure, this, _1, _2));
00193 }
00194
00195 void configure(Config& config, uint32_t level)
00196 {
00197 ROS_DEBUG("Reconfigure request received");
00198
00199 if (level >= (uint32_t)driver_base::SensorLevels::RECONFIGURE_STOP)
00200 stop();
00201
00202
00203 if (config.trigger_mode == "streaming") {
00204 trigger_mode_ = prosilica::Freerun;
00206 desired_freq_ = 1;
00207 }
00208 else if (config.trigger_mode == "syncin1") {
00209 trigger_mode_ = prosilica::SyncIn1;
00210 desired_freq_ = config.trig_rate;
00211 }
00212 else if (config.trigger_mode == "syncin2") {
00213 trigger_mode_ = prosilica::SyncIn2;
00214 desired_freq_ = config.trig_rate;
00215 }
00216 #if 0
00217 else if (config.trigger_mode == "fixedrate") {
00218 ROS_DEBUG("Fixed rate not supported yet implementing software");
00219 trigger_mode_ = prosilica::Software;
00221 desired_freq_ = 0;
00222 }
00223 #endif
00224 else if (config.trigger_mode == "polled") {
00225 trigger_mode_ = prosilica::Software;
00226 desired_freq_ = 0;
00227 }
00228 else {
00229 ROS_ERROR("Invalid trigger mode '%s' in reconfigure request", config.trigger_mode.c_str());
00230 }
00231 trig_timestamp_topic_ = config.trig_timestamp_topic;
00232
00233
00234 if (config.auto_exposure)
00235 {
00236 cam_->setExposure(0, prosilica::Auto);
00237 if (cam_->hasAttribute("ExposureAutoMax"))
00238 {
00239 tPvUint32 us = config.exposure_auto_max*1000000. + 0.5;
00240 cam_->setAttribute("ExposureAutoMax", us);
00241 }
00242 if (cam_->hasAttribute("ExposureAutoTarget"))
00243 cam_->setAttribute("ExposureAutoTarget", (tPvUint32)config.exposure_auto_target);
00244 }
00245 else {
00246 unsigned us = config.exposure*1000000. + 0.5;
00247 cam_->setExposure(us, prosilica::Manual);
00248 }
00249
00250
00251 if (config.auto_gain) {
00252 if (cam_->hasAttribute("GainAutoMax"))
00253 {
00254 cam_->setGain(0, prosilica::Auto);
00255 cam_->setAttribute("GainAutoMax", (tPvUint32)config.gain_auto_max);
00256 cam_->setAttribute("GainAutoTarget", (tPvUint32)config.gain_auto_target);
00257 }
00258 else {
00259 tPvUint32 major, minor;
00260 cam_->getAttribute("FirmwareVerMajor", major);
00261 cam_->getAttribute("FirmwareVerMinor", minor);
00262 ROS_WARN("Auto gain not available for this camera. Auto gain is available "
00263 "on firmware versions 1.36 and above. You are running version %u.%u.",
00264 (unsigned)major, (unsigned)minor);
00265 config.auto_gain = false;
00266 }
00267 }
00268 else
00269 cam_->setGain(config.gain, prosilica::Manual);
00270
00271
00272 if (config.auto_whitebalance) {
00273 if (cam_->hasAttribute("WhitebalMode"))
00274 cam_->setWhiteBalance(0, 0, prosilica::Auto);
00275 else {
00276 ROS_WARN("Auto white balance not available for this camera.");
00277 config.auto_whitebalance = false;
00278 }
00279 }
00280 else
00281 cam_->setWhiteBalance(config.whitebalance_blue, config.whitebalance_red, prosilica::Manual);
00282
00283
00284 if (cam_->hasAttribute("BinningX")) {
00285 tPvUint32 max_binning_x, max_binning_y, dummy;
00286 PvAttrRangeUint32(cam_->handle(), "BinningX", &dummy, &max_binning_x);
00287 PvAttrRangeUint32(cam_->handle(), "BinningY", &dummy, &max_binning_y);
00288 config.binning_x = std::min(config.binning_x, (int)max_binning_x);
00289 config.binning_y = std::min(config.binning_y, (int)max_binning_y);
00290
00291 cam_->setBinning(config.binning_x, config.binning_y);
00292 }
00293 else if (config.binning_x > 1 || config.binning_y > 1)
00294 {
00295 ROS_WARN("Binning not available for this camera.");
00296 config.binning_x = config.binning_y = 1;
00297 }
00298
00299
00300
00301 config.x_offset = std::min(config.x_offset, (int)sensor_width_ - 1);
00302 config.y_offset = std::min(config.y_offset, (int)sensor_height_ - 1);
00303 config.width = std::min(config.width, (int)sensor_width_ - config.x_offset);
00304 config.height = std::min(config.height, (int)sensor_height_ - config.y_offset);
00305
00306 int width = config.width ? config.width : sensor_width_ - config.x_offset;
00307 int height = config.height ? config.height : sensor_height_ - config.y_offset;
00308
00309
00311 int x_offset = config.x_offset / config.binning_x;
00312 int y_offset = config.y_offset / config.binning_y;
00313 unsigned int right_x = (config.x_offset + width + config.binning_x - 1) / config.binning_x;
00314 unsigned int bottom_y = (config.y_offset + height + config.binning_y - 1) / config.binning_y;
00315
00316 right_x = std::min(right_x, (unsigned)(sensor_width_ / config.binning_x));
00317 bottom_y = std::min(bottom_y, (unsigned)(sensor_height_ / config.binning_y));
00318 width = right_x - x_offset;
00319 height = bottom_y - y_offset;
00320
00321 cam_->setRoi(x_offset, y_offset, width, height);
00322
00323
00324 img_.header.frame_id = cam_info_.header.frame_id = config.frame_id;
00325
00326
00327
00328
00329
00330 auto_adjust_stream_bytes_per_second_ = config.auto_adjust_stream_bytes_per_second;
00331 if (!auto_adjust_stream_bytes_per_second_)
00332 cam_->setAttribute("StreamBytesPerSecond", (tPvUint32)config.stream_bytes_per_second);
00333
00335 if (level >= (uint32_t)driver_base::SensorLevels::RECONFIGURE_STOP)
00336 start();
00337 }
00338
00339 ~ProsilicaNode()
00340 {
00341 stop();
00342 cam_.reset();
00343 prosilica::fini();
00344 }
00345
00346 void syncInCallback (const std_msgs::HeaderConstPtr& msg)
00347 {
00349 trig_time_ = msg->stamp;
00350 }
00351
00353
00354 void start()
00355 {
00356 if (running_) return;
00357
00358 if (trigger_mode_ == prosilica::Software) {
00359 poll_srv_ = polled_camera::advertise(nh_, "request_image", &ProsilicaNode::pollCallback, this);
00360
00361
00362 }
00363 else {
00364 if ((trigger_mode_ == prosilica::SyncIn1) || (trigger_mode_ == prosilica::SyncIn2)) {
00365 if (!trig_timestamp_topic_.empty())
00366 trigger_sub_ = nh_.subscribe(trig_timestamp_topic_, 1, &ProsilicaNode::syncInCallback, this);
00367 }
00368 else {
00369 assert(trigger_mode_ == prosilica::Freerun);
00370 }
00371 cam_->setFrameCallback(boost::bind(&ProsilicaNode::publishImage, this, _1));
00372 streaming_pub_ = it_.advertiseCamera("image_raw", 1);
00373 }
00374 cam_->start(trigger_mode_, prosilica::Continuous);
00375 running_ = true;
00376 }
00377
00378 void stop()
00379 {
00380 if (!running_) return;
00381
00382 cam_->stop();
00383 poll_srv_.shutdown();
00384 trigger_sub_.shutdown();
00385 streaming_pub_.shutdown();
00386
00387 running_ = false;
00388 }
00389
00390 void pollCallback(polled_camera::GetPolledImage::Request& req,
00391 polled_camera::GetPolledImage::Response& rsp,
00392 sensor_msgs::Image& image, sensor_msgs::CameraInfo& info)
00393 {
00394 if (trigger_mode_ != prosilica::Software) {
00395 rsp.success = false;
00396 rsp.status_message = "Camera is not in software triggered mode";
00397 return;
00398 }
00399
00400 tPvFrame* frame = NULL;
00401
00402 try {
00403 cam_->setBinning(req.binning_x, req.binning_y);
00404
00405 if (req.roi.x_offset || req.roi.y_offset || req.roi.width || req.roi.height) {
00406
00407
00408 unsigned int left_x = req.roi.x_offset / req.binning_x;
00409 unsigned int top_y = req.roi.y_offset / req.binning_y;
00410 unsigned int right_x = (req.roi.x_offset + req.roi.width + req.binning_x - 1) / req.binning_x;
00411 unsigned int bottom_y = (req.roi.y_offset + req.roi.height + req.binning_y - 1) / req.binning_y;
00412 unsigned int width = right_x - left_x;
00413 unsigned int height = bottom_y - top_y;
00414 cam_->setRoi(left_x, top_y, width, height);
00415 } else {
00416 cam_->setRoiToWholeFrame();
00417 }
00418
00419
00420 unsigned long timeout = req.timeout.isZero() ? PVINFINITE : req.timeout.toNSec() / 1e6;
00421 frame = cam_->grab(timeout);
00422 }
00423 catch (prosilica::ProsilicaException &e) {
00424 if (e.error_code == ePvErrBadSequence)
00425 throw;
00426
00427 rsp.success = false;
00428 rsp.status_message = (boost::format("Prosilica exception: %s") % e.what()).str();
00429 return;
00430 }
00431
00432 if (!frame) {
00434 rsp.success = false;
00435 rsp.status_message = "Failed to capture frame, may have timed out";
00436 return;
00437 }
00438
00439 info = cam_info_;
00440 image.header.frame_id = img_.header.frame_id;
00441 if (!processFrame(frame, image, info)) {
00442 rsp.success = false;
00443 rsp.status_message = "Captured frame but failed to process it";
00444 return;
00445 }
00446 info.roi.do_rectify = req.roi.do_rectify;
00447
00448 rsp.success = true;
00449 }
00450
00451 static bool frameToImage(tPvFrame* frame, sensor_msgs::Image &image)
00452 {
00453
00454 static const char* BAYER_ENCODINGS[] = { "bayer_rggb8", "bayer_gbrg8", "bayer_grbg8", "bayer_bggr8" };
00455
00456 std::string encoding;
00457 if (frame->Format == ePvFmtMono8) encoding = sensor_msgs::image_encodings::MONO8;
00458 else if (frame->Format == ePvFmtBayer8)
00459 {
00460 #if 1
00461 encoding = BAYER_ENCODINGS[frame->BayerPattern];
00462 #else
00463 image.encoding = sensor_msgs::image_encodings::BGR8;
00464 image.height = frame->Height;
00465 image.width = frame->Width;
00466 image.step = frame->Width * 3;
00467 image.data.resize(frame->Height * (frame->Width * 3));
00468 PvUtilityColorInterpolate(frame, &image.data[2], &image.data[1], &image.data[0], 2, 0);
00469 return true;
00470 #endif
00471 }
00472 else if (frame->Format == ePvFmtRgb24) encoding = sensor_msgs::image_encodings::RGB8;
00473 else if (frame->Format == ePvFmtBgr24) encoding = sensor_msgs::image_encodings::BGR8;
00474 else if (frame->Format == ePvFmtRgba32) encoding = sensor_msgs::image_encodings::RGBA8;
00475 else if (frame->Format == ePvFmtBgra32) encoding = sensor_msgs::image_encodings::BGRA8;
00476 else {
00477 ROS_WARN("Received frame with unsupported pixel format %d", frame->Format);
00478 return false;
00479 }
00480
00481 uint32_t step = frame->ImageSize / frame->Height;
00482 return sensor_msgs::fillImage(image, encoding, frame->Height, frame->Width, step, frame->ImageBuffer);
00483 }
00484
00485 bool processFrame(tPvFrame* frame, sensor_msgs::Image &img, sensor_msgs::CameraInfo &cam_info)
00486 {
00488 if ((trigger_mode_ == prosilica::SyncIn1 || trigger_mode_ == prosilica::SyncIn2) && !trig_time_.isZero()) {
00489 img.header.stamp = cam_info.header.stamp = trig_time_;
00490 trig_time_ = ros::Time();
00491 }
00492 else {
00494 img.header.stamp = cam_info.header.stamp = ros::Time::now();
00495 }
00496
00500 tPvUint32 binning_x = 1, binning_y = 1;
00501 if (cam_->hasAttribute("BinningX")) {
00502 cam_->getAttribute("BinningX", binning_x);
00503 cam_->getAttribute("BinningY", binning_y);
00504 }
00505
00506 if (frame->Format == ePvFmtBayer8 && (binning_x > 1 || binning_y > 1))
00507 frame->Format = ePvFmtMono8;
00508
00509 if (!frameToImage(frame, img))
00510 return false;
00511
00512
00513 cam_info.binning_x = binning_x;
00514 cam_info.binning_y = binning_y;
00515
00516 cam_info.roi.x_offset = frame->RegionX * binning_x;
00517 cam_info.roi.y_offset = frame->RegionY * binning_y;
00518 cam_info.roi.height = frame->Height * binning_y;
00519 cam_info.roi.width = frame->Width * binning_x;
00520 cam_info.roi.do_rectify = (frame->Height != sensor_height_ / binning_y) ||
00521 (frame->Width != sensor_width_ / binning_x);
00522
00523 count_++;
00524 return true;
00525 }
00526
00527 void publishImage(tPvFrame* frame)
00528 {
00529 if (processFrame(frame, img_, cam_info_))
00530 streaming_pub_.publish(img_, cam_info_);
00531 }
00532
00533 void loadIntrinsics()
00534 {
00535
00536 std::string buffer(prosilica::Camera::USER_MEMORY_SIZE, '\0');
00537 cam_->readUserMemory(&buffer[0], prosilica::Camera::USER_MEMORY_SIZE);
00538
00539
00540 std::string camera_name;
00541 if (camera_calibration_parsers::parseCalibrationIni(buffer, camera_name, cam_info_))
00542 ROS_INFO("Loaded calibration for camera '%s'", camera_name.c_str());
00543 else
00544 ROS_WARN("Failed to load intrinsics from camera");
00545 }
00546
00547 bool setCameraInfo(sensor_msgs::SetCameraInfo::Request& req,
00548 sensor_msgs::SetCameraInfo::Response& rsp)
00549 {
00550 ROS_INFO("New camera info received");
00551 sensor_msgs::CameraInfo &info = req.camera_info;
00552
00553
00554 tPvUint32 width, height, dummy;
00555 PvAttrRangeUint32(cam_->handle(), "Width", &dummy, &width);
00556 PvAttrRangeUint32(cam_->handle(), "Height", &dummy, &height);
00557 if (info.width != width || info.height != height) {
00558 rsp.success = false;
00559 rsp.status_message = (boost::format("Camera_info resolution %ix%i does not match current video "
00560 "setting, camera running at resolution %ix%i.")
00561 % info.width % info.height % width % height).str();
00562 ROS_ERROR("%s", rsp.status_message.c_str());
00563 return true;
00564 }
00565
00566 stop();
00567
00568 std::string cam_name = "prosilica";
00569 cam_name += hw_id_;
00570 std::stringstream ini_stream;
00571 if (!camera_calibration_parsers::writeCalibrationIni(ini_stream, cam_name, info)) {
00572 rsp.status_message = "Error formatting camera_info for storage.";
00573 rsp.success = false;
00574 }
00575 else {
00576 std::string ini = ini_stream.str();
00577 if (ini.size() > prosilica::Camera::USER_MEMORY_SIZE) {
00578 rsp.success = false;
00579 rsp.status_message = "Unable to write camera_info to camera memory, exceeded storage capacity.";
00580 }
00581 else {
00582 try {
00583 cam_->writeUserMemory(ini.c_str(), ini.size());
00584 cam_info_ = info;
00585 rsp.success = true;
00586 }
00587 catch (prosilica::ProsilicaException &e) {
00588 rsp.success = false;
00589 rsp.status_message = e.what();
00590 }
00591 }
00592 }
00593 if (!rsp.success)
00594 ROS_ERROR("%s", rsp.status_message.c_str());
00595
00596 start();
00597
00598 return true;
00599 }
00600
00601 void normalizeCallback(tPvFrame* frame)
00602 {
00603 unsigned long exposure;
00604 cam_->getAttribute("ExposureValue", exposure);
00605
00606
00607 if (exposure == last_exposure_value_)
00608 consecutive_stable_exposures_++;
00609 else {
00610 last_exposure_value_ = exposure;
00611 consecutive_stable_exposures_ = 0;
00612 }
00613 }
00614
00615 void normalizeExposure()
00616 {
00617 ROS_INFO("Normalizing exposure");
00619
00620 last_exposure_value_ = 0;
00621 consecutive_stable_exposures_ = 0;
00622 cam_->setFrameCallback(boost::bind(&ProsilicaNode::normalizeCallback, this, _1));
00623 cam_->start(prosilica::Freerun);
00624
00626 while (consecutive_stable_exposures_ < 3)
00627 boost::this_thread::sleep(boost::posix_time::millisec(250));
00628
00629 cam_->stop();
00630 }
00631
00633
00635
00636 void runDiagnostics()
00637 {
00638 self_test_->checkTest();
00639 diagnostic_.update();
00640 }
00641
00642 void freqStatus(diagnostic_updater::DiagnosticStatusWrapper& status)
00643 {
00644 double freq = (double)(count_)/diagnostic_.getPeriod();
00645
00646 if (freq < (.9*desired_freq_))
00647 {
00648 status.summary(2, "Desired frequency not met");
00649 }
00650 else
00651 {
00652 status.summary(0, "Desired frequency met");
00653 }
00654
00655 status.add("Images in interval", count_);
00656 status.add("Desired frequency", desired_freq_);
00657 status.add("Actual frequency", freq);
00658
00659 count_ = 0;
00660 }
00661
00662 void frameStatistics(diagnostic_updater::DiagnosticStatusWrapper& status)
00663 {
00664
00665 float frame_rate;
00666 unsigned long completed, dropped;
00667 cam_->getAttribute("StatFrameRate", frame_rate);
00668 cam_->getAttribute("StatFramesCompleted", completed);
00669 cam_->getAttribute("StatFramesDropped", dropped);
00670
00671
00672 frames_completed_acc_.add(completed - frames_completed_total_);
00673 frames_completed_total_ = completed;
00674 unsigned long completed_recent = frames_completed_acc_.sum();
00675
00676 frames_dropped_acc_.add(dropped - frames_dropped_total_);
00677 frames_dropped_total_ = dropped;
00678 unsigned long dropped_recent = frames_dropped_acc_.sum();
00679
00680 float recent_ratio = float(completed_recent) / (completed_recent + dropped_recent);
00681 float total_ratio = float(completed) / (completed + dropped);
00682
00683
00684 if (dropped_recent == 0) {
00685 status.summary(0, "No dropped frames");
00686 }
00687 else if (recent_ratio > 0.8f) {
00688 status.summary(1, "Some dropped frames");
00689 }
00690 else {
00691 status.summary(2, "Excessive proportion of dropped frames");
00692 }
00693
00694 status.add("Camera Frame Rate", frame_rate);
00695 status.add("Recent % Frames Completed", recent_ratio * 100.0f);
00696 status.add("Overall % Frames Completed", total_ratio * 100.0f);
00697 status.add("Frames Completed", completed);
00698 status.add("Frames Dropped", dropped);
00699 }
00700
00701 void packetStatistics(diagnostic_updater::DiagnosticStatusWrapper& status)
00702 {
00703
00704 unsigned long received, missed, requested, resent;
00705 cam_->getAttribute("StatPacketsReceived", received);
00706 cam_->getAttribute("StatPacketsMissed", missed);
00707 cam_->getAttribute("StatPacketsRequested", requested);
00708 cam_->getAttribute("StatPacketsResent", resent);
00709
00710
00711 packets_received_acc_.add(received - packets_received_total_);
00712 packets_received_total_ = received;
00713 unsigned long received_recent = packets_received_acc_.sum();
00714
00715 packets_missed_acc_.add(missed - packets_missed_total_);
00716 packets_missed_total_ = missed;
00717 unsigned long missed_recent = packets_missed_acc_.sum();
00718
00719 float recent_ratio = float(received_recent) / (received_recent + missed_recent);
00720 float total_ratio = float(received) / (received + missed);
00721
00722 if (missed_recent == 0) {
00723 status.summary(0, "No missed packets");
00724 }
00725 else if (recent_ratio > 0.99f) {
00726 status.summary(1, "Some missed packets");
00727 }
00728 else {
00729 status.summary(2, "Excessive proportion of missed packets");
00730 }
00731
00732
00733 unsigned long data_rate = 0;
00734 cam_->getAttribute("StreamBytesPerSecond", data_rate);
00735 unsigned long max_data_rate = cam_->getMaxDataRate();
00736
00737 if (auto_adjust_stream_bytes_per_second_)
00738 {
00739 if (max_data_rate < prosilica::Camera::GIGE_MAX_DATA_RATE)
00740 status.mergeSummary(1, "Max data rate is lower than expected for a GigE port");
00741
00742 try
00743 {
00745 float multiplier = 1.0f;
00746 if (recent_ratio == 1.0f)
00747 {
00748 multiplier = 1.1f;
00749 }
00750 else if (recent_ratio < 0.99f)
00751 {
00752 multiplier = 0.9f;
00753 }
00754 if (multiplier != 1.0f)
00755 {
00756 unsigned long new_data_rate = std::min((unsigned long)(multiplier * data_rate + 0.5), max_data_rate);
00757 new_data_rate = std::max(new_data_rate, max_data_rate/1000);
00758 if (data_rate != new_data_rate)
00759 {
00760 data_rate = new_data_rate;
00761 cam_->setAttribute("StreamBytesPerSecond", data_rate);
00762 ROS_DEBUG("Changed data rate to %lu bytes per second", data_rate);
00763 }
00764 }
00765 }
00766 catch (prosilica::ProsilicaException &e)
00767 {
00768 if (e.error_code == ePvErrUnplugged)
00769 throw;
00770 ROS_ERROR("Exception occurred: '%s'\n"
00771 "Possible network issue. Attempting to reset data rate to the current maximum.",
00772 e.what());
00773 data_rate = max_data_rate;
00774 cam_->setAttribute("StreamBytesPerSecond", data_rate);
00775 }
00776 }
00777
00778 status.add("Recent % Packets Received", recent_ratio * 100.0f);
00779 status.add("Overall % Packets Received", total_ratio * 100.0f);
00780 status.add("Received Packets", received);
00781 status.add("Missed Packets", missed);
00782 status.add("Requested Packets", requested);
00783 status.add("Resent Packets", resent);
00784 status.add("Data Rate (bytes/s)", data_rate);
00785 status.add("Max Data Rate (bytes/s)", max_data_rate);
00786 }
00787
00788 void packetErrorStatus(diagnostic_updater::DiagnosticStatusWrapper& status)
00789 {
00790 unsigned long erroneous;
00791 cam_->getAttribute("StatPacketsErroneous", erroneous);
00792
00793 if (erroneous == 0) {
00794 status.summary(0, "No erroneous packets");
00795 } else {
00796 status.summary(2, "Possible camera hardware failure");
00797 }
00798
00799 status.add("Erroneous Packets", erroneous);
00800 }
00801
00803
00805
00806
00807 void infoTest(diagnostic_updater::DiagnosticStatusWrapper& status)
00808 {
00809 status.name = "Info Test";
00810
00811 self_test_->setID(hw_id_);
00812
00813 std::string camera_name;
00814 try {
00815 cam_->getAttribute("CameraName", camera_name);
00816 status.summary(0, "Connected to Camera");
00817 status.add("Camera Name", camera_name);
00818 }
00819 catch (prosilica::ProsilicaException& e) {
00820 status.summary(2, e.what());
00821 }
00822 }
00823
00824
00825 void attributeTest(diagnostic_updater::DiagnosticStatusWrapper& status)
00826 {
00827 status.name = "Attribute Test";
00828
00829 tPvAttrListPtr list_ptr;
00830 unsigned long list_length;
00831
00832 if (PvAttrList(cam_->handle(), &list_ptr, &list_length) == ePvErrSuccess) {
00833 status.summary(0, "All attributes in valid range");
00834 for (unsigned int i = 0; i < list_length; ++i) {
00835 const char* attribute = list_ptr[i];
00836 tPvErr e = PvAttrIsValid(cam_->handle(), attribute);
00837 if (e != ePvErrSuccess) {
00838 status.summary(2, "One or more invalid attributes");
00839 if (e == ePvErrOutOfRange)
00840 status.add(attribute, "Out of range");
00841 else if (e == ePvErrNotFound)
00842 status.add(attribute, "Does not exist");
00843 else
00844 status.addf(attribute, "Unexpected error code %u", e);
00845 }
00846 }
00847 }
00848 else {
00849 status.summary(2, "Unable to retrieve attribute list");
00850 }
00851 }
00852
00853
00854 void imageTest(diagnostic_updater::DiagnosticStatusWrapper& status)
00855 {
00856 status.name = "Image Capture Test";
00857
00858 try {
00859 if (trigger_mode_ != prosilica::Software) {
00860 tPvUint32 start_completed, current_completed;
00861 cam_->getAttribute("StatFramesCompleted", start_completed);
00862 for (int i = 0; i < 6; ++i) {
00863 boost::this_thread::sleep(boost::posix_time::millisec(500));
00864 cam_->getAttribute("StatFramesCompleted", current_completed);
00865 if (current_completed > start_completed) {
00866 status.summary(0, "Captured a frame, see diagnostics for detailed stats");
00867 return;
00868 }
00869 }
00870
00871
00872 status.summary(2, "No frames captured over 3s in freerun mode");
00873 return;
00874 }
00875
00876
00877 cam_->setRoiToWholeFrame();
00878 tPvFrame* frame = cam_->grab(500);
00879 if (frame)
00880 {
00881 status.summary(0, "Successfully triggered a frame capture");
00882 }
00883 else
00884 {
00885 status.summary(2, "Attempted to grab a frame, but received NULL");
00886 }
00887
00888 }
00889 catch (prosilica::ProsilicaException &e) {
00890 status.summary(2, e.what());
00891 }
00892 }
00893 };
00894
00895 int main(int argc, char **argv)
00896 {
00897 ros::init(argc, argv, "prosilica_driver");
00898
00899 ros::NodeHandle nh("camera");
00900 ProsilicaNode pn(nh);
00901 ros::spin();
00902
00903 return 0;
00904 }