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 <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
00055
00056
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
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
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
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
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
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
00150
00151 socket_.close();
00152 deadline_.expires_at(boost::posix_time::pos_infin);
00153 }
00154
00155
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
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
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
00184
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
00194 return ExitError;
00195 }
00196
00197
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
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
00215 input_buffer_.consume(bytes_transfered_);
00216
00217
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
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
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
00279
00280 std::vector<unsigned char> reply;
00281
00282
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
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;
00302 }
00303
00304 return ExitSuccess;
00305 }
00306
00307 }