r2000_driver.cpp
Go to the documentation of this file.
00001 // Copyright (c) 2014, Pepperl+Fuchs GmbH, Mannheim
00002 // Copyright (c) 2014, Denis Dillenberger
00003 // All rights reserved.
00004 //
00005 // Redistribution and use in source and binary forms, with or without modification,
00006 // are permitted provided that the following conditions are met:
00007 //
00008 // * Redistributions of source code must retain the above copyright notice, this
00009 //   list of conditions and the following disclaimer.
00010 //
00011 // * Redistributions in binary form must reproduce the above copyright notice, this
00012 //   list of conditions and the following disclaimer in the documentation and/or
00013 //   other materials provided with the distribution.
00014 //
00015 // * Neither the name of Pepperl+Fuchs nor the names of its
00016 //   contributors may be used to endorse or promote products derived from
00017 //   this software without specific prior written permission.
00018 //
00019 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00020 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00021 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00022 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
00023 // ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00024 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00025 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
00026 // ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00027 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00028 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00029 
00030 #include <ctime>
00031 #include <algorithm>
00032 #include <pepperl_fuchs_r2000/r2000_driver.h>
00033 #include <pepperl_fuchs_r2000/packet_structure.h>
00034 #include <pepperl_fuchs_r2000/http_command_interface.h>
00035 #include "scan_data_receiver.h"
00036 
00037 namespace pepperl_fuchs {
00038 
00039 R2000Driver::R2000Driver()
00040 {
00041     command_interface_ = 0;
00042     data_receiver_ = 0;
00043     is_connected_ = false;
00044     is_capturing_ = false;
00045     watchdog_feed_time_ = 0;
00046 }
00047 
00048 //-----------------------------------------------------------------------------
00049 bool R2000Driver::connect(const std::string hostname, int port)
00050 {
00051     command_interface_ = new HttpCommandInterface(hostname,port);
00052     auto opi = command_interface_->getProtocolInfo();
00053     if( !opi || (*opi).version_major != 1 )
00054     {
00055         std::cerr << "ERROR: Could not connect to laser range finder!" << std::endl;
00056         return false;
00057     }
00058 
00059     if( (*opi).version_major != 1 )
00060     {
00061         std::cerr << "ERROR: Wrong protocol version (version_major=" << (*opi).version_major << ", version_minor=" << (*opi).version_minor << ")" << std::endl;
00062         return false;
00063     }
00064 
00065     protocol_info_ = *opi;
00066     parameters_ = command_interface_->getParameters(command_interface_->getParameterList());
00067     is_connected_ = true;
00068     return true;
00069 }
00070 
00071 //-----------------------------------------------------------------------------
00072 R2000Driver::~R2000Driver()
00073 {
00074     disconnect();
00075 }
00076 
00077 //-----------------------------------------------------------------------------
00078 bool R2000Driver::startCapturingTCP()
00079 {
00080     if( !checkConnection() )
00081         return false;
00082 
00083     handle_info_ = command_interface_->requestHandleTCP();
00084     if( !handle_info_ )
00085         return false;
00086 
00087     data_receiver_ = new ScanDataReceiver(handle_info_->hostname, handle_info_->port);
00088     if( !data_receiver_->isConnected() || !command_interface_->startScanOutput((*handle_info_).handle))
00089         return false;
00090 
00091     food_timeout_ = std::floor(std::max((handle_info_->watchdog_timeout/1000.0/3.0),1.0));
00092     is_capturing_ = true;
00093     return true;
00094 }
00095 
00096 //-----------------------------------------------------------------------------
00097 bool R2000Driver::startCapturingUDP()
00098 {
00099     if( !checkConnection() )
00100         return false;
00101 
00102     data_receiver_ = new ScanDataReceiver();
00103     if( !data_receiver_->isConnected() )
00104         return false;
00105     int udp_port = data_receiver_->getUDPPort();
00106 
00107     handle_info_ = command_interface_->requestHandleUDP(udp_port);
00108     if( !handle_info_ || !command_interface_->startScanOutput((*handle_info_).handle) )
00109         return false;
00110 
00111     food_timeout_ = std::floor(std::max((handle_info_->watchdog_timeout/1000.0/3.0),1.0));
00112     is_capturing_ = true;
00113     return true;
00114 }
00115 
00116 //-----------------------------------------------------------------------------
00117 bool R2000Driver::stopCapturing()
00118 {
00119     if( !is_capturing_ || !command_interface_ )
00120         return false;
00121 
00122     bool return_val = checkConnection();
00123 
00124     return_val = return_val && command_interface_->stopScanOutput((*handle_info_).handle);
00125 
00126     delete data_receiver_;
00127     data_receiver_ = 0;
00128 
00129     is_capturing_ = false;
00130     return_val = return_val && command_interface_->releaseHandle(handle_info_->handle);
00131     handle_info_ = boost::optional<HandleInfo>();
00132     return return_val;
00133 }
00134 
00135 //-----------------------------------------------------------------------------
00136 bool R2000Driver::checkConnection()
00137 {
00138     if( !command_interface_ || !isConnected() || !command_interface_->getProtocolInfo() )
00139     {
00140         std::cerr << "ERROR: No connection to laser range finder or connection lost!" << std::endl;
00141         return false;
00142     }
00143     return true;
00144 }
00145 
00146 //-----------------------------------------------------------------------------
00147 ScanData R2000Driver::getScan()
00148 {
00149     feedWatchdog();
00150     if( data_receiver_ )
00151         return data_receiver_->getScan();
00152     else
00153     {
00154         std::cerr << "ERROR: No scan capturing started!" << std::endl;
00155         return ScanData();
00156     }
00157 }
00158 
00159 //-----------------------------------------------------------------------------
00160 ScanData R2000Driver::getFullScan()
00161 {
00162     feedWatchdog();
00163     if( data_receiver_ )
00164         return data_receiver_->getFullScan();
00165     else
00166     {
00167         std::cerr << "ERROR: No scan capturing started!" << std::endl;
00168         return ScanData();
00169     }
00170 }
00171 
00172 //-----------------------------------------------------------------------------
00173 std::size_t R2000Driver::getScansAvailable() const
00174 {
00175     if( data_receiver_ )
00176         return data_receiver_->getScansAvailable();
00177     else
00178     {
00179         std::cerr << "ERROR: No scan capturing started!" << std::endl;
00180         return 0;
00181     }
00182 }
00183 
00184 //-----------------------------------------------------------------------------
00185 std::size_t R2000Driver::getFullScansAvailable() const
00186 {
00187     if( data_receiver_ )
00188         return data_receiver_->getFullScansAvailable();
00189     else
00190     {
00191         std::cerr << "ERROR: No scan capturing started!" << std::endl;
00192         return 0;
00193     }
00194 }
00195 
00196 //-----------------------------------------------------------------------------
00197 void R2000Driver::disconnect()
00198 {
00199     if( isCapturing() )
00200         stopCapturing();
00201 
00202     delete data_receiver_;
00203     delete command_interface_;
00204     data_receiver_ = 0;
00205     command_interface_ = 0;
00206 
00207     is_capturing_ = false;
00208     is_connected_ = false;
00209 
00210     handle_info_ = boost::optional<HandleInfo>();
00211     protocol_info_ = ProtocolInfo();
00212     parameters_ = std::map< std::string, std::string >();
00213 }
00214 
00215 //-----------------------------------------------------------------------------
00216 bool R2000Driver::isCapturing()
00217 {
00218     return is_capturing_ && data_receiver_->isConnected();
00219 }
00220 
00221 //-----------------------------------------------------------------------------
00222 const std::map< std::string, std::string >& R2000Driver::getParameters()
00223 {
00224     if( command_interface_ )
00225         parameters_ = command_interface_->getParameters(command_interface_->getParameterList());
00226     return parameters_;
00227 }
00228 
00229 //-----------------------------------------------------------------------------
00230 bool R2000Driver::setScanFrequency(unsigned int frequency)
00231 {
00232     if( !command_interface_ )
00233         return false;
00234     return command_interface_->setParameter("scan_frequency",std::to_string(frequency));
00235 }
00236 
00237 //-----------------------------------------------------------------------------
00238 bool R2000Driver::setSamplesPerScan(unsigned int samples)
00239 {
00240     if( !command_interface_ )
00241         return false;
00242     return command_interface_->setParameter("samples_per_scan",std::to_string(samples));
00243 }
00244 
00245 //-----------------------------------------------------------------------------
00246 bool R2000Driver::rebootDevice()
00247 {
00248     if( !command_interface_ )
00249         return false;
00250     return command_interface_->rebootDevice();
00251 }
00252 
00253 //-----------------------------------------------------------------------------
00254 bool R2000Driver::resetParameters(const std::vector<std::string> &names)
00255 {
00256     if( !command_interface_ )
00257         return false;
00258     return command_interface_->resetParameters(names);
00259 }
00260 
00261 //-----------------------------------------------------------------------------
00262 bool R2000Driver::setParameter(const std::string &name, const std::string &value)
00263 {
00264     if( !command_interface_ )
00265         return false;
00266     return command_interface_->setParameter(name,value);
00267 }
00268 
00269 //-----------------------------------------------------------------------------
00270 void R2000Driver::feedWatchdog(bool feed_always)
00271 {
00272     const double current_time = std::time(0);
00273 
00274     if( (feed_always || watchdog_feed_time_<(current_time-food_timeout_)) && handle_info_ && command_interface_  )
00275     {
00276         if( !command_interface_->feedWatchdog(handle_info_->handle) )
00277             std::cerr << "ERROR: Feeding watchdog failed!" << std::endl;
00278         watchdog_feed_time_ = current_time;
00279     }
00280 }
00281 
00282 //-----------------------------------------------------------------------------
00283 //-----------------------------------------------------------------------------
00284 } // NS


pepperl_fuchs_r2000
Author(s): Denis Dillenberger
autogenerated on Sat Sep 17 2016 04:01:08