Debian Codegraph @

libs/hwdrivers/src/CNTRIPEmitter.cpp

raw
1 /* +------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | https://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2026, Individual contributors, see AUTHORS file |
6 | See: https://www.mrpt.org/Authors - All rights reserved. |
7 | Released under BSD License. See: https://www.mrpt.org/License |
8 +------------------------------------------------------------------------+ */
9
10 #include "hwdrivers-precomp.h" // Precompiled headers
11 //
12 #include <mrpt/hwdrivers/CNTRIPEmitter.h>
13 #include <mrpt/system/filesystem.h>
14 #include <mrpt/system/string_utils.h>
15
16 #include <chrono>
17 #include <iostream>
18 #include <thread>
19
20 IMPLEMENTS_GENERIC_SENSOR(CNTRIPEmitter, mrpt::hwdrivers)
21
22 using namespace std;
23 using namespace mrpt;
24 using namespace mrpt::obs;
25 using namespace mrpt::hwdrivers;
26
27 /*-------------------------------------------------------------
28 CNTRIPEmitter
29 -------------------------------------------------------------*/
30 CNTRIPEmitter::CNTRIPEmitter() : m_client(), m_com_port("") {}
31 /*-------------------------------------------------------------
32 ~CNTRIPEmitter
33 -------------------------------------------------------------*/
34 CNTRIPEmitter::~CNTRIPEmitter()
35 {
36 m_client.close();
37 if (m_out_COM.isOpen()) m_out_COM.close();
38 if (m_raw_output_file_stream.is_open()) m_raw_output_file_stream.close();
39 }
40
41 /*-------------------------------------------------------------
42 doProcess
43 -------------------------------------------------------------*/
44 void CNTRIPEmitter::doProcess()
45 {
46 std::vector<uint8_t> buf;
47 m_client.stream_data.readAndClear(buf);
48
49 if (!buf.empty())
50 {
51 if (m_verbose)
52 {
53 const double At = m_rate_timer.Tac();
54 m_rate_count += buf.size();
55 if (At > 5.0)
56 {
57 const double estim_rate_Bps = m_rate_count / At;
58 cout << mrpt::format(
59 "[NTRIP %s] Rate: %.02f B/s\n",
60 mrpt::system::timeLocalToString(mrpt::Clock::now()).c_str(), estim_rate_Bps);
61 m_rate_timer.Tic();
62 m_rate_count = 0;
63 }
64
65 cout << mrpt::format(
66 "[NTRIP %s] RX (%u bytes)\n", mrpt::system::timeLocalToString(mrpt::Clock::now()).c_str(),
67 (unsigned int)buf.size());
68 }
69 if (m_out_COM.isOpen())
70 {
71 // Send through the serial port:
72 cout << mrpt::format(
73 "[NTRIP %s] RX: %u bytes\n", mrpt::system::timeLocalToString(mrpt::Clock::now()).c_str(),
74 (unsigned)buf.size());
75 m_out_COM.Write(&buf[0], buf.size());
76 }
77
78 if (m_raw_output_file_stream.is_open())
79 {
80 m_raw_output_file_stream.write(reinterpret_cast<const char*>(&buf[0]), buf.size());
81 }
82 }
83
84 // Try to read a msg from the receiver -> NTRIP caster
85 if (m_transmit_to_server && m_out_COM.isOpen())
86 {
87 char rxbuf[50];
88 const size_t nReadActual = m_out_COM.Read(rxbuf, sizeof(rxbuf) - 1);
89 if (nReadActual)
90 {
91 rxbuf[nReadActual] = 0;
92 if (m_verbose)
93 cout << mrpt::format(
94 "[NTRIP %s] TX (%u bytes)\n",
95 mrpt::system::timeLocalToString(mrpt::Clock::now()).c_str(), (unsigned int)nReadActual);
96 }
97 }
98
99 std::this_thread::sleep_for(1ms);
100 }
101
102 /*-------------------------------------------------------------
103 initialize
104 -------------------------------------------------------------*/
105 void CNTRIPEmitter::initialize()
106 {
107 if (m_out_COM.isOpen()) m_out_COM.close();
108
109 if (!m_com_port.empty())
110 {
111 cout << mrpt::format("[NTRIP] Opening %s...\n", m_com_port.c_str());
112 m_out_COM.open(m_com_port);
113 m_out_COM.setConfig(m_com_bauds);
114 m_out_COM.setTimeouts(0, 0, 10, 0, 1);
115 m_out_COM.purgeBuffers();
116 cout << mrpt::format("[NTRIP] Open %s Ok.\n", m_com_port.c_str());
117 }
118
119 if (m_raw_output_file_stream.is_open())
120 {
121 m_raw_output_file_stream.close();
122 }
123
124 if (!m_raw_output_file_prefix.empty())
125 {
126 const string fil = mrpt::system::fileNameStripInvalidChars(
127 m_raw_output_file_prefix + mrpt::system::dateTimeLocalToString(mrpt::Clock::now()) +
128 string(".bin"));
129 m_raw_output_file_stream.open(fil.c_str(), std::ofstream::out | std::ofstream::binary);
130 if (!m_raw_output_file_stream.is_open())
131 THROW_EXCEPTION_FMT("Error opening output raw file: `%s`", fil.c_str());
132 }
133
134 string errstr;
135 if (!m_client.open(m_ntrip_args, errstr))
136 THROW_EXCEPTION_FMT("ERROR trying to connect to NTRIP caster: %s", errstr.c_str());
137 }
138
139 /* -----------------------------------------------------
140 loadConfig_sensorSpecific
141 ----------------------------------------------------- */
142 void CNTRIPEmitter::loadConfig_sensorSpecific(
143 const mrpt::config::CConfigFileBase& c, const std::string& s)
144 {
145 #ifdef _WIN32
146 m_com_port = c.read_string(s, "COM_port_WIN", "");
147 #else
148 m_com_port = c.read_string(s, "COM_port_LIN", "");
149 #endif
150
151 m_raw_output_file_prefix = c.read_string(s, "raw_output_file_prefix", "");
152
153 ASSERTMSG_(
154 !m_raw_output_file_prefix.empty() || !m_com_port.empty(),
155 "At least one of either raw file output or serial COM file must be "
156 "specified in configuration file!");
157
158 if (!m_com_port.empty())
159 {
160 m_com_bauds = c.read_int(s, "baudRate", m_com_bauds, true);
161 }
162
163 m_transmit_to_server = c.read_bool(s, "transmit_to_server", m_transmit_to_server);
164
165 m_ntrip_args.mountpoint = mrpt::system::trim(c.read_string(s, "mountpoint", "", true));
166 m_ntrip_args.server = mrpt::system::trim(c.read_string(s, "server", "", true));
167 m_ntrip_args.port = c.read_int(s, "port", 2101, true);
168
169 m_ntrip_args.user = mrpt::system::trim(c.read_string(s, "user", ""));
170 m_ntrip_args.password = mrpt::system::trim(c.read_string(s, "password", ""));
171 }