33 #include <boost/asio.hpp> 34 #include <boost/array.hpp> 35 #include <boost/format.hpp> 36 #include <boost/bind.hpp> 37 #include <boost/serialization/access.hpp> 38 #include <boost/archive/binary_iarchive.hpp> 39 #include <boost/shared_array.hpp> 40 #include <boost/algorithm/string.hpp> 41 #include <boost/chrono.hpp> 70 const boost::posix_time::ptime&)>
cb_aux_;
80 std::vector<boost::shared_array<const TableSincos>>
tbl_v_;
86 boost::asio::streambuf
buf_;
90 : socket_(io_service_)
97 , tbl_h_loaded_(false)
98 , tbl_v_loaded_(false)
99 , timeout_(
boost::posix_time::seconds(1))
104 timeout_ = boost::posix_time::milliseconds(static_cast<int64_t>(1000 * to));
106 void connect(
const char* ip,
const unsigned int port, decltype(cb_connect_) cb)
109 boost::asio::ip::tcp::endpoint endpoint(boost::asio::ip::address::from_string(ip), port);
110 timer_.expires_from_now(timeout_);
133 send(std::string(
"SET:_ars=1\n"));
135 send(std::string(
"SET:_ars=0\n"));
137 [[deprecated(
"use setHorizontalInterlace() instead of setInterlace()")]]
void setInterlace(
const int itl)
143 send((boost::format(
"SET:_itl=0,%02d\n") % itl).str());
147 send((boost::format(
"SET:_itv=0,%02d\n") % itl).str());
152 tbl_vn_loaded_.resize(itl);
155 send(std::string(
"GET:tblv\n"));
159 for (
int i = 0; i < itl; ++i)
161 send((boost::format(
"GET:tv%02d\n") % i).str());
162 tbl_vn_loaded_[i] =
false;
168 send(std::string(
"GET:tblh\n"));
172 send(std::string(
"PNG\n"));
176 send((boost::format(
"DAT:ax=%d\n") % static_cast<int>(
start)).str());
182 send((boost::format(
"DAT:ri=%d\n") % static_cast<int>(
start)).str());
186 send((boost::format(
"DAT:ro=%d\n") % static_cast<int>(
start)).str());
192 timer_.expires_from_now(timeout_);
196 boost::asio::async_read(socket_, buf_, boost::asio::transfer_at_least(4),
201 boost::system::error_code ec;
202 io_service_.poll(ec);
221 void send(
const std::string cmd)
224 boost::asio::async_write(socket_, boost::asio::buffer(*data),
262 template <
class DATA_TYPE>
276 const DATA_TYPE* data = boost::asio::buffer_cast<
const DATA_TYPE*>(buf_.data());
278 for (
int s = 0;
s < range_index.
nspots;
s++)
280 const double spot =
s + range_header.
spot;
281 const double h_rad = h_head + (h_tail - h_head) * tbl_h_[spot];
282 const double h_cos = cos(h_rad);
283 const double h_sin = sin(h_rad);
284 const vssp::XYZI dir(tbl_v_[tv][spot].
s, tbl_v_[tv][spot].c, h_sin, h_cos);
285 for (
int e = index[s]; e < index[s + 1]; e++)
286 points[i++] = dir * data[e];
290 void onRead(
const boost::system::error_code& error)
292 const auto time_read = boost::posix_time::microsec_clock::universal_time();
293 if (error == boost::asio::error::eof)
318 const uint8_t* data = boost::asio::buffer_cast<
const uint8_t*>(buf_.data());
319 for (
size_t i = 1; i < buf_.size() -
sizeof(uint32_t); i++)
321 const uint32_t* mark =
reinterpret_cast<const uint32_t*
>(data + i);
330 if (buf_.size() < header.
length)
336 for (
int i = 0; i < 1; ++i)
344 const std::string data(boost::asio::buffer_cast<const char*>(buf_.data()));
361 const std::string data(boost::asio::buffer_cast<const char*>(buf_.data()));
362 std::vector<std::string> lines;
363 boost::algorithm::split(lines, data, boost::algorithm::is_any_of(
"\n\r"));
364 if (lines.size() == 0)
367 if (lines[0].compare(0, 7,
"GET:tbl") == 0 ||
368 lines[0].compare(0, 6,
"GET:tv") == 0)
370 if (lines.size() < 2)
372 std::vector<std::string> cells;
373 boost::algorithm::split(cells, lines[1], boost::algorithm::is_any_of(
","));
375 if (lines[0].compare(
"GET:tblv") == 0 ||
376 lines[0].compare(0, 6,
"GET:tv") == 0)
379 if (lines[0].compare(0, 6,
"GET:tv") == 0)
380 tv = std::stoi(lines[0].substr(6));
382 if (tv >= static_cast<int>(tbl_v_.size()))
387 for (
auto& cell : cells)
389 const double rad(std::strtol(cell.c_str(),
nullptr, 16) * 2.0 * M_PI / 65535.0);
390 sincos(rad, &tbl_v[i].
s, &tbl_v[i].c);
394 tbl_vn_loaded_[tv] =
true;
396 bool load_finished =
true;
397 for (
auto loaded : tbl_vn_loaded_)
400 load_finished =
false;
402 tbl_v_loaded_ = load_finished;
404 else if (lines[0].compare(
"GET:tblh") == 0)
408 for (
auto& cell : cells)
410 tbl_h[i] = std::strtol(cell.c_str(),
nullptr, 16) / 65535.0;
414 tbl_h_loaded_ =
true;
436 if (!tbl_h_loaded_ || !tbl_v_loaded_ || !cb_point_)
462 std::memcpy(index.get(), boost::asio::buffer_cast<
const vssp::RangeIndex*>(buf_.data()),
463 sizeof(uint16_t) * (range_index.
nspots + 1));
464 buf_.consume(index_length);
465 length -= index_length;
474 success = rangeToXYZ<vssp::DataRangeIntensity>(
475 range_header, range_header_v2r1, range_index, index, points);
479 success = rangeToXYZ<vssp::DataRangeOnly>(
480 range_header, range_header_v2r1, range_index, index, points);
488 cb_point_(header, range_header, range_index, index, points, time_read);
501 for (
int i = 0; i < aux_header.
data_count; i++)
508 auxs[i][b] = aux_factor_[b] * data[offset++].
val;
510 buf_.consume(
sizeof(int32_t) * offset);
511 length -=
sizeof(int32_t) * offset;
514 cb_aux_(header, aux_header, auxs, time_read);
521 buf_.consume(length);
static const uint32_t TYPE_PNG
void setAutoReset(const bool enable)
static const uint32_t TYPE_SET
void onTimeout(const boost::system::error_code &error)
boost::asio::io_service & getIoService()
void onRead(const boost::system::error_code &error)
bool rangeToXYZ(const vssp::RangeHeader &range_header, const vssp::RangeHeaderV2R1 &range_header_v2r1, const vssp::RangeIndex &range_index, const boost::shared_array< const uint16_t > &index, const boost::shared_array< vssp::XYZI > &points)
void requestData(const bool intensity=1, const bool start=1)
void registerAuxCallback(decltype(cb_aux_) cb)
boost::asio::io_service io_service_
void requestVerticalTable(const int itl=1)
void setHorizontalInterlace(const int itl)
void setTimeout(const double to)
void setVerticalInterlace(const int itl)
boost::function< void(bool)> cb_connect_
static const uint32_t TYPE_AX
std_msgs::Header * header(M &m)
void registerCallback(decltype(cb_point_) cb)
void registerErrorCallback(decltype(cb_error_) cb)
boost::asio::ip::tcp::socket socket_
static const AuxFactorArray AUX_FACTOR_DEFAULT
void requestAuxData(const bool start=1)
boost::asio::deadline_timer timer_
AuxFactorArray aux_factor_
static const uint32_t TYPE_RI
void registerPingCallback(decltype(cb_ping_) cb)
void requestHorizontalTable()
boost::asio::streambuf buf_
void onTimeoutConnect(const boost::system::error_code &error)
static const uint32_t TYPE_ERR
static const uint32_t TYPE_DAT
boost::function< void(const vssp::Header &, const boost::posix_time::ptime &)> cb_ping_
void setInterlace(const int itl)
std::vector< bool > tbl_vn_loaded_
void onConnect(const boost::system::error_code &error)
boost::function< void(const vssp::Header &, const std::string &, const boost::posix_time::ptime &)> cb_error_
static const uint32_t TYPE_RO
void onSend(const boost::system::error_code &error, boost::shared_ptr< std::string > data)
static const uint32_t TYPE_ER
const RangeHeaderV2R1 RANGE_HEADER_V2R1_DEFAULT
void connect(const char *ip, const unsigned int port, decltype(cb_connect_) cb)
static const uint32_t STATUS_OK
static const uint32_t VSSP_MARK
static const uint32_t TYPE_VER
boost::posix_time::time_duration timeout_
boost::function< void(const vssp::Header &, const vssp::AuxHeader &, const boost::shared_array< vssp::Aux > &, const boost::posix_time::ptime &)> cb_aux_
std::vector< boost::shared_array< const TableSincos > > tbl_v_
boost::shared_array< const double > tbl_h_
boost::function< void(const vssp::Header &, const vssp::RangeHeader &, const vssp::RangeIndex &, const boost::shared_array< uint16_t > &, const boost::shared_array< vssp::XYZI > &, const boost::posix_time::ptime &)> cb_point_
static const uint32_t TYPE_GET
void send(const std::string cmd)