Goby3  3.1.5a
2024.05.23
common.h
Go to the documentation of this file.
1 // Copyright 2019-2021:
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_MAVLINK_COMMON_H
25 #define GOBY_MIDDLEWARE_IO_MAVLINK_COMMON_H
26 
27 #include <cstdint> // for uint8_t
28 #include <memory> // for shared_ptr, __sh...
29 #include <sstream> // for basic_ostream<>:...
30 #include <string> // for string
31 #include <vector> // for vector
32 
33 #include <mavlink/v2.0/common/common.hpp>
34 
35 #include "goby/exception.h" // for Exception
36 #include "goby/middleware/io/detail/io_interface.h" // for PubSubLayer, Pub...
37 #include "goby/middleware/marshalling/interface.h" // for MarshallingScheme
38 #include "goby/middleware/marshalling/mavlink.h" // for SerializerParser...
39 #include "goby/middleware/protobuf/io.pb.h" // for IOData
40 #include "goby/util/debug_logger/flex_ostream.h" // for operator<<, Flex...
41 
42 namespace goby
43 {
44 namespace middleware
45 {
46 class Group;
47 }
48 } // namespace goby
49 
50 namespace goby
51 {
52 namespace middleware
53 {
54 namespace io
55 {
56 template <const goby::middleware::Group& line_in_group,
57  const goby::middleware::Group& line_out_group, PubSubLayer publish_layer,
58  PubSubLayer subscribe_layer, typename IOThreadBase, typename IOConfig>
60 {
61  public:
63  {
64  // subscribe directly to mavlink_message_t as well
65  if (subscribe_layer == PubSubLayer::INTERPROCESS)
66  {
67  auto msg_out_callback = [this](std::shared_ptr<const mavlink::mavlink_message_t> msg,
68  const std::string& type) {
70  goby::glog << "writing msg [sysid: " << static_cast<int>(msg->sysid)
71  << ", compid: " << static_cast<int>(msg->compid)
72  << "] of msgid: " << static_cast<int>(msg->msgid) << std::endl;
73  auto io_msg = std::make_shared<goby::middleware::protobuf::IOData>();
74  auto data = SerializerParserHelper<mavlink::mavlink_message_t,
75  MarshallingScheme::MAVLINK>::serialize(*msg);
76  io_msg->set_data(&data[0], data.size());
77  this->write(io_msg);
78  };
79 
80  this->interprocess()
81  .template subscribe_type_regex<line_out_group, mavlink::mavlink_message_t>(
82  msg_out_callback);
83  }
84  }
85 
87 
88  protected:
89  void try_parse(std::size_t bytes_transferred);
90  std::array<char, MAVLINK_MAX_PACKET_LEN>& buffer() { return buffer_; }
91 
92  private:
93  void clear_buffers()
94  {
95  msg_ = std::make_shared<mavlink::mavlink_message_t>();
96  status_ = {};
97  msg_buffer_ = {};
98  status_buffer_ = {};
99  }
100 
101  private:
102  std::array<char, MAVLINK_MAX_PACKET_LEN> buffer_;
103  std::shared_ptr<mavlink::mavlink_message_t> msg_ =
104  std::make_shared<mavlink::mavlink_message_t>();
105  mavlink::mavlink_message_t msg_buffer_{};
106  mavlink::mavlink_status_t status_{}, status_buffer_{};
107 };
108 } // namespace io
109 } // namespace middleware
110 } // namespace goby
111 
112 template <
113  const goby::middleware::Group& line_in_group, const goby::middleware::Group& line_out_group,
114  goby::middleware::io::PubSubLayer publish_layer,
115  goby::middleware::io::PubSubLayer subscribe_layer, typename IOThreadBase, typename IOConfig>
116 void goby::middleware::io::IOThreadMAVLink<line_in_group, line_out_group, publish_layer,
117  subscribe_layer, IOThreadBase,
118  IOConfig>::try_parse(std::size_t bytes_transferred)
119 {
120  auto bytes_begin = buffer_.begin(), bytes_end = buffer_.begin() + bytes_transferred;
121  for (auto c = bytes_begin; c != bytes_end; ++c)
122  {
123  try
124  {
125  auto res = mavlink::mavlink_frame_char_buffer(&msg_buffer_, &status_buffer_, *c,
126  msg_.get(), &status_);
127  switch (res)
128  {
129  case mavlink::MAVLINK_FRAMING_BAD_CRC:
130  if (status_.parse_state != mavlink::MAVLINK_PARSE_STATE_IDLE)
131  {
132  break; // keep parsing
133  }
134  else if (mavlink::mavlink_get_msg_entry(msg_->msgid) == nullptr)
135  {
136  goby::glog.is_debug3() &&
137  goby::glog << "BAD CRC decoding MAVLink msg, but "
138  "forwarding because we don't know this msgid"
139  << std::endl;
140  // forward anyway as it might be a msgid we don't know
141  // so flow through here (no break) is intentional!!
142  }
143  else
144  {
145  goby::glog.is_warn() && goby::glog << "BAD CRC decoding MAVLink msg"
146  << std::endl;
147  break;
148  }
149 
150  case mavlink::MAVLINK_FRAMING_OK:
151  {
152  goby::glog.is_debug3() && goby::glog << "Parsed message of id: " << msg_->msgid
153  << std::endl;
154 
155  this->publish_in(msg_);
156 
157  std::array<uint8_t, MAVLINK_MAX_PACKET_LEN> buffer;
158  auto length = mavlink::mavlink_msg_to_send_buffer(&buffer[0], msg_.get());
159  std::string bytes(buffer.begin(), buffer.begin() + length);
160  this->handle_read_success(length, bytes);
161  break;
162  }
163 
164  case mavlink::MAVLINK_FRAMING_INCOMPLETE: break;
165 
166  case mavlink::MAVLINK_FRAMING_BAD_SIGNATURE:
167  goby::glog.is_warn() && goby::glog << "BAD SIGNATURE decoding MAVLink msg"
168  << std::endl;
169  break;
170  default:
171  goby::glog.is_warn() && goby::glog << "Unknown value " << res
172  << " returned while decoding MAVLink msg"
173  << std::endl;
174  break;
175  }
176  }
177  catch (goby::Exception& e)
178  {
179  goby::glog.is_warn() && goby::glog << "Exception decoding MAVLink msg: " << e.what()
180  << std::endl;
181  clear_buffers();
182  }
183  }
184 }
185 
186 #endif
io.pb.h
interface.h
goby
The global namespace for the Goby project.
Definition: acomms_constants.h:33
goby::middleware::SerializerParserHelper
Class for parsing and serializing a given marshalling scheme. Must be specialized for a particular sc...
Definition: interface.h:97
goby::util::FlexOstream::is_warn
bool is_warn()
Definition: flex_ostream.h:82
goby::util::FlexOstream::is_debug3
bool is_debug3()
Definition: flex_ostream.h:86
goby::acomms::abc::protobuf::config
extern ::google::protobuf::internal::ExtensionIdentifier< ::goby::acomms::protobuf::DriverConfig, ::google::protobuf::internal::MessageTypeTraits< ::goby::acomms::abc::protobuf::Config >, 11, false > config
Definition: abc_driver.pb.h:203
goby::util::e
constexpr T e
Definition: constants.h:35
goby::util::FlexOstream::is_debug2
bool is_debug2()
Definition: flex_ostream.h:85
goby::middleware::io::PubSubLayer
PubSubLayer
Definition: io_transporters.h:38
jwt::json::type
type
Generic JSON types used in JWTs.
Definition: jwt.h:2071
flex_ostream.h
goby::msg
extern ::google::protobuf::internal::ExtensionIdentifier< ::google::protobuf::MessageOptions, ::google::protobuf::internal::MessageTypeTraits< ::goby::GobyMessageOptions >, 11, false > msg
Definition: option_extensions.pb.h:1327
goby::middleware::Group
Class for grouping publications in the Goby middleware. Analogous to "topics" in ROS,...
Definition: group.h:59
IOThreadBase
io_interface.h
goby::middleware::io::PubSubLayer::INTERPROCESS
@ INTERPROCESS
goby::Exception
simple exception class for goby applications
Definition: exception.h:34
exception.h
goby::glog
util::FlexOstream glog
Access the Goby logger through this object.
goby::middleware::MarshallingScheme::MAVLINK
@ MAVLINK
Definition: interface.h:58