Goby3  3.1.5a
2024.05.23
ais.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_AIS_H
25 #define GOBY_MIDDLEWARE_AIS_H
26 
27 #include <numeric>
28 
29 #include <boost/algorithm/string.hpp>
30 #include <boost/circular_buffer.hpp>
31 #include <boost/units/cmath.hpp>
32 
34 #include "goby/util/geodesy.h"
36 
37 namespace goby
38 {
39 namespace middleware
40 {
42 {
43  public:
44  AISConverter(int mmsi, int history_length = 2) : mmsi_(mmsi), status_reports_(history_length)
45  {
46  if (history_length < 2)
47  throw(std::runtime_error("History length must be >= 2"));
48  }
49 
51  {
52  // reject duplications
53  if (status_reports_.empty() ||
54  status.SerializeAsString() != status_reports_.back().SerializeAsString())
55  status_reports_.push_back(status);
56  }
57 
58  bool empty() { return status_reports_.empty(); }
59 
60  std::pair<goby::util::ais::protobuf::Position, goby::util::ais::protobuf::Voyage>
62  {
63  using namespace boost::units;
64  using boost::units::quantity;
67 
68  if (status_reports_.size() == 0)
69  throw(std::runtime_error("No status reports"));
70 
71  const goby::middleware::frontseat::protobuf::NodeStatus& status = status_reports_.back();
72 
73  Position pos;
74  pos.set_message_id(18); // Class B position report
75  pos.set_mmsi(mmsi_);
77  if (status.global_fix().has_lat())
78  pos.set_lat_with_units(status.global_fix().lat_with_units());
79  if (status.global_fix().has_lon())
80  pos.set_lon_with_units(status.global_fix().lon_with_units());
81  if (status.pose().has_heading())
82  pos.set_true_heading_with_units(status.pose().heading_with_units());
83 
84  std::vector<quantity<si::velocity>> sogs;
85  std::vector<double> cogs_cos;
86  std::vector<double> cogs_sin;
87 
88  auto ninety_degrees(90. * boost::units::degree::degrees);
89 
90  // convert to local projection to perform cog and sog calculations
91  goby::util::UTMGeodesy geo({status_reports_.front().global_fix().lat_with_units(),
92  status_reports_.front().global_fix().lon_with_units()});
93  for (int i = 1, n = status_reports_.size(); i < n; ++i)
94  {
95  auto& status0 = status_reports_[i - 1];
96  auto& status1 = status_reports_[i];
97 
98  auto xy0 = geo.convert(
99  {status0.global_fix().lat_with_units(), status0.global_fix().lon_with_units()});
100  auto xy1 = geo.convert(
101  {status1.global_fix().lat_with_units(), status1.global_fix().lon_with_units()});
102 
103  auto dy = xy1.y - xy0.y;
104  auto dx = xy1.x - xy0.x;
105  auto dt = status1.time_with_units() - status0.time_with_units();
106 
107  decltype(ninety_degrees) cog_angle(boost::units::atan2(dy, dx));
108 
109  sogs.push_back(boost::units::sqrt(dy * dy + dx * dx) / dt);
110  cogs_cos.push_back(boost::units::cos(cog_angle));
111  cogs_sin.push_back(boost::units::sin(cog_angle));
112  }
113  auto sog_sum =
114  std::accumulate(sogs.begin(), sogs.end(), 0. * boost::units::si::meters_per_second);
115 
116  auto cogs_cos_mean =
117  std::accumulate(cogs_cos.begin(), cogs_cos.end(), 0.0) / cogs_cos.size();
118  auto cogs_sin_mean =
119  std::accumulate(cogs_sin.begin(), cogs_sin.end(), 0.0) / cogs_sin.size();
120 
121  if (status.speed().has_over_ground())
122  pos.set_speed_over_ground_with_units(status.speed().over_ground_with_units());
123  else
124  pos.set_speed_over_ground_with_units(sog_sum /
125  quantity<si::dimensionless>(sogs.size()));
126 
127  decltype(ninety_degrees) cog_heading_mean(
128  boost::units::atan2(quantity<si::dimensionless>(cogs_sin_mean),
129  quantity<si::dimensionless>(cogs_cos_mean)));
130 
131  pos.set_course_over_ground_with_units(ninety_degrees - cog_heading_mean);
132 
133  Voyage voy;
134  voy.set_message_id(24); // Class B voyage
135  voy.set_mmsi(mmsi_);
136  voy.set_name(boost::to_upper_copy(status.name()));
137  voy.set_type(Voyage::TYPE__OTHER);
138 
139  return std::make_pair(pos, voy);
140  }
141 
142  private:
143  int mmsi_;
144  boost::circular_buffer<goby::middleware::frontseat::protobuf::NodeStatus> status_reports_;
145 };
146 
147 } // namespace middleware
148 } // namespace goby
149 
150 #endif
goby::middleware::AISConverter::AISConverter
AISConverter(int mmsi, int history_length=2)
Definition: ais.h:44
goby::middleware::frontseat::protobuf::NodeStatus
Definition: frontseat_data.pb.h:227
goby
The global namespace for the Goby project.
Definition: acomms_constants.h:33
goby::middleware::AISConverter::latest_node_status_to_ais_b
std::pair< goby::util::ais::protobuf::Position, goby::util::ais::protobuf::Voyage > latest_node_status_to_ais_b()
Definition: ais.h:61
goby::util::ais::protobuf::AIS_STATUS__UNDER_WAY_USING_ENGINE
@ AIS_STATUS__UNDER_WAY_USING_ENGINE
Definition: ais.pb.h:268
frontseat_data.pb.h
goby::middleware::acomms::groups::status
constexpr Group status
Definition: groups.h:43
goby::middleware::AISConverter::add_status
void add_status(const goby::middleware::frontseat::protobuf::NodeStatus &status)
Definition: ais.h:50
goby::util::ais::protobuf::Position
Definition: ais.pb.h:302
goby::util::UTMGeodesy
Definition: geodesy.h:36
goby::middleware::AISConverter
Definition: ais.h:41
ais.pb.h
goby::middleware::AISConverter::empty
bool empty()
Definition: ais.h:58
goby::util::UTMGeodesy::convert
LatLonPoint convert(const XYPoint &utm) const
geodesy.h
boost::units
Definition: time.hpp:18
goby::util::ais::protobuf::Voyage
Definition: ais.pb.h:700