Goby3  3.1.5a
2024.05.23
common.h
Go to the documentation of this file.
1 // Copyright 2022:
2 // GobySoft, LLC (2013-)
3 // Community contributors (see AUTHORS file)
4 // File authors:
5 // Toby Schneider <toby@gobysoft.org>
6 //
7 //
8 // This file is part of the Goby Underwater Autonomy Project Libraries
9 // ("The Goby Libraries").
10 //
11 // The Goby Libraries are free software: you can redistribute them and/or modify
12 // them under the terms of the GNU Lesser General Public License as published by
13 // the Free Software Foundation, either version 2.1 of the License, or
14 // (at your option) any later version.
15 //
16 // The Goby Libraries are distributed in the hope that they will be useful,
17 // but WITHOUT ANY WARRANTY; without even the implied warranty of
18 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19 // GNU Lesser General Public License for more details.
20 //
21 // You should have received a copy of the GNU Lesser General Public License
22 // along with Goby. If not, see <http://www.gnu.org/licenses/>.
23 
24 #ifndef GOBY_MIDDLEWARE_IO_COBS_COMMON_H
25 #define GOBY_MIDDLEWARE_IO_COBS_COMMON_H
26 
27 #include <memory>
28 
29 #include <boost/asio/read.hpp> // for async_read
30 #include <boost/asio/write.hpp> // for async_write
31 
33 #include "goby/util/binary.h"
34 #include "goby/util/debug_logger.h" // for glog
36 
37 namespace goby
38 {
39 namespace middleware
40 {
41 namespace io
42 {
43 template <class Thread>
44 void cobs_async_write(Thread* this_thread,
45  std::shared_ptr<const goby::middleware::protobuf::IOData> io_msg)
46 {
47  constexpr static char cobs_eol{0};
48 
49  // COBS worst case is 1 byte for every 254 bytes of input
50  auto input_size = io_msg->data().size();
51  auto output_size_max = input_size + (input_size / 254) + 1;
52  std::string cobs_encoded(output_size_max, '\0');
53 
54  auto cobs_size = cobs_encode(reinterpret_cast<const uint8_t*>(io_msg->data().data()),
55  input_size, reinterpret_cast<uint8_t*>(&cobs_encoded[0]));
56 
57  if (cobs_size)
58  {
59  cobs_encoded.resize(cobs_size);
60  cobs_encoded += cobs_eol;
61 
62  goby::glog.is_debug2() && goby::glog << group(this_thread->glog_group()) << "COBS ("
63  << cobs_encoded.size() << "B) <"
64  << " " << goby::util::hex_encode(cobs_encoded)
65  << std::endl;
66 
67  boost::asio::async_write(this_thread->mutable_socket(), boost::asio::buffer(cobs_encoded),
68  [this_thread, cobs_encoded](const boost::system::error_code& ec,
69  std::size_t bytes_transferred) {
70  if (!ec && bytes_transferred > 0)
71  {
72  this_thread->handle_write_success(bytes_transferred);
73  }
74  else
75  {
76  this_thread->handle_write_error(ec);
77  }
78  });
79  }
80  else
81  {
82  goby::glog.is_warn() && goby::glog << group(this_thread->glog_group())
83  << "Failed to encode COBS message: "
84  << goby::util::hex_encode(io_msg->data()) << std::endl;
85  this_thread->handle_write_error(boost::system::error_code());
86  }
87 }
88 
89 template <class Thread, class ThreadBase = Thread>
90 void cobs_async_read(Thread* this_thread,
91  std::shared_ptr<ThreadBase> self = std::shared_ptr<ThreadBase>())
92 {
93  constexpr static char cobs_eol{0};
94 
95  boost::asio::async_read_until(
96  this_thread->mutable_socket(), this_thread->buffer_, cobs_eol,
97  [this_thread, self](const boost::system::error_code& ec, std::size_t bytes_transferred) {
98  if (!ec && bytes_transferred > 0)
99  {
100  std::string bytes(bytes_transferred, '\0');
101  std::istream is(&this_thread->buffer_);
102  is.read(&bytes[0], bytes_transferred);
103 
104  goby::glog.is_debug2() && goby::glog << group(this_thread->glog_group()) << "COBS ("
105  << bytes.size() << "B) >"
106  << " " << goby::util::hex_encode(bytes)
107  << std::endl;
108 
109  auto io_msg = std::make_shared<goby::middleware::protobuf::IOData>();
110  auto& cobs_decoded = *io_msg->mutable_data();
111  cobs_decoded = std::string(bytes_transferred, '\0');
112 
113  int decoded_size =
114  cobs_decode(reinterpret_cast<const uint8_t*>(bytes.data()), bytes_transferred,
115  reinterpret_cast<uint8_t*>(&cobs_decoded[0]));
116  if (decoded_size)
117  {
118  // decoded size includes final 0 so remove last byte?
119  cobs_decoded.resize(decoded_size - 1);
120  this_thread->handle_read_success(bytes_transferred, io_msg);
121  this_thread->async_read();
122  }
123  else
124  {
125  goby::glog.is_warn() && goby::glog << group(this_thread->glog_group())
126  << "Failed to decode COBS message: "
127  << goby::util::hex_encode(bytes)
128  << std::endl;
129  this_thread->handle_read_error(ec);
130  }
131  }
132  else
133  {
134  this_thread->handle_read_error(ec);
135  }
136  });
137 }
138 
139 } // namespace io
140 } // namespace middleware
141 } // namespace goby
142 
143 #endif
io.pb.h
goby
The global namespace for the Goby project.
Definition: acomms_constants.h:33
goby::util::FlexOstream::is_warn
bool is_warn()
Definition: flex_ostream.h:82
group
goby::util::logger::GroupSetter group(std::string n)
Definition: logger_manipulators.h:134
goby::util::FlexOstream::is_debug2
bool is_debug2()
Definition: flex_ostream.h:85
goby::middleware::io::cobs_async_write
void cobs_async_write(Thread *this_thread, std::shared_ptr< const goby::middleware::protobuf::IOData > io_msg)
Definition: common.h:44
goby::middleware::Thread
Represents a thread of execution within the Goby middleware, interleaving periodic events (loop()) wi...
Definition: thread.h:60
goby::middleware::protobuf::IOData::data
const ::std::string & data() const
Definition: io.pb.h:2103
binary.h
debug_logger.h
cobs_encode
std::size_t cobs_encode(const Byte *input, std::size_t length, Byte *output)
Definition: cobs.h:18
cobs.h
goby::util::hex_encode
void hex_encode(const std::string &in, std::string *out, bool upper_case=false)
Encodes a (little-endian) hexadecimal string from a byte string. Index 0 of in is written to index 0 ...
Definition: binary.h:94
goby::glog
util::FlexOstream glog
Access the Goby logger through this object.
goby::middleware::io::cobs_async_read
void cobs_async_read(Thread *this_thread, std::shared_ptr< ThreadBase > self=std::shared_ptr< ThreadBase >())
Definition: common.h:90