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 #include <ros/ros.h>
00036 #include "trigger.h"
00037
00045
00046 const std::string Trigger::trigger_mode_names_[DC1394_TRIGGER_MODE_NUM] = {"mode_0", "mode_1", "mode_2", "mode_3", "mode_4",
00047 "mode_5", "mode_14", "mode_15", };
00048 const std::string Trigger::trigger_source_names_[DC1394_TRIGGER_SOURCE_NUM] = {"source_0", "source_1", "source_2",
00049 "source_3", "source_software", };
00050 const std::string Trigger::trigger_polarity_names_[DC1394_TRIGGER_ACTIVE_NUM] = {"active_low", "active_high", };
00051
00052 bool Trigger::findTriggerMode(std::string str)
00053 {
00054 if (str == "mode_0")
00055 triggerMode_ = DC1394_TRIGGER_MODE_0;
00056 else if(str == "mode_1")
00057 triggerMode_ = DC1394_TRIGGER_MODE_1;
00058 else if(str == "mode_2")
00059 triggerMode_ = DC1394_TRIGGER_MODE_2;
00060 else if(str == "mode_3")
00061 triggerMode_ = DC1394_TRIGGER_MODE_3;
00062 else if(str == "mode_4")
00063 triggerMode_ = DC1394_TRIGGER_MODE_4;
00064 else if(str == "mode_5")
00065 triggerMode_ = DC1394_TRIGGER_MODE_5;
00066 else if(str == "mode_14")
00067 triggerMode_ = DC1394_TRIGGER_MODE_14;
00068 else if(str == "mode_15")
00069 triggerMode_ = DC1394_TRIGGER_MODE_15;
00070 else
00071 {
00072 triggerMode_ = (dc1394trigger_mode_t) DC1394_TRIGGER_MODE_NUM;
00073 return false;
00074 }
00075
00076 return true;
00077 }
00078
00079 bool Trigger::findTriggerSource(std::string str)
00080 {
00081 if (str == "source_0")
00082 triggerSource_ = DC1394_TRIGGER_SOURCE_0;
00083 else if(str == "source_1")
00084 triggerSource_ = DC1394_TRIGGER_SOURCE_1;
00085 else if(str == "source_2")
00086 triggerSource_ = DC1394_TRIGGER_SOURCE_2;
00087 else if(str == "source_3")
00088 triggerSource_ = DC1394_TRIGGER_SOURCE_3;
00089 else if(str == "source_software")
00090 triggerSource_ = DC1394_TRIGGER_SOURCE_SOFTWARE;
00091 else
00092 {
00093 triggerSource_ = (dc1394trigger_source_t) DC1394_TRIGGER_SOURCE_NUM;
00094 return false;
00095 }
00096
00097 return true;
00098 }
00099
00100 bool Trigger::findTriggerPolarity(std::string str)
00101 {
00102 if(str == "active_low")
00103 triggerPolarity_ = DC1394_TRIGGER_ACTIVE_LOW;
00104 else if(str == "active_high")
00105 triggerPolarity_ = DC1394_TRIGGER_ACTIVE_HIGH;
00106 else
00107 {
00108 triggerPolarity_ = (dc1394trigger_polarity_t) DC1394_TRIGGER_ACTIVE_NUM;
00109 return false;
00110 }
00111
00112 return true;
00113 }
00114
00115 bool Trigger::checkTriggerSource(dc1394trigger_source_t source)
00116 {
00117 for(size_t i = 0; i < triggerSources_.num; i++)
00118 if(source == triggerSources_.sources[i]) return true;
00119
00120 return false;
00121 }
00122
00128 bool Trigger::enumSources(dc1394camera_t *camera, dc1394trigger_sources_t &sources)
00129 {
00130 dc1394error_t err = dc1394_external_trigger_get_supported_sources(camera, &sources);
00131 if (err != DC1394_SUCCESS)
00132 {
00133 ROS_FATAL("enumTriggerSources() failed: %d", err);
00134 return false;
00135 }
00136
00137 std::ostringstream ss;
00138 if(sources.num != 0)
00139 {
00140 for(size_t i = 0; i < sources.num - 1; i++)
00141 ss << triggerSourceName(sources.sources[i]) << ", ";
00142 ss << triggerSourceName(sources.sources[sources.num - 1]);
00143 }
00144 else
00145 {
00146 ss << "none";
00147 }
00148
00149 ROS_DEBUG_STREAM("Trigger sources: " << ss.str());
00150 return true;
00151 }
00152
00159 dc1394trigger_polarity_t Trigger::getPolarity(dc1394camera_t *camera)
00160 {
00161 dc1394trigger_polarity_t current_polarity;
00162
00163 dc1394bool_t has_polarity;
00164 dc1394error_t err = dc1394_external_trigger_has_polarity(camera, &has_polarity);
00165 if (err != DC1394_SUCCESS)
00166 {
00167 ROS_FATAL("getPolarity() failed: %d", err);
00168 return (dc1394trigger_polarity_t) DC1394_TRIGGER_ACTIVE_NUM;
00169 }
00170
00171 if (has_polarity == DC1394_TRUE)
00172 {
00173 err = dc1394_external_trigger_get_polarity(camera, ¤t_polarity);
00174 if (err != DC1394_SUCCESS)
00175 {
00176 ROS_FATAL("getPolarity() failed: %d", err);
00177 return (dc1394trigger_polarity_t) DC1394_TRIGGER_ACTIVE_NUM;
00178 }
00179 }
00180 else
00181 {
00182 ROS_ERROR("Polarity is not supported");
00183 return (dc1394trigger_polarity_t) DC1394_TRIGGER_ACTIVE_NUM;
00184 }
00185
00186 return current_polarity;
00187 }
00188
00196 bool Trigger::setPolarity(dc1394camera_t *camera, dc1394trigger_polarity_t &polarity)
00197 {
00198 dc1394trigger_polarity_t current_polarity = getPolarity(camera);
00199
00200 dc1394bool_t has_polarity;
00201 dc1394error_t err = dc1394_external_trigger_has_polarity(camera, &has_polarity);
00202 if (err != DC1394_SUCCESS)
00203 {
00204 ROS_FATAL("setPolarity() failed: %d", err);
00205 return false;
00206 }
00207
00208 if (has_polarity == DC1394_TRUE)
00209 {
00210
00211 if(current_polarity == polarity) return true;
00212
00213 err = dc1394_external_trigger_set_polarity(camera, polarity);
00214 if (err != DC1394_SUCCESS)
00215 {
00216 polarity = current_polarity;
00217 ROS_FATAL("setPolarity() failed: %d", err);
00218 return false;
00219 }
00220 ROS_DEBUG("setPolarity(): %s", triggerPolarityName(polarity).c_str());
00221 }
00222 else
00223 {
00224 ROS_FATAL("Polarity is not supported");
00225 return false;
00226 }
00227
00228 return true;
00229 }
00230
00236 dc1394switch_t Trigger::getExternalTriggerPowerState(dc1394camera_t *camera)
00237 {
00238 dc1394switch_t state;
00239
00240 dc1394error_t err = dc1394_external_trigger_get_power(camera, &state);
00241 if (err != DC1394_SUCCESS)
00242 {
00243 ROS_FATAL("getExternalTriggerPowerState() failed: %d", err);
00244 return (dc1394switch_t)-1;
00245 }
00246 externalTriggerPowerState_ = state;
00247 return state;
00248 }
00249
00257 bool Trigger::setExternalTriggerPowerState(dc1394camera_t *camera, dc1394switch_t &state)
00258 {
00259 dc1394switch_t current_state = getExternalTriggerPowerState(camera);
00260
00261
00262 if(current_state == state) return true;
00263
00264 dc1394error_t err = dc1394_external_trigger_set_power(camera, state);
00265 if (err != DC1394_SUCCESS)
00266 {
00267 state = current_state;
00268 ROS_FATAL("setExternalTriggerPowerState() failed: %d", err);
00269 return false;
00270 }
00271 externalTriggerPowerState_ = state;
00272 ROS_DEBUG("setExternalTriggerPowerState(): %s", (state == DC1394_ON ? "ON" : "OFF"));
00273 return true;
00274 }
00275
00281 dc1394switch_t Trigger::getSoftwareTriggerPowerState(dc1394camera_t *camera)
00282 {
00283 dc1394switch_t state;
00284
00285 dc1394error_t err = dc1394_software_trigger_get_power(camera, &state);
00286 if (err != DC1394_SUCCESS)
00287 {
00288 ROS_FATAL("getSoftwareTriggerPowerState() failed: %d", err);
00289 return (dc1394switch_t)-1;
00290 }
00291 return state;
00292 }
00293
00301 bool Trigger::setSoftwareTriggerPowerState(dc1394camera_t *camera, dc1394switch_t &state)
00302 {
00303 dc1394switch_t current_state = getSoftwareTriggerPowerState(camera);
00304
00305
00306 if(current_state == state) return true;
00307
00308 dc1394error_t err = dc1394_software_trigger_set_power(camera, state);
00309 if (err != DC1394_SUCCESS)
00310 {
00311 state = current_state;
00312 ROS_FATAL("setSoftwareTriggerPowerState() failed: %d", err);
00313 return false;
00314 }
00315 ROS_DEBUG("setSoftwareTriggerPowerState(): %s", (state == DC1394_ON ? "ON" : "OFF"));
00316 return true;
00317 }
00318
00325 dc1394trigger_mode_t Trigger::getMode(dc1394camera_t *camera)
00326 {
00327 dc1394trigger_mode_t mode;
00328
00329 dc1394error_t err = dc1394_external_trigger_get_mode(camera, &mode);
00330 if (err != DC1394_SUCCESS)
00331 {
00332 ROS_FATAL("getTriggerMode() failed: %d", err);
00333 return (dc1394trigger_mode_t) DC1394_TRIGGER_MODE_NUM;
00334 }
00335
00336 return mode;
00337 }
00338
00346 bool Trigger::setMode(dc1394camera_t *camera, dc1394trigger_mode_t &mode)
00347 {
00348 dc1394trigger_mode_t current_mode = getMode(camera);
00349
00350
00351 if(current_mode == mode) return true;
00352
00353 dc1394error_t err = dc1394_external_trigger_set_mode(camera, mode);
00354 if (err != DC1394_SUCCESS)
00355 {
00356 mode = current_mode;
00357 ROS_FATAL("setTriggerMode() failed: %d", err);
00358 return false;
00359 }
00360 ROS_DEBUG("setMode(): %s", triggerModeName(mode).c_str());
00361 return true;
00362 }
00363
00370 dc1394trigger_source_t Trigger::getSource(dc1394camera_t *camera)
00371 {
00372 dc1394trigger_source_t source;
00373
00374 dc1394error_t err = dc1394_external_trigger_get_source(camera, &source);
00375 if (err != DC1394_SUCCESS)
00376 {
00377 ROS_FATAL("getTriggerSource() failed: %d", err);
00378 return (dc1394trigger_source_t) DC1394_TRIGGER_SOURCE_NUM;
00379 }
00380
00381 return source;
00382 }
00383
00391 bool Trigger::setSource(dc1394camera_t *camera, dc1394trigger_source_t &source)
00392 {
00393 dc1394trigger_source_t current_source = getSource(camera);
00394
00395
00396 if(current_source == source) return true;
00397
00398 dc1394error_t err = dc1394_external_trigger_set_source(camera, source);
00399
00400 if (err != DC1394_SUCCESS)
00401 {
00402 source = current_source;
00403 ROS_FATAL("setTriggerSource() failed: %d", err);
00404 return false;
00405 }
00406 ROS_DEBUG("setSource(): %s", triggerSourceName(source).c_str());
00407
00408 return true;
00409 }
00410
00417 bool Trigger::reconfigure(Config *newconfig)
00418 {
00419 bool is_ok = true;
00420
00422
00424 dc1394switch_t on_off = (dc1394switch_t) newconfig->external_trigger;
00425 if (!Trigger::setExternalTriggerPowerState(camera_, on_off))
00426 {
00427 newconfig->external_trigger = on_off;
00428 ROS_ERROR("Failed to set external trigger power");
00429 is_ok = false;
00430 }
00431
00432 on_off = (dc1394switch_t) newconfig->software_trigger;
00433 if (!Trigger::setSoftwareTriggerPowerState(camera_, on_off))
00434 {
00435 newconfig->software_trigger = on_off;
00436 ROS_ERROR("Failed to set software trigger power");
00437 is_ok = false;
00438 }
00439
00440 if (findTriggerMode(newconfig->trigger_mode))
00441 {
00442 if (!Trigger::setMode(camera_, triggerMode_))
00443 {
00444
00445 ROS_ASSERT(triggerMode_ <= DC1394_TRIGGER_MODE_MAX);
00446 newconfig->trigger_mode = Trigger::trigger_mode_names_[triggerMode_ - DC1394_TRIGGER_MODE_MIN];
00447 ROS_ERROR("Failed to set trigger mode");
00448 is_ok = false;
00449 }
00450 }
00451 else
00452 {
00453 ROS_ERROR_STREAM("Unknown trigger mode: " << newconfig->trigger_mode);
00454 is_ok = false;
00455 }
00456
00457 if (triggerSources_.num != 0)
00458 {
00459 if (findTriggerSource(newconfig->trigger_source) && checkTriggerSource(triggerSource_))
00460 {
00461 if (!Trigger::setSource(camera_, triggerSource_))
00462 {
00463
00464 ROS_ASSERT(triggerSource_ <= DC1394_TRIGGER_SOURCE_MAX);
00465 newconfig->trigger_source = Trigger::trigger_source_names_[triggerSource_ - DC1394_TRIGGER_SOURCE_MIN];
00466 ROS_ERROR("Failed to set trigger source");
00467 is_ok = false;
00468 }
00469 }
00470 else
00471 {
00472 ROS_ERROR_STREAM("Unknown trigger source: " << newconfig->trigger_source);
00473 is_ok = false;
00474 }
00475 }
00476 else
00477 {
00478 ROS_DEBUG("No triggering sources available");
00479 }
00480
00481 if (findTriggerPolarity(newconfig->trigger_polarity))
00482 {
00483 if (!Trigger::setPolarity(camera_, triggerPolarity_))
00484 {
00485
00486 ROS_ASSERT(triggerPolarity_ <= DC1394_TRIGGER_ACTIVE_MAX);
00487 newconfig->trigger_polarity = Trigger::trigger_polarity_names_[triggerPolarity_ - DC1394_TRIGGER_ACTIVE_MIN];
00488 ROS_ERROR("Failed to set trigger polarity");
00489 is_ok = false;
00490 }
00491 }
00492 else
00493 {
00494 ROS_ERROR_STREAM("Unknown trigger polarity: " << newconfig->trigger_polarity);
00495 is_ok = false;
00496 }
00497
00498 return is_ok;
00499 }
00500
00508 bool Trigger::initialize(Config *newconfig)
00509 {
00510 ROS_INFO("[%016lx] has trigger support", camera_->guid);
00511
00512
00513 if (!Trigger::enumSources(camera_, triggerSources_))
00514 {
00515 ROS_ERROR("Failed to enumerate trigger sources");
00516 return false;
00517 }
00518
00519
00520 Trigger::getExternalTriggerPowerState(camera_);
00521
00522
00523 return reconfigure(newconfig);
00524 }