trigger.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013 Boris Gromov, BioRobotics Lab at Korea Tech
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the author nor other contributors may be
00018  *     used to endorse or promote products derived from this software
00019  *     without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 #include <ros/ros.h>
00036 #include "trigger.h"
00037 
00045 // initializing constants
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; // failure
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; // failure
00169   }
00170 
00171   if (has_polarity == DC1394_TRUE)
00172   {
00173     err = dc1394_external_trigger_get_polarity(camera, &current_polarity);
00174     if (err != DC1394_SUCCESS)
00175     {
00176       ROS_FATAL("getPolarity() failed: %d", err);
00177       return (dc1394trigger_polarity_t) DC1394_TRIGGER_ACTIVE_NUM; // failure
00178     }
00179   }
00180   else
00181   {
00182     ROS_ERROR("Polarity is not supported");
00183     return (dc1394trigger_polarity_t) DC1394_TRIGGER_ACTIVE_NUM; // failure
00184   }
00185 
00186   return current_polarity; // success
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; // failure
00206   }
00207 
00208   if (has_polarity == DC1394_TRUE)
00209   {
00210     // if config not changed, then do nothing
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; // failure
00219     }
00220     ROS_DEBUG("setPolarity(): %s", triggerPolarityName(polarity).c_str());
00221   }
00222   else
00223   {
00224     ROS_FATAL("Polarity is not supported");
00225     return false; // failure
00226   }
00227 
00228   return true; // success
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; // failure
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   // if config not changed, then do nothing
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; // failure
00270   }
00271   externalTriggerPowerState_ = state;
00272   ROS_DEBUG("setExternalTriggerPowerState(): %s", (state == DC1394_ON ? "ON" : "OFF"));
00273   return true; // success
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; // failure
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   // if config not changed, then do nothing
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; // failure
00314   }
00315   ROS_DEBUG("setSoftwareTriggerPowerState(): %s", (state == DC1394_ON ? "ON" : "OFF"));
00316   return true; // success
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; // failure
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   // if config not changed, then do nothing
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; // failure
00359   }
00360   ROS_DEBUG("setMode(): %s", triggerModeName(mode).c_str());
00361   return true; // success
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; // failure
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   // if config not changed, then do nothing
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; // failure
00405   }
00406   ROS_DEBUG("setSource(): %s", triggerSourceName(source).c_str());
00407 
00408   return true; // success
00409 }
00410 
00417 bool Trigger::reconfigure(Config *newconfig)
00418 {
00419   bool is_ok = true;
00420 
00422   // set triggering modes
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       // Possible if driver compiled against different version of libdc1394
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         // Possible if driver compiled against different version of libdc1394
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       // Possible if driver compiled against different version of libdc1394
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   // Enumerate trigger sources
00513   if (!Trigger::enumSources(camera_, triggerSources_))
00514   {
00515     ROS_ERROR("Failed to enumerate trigger sources");
00516     return false;
00517   }
00518 
00519   // Update externalTriggerPowerState_ variable with current value
00520   Trigger::getExternalTriggerPowerState(camera_);
00521 
00522   // configure trigger features
00523   return reconfigure(newconfig);
00524 }


camera1394stereo
Author(s): Joan Pau Beltran
autogenerated on Thu Jun 6 2019 21:29:10