Goby3  3.1.5a
2024.05.23
can.h
Go to the documentation of this file.
1 // Copyright 2020-2023:
2 // GobySoft, LLC (2013-)
3 // Community contributors (see AUTHORS file)
4 // File authors:
5 // Toby Schneider <toby@gobysoft.org>
6 // Shawn Dooley <shawn@shawndooley.net>
7 //
8 //
9 // This file is part of the Goby Underwater Autonomy Project Libraries
10 // ("The Goby Libraries").
11 //
12 // The Goby Libraries are free software: you can redistribute them and/or modify
13 // them under the terms of the GNU Lesser General Public License as published by
14 // the Free Software Foundation, either version 2.1 of the License, or
15 // (at your option) any later version.
16 //
17 // The Goby Libraries are distributed in the hope that they will be useful,
18 // but WITHOUT ANY WARRANTY; without even the implied warranty of
19 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
20 // GNU Lesser General Public License for more details.
21 //
22 // You should have received a copy of the GNU Lesser General Public License
23 // along with Goby. If not, see <http://www.gnu.org/licenses/>.
24 
25 #ifndef GOBY_MIDDLEWARE_IO_CAN_H
26 #define GOBY_MIDDLEWARE_IO_CAN_H
27 
28 #include <errno.h> // for errno
29 #include <linux/can.h> // for can_frame, socka...
30 #include <linux/can/raw.h> // for CAN_RAW_FILTER
31 #include <memory> // for make_shared, sha...
32 #include <net/if.h> // for ifreq, ifr_ifindex
33 #include <stdint.h> // for uint32_t, uint8_t
34 #include <string.h> // for strcpy, strerror
35 #include <string> // for string, operator+
36 #include <sys/ioctl.h> // for ioctl, SIOCGIFINDEX
37 #include <sys/socket.h> // for bind, setsockopt
38 #include <tuple> // for make_tuple, tuple
39 #include <vector> // for vector
40 
41 #include <boost/asio/buffer.hpp> // for buffer
42 #include <boost/asio/posix/stream_descriptor.hpp> // for stream_descriptor
43 #include <boost/asio/read.hpp> // for async_read
44 #include <boost/bind/bind.hpp> // for bind
45 #include <boost/core/ref.hpp> // for ref
46 
47 #include "goby/exception.h" // for Exception
48 #include "goby/middleware/io/detail/io_interface.h" // for PubSubLayer, IOT...
49 #include "goby/middleware/protobuf/can_config.pb.h" // for CanConfig, CanCo...
50 #include "goby/middleware/protobuf/io.pb.h" // for IOData
51 namespace goby
52 {
53 namespace middleware
54 {
55 class Group;
56 }
57 } // namespace goby
58 
59 namespace goby
60 {
61 namespace middleware
62 {
63 namespace io
64 {
65 inline std::uint32_t make_extended_format_can_id(std::uint32_t pgn, std::uint8_t priority,
66  std::uint8_t source = 0)
67 {
68  return (pgn & 0x1FFFF) << 8 | (priority & 0x7) << 26 | CAN_EFF_FLAG | (source & 0xFF);
69 }
70 
71 // tuple of pgn, priority, source
72 namespace can_id
73 {
74 constexpr int pgn_index{0};
75 constexpr int priority_index{1};
76 constexpr int source_index{2};
77 } // namespace can_id
78 
79 inline std::tuple<std::uint32_t, std::uint8_t, std::uint8_t>
80 parse_extended_format_can_id(std::uint32_t can_id)
81 {
82  return std::make_tuple((can_id >> 8) & 0x1FFFF, (can_id >> 26) & 0x7, can_id & 0xFF);
83 }
84 
85 template <const goby::middleware::Group& line_in_group,
86  const goby::middleware::Group& line_out_group,
87  // by default publish all incoming traffic to interprocess for logging
88  PubSubLayer publish_layer = PubSubLayer::INTERPROCESS,
89  // but only subscribe on interthread for outgoing traffic
90  PubSubLayer subscribe_layer = PubSubLayer::INTERTHREAD,
91  template <class> class ThreadType = goby::middleware::SimpleThread,
92  bool use_indexed_groups = false>
93 class CanThread
94  : public detail::IOThread<line_in_group, line_out_group, publish_layer, subscribe_layer,
95  goby::middleware::protobuf::CanConfig,
96  boost::asio::posix::stream_descriptor, ThreadType, use_indexed_groups>
97 {
98  using Base =
99  detail::IOThread<line_in_group, line_out_group, publish_layer, subscribe_layer,
101  boost::asio::posix::stream_descriptor, ThreadType, use_indexed_groups>;
102 
103  public:
108  : Base(config, index, std::string("can: ") + config.interface())
109  {
111  this->interthread().template publish<line_in_group>(ready);
112  }
113 
114  ~CanThread() override {}
115 
116  private:
117  void async_read() override;
118  void async_write(std::shared_ptr<const goby::middleware::protobuf::IOData> io_msg) override
119  {
120  detail::basic_async_write(this, io_msg);
121  }
122 
123  void open_socket() override;
124 
125  void data_rec(struct can_frame& receive_frame_, boost::asio::posix::stream_descriptor& stream);
126 
127  private:
128  struct can_frame receive_frame_;
129 };
130 } // namespace io
131 } // namespace middleware
132 } // namespace goby
133 
134 template <const goby::middleware::Group& line_in_group,
135  const goby::middleware::Group& line_out_group,
136  goby::middleware::io::PubSubLayer publish_layer,
137  goby::middleware::io::PubSubLayer subscribe_layer, template <class> class ThreadType,
138  bool use_indexed_groups>
139 void goby::middleware::io::CanThread<line_in_group, line_out_group, publish_layer, subscribe_layer,
140  ThreadType, use_indexed_groups>::open_socket()
141 {
142  int can_socket;
143 
144  struct sockaddr_can addr_
145  {
146  };
147  struct can_frame receive_frame_;
148  struct ifreq ifr_;
149  can_socket = socket(PF_CAN, SOCK_RAW, CAN_RAW);
150 
151  std::vector<struct can_filter> filters;
152 
153  for (auto x : this->cfg().filter())
154  {
155  auto id = x.can_id();
156  auto mask = x.has_can_mask_custom() ? x.can_mask_custom() : x.can_mask();
157 
158  filters.push_back({id, mask});
159  }
160 
161  for (std::uint32_t x : this->cfg().pgn_filter())
162  {
163  constexpr std::uint32_t one_byte = 8; // bits
164  auto id = x << one_byte;
165  constexpr auto mask = protobuf::CanConfig::CanFilter::PGNOnly; // PGN mask
166  filters.push_back({id, mask});
167  }
168 
169  if (filters.size())
170  {
171  setsockopt(can_socket, SOL_CAN_RAW, CAN_RAW_FILTER, filters.data(),
172  sizeof(can_filter) * filters.size());
173  }
174  std::strcpy(ifr_.ifr_name, this->cfg().interface().c_str());
175 
176  ioctl(can_socket, SIOCGIFINDEX, &ifr_);
177 
178  addr_.can_family = AF_CAN;
179  addr_.can_ifindex = ifr_.ifr_ifindex;
180  if (bind(can_socket, (struct sockaddr*)&addr_, sizeof(addr_)) < 0)
181  throw(goby::Exception(std::string("Error in socket bind to interface ") +
182  this->cfg().interface() + ": " + std::strerror(errno)));
183 
184  this->mutable_socket().assign(can_socket);
185 
186  this->interthread().template subscribe<line_out_group, can_frame>(
187  [this](const can_frame& frame)
188  {
189  auto io_msg = std::make_shared<goby::middleware::protobuf::IOData>();
190  std::string& bytes = *io_msg->mutable_data();
191 
192  const int frame_size = sizeof(can_frame);
193 
194  for (int i = 0; i < frame_size; ++i)
195  {
196  bytes += *(reinterpret_cast<const char*>(&frame) + i);
197  }
198  this->write(io_msg);
199  });
200 }
201 
202 template <const goby::middleware::Group& line_in_group,
203  const goby::middleware::Group& line_out_group,
204  goby::middleware::io::PubSubLayer publish_layer,
205  goby::middleware::io::PubSubLayer subscribe_layer, template <class> class ThreadType,
206  bool use_indexed_groups>
207 void goby::middleware::io::CanThread<line_in_group, line_out_group, publish_layer, subscribe_layer,
208  ThreadType, use_indexed_groups>::async_read()
209 {
210  boost::asio::async_read(this->mutable_socket(),
211  boost::asio::buffer(&receive_frame_, sizeof(receive_frame_)),
212  boost::bind(&CanThread::data_rec, this, boost::ref(receive_frame_),
213  boost::ref(this->mutable_socket())));
214 }
215 
216 template <const goby::middleware::Group& line_in_group,
217  const goby::middleware::Group& line_out_group,
218  goby::middleware::io::PubSubLayer publish_layer,
219  goby::middleware::io::PubSubLayer subscribe_layer, template <class> class ThreadType,
220  bool use_indexed_groups>
222  line_in_group, line_out_group, publish_layer, subscribe_layer, ThreadType,
223  use_indexed_groups>::data_rec(struct can_frame& receive_frame_,
224  boost::asio::posix::stream_descriptor& stream)
225 {
226  // Within a process raw can frames are probably what we are looking for.
227  this->interthread().template publish<line_in_group>(receive_frame_);
228 
229  std::string bytes;
230  const int frame_size = sizeof(can_frame);
231 
232  for (int i = 0; i < frame_size; ++i)
233  {
234  bytes += *(reinterpret_cast<char*>(&receive_frame_) + i);
235  }
236 
237  this->handle_read_success(bytes.size(), bytes);
238 
239  boost::asio::async_read(
240  stream, boost::asio::buffer(&receive_frame_, sizeof(receive_frame_)),
241  boost::bind(&CanThread::data_rec, this, boost::ref(receive_frame_), boost::ref(stream)));
242 }
243 
244 #endif
io.pb.h
goby
The global namespace for the Goby project.
Definition: acomms_constants.h:33
goby::middleware::io::CanThread::~CanThread
~CanThread() override
Definition: can.h:114
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
can_config.pb.h
goby::middleware::Thread< goby::middleware::protobuf::CanConfig, InterVehicleForwarder< InterProcessForwarder< InterThreadTransporter > > >::index
int index() const
Definition: thread.h:145
goby::middleware::SimpleThread
Implements Thread for a three layer middleware setup ([ intervehicle [ interprocess [ interthread ] ]...
Definition: simple_thread.h:43
goby::middleware::io::PubSubLayer
PubSubLayer
Definition: io_transporters.h:38
goby::middleware::io::can_id::pgn_index
constexpr int pgn_index
Definition: can.h:74
goby::middleware::io::CanThread
Definition: can.h:93
goby::middleware::io::CanThread::CanThread
CanThread(const goby::middleware::protobuf::CanConfig &config, int index=-1)
Constructs the thread.
Definition: can.h:107
goby::middleware::io::parse_extended_format_can_id
std::tuple< std::uint32_t, std::uint8_t, std::uint8_t > parse_extended_format_can_id(std::uint32_t can_id)
Definition: can.h:80
goby::middleware::io::can_id::priority_index
constexpr int priority_index
Definition: can.h:75
goby::middleware::Group
Class for grouping publications in the Goby middleware. Analogous to "topics" in ROS,...
Definition: group.h:59
goby::acomms::bind
void bind(ModemDriverBase &driver, QueueManager &queue_manager)
binds the driver link-layer callbacks to the QueueManager
Definition: bind.h:45
goby::middleware::SimpleThread< goby::middleware::protobuf::CanConfig >::interthread
InterThreadTransporter & interthread()
Access the transporter on the interthread layer (this is the innermost transporter)
Definition: simple_thread.h:96
goby::middleware::io::detail::basic_async_write
void basic_async_write(IOThreadImplementation *this_thread, std::shared_ptr< const goby::middleware::protobuf::IOData > io_msg)
Definition: io_interface.h:261
goby::middleware::io::can_id::source_index
constexpr int source_index
Definition: can.h:76
io_interface.h
goby::middleware::io::detail::IOThread
Definition: io_interface.h:79
goby::middleware::io::PubSubLayer::INTERPROCESS
@ INTERPROCESS
goby::Exception
simple exception class for goby applications
Definition: exception.h:34
exception.h
goby::middleware::io::ThreadState::SUBSCRIPTIONS_COMPLETE
@ SUBSCRIPTIONS_COMPLETE
goby::middleware::protobuf::CanConfig
Definition: can_config.pb.h:277
goby::middleware::io::make_extended_format_can_id
std::uint32_t make_extended_format_can_id(std::uint32_t pgn, std::uint8_t priority, std::uint8_t source=0)
Definition: can.h:65
goby::middleware::io::PubSubLayer::INTERTHREAD
@ INTERTHREAD