sick_tim3xx_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_tim3xx/sick_tim3xx_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_tim3xx
00043 {
00044 
00045 SickTim3xxCommonTcp::SickTim3xxCommonTcp(const std::string & hostname, AbstractParser* parser) 
00046 :
00047     SickTim3xxCommon(parser),
00048     socket_(io_service_),
00049     deadline_(io_service_),
00050     hostname_(hostname)
00051 {
00052     // Set up the deadline actor to implement timeouts.
00053     // Based on blocking TCP example on:
00054     // http://www.boost.org/doc/libs/1_46_0/doc/html/boost_asio/example/timeouts/blocking_tcp_client.cpp
00055     deadline_.expires_at(boost::posix_time::pos_infin);
00056     checkDeadline();
00057 }
00058 
00059 SickTim3xxCommonTcp::~SickTim3xxCommonTcp()
00060 {
00061   stop_scanner();
00062   close_device();
00063 }
00064 
00065 using boost::asio::ip::tcp;
00066 using boost::lambda::var;
00067 using boost::lambda::_1;
00068 
00069 int SickTim3xxCommonTcp::init_device()
00070 {
00071     // Resolve the supplied hostname
00072     tcp::resolver::iterator iterator;
00073     try
00074     {
00075         tcp::resolver resolver(io_service_);
00076         tcp::resolver::query query(hostname_, "2112");
00077         iterator = resolver.resolve(query);
00078     }
00079     catch (boost::system::system_error &e)
00080     {
00081         ROS_FATAL("Could not resolve host: ... (%d)(%s)", e.code().value(), e.code().message().c_str());
00082         diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "Could not resolve host.");
00083         return EXIT_FAILURE;
00084     }
00085 
00086     // Try to connect to all possible endpoints
00087     boost::system::error_code ec;
00088     bool success = false;
00089     for ( ; iterator != tcp::resolver::iterator(); ++iterator)
00090     {
00091         std::string repr = boost::lexical_cast<std::string>(iterator->endpoint());
00092         socket_.close();
00093 
00094         // Time out in 5 seconds
00095         deadline_.expires_from_now(boost::posix_time::seconds(5));
00096 
00097         ec = boost::asio::error::would_block;
00098         ROS_DEBUG("Attempting to connect to %s", repr.c_str());
00099         socket_.async_connect(iterator->endpoint(), boost::lambda::var(ec) = _1);
00100 
00101         // Wait until timeout
00102         do io_service_.run_one(); while (ec == boost::asio::error::would_block);
00103 
00104         if (!ec && socket_.is_open())
00105         {
00106             success = true;
00107             ROS_INFO("Succesfully connected to %s", repr.c_str());
00108             break;
00109         }
00110         ROS_ERROR("Failed to connect to %s", repr.c_str());
00111     }
00112 
00113     // Check if connecting succeeded
00114     if (!success)
00115     {
00116         ROS_FATAL("Could not connect to host %s", hostname_.c_str());
00117         diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "Could not connect to host.");
00118         return EXIT_FAILURE;
00119     }
00120 
00121     input_buffer_.consume(input_buffer_.size());
00122 
00123     return EXIT_SUCCESS;
00124 }
00125 
00126 int SickTim3xxCommonTcp::close_device()
00127 {
00128     if (socket_.is_open())
00129     {
00130         try
00131         {
00132             socket_.close();
00133         }
00134         catch (boost::system::system_error &e)
00135         {
00136             ROS_ERROR("An error occured during closing of the connection: %d:%s", e.code().value(), e.code().message().c_str());
00137         }
00138     }
00139     return 0;
00140 }
00141 
00142 void SickTim3xxCommonTcp::checkDeadline()
00143 {
00144     if (deadline_.expires_at() <= boost::asio::deadline_timer::traits_type::now())
00145     {
00146         // The reason the function is called is that the deadline expired. Close
00147         // the socket to return all IO operations and reset the deadline
00148         socket_.close();
00149         deadline_.expires_at(boost::posix_time::pos_infin);
00150     }
00151 
00152     // Nothing bad happened, go back to sleep
00153     deadline_.async_wait(boost::bind(&SickTim3xxCommonTcp::checkDeadline, this));
00154 }
00155 
00156 int SickTim3xxCommonTcp::readWithTimeout(size_t timeout_ms, char *buffer, int buffer_size, int *bytes_read, bool *exception_occured)
00157 {
00158     // Set up the deadline to the proper timeout, error and delimiters
00159     deadline_.expires_from_now(boost::posix_time::milliseconds(timeout_ms));
00160     const char end_delim = static_cast<char>(0x03);
00161     ec_ = boost::asio::error::would_block;
00162     bytes_transfered_ = 0;
00163 
00164     // Read until 0x03 ending indicator
00165     boost::asio::async_read_until(
00166         socket_, 
00167         input_buffer_,
00168         end_delim,
00169         boost::bind(
00170             &SickTim3xxCommonTcp::handleRead,
00171             this,
00172             boost::asio::placeholders::error,
00173             boost::asio::placeholders::bytes_transferred
00174         )
00175     );
00176     do io_service_.run_one(); while (ec_ == boost::asio::error::would_block);
00177 
00178     if (ec_)
00179     {
00180         // would_block means the connectio is ok, but nothing came in in time.
00181         // If any other error code is set, this means something bad happened.
00182         if (ec_ != boost::asio::error::would_block)
00183         {
00184             ROS_ERROR("sendSOPASCommand: failed attempt to read from socket: %d: %s", ec_.value(), ec_.message().c_str());
00185             diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "sendSOPASCommand: exception during read_until().");
00186             if (exception_occured != 0)
00187                 *exception_occured = true;
00188         }
00189 
00190         // For would_block, just return and indicate nothing bad happend
00191         return EXIT_FAILURE;
00192     }
00193     
00194     // Avoid a buffer overflow by limiting the data we read
00195     size_t to_read = bytes_transfered_ > buffer_size - 1 ? buffer_size - 1 : bytes_transfered_;
00196     size_t i = 0;
00197     std::istream istr(&input_buffer_);
00198     if (buffer != 0)
00199     {
00200         istr.read(buffer, to_read);
00201         buffer[to_read] = 0;
00202 
00203         // Consume the rest of the message if necessary
00204         if (to_read < bytes_transfered_)
00205         {
00206             ROS_WARN("Dropping %zu bytes to avoid buffer overflow", bytes_transfered_ - to_read);
00207             input_buffer_.consume(bytes_transfered_ - to_read);
00208         }
00209     }
00210     else
00211         // No buffer was provided, just drop the data
00212         input_buffer_.consume(bytes_transfered_);
00213     
00214     // Set the return variable to the size of the read message
00215     if (bytes_read != 0)
00216         *bytes_read = to_read;
00217 
00218     return EXIT_SUCCESS;
00219 }
00220 
00224 int SickTim3xxCommonTcp::sendSOPASCommand(const char* request, std::vector<unsigned char> * reply)
00225 {
00226     if (!socket_.is_open()) {
00227         ROS_ERROR("sendSOPASCommand: socket not open");
00228         diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "sendSOPASCommand: socket not open.");
00229         return EXIT_FAILURE;
00230     }
00231 
00232     /*
00233      * Write a SOPAS variable read request to the device.
00234      */
00235     try
00236     {
00237         boost::asio::write(socket_, boost::asio::buffer(request, strlen(request)));
00238     }
00239     catch (boost::system::system_error &e)
00240     {
00241         ROS_ERROR("write error for command: %s", request);
00242         diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "Write error for sendSOPASCommand.");
00243         return EXIT_FAILURE;
00244     }
00245 
00246     // Set timeout in 5 seconds
00247     const int BUF_SIZE = 1000;
00248     char buffer[BUF_SIZE];
00249     int bytes_read;
00250     if (readWithTimeout(1000, buffer, BUF_SIZE, &bytes_read, 0) == EXIT_FAILURE)
00251     {
00252         ROS_ERROR_THROTTLE(1.0, "sendSOPASCommand: no full reply available for read after 1s");
00253         diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "sendSOPASCommand: no full reply available for read after 5 s.");
00254         return EXIT_FAILURE;
00255     }
00256 
00257     if (reply)
00258     {
00259         reply->resize(bytes_read);
00260         std::copy(buffer, buffer + bytes_read, &(*reply)[0]);
00261     }
00262 
00263     return EXIT_SUCCESS;
00264 }
00265 
00266 int SickTim3xxCommonTcp::get_datagram(unsigned char* receiveBuffer, int bufferSize, int* actual_length)
00267 {
00268     if (!socket_.is_open()) {
00269         ROS_ERROR("get_datagram: socket not open");
00270         diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "get_datagram: socket not open.");
00271         return EXIT_FAILURE;
00272     }
00273 
00274     /*
00275      * Write a SOPAS variable read request to the device.
00276      */
00277     std::vector<unsigned char> reply;
00278 
00279     // Wait at most 500ms for a new scan
00280     size_t timeout = 500;
00281     bool exception_occured = false;
00282 
00283     char *buffer = reinterpret_cast<char *>(receiveBuffer);
00284 
00285     if (readWithTimeout(timeout, buffer, bufferSize, actual_length, &exception_occured) != EXIT_SUCCESS)
00286     {
00287         ROS_ERROR_THROTTLE(1.0, "get_datagram: no data available for read after %zu ms", timeout);
00288         diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "get_datagram: no data available for read after timeout.");
00289 
00290         // Attempt to reconnect when the connection was terminated
00291         if (!socket_.is_open())
00292         {
00293             sleep(1);
00294             ROS_INFO("Failure - attempting to reconnect");
00295             int ret = init_device();
00296             if (ret != EXIT_SUCCESS)
00297                 return ret;
00298 
00299             return init_scanner();
00300         }
00301 
00302         return exception_occured ? EXIT_FAILURE : EXIT_SUCCESS;    // keep on trying
00303     }
00304 
00305     return EXIT_SUCCESS;
00306 }
00307 
00308 } /* namespace sick_tim3xx */


sick_tim3xx
Author(s): Jochen Sprickerhof, Martin Günther
autogenerated on Fri Aug 15 2014 11:54:25