sick_tim_common_tcp.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2013, Freiburg University
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of Osnabrück University nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  *
00029  *  Created on: 15.11.2013
00030  *
00031  *      Authors:
00032  *         Christian Dornhege <c.dornhege@googlemail.com>
00033  */
00034 
00035 #include <sick_tim/sick_tim_common_tcp.h>
00036 #include <boost/asio.hpp>
00037 #include <boost/lambda/lambda.hpp>
00038 #include <algorithm>
00039 #include <iterator>
00040 #include <boost/lexical_cast.hpp>
00041 
00042 namespace sick_tim
00043 {
00044 
00045 SickTimCommonTcp::SickTimCommonTcp(const std::string &hostname, const std::string &port, int &timelimit, AbstractParser* parser)
00046 :
00047     SickTimCommon(parser),
00048     socket_(io_service_),
00049     deadline_(io_service_),
00050     hostname_(hostname),
00051     port_(port),
00052     timelimit_(timelimit)
00053 {
00054     // Set up the deadline actor to implement timeouts.
00055     // Based on blocking TCP example on:
00056     // http://www.boost.org/doc/libs/1_46_0/doc/html/boost_asio/example/timeouts/blocking_tcp_client.cpp
00057     deadline_.expires_at(boost::posix_time::pos_infin);
00058     checkDeadline();
00059 }
00060 
00061 SickTimCommonTcp::~SickTimCommonTcp()
00062 {
00063   stop_scanner();
00064   close_device();
00065 }
00066 
00067 using boost::asio::ip::tcp;
00068 using boost::lambda::var;
00069 using boost::lambda::_1;
00070 
00071 int SickTimCommonTcp::init_device()
00072 {
00073     // Resolve the supplied hostname
00074     tcp::resolver::iterator iterator;
00075     try
00076     {
00077         tcp::resolver resolver(io_service_);
00078         tcp::resolver::query query(hostname_, port_);
00079         iterator = resolver.resolve(query);
00080     }
00081     catch (boost::system::system_error &e)
00082     {
00083         ROS_FATAL("Could not resolve host: ... (%d)(%s)", e.code().value(), e.code().message().c_str());
00084         diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "Could not resolve host.");
00085         return ExitError;
00086     }
00087 
00088     // Try to connect to all possible endpoints
00089     boost::system::error_code ec;
00090     bool success = false;
00091     for ( ; iterator != tcp::resolver::iterator(); ++iterator)
00092     {
00093         std::string repr = boost::lexical_cast<std::string>(iterator->endpoint());
00094         socket_.close();
00095 
00096         // Set the time out length
00097         ROS_INFO("Waiting %i seconds for device to connect.", timelimit_);
00098         deadline_.expires_from_now(boost::posix_time::seconds(timelimit_));
00099 
00100         ec = boost::asio::error::would_block;
00101         ROS_DEBUG("Attempting to connect to %s", repr.c_str());
00102         socket_.async_connect(iterator->endpoint(), boost::lambda::var(ec) = _1);
00103 
00104         // Wait until timeout
00105         do io_service_.run_one(); while (ec == boost::asio::error::would_block);
00106 
00107         if (!ec && socket_.is_open())
00108         {
00109             success = true;
00110             ROS_INFO("Succesfully connected to %s", repr.c_str());
00111             break;
00112         }
00113         ROS_ERROR("Failed to connect to %s", repr.c_str());
00114     }
00115 
00116     // Check if connecting succeeded
00117     if (!success)
00118     {
00119         ROS_FATAL("Could not connect to host %s:%s", hostname_.c_str(), port_.c_str());
00120         diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "Could not connect to host.");
00121         return ExitError;
00122     }
00123 
00124     input_buffer_.consume(input_buffer_.size());
00125 
00126     return ExitSuccess;
00127 }
00128 
00129 int SickTimCommonTcp::close_device()
00130 {
00131     if (socket_.is_open())
00132     {
00133         try
00134         {
00135             socket_.close();
00136         }
00137         catch (boost::system::system_error &e)
00138         {
00139             ROS_ERROR("An error occured during closing of the connection: %d:%s", e.code().value(), e.code().message().c_str());
00140         }
00141     }
00142     return 0;
00143 }
00144 
00145 void SickTimCommonTcp::checkDeadline()
00146 {
00147     if (deadline_.expires_at() <= boost::asio::deadline_timer::traits_type::now())
00148     {
00149         // The reason the function is called is that the deadline expired. Close
00150         // the socket to return all IO operations and reset the deadline
00151         socket_.close();
00152         deadline_.expires_at(boost::posix_time::pos_infin);
00153     }
00154 
00155     // Nothing bad happened, go back to sleep
00156     deadline_.async_wait(boost::bind(&SickTimCommonTcp::checkDeadline, this));
00157 }
00158 
00159 int SickTimCommonTcp::readWithTimeout(size_t timeout_ms, char *buffer, int buffer_size, int *bytes_read, bool *exception_occured)
00160 {
00161     // Set up the deadline to the proper timeout, error and delimiters
00162     deadline_.expires_from_now(boost::posix_time::milliseconds(timeout_ms));
00163     const char end_delim = static_cast<char>(0x03);
00164     ec_ = boost::asio::error::would_block;
00165     bytes_transfered_ = 0;
00166 
00167     // Read until 0x03 ending indicator
00168     boost::asio::async_read_until(
00169         socket_, 
00170         input_buffer_,
00171         end_delim,
00172         boost::bind(
00173             &SickTimCommonTcp::handleRead,
00174             this,
00175             boost::asio::placeholders::error,
00176             boost::asio::placeholders::bytes_transferred
00177         )
00178     );
00179     do io_service_.run_one(); while (ec_ == boost::asio::error::would_block);
00180 
00181     if (ec_)
00182     {
00183         // would_block means the connectio is ok, but nothing came in in time.
00184         // If any other error code is set, this means something bad happened.
00185         if (ec_ != boost::asio::error::would_block)
00186         {
00187             ROS_ERROR("sendSOPASCommand: failed attempt to read from socket: %d: %s", ec_.value(), ec_.message().c_str());
00188             diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "sendSOPASCommand: exception during read_until().");
00189             if (exception_occured != 0)
00190                 *exception_occured = true;
00191         }
00192 
00193         // For would_block, just return and indicate nothing bad happend
00194         return ExitError;
00195     }
00196     
00197     // Avoid a buffer overflow by limiting the data we read
00198     size_t to_read = bytes_transfered_ > buffer_size - 1 ? buffer_size - 1 : bytes_transfered_;
00199     size_t i = 0;
00200     std::istream istr(&input_buffer_);
00201     if (buffer != 0)
00202     {
00203         istr.read(buffer, to_read);
00204         buffer[to_read] = 0;
00205 
00206         // Consume the rest of the message if necessary
00207         if (to_read < bytes_transfered_)
00208         {
00209             ROS_WARN("Dropping %zu bytes to avoid buffer overflow", bytes_transfered_ - to_read);
00210             input_buffer_.consume(bytes_transfered_ - to_read);
00211         }
00212     }
00213     else
00214         // No buffer was provided, just drop the data
00215         input_buffer_.consume(bytes_transfered_);
00216     
00217     // Set the return variable to the size of the read message
00218     if (bytes_read != 0)
00219         *bytes_read = to_read;
00220 
00221     return ExitSuccess;
00222 }
00223 
00227 int SickTimCommonTcp::sendSOPASCommand(const char* request, std::vector<unsigned char> * reply)
00228 {
00229     if (!socket_.is_open()) {
00230         ROS_ERROR("sendSOPASCommand: socket not open");
00231         diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "sendSOPASCommand: socket not open.");
00232         return ExitError;
00233     }
00234 
00235     /*
00236      * Write a SOPAS variable read request to the device.
00237      */
00238     try
00239     {
00240         boost::asio::write(socket_, boost::asio::buffer(request, strlen(request)));
00241     }
00242     catch (boost::system::system_error &e)
00243     {
00244         ROS_ERROR("write error for command: %s", request);
00245         diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "Write error for sendSOPASCommand.");
00246         return ExitError;
00247     }
00248 
00249     // Set timeout in 5 seconds
00250     const int BUF_SIZE = 1000;
00251     char buffer[BUF_SIZE];
00252     int bytes_read;
00253     if (readWithTimeout(1000, buffer, BUF_SIZE, &bytes_read, 0) == ExitError)
00254     {
00255         ROS_ERROR_THROTTLE(1.0, "sendSOPASCommand: no full reply available for read after 1s");
00256         diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "sendSOPASCommand: no full reply available for read after 5 s.");
00257         return ExitError;
00258     }
00259 
00260     if (reply)
00261     {
00262         reply->resize(bytes_read);
00263         std::copy(buffer, buffer + bytes_read, &(*reply)[0]);
00264     }
00265 
00266     return ExitSuccess;
00267 }
00268 
00269 int SickTimCommonTcp::get_datagram(unsigned char* receiveBuffer, int bufferSize, int* actual_length)
00270 {
00271     if (!socket_.is_open()) {
00272         ROS_ERROR("get_datagram: socket not open");
00273         diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "get_datagram: socket not open.");
00274         return ExitError;
00275     }
00276 
00277     /*
00278      * Write a SOPAS variable read request to the device.
00279      */
00280     std::vector<unsigned char> reply;
00281 
00282     // Wait at most 1000ms for a new scan
00283     size_t timeout = 1000;
00284     bool exception_occured = false;
00285 
00286     char *buffer = reinterpret_cast<char *>(receiveBuffer);
00287 
00288     if (readWithTimeout(timeout, buffer, bufferSize, actual_length, &exception_occured) != ExitSuccess)
00289     {
00290         ROS_ERROR_THROTTLE(1.0, "get_datagram: no data available for read after %zu ms", timeout);
00291         diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "get_datagram: no data available for read after timeout.");
00292 
00293         // Attempt to reconnect when the connection was terminated
00294         if (!socket_.is_open())
00295         {
00296             sleep(1);
00297             ROS_INFO("Failure - attempting to reconnect");
00298             return init();
00299         }
00300 
00301         return exception_occured ? ExitError : ExitSuccess;    // keep on trying
00302     }
00303 
00304     return ExitSuccess;
00305 }
00306 
00307 } /* namespace sick_tim */


sick_tim
Author(s): Jochen Sprickerhof , Martin Günther , Sebastian Pütz
autogenerated on Thu May 9 2019 02:32:02