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
00037
00038
00039
00040
00041 #include <ros/node_handle.h>
00042 #include <sensor_msgs/Image.h>
00043 #include <sensor_msgs/CameraInfo.h>
00044 #include <sensor_msgs/fill_image.h>
00045 #include <sensor_msgs/SetCameraInfo.h>
00046 #include <driver_base/SensorLevels.h>
00047 #include <diagnostic_updater/diagnostic_updater.h>
00048 #include <diagnostic_updater/update_functions.h>
00049 #include <diagnostic_updater/publisher.h>
00050 #include <self_test/self_test.h>
00051 #include <stdlib.h>
00052 #include <limits>
00053 #include <math.h>
00054 #include <linux/sysctl.h>
00055 #include <iostream>
00056 #include <fstream>
00057 #include <wge100_camera/BoardConfig.h>
00058 #include <boost/tokenizer.hpp>
00059 #include <boost/format.hpp>
00060 #include <driver_base/driver.h>
00061 #include <driver_base/driver_node.h>
00062 #include <timestamp_tools/trigger_matcher.h>
00063 #include <camera_calibration_parsers/parse.h>
00064 #include <image_transport/image_transport.h>
00065 #include <sstream>
00066
00067 #include <wge100_camera/WGE100CameraConfig.h>
00068
00069 #include "wge100_camera/wge100lib.h"
00070 #include "wge100_camera/host_netutil.h"
00071 #include "wge100_camera/mt9v.h"
00072
00073 #define RMEM_MAX_RECOMMENDED 20000000
00074
00075
00076 struct SimpleMatrix
00077 {
00078 int rows;
00079 int cols;
00080 const double* data;
00081
00082 SimpleMatrix(int rows, int cols, const double* data)
00083 : rows(rows), cols(cols), data(data)
00084 {}
00085 };
00086
00087 std::ostream& operator << (std::ostream& out, const SimpleMatrix& m)
00088 {
00089 for (int i = 0; i < m.rows; ++i) {
00090 for (int j = 0; j < m.cols; ++j) {
00091 out << m.data[m.cols*i+j] << " ";
00092 }
00093 out << std::endl;
00094 }
00095 return out;
00096 }
00097
00098 bool writeCalibrationIni(std::ostream& out, const std::string& camera_name,
00099 const sensor_msgs::CameraInfo& cam_info)
00100 {
00101 out.precision(5);
00102 out << std::fixed;
00103
00104 out << "# Camera intrinsics\n\n";
00106 out << "[image]\n\n";
00107 out << "width\n" << cam_info.width << "\n\n";
00108 out << "height\n" << cam_info.height << "\n\n";
00109 out << "[" << camera_name << "]\n\n";
00110
00111 out << "camera matrix\n" << SimpleMatrix(3, 3, &cam_info.K[0]);
00112 out << "\ndistortion\n" << SimpleMatrix(1, 5, &cam_info.D[0]);
00113 out << "\n\nrectification\n" << SimpleMatrix(3, 3, &cam_info.R[0]);
00114 out << "\nprojection\n" << SimpleMatrix(3, 4, &cam_info.P[0]);
00115
00116 return true;
00117 }
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152 class FrameTimeFilter
00153 {
00154 public:
00155 FrameTimeFilter(double frame_rate = 1., double late_locked_threshold = 1e-3, double early_locked_threshold = 3e-3, int early_recovery_frames = 5, int max_recovery_frames = 10, double max_skew = 1.001, double max_skipped_frames = 100)
00156 {
00157 frame_period_ = max_skew / frame_rate;
00158 max_recovery_frames_ = max_recovery_frames;
00159 early_recovery_frames_ = early_recovery_frames;
00160 late_locked_threshold_ = late_locked_threshold;
00161 early_locked_threshold_ = early_locked_threshold;
00162 max_skipped_frames_ = max_skipped_frames;
00163 last_frame_number_ = 0;
00164 reset_filter();
00165 }
00166
00167 void reset_filter()
00168 {
00169 relock_time_ = anticipated_time_ = std::numeric_limits<double>::infinity();
00170 unlocked_count_ = late_locked_threshold_;
00171 }
00172
00173 double run(double in_time, int frame_number)
00174 {
00175 double out_time = in_time;
00176 int delta = (frame_number - last_frame_number_) & 0xFFFF;
00177
00178 if (delta > max_skipped_frames_)
00179 {
00180 ROS_WARN_NAMED("frame_time_filter", "FrameTimeFilter missed too many frames from #%u to #%u. Resetting.", last_frame_number_, frame_number);
00181 reset_filter();
00182 }
00183
00184 anticipated_time_ += frame_period_ * delta;
00185 relock_time_ += frame_period_ * delta;
00186
00187 if (out_time < relock_time_)
00188 relock_time_ = out_time;
00189
00190
00191
00192 bool early_trigger = false;
00193 if (out_time < anticipated_time_ - early_locked_threshold_ && finite(anticipated_time_))
00194 {
00195 ROS_WARN_NAMED("frame_time_filter", "FrameTimeFilter saw frame #%u was early by %f.", frame_number, anticipated_time_ - out_time);
00196 early_trigger = true;
00197 }
00198 if (out_time < anticipated_time_)
00199 {
00200 anticipated_time_ = out_time;
00201 relock_time_ = std::numeric_limits<double>::infinity();
00202 unlocked_count_ = 0;
00203 }
00204 else if (out_time > anticipated_time_ + late_locked_threshold_)
00205 {
00206 unlocked_count_++;
00207 if (unlocked_count_ > max_recovery_frames_)
00208 {
00209 ROS_DEBUG_NAMED("frame_time_filter", "FrameTimeFilter lost lock at frame #%u, shifting by %f.", frame_number, relock_time_ - anticipated_time_);
00210 anticipated_time_ = relock_time_;
00211 relock_time_ = std::numeric_limits<double>::infinity();
00212 }
00213
00214
00215 out_time = anticipated_time_;
00216 }
00217
00218 last_frame_number_ = frame_number;
00219
00220 return early_trigger ? -out_time : out_time;
00221 }
00222
00223 private:
00224 int max_recovery_frames_;
00225 int unlocked_count_;
00226 int last_frame_number_;
00227 int max_skipped_frames_;
00228 int early_recovery_frames_;
00229 double early_locked_threshold_;
00230 double late_locked_threshold_;
00231 double anticipated_time_;
00232 double relock_time_;
00233 double frame_period_;
00234 };
00235
00239 class SlowTriggerFilter
00240 {
00241 private:
00242 bool has_warned_;
00243 int before_warn_;
00244 int before_clear_;
00245 int countdown_;
00246
00247 public:
00248 SlowTriggerFilter(int before_warn, int before_clear)
00249 {
00250 has_warned_ = false;
00251 before_clear_ = before_clear;
00252 before_warn_ = before_warn;
00253 countdown_ = before_warn_;
00254 }
00255
00256 bool badFrame()
00257 {
00258 if (!has_warned_)
00259 {
00260 if (!countdown_--)
00261 {
00262 has_warned_ = true;
00263 countdown_ = before_warn_;
00264 return true;
00265 }
00266 }
00267 else
00268 countdown_ = before_clear_;
00269
00270 return false;
00271 }
00272
00273 void goodFrame()
00274 {
00275 if (has_warned_)
00276 {
00277 if (!countdown_--)
00278 {
00279 has_warned_ = false;
00280 countdown_ = before_clear_;
00281 }
00282 }
00283 else
00284 countdown_ = before_warn_;
00285 }
00286 };
00287
00288 class WGE100CameraDriver : public driver_base::Driver
00289 {
00290 friend class WGE100CameraNode;
00291
00292 public:
00293 typedef wge100_camera::WGE100CameraConfig Config;
00294 Config config_;
00295
00296 private:
00297
00298 ros::ServiceClient trig_service_;
00299
00300
00301 double desired_freq_;
00302
00303
00304 int rmem_max_;
00305 int missed_eof_count_;
00306 int dropped_packets_;
00307 int massive_frame_losses_;
00308 bool dropped_packet_event_;
00309 int missed_line_count_;
00310 int trigger_matcher_drop_count_;
00311 double last_image_time_;
00312 double driver_start_time_;
00313 unsigned int last_frame_number_;
00314 unsigned int last_partial_frame_number_;
00315 int lost_image_thread_count_;
00316
00317
00318 FrameTimeFilter frame_time_filter_;
00319 timestamp_tools::TriggerMatcher trigger_matcher_;
00320
00321
00322 sockaddr localMac_;
00323 in_addr localIp_;
00324
00325
00326 std::string hwinfo_;
00327 IpCamList camera_;
00328 MT9VImagerPtr imager_;
00329 MT9VImagerPtr alternate_imager_;
00330 double last_camera_ok_time_;
00331 std::string image_encoding_;
00332 bool next_is_alternate_;
00333 bool first_frame_;
00334 bool enable_alternate_;
00335 bool enable_primary_;
00336
00337
00338 boost::shared_ptr<boost::thread> image_thread_;
00339
00340
00341 typedef boost::function<int(size_t, size_t, uint8_t*, ros::Time, bool)> UseFrameFunction;
00342 UseFrameFunction useFrame_;
00343
00344 struct ImagerSettings
00345 {
00346 int height;
00347 int width;
00348 int x_offset;
00349 int y_offset;
00350 int horizontal_binning;
00351 int vertical_binning;
00352 bool auto_black_level;
00353 int black_level;
00354 int black_level_filter_length;
00355 int black_level_step_size;
00356 int brightness;
00357 int gain;
00358 double exposure;
00359 double max_exposure;
00360 double imager_rate;
00361 bool companding;
00362 bool auto_gain;
00363 bool auto_exposure;
00364 bool mirror_x;
00365 bool mirror_y;
00366 bool rotate_180;
00367 };
00368
00369 int setImagerSettings(MT9VImager &imager, ImagerSettings &cfg)
00370 {
00371 int bintable[5] = { -1, 0, 1, -1, 2 };
00372
00373 if (imager.setMode(cfg.width, cfg.height, bintable[cfg.horizontal_binning], bintable[cfg.vertical_binning], cfg.imager_rate, cfg.x_offset, cfg.y_offset))
00374 {
00375 setStatusMessage("Error setting video mode.");
00376 return 1;
00377 }
00378
00379 if (imager.setCompanding(cfg.companding))
00380 {
00381 setStatusMessage("Error setting companding mode.");
00382 return 1;
00383 }
00384
00385 if (imager.setAgcAec(cfg.auto_gain, cfg.auto_exposure))
00386 {
00387 setStatusMessage("Error setting AGC/AEC mode. Exposure and gain may be incorrect.");
00388 return 1;
00389 }
00390
00391 if (imager.setGain(cfg.gain))
00392 {
00393 setStatusMessage("Error setting analog gain.");
00394 return 1;
00395 }
00396
00397 if (imager.setMirror(cfg.mirror_x != cfg.rotate_180, cfg.mirror_y != cfg.rotate_180))
00398 {
00399 setStatusMessage("Error setting mirroring properties.");
00400 return 1;
00401 }
00402
00403 if (imager.setExposure(cfg.exposure))
00404 {
00405 setStatusMessage("Error setting exposure.");
00406 return 1;
00407 }
00408
00409 if (imager.setBlackLevel(!cfg.auto_black_level, cfg.black_level, cfg.black_level_step_size,
00410 cfg.black_level_filter_length))
00411 {
00412 setStatusMessage("Error setting black level.");
00413 return 1;
00414 }
00415
00416 if (imager.setBrightness(cfg.brightness))
00417 {
00418 setStatusMessage("Error setting brightness.");
00419 return 1;
00420 }
00421
00422 if (imager.setMaxExposure(cfg.max_exposure ? cfg.max_exposure : 0.99 / cfg.imager_rate))
00423 {
00424 setStatusMessage("Error setting maximum exposure.");
00425 return 1;
00426 }
00427
00428 return 0;
00429 }
00430
00431
00432 public:
00433 WGE100CameraDriver() :
00434 trigger_matcher_(5, 60),
00435 no_timestamp_warning_filter_(10, 10)
00436 {
00437
00438
00439
00440
00441 last_camera_ok_time_ = 0;
00442 last_image_time_ = 0;
00443 driver_start_time_ = ros::Time::now().toSec();
00444 missed_line_count_ = 0;
00445 trigger_matcher_drop_count_ = 0;
00446 missed_eof_count_ = 0;
00447 dropped_packets_ = 0;
00448 lost_image_thread_count_ = 0;
00449 massive_frame_losses_ = 0;
00450 dropped_packet_event_ = false;
00451 }
00452
00453 void config_update(const Config &new_cfg, uint32_t level = 0)
00454 {
00455 config_ = new_cfg;
00456
00457 if (config_.horizontal_binning == 3)
00458 {
00459 config_.horizontal_binning = 2;
00460 ROS_WARN("horizontal_binning = 3 is invalid. Setting to 2.");
00461 }
00462
00463 if (config_.vertical_binning == 3)
00464 {
00465 config_.vertical_binning = 2;
00466 ROS_WARN("horizontal_binning = 3 is invalid. Setting to 2.");
00467 }
00468
00469 desired_freq_ = config_.ext_trig ? config_.trig_rate : config_.imager_rate;
00470
00472
00473 if (!config_.auto_exposure && config_.exposure > 0.99 / desired_freq_)
00474 {
00475 ROS_WARN("Exposure (%f s) is greater than 99%% of frame period (%f s). Setting to 99%% of frame period.",
00476 config_.exposure, 0.99 / desired_freq_);
00477 config_.exposure = 0.99 * 1 / desired_freq_;
00478 }
00479
00480
00481
00482
00483
00484 if (config_.auto_exposure && config_.max_exposure && config_.max_exposure > 0.99 / desired_freq_)
00485 {
00486 ROS_WARN("Maximum exposure (%f s) is greater than 99%% of frame period (%f s). Setting to 99%% of frame period.",
00487 config_.max_exposure, 0.99 / desired_freq_);
00488 config_.max_exposure = 0.99 * 1 / desired_freq_;
00489 }
00490
00491
00492
00493
00494
00495
00496 enable_primary_ = (config_.register_set != wge100_camera::WGE100Camera_AlternateRegisterSet);
00497 enable_alternate_ = (config_.register_set != wge100_camera::WGE100Camera_PrimaryRegisterSet);
00498 }
00499
00500 ~WGE100CameraDriver()
00501 {
00502
00503 close();
00504 }
00505
00506 int get_rmem_max()
00507 {
00508 int rmem_max;
00509 {
00510 std::ifstream f("/proc/sys/net/core/rmem_max");
00511 f >> rmem_max;
00512 }
00513 return rmem_max;
00514 }
00515
00516 void doOpen()
00517 {
00518 if (state_ != CLOSED)
00519 {
00520 ROS_ERROR("Assertion failing. state_ = %i", state_);
00521 ROS_ASSERT(state_ == CLOSED);
00522 }
00523
00524 ROS_DEBUG("open()");
00525
00526 int retval;
00527
00528
00529 rmem_max_ = get_rmem_max();
00530 if (rmem_max_ < RMEM_MAX_RECOMMENDED)
00531 {
00532 ROS_WARN_NAMED("rmem_max", "rmem_max is %i. Buffer overflows and packet loss may occur. Minimum recommended value is 20000000. Updates may not take effect until the driver is restarted. See http://www.ros.org/wiki/wge100_camera/Troubleshooting", rmem_max_);
00533 }
00534
00535 ROS_DEBUG("Redoing discovery.");
00536 const char *errmsg;
00537 int findResult = wge100FindByUrl(config_.camera_url.c_str(), &camera_, SEC_TO_USEC(0.2), &errmsg);
00538
00539 if (findResult)
00540 {
00541 setStatusMessagef("Matching URL %s : %s", config_.camera_url.c_str(), errmsg);
00542 strncpy(camera_.cam_name, "unknown", sizeof(camera_.cam_name));
00543 camera_.serial = -1;
00544 return;
00545 }
00546
00547 retval = wge100Configure(&camera_, camera_.ip_str, SEC_TO_USEC(0.5));
00548 if (retval != 0) {
00549 setStatusMessage("IP address configuration failed");
00550 return;
00551 }
00552
00553 ROS_INFO("Configured camera. Complete URLs: serial://%u@%s#%s name://%s@%s#%s",
00554 camera_.serial, camera_.ip_str, camera_.ifName, camera_.cam_name, camera_.ip_str, camera_.ifName);
00555
00556 camera_.serial = camera_.serial;
00557 hwinfo_ = camera_.hwinfo;
00558
00559
00560 if ( wge100EthGetLocalMac(camera_.ifName, &localMac_) != 0 ) {
00561 setStatusMessagef("Unable to get local MAC address for interface %s", camera_.ifName);
00562 return;
00563 }
00564
00565
00566 if ( wge100IpGetLocalAddr(camera_.ifName, &localIp_) != 0) {
00567 setStatusMessagef("Unable to get local IP address for interface %s", camera_.ifName);
00568 return;
00569 }
00570
00571 if (config_.ext_trig && config_.trig_timestamp_topic.empty())
00572 {
00573 setStatusMessage("External triggering is selected, but no \"trig_timestamp_topic\" was specified.");
00574 return;
00575 }
00576
00577
00578 if ( wge100ImagerSetRes( &camera_, config_.width, config_.height ) != 0) {
00579 setStatusMessage("Error selecting image resolution.");
00580 return;
00581 }
00582
00583 ImagerSettings primary_settings, alternate_settings;
00584
00585 primary_settings.height = config_.height;
00586 primary_settings.width = config_.width;
00587 primary_settings.x_offset = config_.x_offset;
00588 primary_settings.y_offset = config_.y_offset;
00589 primary_settings.horizontal_binning = config_.horizontal_binning;
00590 primary_settings.vertical_binning = config_.vertical_binning;
00591 primary_settings.imager_rate = config_.imager_rate;
00592 primary_settings.companding = config_.companding;
00593 primary_settings.auto_gain = config_.auto_gain;
00594 primary_settings.gain = config_.gain;
00595 primary_settings.auto_exposure = config_.auto_exposure;
00596 primary_settings.exposure = config_.exposure;
00597 primary_settings.mirror_x = config_.mirror_x;
00598 primary_settings.mirror_y = config_.mirror_y;
00599 primary_settings.rotate_180 = config_.rotate_180;
00600 primary_settings.auto_black_level = config_.auto_black_level;
00601 primary_settings.black_level_filter_length = config_.black_level_filter_length;
00602 primary_settings.black_level_step_size = config_.black_level_step_size;
00603 primary_settings.black_level = config_.black_level;
00604 primary_settings.brightness = config_.brightness;
00605 primary_settings.max_exposure = config_.max_exposure;
00606
00607 alternate_settings.height = config_.height;
00608 alternate_settings.width = config_.width;
00609 alternate_settings.x_offset = config_.x_offset;
00610 alternate_settings.y_offset = config_.y_offset;
00611 alternate_settings.horizontal_binning = config_.horizontal_binning;
00612 alternate_settings.vertical_binning = config_.vertical_binning;
00613 alternate_settings.imager_rate = config_.imager_rate;
00614 alternate_settings.companding = config_.companding_alternate;
00615 alternate_settings.auto_gain = config_.auto_gain_alternate;
00616 alternate_settings.gain = config_.gain_alternate;
00617 alternate_settings.auto_exposure = config_.auto_exposure_alternate;
00618 alternate_settings.exposure = config_.exposure_alternate;
00619 alternate_settings.mirror_x = config_.mirror_x;
00620 alternate_settings.mirror_y = config_.mirror_y;
00621 alternate_settings.rotate_180 = config_.rotate_180;
00622 alternate_settings.auto_black_level = config_.auto_black_level;
00623 alternate_settings.black_level = config_.black_level;
00624 alternate_settings.black_level_filter_length = config_.black_level_filter_length;
00625 alternate_settings.black_level_step_size = config_.black_level_step_size;
00626 alternate_settings.brightness = config_.brightness;
00627 alternate_settings.max_exposure = config_.max_exposure;
00628
00629 imager_ = MT9VImager::getInstance(camera_);
00630 if (!imager_)
00631 {
00632 setStatusMessage("Unknown imager model.");
00633 return;
00634 }
00635 alternate_imager_ = imager_->getAlternateContext();
00636
00637 if (enable_primary_ && enable_alternate_)
00638 {
00639 if (!alternate_imager_)
00640 {
00641 setStatusMessage("Unable to find alternate imager context, but enable_alternate is true.");
00642 return;
00643 }
00644
00645
00646 primary_settings.auto_exposure = config_.auto_exposure_alternate;
00647 primary_settings.gain = config_.gain_alternate;
00648 primary_settings.companding = config_.companding_alternate;
00649 alternate_settings.auto_exposure = config_.auto_exposure;
00650 alternate_settings.gain = config_.gain;
00651 alternate_settings.companding = config_.companding;
00652
00653 if (setImagerSettings(*imager_, primary_settings) ||
00654 setImagerSettings(*alternate_imager_, alternate_settings))
00655 return;
00656 }
00657 else if (enable_primary_)
00658 {
00659 if (setImagerSettings(*imager_, primary_settings))
00660 return;
00661 }
00662 else
00663 {
00664 if (setImagerSettings(*imager_, alternate_settings))
00665 return;
00666 }
00667
00668 if (enable_alternate_ && enable_primary_ && wge100SensorSelect(&camera_, 0, 0x07))
00669 {
00670 setStatusMessage("Error configuring camera to report alternate frames.");
00671 return;
00672 }
00673
00674 next_is_alternate_ = !enable_primary_;
00675
00676
00677 frame_time_filter_ = FrameTimeFilter(desired_freq_, 0.001, 0.5 / config_.imager_rate);
00678 trigger_matcher_.setTrigDelay(1. / config_.imager_rate);
00679
00680
00681
00682 bool is_027 = ((camera_.serial / 100000) % 100) == 27;
00683 bool is_MT9V032 = imager_->getModel() == "MT9V032";
00684 if (camera_.serial != 0 && is_027 == is_MT9V032)
00685 {
00686 boost::recursive_mutex::scoped_lock lock(mutex_);
00687 setStatusMessagef("Camera has %s serial number, but has %s imager. This is a serious problem with the hardware.",
00688 is_027 ? "027" : "non-027", imager_->getModel().c_str());
00689 return;
00690 }
00691
00692 if (config_.test_pattern && (
00693 wge100ReliableSensorWrite( &camera_, 0x7F, 0x3800, NULL ) != 0 ||
00694 wge100ReliableSensorWrite( &camera_, 0x0F, 0x0006, NULL ) != 0)) {
00695 setStatusMessage("Error turning on test pattern.");
00696 return;
00697 }
00698
00699 setStatusMessage("Camera is opened.");
00700 state_ = OPENED;
00701 last_camera_ok_time_ = ros::Time::now().toSec();
00702 }
00703
00704 void doClose()
00705 {
00706 ROS_DEBUG("close()");
00707 ROS_ASSERT(state_ == OPENED);
00708 state_ = CLOSED;
00709 setStatusMessage("Camera is closed.");
00710 }
00711
00712 void doStart()
00713 {
00714 last_frame_number_ = 0xFFFF;
00715 last_partial_frame_number_ = 0xFFFF;
00716
00717
00718 int trig_mode = config_.ext_trig ? TRIG_STATE_EXTERNAL : TRIG_STATE_INTERNAL;
00719 if (enable_alternate_ && enable_primary_)
00720 trig_mode |= TRIG_STATE_ALTERNATE;
00721 if (config_.rising_edge_trig)
00722 trig_mode |= TRIG_STATE_RISING;
00723 if ( wge100TriggerControl( &camera_, trig_mode ) != 0) {
00724 setStatusMessagef("Trigger mode set error. Is %s accessible from interface %s? (Try running route to check.)", camera_.ip_str, camera_.ifName);
00725 state_ = CLOSED;
00726 return;
00727 }
00728
00729 trigger_matcher_.reset();
00730
00731
00732
00733
00734
00735
00736
00737
00738
00739
00740
00741
00742
00743 ROS_DEBUG("start()");
00744 ROS_ASSERT(state_ == OPENED);
00745 setStatusMessage("Waiting for first frame...");
00746 state_ = RUNNING;
00747 image_thread_.reset(new boost::thread(boost::bind(&WGE100CameraDriver::imageThread, this)));
00748 }
00749
00750 void doStop()
00751 {
00752 ROS_DEBUG("stop()");
00753 if (state_ != RUNNING)
00754 return;
00755
00757 setStatusMessage("Stopped");
00758
00759 state_ = OPENED;
00760
00761 if (image_thread_ && !image_thread_->timed_join((boost::posix_time::milliseconds) 2000))
00762 {
00763 ROS_ERROR("image_thread_ did not die after two seconds. Pretending that it did. This is probably a bad sign.");
00764 lost_image_thread_count_++;
00765 }
00766 image_thread_.reset();
00767 }
00768
00769 int setTestMode(uint16_t mode, diagnostic_updater::DiagnosticStatusWrapper &status)
00770 {
00771 if ( wge100ReliableSensorWrite( &camera_, 0x7F, mode, NULL ) != 0) {
00772 status.summary(2, "Could not set imager into test mode.");
00773 status.add("Writing imager test mode", "Fail");
00774 return 1;
00775 }
00776 else
00777 {
00778 status.add("Writing imager test mode", "Pass");
00779 }
00780
00781 usleep(100000);
00782 uint16_t inmode;
00783 if ( wge100ReliableSensorRead( &camera_, 0x7F, &inmode, NULL ) != 0) {
00784 status.summary(2, "Could not read imager mode back.");
00785 status.add("Reading imager test mode", "Fail");
00786 return 1;
00787 }
00788 else
00789 {
00790 status.add("Reading imager test mode", "Pass");
00791 }
00792
00793 if (inmode != mode) {
00794 status.summary(2, "Imager test mode read back did not match.");
00795 status.addf("Comparing read back value", "Fail (%04x != %04x)", inmode, mode);
00796 return 1;
00797 }
00798 else
00799 {
00800 status.add("Comparing read back value", "Pass");
00801 }
00802
00803 return 0;
00804 }
00805
00806 std::string getID()
00807 {
00808 if (!camera_.serial)
00809 return "unknown";
00810 else
00811 return (boost::format("WGE100_%05u") % camera_.serial).str();
00812 }
00813
00814 private:
00815 void imageThread()
00816 {
00817
00818 frame_time_filter_.reset_filter();
00819
00820 first_frame_ = true;
00821 int ret_val = wge100VidReceiveAuto( &camera_, config_.height, config_.width, &WGE100CameraDriver::frameHandler, this);
00822
00823 if (ret_val == IMAGER_LINENO_OVF)
00824 setStatusMessage("Camera terminated streaming with an overflow error.");
00825 else if (ret_val == IMAGER_LINENO_ERR)
00826 setStatusMessage("Camera terminated streaming with an error.");
00827 else if (ret_val)
00828 setStatusMessage("wge100VidReceiveAuto exited with an error code.");
00829
00830
00831
00832
00833
00834 if (state_ == RUNNING)
00835 state_ = OPENED;
00836 ROS_DEBUG("Image thread exiting.");
00837 }
00838
00839 void cameraStatus(diagnostic_updater::DiagnosticStatusWrapper& stat)
00840 {
00841 double since_last_frame_ = ros::Time::now().toSec() - (last_image_time_ ? last_image_time_ : driver_start_time_);
00842 if (since_last_frame_ > 10)
00843 {
00844 stat.summary(2, "Next frame is way past due.");
00845 }
00846 else if (since_last_frame_ > 5 / desired_freq_)
00847 {
00848 stat.summary(1, "Next frame is past due.");
00849 }
00850 else if (rmem_max_ < RMEM_MAX_RECOMMENDED)
00851 {
00852 stat.summaryf(1, "Frames are streaming, but the receive buffer is small (rmem_max=%u). Please increase rmem_max to %u to avoid dropped frames. For details: http://www.ros.org/wiki/wge100_camera/Troubleshooting", rmem_max_, RMEM_MAX_RECOMMENDED);
00853 }
00854 else if (dropped_packet_event_ && config_.packet_debug)
00855 {
00856
00857 stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "There have been dropped packets.");
00858 dropped_packet_event_ = 0;
00859 }
00860 else
00861 {
00862 stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Frames are streaming.");
00863 dropped_packet_event_ = 0;
00864 }
00865
00866 stat.addf("Missing image line frames", "%d", missed_line_count_);
00867 stat.addf("Missing EOF frames", "%d", missed_eof_count_);
00868 stat.addf("Dropped packet estimate", "%d", dropped_packets_);
00869 stat.addf("Frames dropped by trigger filter", "%d", trigger_matcher_drop_count_);
00870 stat.addf("Massive frame losses", "%d", massive_frame_losses_);
00871 stat.addf("Losses of image thread", "%d", lost_image_thread_count_);
00872 stat.addf("First packet offset", "%d", config_.first_packet_offset);
00873 if (isClosed())
00874 {
00875 static const std::string not_opened = "not_opened";
00876 stat.add("Camera Serial #", not_opened);
00877 stat.add("Camera Name", not_opened);
00878 stat.add("Camera Hardware", not_opened);
00879 stat.add("Camera MAC", not_opened);
00880 stat.add("Interface", not_opened);
00881 stat.add("Camera IP", not_opened);
00882 stat.add("Image encoding", not_opened);
00883 }
00884 else
00885 {
00886 stat.add("Camera Serial #", camera_.serial);
00887 stat.add("Camera Name", camera_.cam_name);
00888 stat.add("Camera Hardware", hwinfo_);
00889 stat.addf("Camera MAC", "%02X:%02X:%02X:%02X:%02X:%02X", camera_.mac[0], camera_.mac[1], camera_.mac[2], camera_.mac[3], camera_.mac[4], camera_.mac[5]);
00890 stat.add("Interface", camera_.ifName);
00891 stat.add("Camera IP", camera_.ip_str);
00892 stat.add("Image encoding", image_encoding_);
00893 }
00894 stat.add("Image width", config_.width);
00895 stat.add("Image height", config_.height);
00896 stat.add("Image horizontal binning", config_.horizontal_binning);
00897 stat.add("Image vertical binning", config_.vertical_binning);
00898 stat.add("Requested imager rate", config_.imager_rate);
00899 stat.add("Latest frame time", last_image_time_);
00900 stat.add("Latest frame #", last_frame_number_);
00901 stat.add("External trigger controller", config_.trig_timestamp_topic);
00902 stat.add("Trigger mode", config_.ext_trig ? "external" : "internal");
00903 boost::shared_ptr<MT9VImager> imager = imager_;
00904 if (imager)
00905 {
00906 stat.add("Imager model", imager->getModel());
00907 stat.addf("Imager version", "0x%04x", imager->getVersion());
00908 }
00909 }
00910
00911 double getTriggeredFrameTime(double firstPacketTime)
00912 {
00913
00914
00915
00916
00917
00918
00919
00920
00921
00922
00923
00924
00925
00926
00927
00928 return 0;
00929 }
00930
00931 double getExternallyTriggeredFrameTime(double firstPacketTime)
00932 {
00933
00934
00935
00936
00937 double exposeEndTime = firstPacketTime - config_.first_packet_offset +
00938 1 / config_.imager_rate -
00939 1 / config_.trig_rate;
00940
00941 return exposeEndTime;
00942 }
00943
00944 double getFreeRunningFrameTime(double firstPacketTime)
00945 {
00946
00947
00948 return firstPacketTime - config_.first_packet_offset;
00949 }
00950
00951
00952
00953
00954
00955
00956
00957
00958
00959
00960
00961
00962
00963
00964
00965
00966 SlowTriggerFilter no_timestamp_warning_filter_;
00967
00968 int frameHandler(wge100FrameInfo *frame_info)
00969 {
00970 boost::recursive_mutex::scoped_lock(diagnostics_lock_);
00971
00972 if (state_ != RUNNING)
00973 return 1;
00974
00975 if (frame_info == NULL)
00976 {
00977 boost::recursive_mutex::scoped_lock lock(mutex_);
00978
00980 if (config_.ext_trig && !trigger_matcher_.hasTimestamp())
00981 {
00982 const char *msg = "More than one second elapsed without receiving any trigger. Is this normal?";
00983 if (no_timestamp_warning_filter_.badFrame())
00984 ROS_WARN_NAMED("slow_trigger", "%s", msg);
00985 else
00986 ROS_DEBUG("%s", msg);
00987 return 0;
00988 }
00989
00991
00992
00993
00994 setStatusMessagef("No data have arrived for more than one second. Assuming that camera is no longer streaming.");
00995 return 1;
00996 }
00997
00998 no_timestamp_warning_filter_.goodFrame();
00999
01000 if (first_frame_)
01001 {
01002 ROS_INFO_COND(first_frame_, "Camera streaming.");
01003 setStatusMessage("Camera streaming.");
01004 }
01005 first_frame_ = false;
01006
01007 double firstPacketTime = frame_info->startTime.tv_sec + frame_info->startTime.tv_usec / 1e6;
01008
01009
01010
01011 double firstPacketTimeFiltered = fabs(frame_time_filter_.run(firstPacketTime, frame_info->frame_number));
01012
01013
01014
01015
01016
01017 ros::Time imageTime;
01018 if (!config_.ext_trig || config_.trig_timestamp_topic.empty())
01019 {
01020
01021
01022
01023 double triggerTimeGuess = firstPacketTimeFiltered - config_.first_packet_offset;
01024
01025 trigger_matcher_.triggerCallback(triggerTimeGuess);
01026
01027
01028 imageTime = trigger_matcher_.getTimestampNoblock(ros::Time(firstPacketTime));
01029
01030 }
01031 else
01032 {
01033
01034
01035 imageTime = trigger_matcher_.getTimestampBlocking(ros::Time(firstPacketTime), 0.01);
01036
01037
01038 }
01039
01040
01041
01042
01043
01044 int16_t frame_delta = (frame_info->frame_number - last_partial_frame_number_) & 0xFFFF;
01045 last_partial_frame_number_ = frame_info->frame_number;
01046 if (frame_delta > 1)
01047 {
01048 dropped_packet_event_ = true;
01049 ROS_DEBUG_NAMED("dropped_frame", "Some frames were never seen. The dropped packet count will be incorrect.");
01050 massive_frame_losses_++;
01051 }
01052
01053
01054 if (frame_info->eofInfo == NULL) {
01055 missed_eof_count_++;
01056 dropped_packets_++;
01057 dropped_packet_event_ = true;
01058 ROS_DEBUG_NAMED("no_eof", "Frame %u was missing EOF", frame_info->frame_number);
01059 return 0;
01060 }
01061
01062
01063 if (frame_info->shortFrame) {
01064 missed_line_count_++;
01065 dropped_packets_ += frame_info->missingLines;
01066 dropped_packet_event_ = true;
01067 ROS_DEBUG_NAMED("short_frame", "Short frame #%u (%zu video lines were missing, last was %zu)", frame_info->frame_number,
01068 frame_info->missingLines, frame_info->lastMissingLine);
01069 return 0;
01070 }
01071
01072 if (imageTime == timestamp_tools::TriggerMatcher::RetryLater)
01073 {
01074 trigger_matcher_drop_count_++;
01075 ROS_DEBUG_NAMED("missing_timestamp", "TriggerMatcher did not get timestamp for frame #%u", frame_info->frame_number);
01076 return 0;
01077 }
01078
01079 if (imageTime == timestamp_tools::TriggerMatcher::DropData)
01080 {
01081 trigger_matcher_drop_count_++;
01082 ROS_DEBUG_NAMED("trigger_match_fail", "TriggerMatcher dropped frame #%u", frame_info->frame_number);
01083 return 0;
01084 }
01085
01086 last_image_time_ = imageTime.toSec();
01087 last_frame_number_ = frame_info->frame_number;
01088
01089 image_encoding_ = isColor() ? sensor_msgs::image_encodings::BAYER_BGGR8 : sensor_msgs::image_encodings::MONO8;
01090
01091 bool alternate = next_is_alternate_;
01092 next_is_alternate_ = enable_alternate_ && (!enable_primary_ || (frame_info->eofInfo->i2c[0] & 0x8000));
01093
01094 return useFrame_(frame_info->width, frame_info->height, frame_info->frameData, imageTime, alternate);
01095 }
01096
01097 bool isColor()
01098 {
01099 return imager_->getModel() == "MT9V032";
01100
01101
01102
01103
01104 }
01105
01106 static int frameHandler(wge100FrameInfo *frameInfo, void *userData)
01107 {
01108 WGE100CameraDriver &fa_node = *(WGE100CameraDriver*)userData;
01109 return fa_node.frameHandler(frameInfo);
01110 }
01111
01112 uint16_t intrinsicsChecksum(uint16_t *data, int words)
01113 {
01114 uint16_t sum = 0;
01115 for (int i = 0; i < words; i++)
01116 sum += htons(data[i]);
01117 return htons(0xFFFF - sum);
01118 }
01119
01120 bool loadIntrinsics(sensor_msgs::CameraInfo &cam_info)
01121 {
01122
01123 std::string calbuff(2 * FLASH_PAGE_SIZE, 0);
01124
01125 if(wge100ReliableFlashRead(&camera_, FLASH_CALIBRATION_PAGENO, (uint8_t *) &calbuff[0], NULL) != 0 ||
01126 wge100ReliableFlashRead(&camera_, FLASH_CALIBRATION_PAGENO+1, (uint8_t *) &calbuff[FLASH_PAGE_SIZE], NULL) != 0)
01127 {
01128 ROS_WARN("Error reading camera intrinsics from flash.\n");
01129 return false;
01130 }
01131
01132 uint16_t chk = intrinsicsChecksum((uint16_t *) &calbuff[0], calbuff.length() / 2);
01133 if (chk)
01134 {
01135 ROS_WARN_NAMED("no_intrinsics", "Camera intrinsics have a bad checksum. They have probably never been set.");
01136 return false;
01137 }
01138
01139 std::string name;
01140 bool success = camera_calibration_parsers::parseCalibration(calbuff, "ini", name, cam_info);
01141
01142 if (success && (cam_info.width != (unsigned int) config_.width || cam_info.height != (unsigned int) config_.height)) {
01143 ROS_ERROR("Image resolution from intrinsics file does not match current video setting, "
01144 "intrinsics expect %ix%i but running at %ix%i", cam_info.width, cam_info.height, config_.width, config_.height);
01145 }
01146
01147 if (!success)
01148 ROS_ERROR("Could not parse camera intrinsics downloaded from camera.");
01149
01150 return success;
01151 }
01152
01153 bool saveIntrinsics(const sensor_msgs::CameraInfo &cam_info)
01154 {
01155 char calbuff[2 * FLASH_PAGE_SIZE];
01156
01157 std::stringstream inifile;
01158 writeCalibrationIni(inifile, camera_.cam_name, cam_info);
01159 strncpy(calbuff, inifile.str().c_str(), 2 * FLASH_PAGE_SIZE - 2);
01160
01161 uint16_t *chk = ((uint16_t *) calbuff) + FLASH_PAGE_SIZE - 1;
01162 *chk = 0;
01163 *chk = intrinsicsChecksum((uint16_t *) calbuff, sizeof(calbuff) / 2);
01164
01165 ROS_INFO("Writing camera info:\n%s", calbuff);
01166
01167 boost::recursive_mutex::scoped_lock lock_(mutex_);
01168 goOpened();
01169
01170
01171
01172 bool ret = false;
01173 if (state_ != OPENED)
01174 ROS_ERROR("Error putting camera into OPENED state to write to flash.\n");
01175 else if(wge100ReliableFlashWrite(&camera_, FLASH_CALIBRATION_PAGENO, (uint8_t *) calbuff, NULL) != 0 ||
01176 wge100ReliableFlashWrite(&camera_, FLASH_CALIBRATION_PAGENO+1, (uint8_t *) calbuff + FLASH_PAGE_SIZE, NULL) != 0)
01177 ROS_ERROR("Error writing camera intrinsics to flash.\n");
01178 else
01179 {
01180 ROS_INFO("Camera_info successfully written to camera.");
01181 ret = true;
01182 }
01183
01184 goClosed();
01185 goRunning();
01186 return ret;
01187 }
01188
01189 bool boardConfig(wge100_camera::BoardConfig::Request &req, wge100_camera::BoardConfig::Response &rsp)
01190 {
01191 if (state_ != RUNNING)
01192 {
01193 ROS_ERROR("board_config sevice called while camera was not running. To avoid programming the wrong camera, board_config can only be called while viewing the camera stream.");
01194 rsp.success = 0;
01195 return 1;
01196 }
01197
01198 MACAddress mac;
01199 if (req.mac.size() != 6)
01200 {
01201 ROS_ERROR("board_config service called with %zu bytes in MAC. Should be 6.", req.mac.size());
01202 rsp.success = 0;
01203 return 1;
01204 }
01205 for (int i = 0; i < 6; i++)
01206 mac[i] = req.mac[i];
01207 ROS_INFO("board_config called s/n #%i, and MAC %02x:%02x:%02x:%02x:%02x:%02x.", req.serial, mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
01208 stop();
01209 rsp.success = !wge100ConfigureBoard( &camera_, req.serial, &mac);
01210
01211 if (rsp.success)
01212 ROS_INFO("board_config succeeded.");
01213 else
01214 ROS_INFO("board_config failed.");
01215
01216 return 1;
01217 }
01218
01219 };
01220
01221 class WGE100CameraNode : public driver_base::DriverNode<WGE100CameraDriver>
01222 {
01223 public:
01224 WGE100CameraNode(ros::NodeHandle &nh) :
01225 driver_base::DriverNode<WGE100CameraDriver>(nh),
01226 camera_node_handle_(nh.resolveName("camera")),
01227 camera_node_handle_alt_(nh.resolveName("camera_alternate")),
01228 cam_pub_diag_(cam_pub_.getTopic(),
01229 diagnostic_,
01230 diagnostic_updater::FrequencyStatusParam(&driver_.desired_freq_, &driver_.desired_freq_, 0.05),
01231 diagnostic_updater::TimeStampStatusParam()),
01232 config_bord_service_(private_node_handle_.advertiseService("board_config", &WGE100CameraDriver::boardConfig, &driver_)),
01233 set_camera_info_service_(camera_node_handle_.advertiseService("set_camera_info", &WGE100CameraNode::setCameraInfo, this)),
01234 wge100_diagnostic_task_("WGE100 Diagnostics", boost::bind(&WGE100CameraDriver::cameraStatus, &driver_, _1)),
01235 calibrated_(false)
01236 {
01237
01238
01239
01240 std::string topic = camera_node_handle_.resolveName("image_raw");
01241 std::string alt_topic = camera_node_handle_alt_.resolveName("image_raw");
01242 image_transport::ImageTransport it(nh);
01243 cam_pub_ = it.advertiseCamera(topic, 1);
01244 if (topic == alt_topic)
01245 cam_pub_alt_ = cam_pub_;
01246 else
01247 cam_pub_alt_ = it.advertiseCamera(alt_topic, 1);
01248
01249 driver_.useFrame_ = boost::bind(&WGE100CameraNode::publishImage, this, _1, _2, _3, _4, _5);
01250 driver_.setPostOpenHook(boost::bind(&WGE100CameraNode::postOpenHook, this));
01251 }
01252
01253 private:
01254 ros::NodeHandle camera_node_handle_;
01255 ros::NodeHandle camera_node_handle_alt_;
01256
01257
01258 image_transport::CameraPublisher cam_pub_;
01259 image_transport::CameraPublisher cam_pub_alt_;
01260 diagnostic_updater::TopicDiagnostic cam_pub_diag_;
01261 sensor_msgs::Image image_;
01262 sensor_msgs::CameraInfo camera_info_;
01263 ros::ServiceServer config_bord_service_;
01264 ros::ServiceServer set_camera_info_service_;
01265 ros::Subscriber trigger_sub_;
01266
01267 diagnostic_updater::FunctionDiagnosticTask wge100_diagnostic_task_;
01268
01269
01270 bool calibrated_;
01271
01272 int publishImage(size_t width, size_t height, uint8_t *frameData, ros::Time t, bool alternate)
01273 {
01274
01275 fillImage(image_, driver_.image_encoding_, height, width, width, frameData);
01276
01277 fflush(stdout);
01278 (alternate ? cam_pub_alt_ : cam_pub_).publish(image_, camera_info_, t);
01279 cam_pub_diag_.tick(t);
01280
01281 return 0;
01282 }
01283
01284 virtual void reconfigureHook(int level)
01285 {
01286 ROS_DEBUG("WGE100CameraNode::reconfigureHook called at level %x", level);
01287
01288 image_.header.frame_id = driver_.config_.frame_id;
01289 camera_info_.header.frame_id = driver_.config_.frame_id;
01290 }
01291
01292 void postOpenHook()
01293 {
01294
01295 calibrated_ = driver_.loadIntrinsics(camera_info_);
01296 if (calibrated_)
01297 ROS_INFO("Loaded intrinsics from camera.");
01298 else
01299 {
01300 ROS_WARN_NAMED("no_intrinsics", "Failed to load intrinsics from camera");
01301 camera_info_ = sensor_msgs::CameraInfo();
01302 camera_info_.width = driver_.config_.width;
01303 camera_info_.height = driver_.config_.height;
01304 }
01305
01306 if (driver_.config_.ext_trig && !driver_.config_.trig_timestamp_topic.empty())
01307 {
01308 void (timestamp_tools::TriggerMatcher::*cb)(const roslib::HeaderConstPtr &) = ×tamp_tools::TriggerMatcher::triggerCallback;
01309 trigger_sub_ = node_handle_.subscribe(driver_.config_.trig_timestamp_topic, 100, cb, &driver_.trigger_matcher_);
01310 }
01311 else
01312 trigger_sub_.shutdown();
01313
01314 diagnostic_.setHardwareID(driver_.getID());
01315 }
01316
01317 virtual void addDiagnostics()
01318 {
01319
01320 driver_status_diagnostic_.addTask(&wge100_diagnostic_task_);
01321 }
01322
01323 virtual void addRunningTests()
01324 {
01325 self_test_.add( "Streaming Test", this, &WGE100CameraNode::streamingTest);
01326 }
01327
01328 bool setCameraInfo(sensor_msgs::SetCameraInfo::Request &req, sensor_msgs::SetCameraInfo::Response &rsp)
01329 {
01330 sensor_msgs::CameraInfo &cam_info = req.camera_info;
01331
01332 if (driver_.getState() != driver_base::Driver::RUNNING)
01333 {
01334 ROS_ERROR("set_camera_info service called while camera was not running. To avoid programming the wrong camera, set_camera_info can only be called while viewing the camera stream.");
01335 rsp.status_message = "Camera not running. Camera must be running to avoid setting the intrinsics of the wrong camera.";
01336 rsp.success = false;
01337 return 1;
01338 }
01339
01340 if ((cam_info.width != (unsigned int) driver_.config_.width || cam_info.height != (unsigned int) driver_.config_.height)) {
01341 ROS_ERROR("Image resolution of camera_info passed to set_camera_info service does not match current video setting, "
01342 "camera_info contains %ix%i but camera running at %ix%i", cam_info.width, cam_info.height, driver_.config_.width, driver_.config_.height);
01343 rsp.status_message = (boost::format("Camera_info resolution %ix%i does not match camera resolution %ix%i.")%cam_info.width%cam_info.height%driver_.config_.width%driver_.config_.height).str();
01344 rsp.success = false;
01345 }
01346
01347 rsp.success = driver_.saveIntrinsics(cam_info);
01348 if (!rsp.success)
01349 rsp.status_message = "Error writing camera_info to flash.";
01350
01351 return 1;
01352 }
01353
01354 virtual void addOpenedTests()
01355 {
01356 }
01357
01358 virtual void addStoppedTests()
01359 {
01360 ROS_DEBUG("Adding wge100 camera video mode tests.");
01361 for (int i = 0; i < MT9V_NUM_MODES; i++)
01362 {
01363 if (MT9VModes[i].width != 640)
01364 continue;
01365 diagnostic_updater::TaskFunction f = boost::bind(&WGE100CameraNode::videoModeTest, this, i, _1);
01366 self_test_.add( str(boost::format("Test Pattern in mode %s")%MT9VModes[i].name), f );
01367 }
01368 }
01369
01370 void streamingTest(diagnostic_updater::DiagnosticStatusWrapper& status)
01371 {
01372
01373 for (int i = 0; i < 10; i++)
01374 {
01375 cam_pub_diag_.clear_window();
01376 sleep(1);
01377
01378 cam_pub_diag_.run(status);
01379 if (!status.level)
01380 break;
01381 driver_.start();
01382 }
01383 }
01384
01385 class VideoModeTestFrameHandler
01386 {
01387 public:
01388 VideoModeTestFrameHandler(diagnostic_updater::DiagnosticStatusWrapper &status, int imager_id) : got_frame_(false), frame_count_(0), status_(status), imager_id_(imager_id)
01389 {}
01390
01391 volatile bool got_frame_;
01392 int frame_count_;
01393
01394 int run(size_t width, size_t height, uint8_t *data, ros::Time stamp, bool alternate)
01395 {
01396 status_.add("Got a frame", "Pass");
01397
01398 bool final = frame_count_++ > 10;
01399 int mode;
01400
01401 switch (imager_id_)
01402 {
01403 case 0x1313:
01404 mode = 0;
01405 break;
01406 case 0x1324:
01407 mode = 2;
01408 break;
01409 default:
01410 ROS_WARN("Unexpected imager_id, autodetecting test pattern for this camera.");
01411 mode = -1;
01412 break;
01413 }
01414
01415 for (size_t y = 0; y < height; y++)
01416 for (size_t x = 0; x < width; x++, data++)
01417 {
01418 std::string msg;
01419
01420
01421
01422 msg = check_expected(x, y, *data, final, mode);
01423
01424
01425
01426 if (msg.empty())
01427 continue;
01428
01429
01432
01433
01434 if (!final)
01435 {
01436 ROS_WARN("Pattern mismatch, retrying...");
01437 return 0;
01438 }
01439
01440 status_.summary(2, msg);
01441 status_.add("Frame content", msg);
01442
01443 got_frame_ = true;
01444 return 1;
01445 }
01446
01447 status_.addf("Frame content", "Pass");
01448 got_frame_ = true;
01449 return 1;
01450 }
01451
01452 private:
01453 std::string check_expected(int x, int y, uint8_t actual_col, bool final, int &mode)
01454 {
01455 #define NUM_TEST_PATTERNS 3
01456
01457
01458 uint8_t col[NUM_TEST_PATTERNS];
01459 col[0] = (x + 2 * y + 25) / 4;
01460 if ((x + 1) / 2 + y < 500)
01461 col[1] = 14 + x / 4;
01462 else
01463 col[1] = 0;
01464 col[2] = (x + 2 * y + 43) / 4;
01465
01466 std::string msg;
01467
01468 if (mode != -1)
01469 {
01470 if (col[mode] == actual_col)
01471 return "";
01472
01473 msg = (boost::format("Unexpected value %u instead of %u at (x=%u, y=%u)")%(int)actual_col%(int)col[mode]%x%y).str();
01474 }
01475 else
01476 {
01477 for (int i = 0; i < NUM_TEST_PATTERNS; i++)
01478 if (col[i] == actual_col)
01479 {
01480 mode = i;
01481 return "";
01482 }
01483
01484 msg = (boost::format("Unexpected value in first pixel %u, expected: %u, %u or %u")%(int)actual_col%(int)col[0]%(int)col[1]%(int)col[2]).str();
01485 }
01486
01487 ROS_ERROR("%s", msg.c_str());
01488
01489 return msg;
01490 }
01491 diagnostic_updater::DiagnosticStatusWrapper &status_;
01492 int imager_id_;
01493 };
01494
01495 void videoModeTest(int mode, diagnostic_updater::DiagnosticStatusWrapper& status)
01496 {
01497 if (mode < 0 || mode > MT9V_NUM_MODES)
01498 {
01499 ROS_ERROR("Tried to call videoModeTest with out of range mode %u.", mode);
01500 return;
01501 }
01502
01503 const Config old_config = driver_.config_;
01504 WGE100CameraDriver::UseFrameFunction oldUseFrame = driver_.useFrame_;
01505
01506 Config new_config = driver_.config_;
01507 new_config.width = MT9VModes[mode].width;
01508 new_config.height = MT9VModes[mode].height;
01509 new_config.x_offset = 0;
01510 new_config.y_offset = 0;
01511 new_config.imager_rate = MT9VModes[mode].fps;
01512 new_config.ext_trig = 0;
01513 new_config.register_set = wge100_camera::WGE100Camera_PrimaryRegisterSet;
01514 new_config.test_pattern = true;
01515 driver_.config_update(new_config);
01516 boost::shared_ptr<MT9VImager> imager = driver_.imager_;
01517 VideoModeTestFrameHandler callback(status, imager ? driver_.imager_->getVersion() : 0);
01518 driver_.useFrame_ = boost::bind(&VideoModeTestFrameHandler::run, boost::ref(callback), _1, _2, _3, _4, _5);
01519
01520 status.name = (boost::format("Pattern Test: %ix%i at %.1f fps.")%new_config.width%new_config.height%new_config.imager_rate).str();
01521 status.summary(0, "Passed");
01522
01523 int oldcount = driver_.lost_image_thread_count_;
01524 for (int i = 0; i < 50 && !callback.got_frame_; i++)
01525 {
01526 driver_.start();
01527 usleep(100000);
01528 }
01529 driver_.close();
01530 if (!callback.got_frame_)
01531 {
01532 ROS_ERROR("Got no frame during pattern test.");
01533 status.summary(2, "Got no frame during pattern test.");
01534 }
01535 if (oldcount < driver_.lost_image_thread_count_)
01536 {
01537 ROS_ERROR("Lost the image_thread. This should never happen.");
01538 status.summary(2, "Lost the image_thread. This should never happen.");
01539 }
01540
01541 driver_.useFrame_ = oldUseFrame;
01542 driver_.config_update(old_config);
01543
01544 ROS_INFO("Exiting test %s with status %i: %s", status.name.c_str(), status.level, status.message.c_str());
01545 }
01546 };
01547
01548 int main(int argc, char **argv)
01549 {
01550 return driver_base::main<WGE100CameraNode>(argc, argv, "wge100_camera");
01551 }