Goby3 3.2.3
2025.05.13
Loading...
Searching...
No Matches
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
37namespace goby
38{
39namespace 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
std::pair< goby::util::ais::protobuf::Position, goby::util::ais::protobuf::Voyage > latest_node_status_to_ais_b()
Definition ais.h:61
AISConverter(int mmsi, int history_length=2)
Definition ais.h:44
void add_status(const goby::middleware::frontseat::protobuf::NodeStatus &status)
Definition ais.h:50
The global namespace for the Goby project.