Debian Codegraph @

libs/hwdrivers/src/CNTRIPClient.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/comms/CClientTCPSocket.h>
13 #include <mrpt/comms/net_utils.h>
14 #include <mrpt/core/bits_math.h>
15 #include <mrpt/core/format.h>
16 #include <mrpt/hwdrivers/CNTRIPClient.h>
17 #include <mrpt/math/wrap2pi.h>
18 #include <mrpt/system/string_utils.h>
19
20 #include <chrono>
21 #include <cstring>
22 #include <iostream>
23 #include <thread>
24
25 using namespace mrpt;
26 using namespace mrpt::comms;
27 using namespace mrpt::system;
28 using namespace mrpt::hwdrivers;
29 using namespace mrpt::math;
30 using namespace std;
31
32 /* --------------------------------------------------------
33 CNTRIPClient
34 -------------------------------------------------------- */
35 CNTRIPClient::CNTRIPClient() : m_thread(), m_args()
36 {
37 m_thread = std::thread(&CNTRIPClient::private_ntrip_thread, this);
38 }
39
40 /* --------------------------------------------------------
41 ~CNTRIPClient
42 -------------------------------------------------------- */
43 CNTRIPClient::~CNTRIPClient()
44 {
45 this->close();
46 if (m_thread.joinable())
47 {
48 m_thread_exit = true;
49 m_thread.join();
50 }
51 }
52
53 /* --------------------------------------------------------
54 close
55 -------------------------------------------------------- */
56 void CNTRIPClient::close()
57 {
58 m_upload_data.clear();
59 if (!m_thread_do_process) return;
60 m_thread_do_process = false;
61 m_sem_sock_closed.get_future().wait_for(500ms);
62 }
63
64 /* --------------------------------------------------------
65 open
66 -------------------------------------------------------- */
67 bool CNTRIPClient::open(const NTRIPArgs& params, string& out_errmsg)
68 {
69 this->close();
70
71 if (params.mountpoint.empty())
72 {
73 out_errmsg = "MOUNTPOINT cannot be empty.";
74 return false;
75 }
76 if (params.server.empty())
77 {
78 out_errmsg = "Server address cannot be empty.";
79 return false;
80 }
81
82 // Try to open it:
83 m_waiting_answer_connection = true;
84 m_answer_connection = connError;
85 out_errmsg.clear();
86
87 m_args = params;
88 m_thread_do_process = true;
89
90 // Wait until the thread tell us the initial result...
91 if (m_sem_first_connect_done.get_future().wait_for(6s) == std::future_status::timeout)
92 {
93 out_errmsg = "Timeout waiting thread response";
94 return false;
95 }
96
97 switch (m_answer_connection)
98 {
99 case connOk:
100 return true;
101 case connError:
102 out_errmsg = mrpt::format("Error trying to connect to server '%s'", params.server.c_str());
103 return false;
104 case connUnauthorized:
105 out_errmsg = mrpt::format("Authentication failed for server '%s'", params.server.c_str());
106 return false;
107
108 default:
109 out_errmsg = "UNKNOWN m_answer_connection!!";
110 return false;
111 }
112 }
113
114 /* --------------------------------------------------------
115 THE WORKING THREAD
116 -------------------------------------------------------- */
117 void CNTRIPClient::private_ntrip_thread()
118 {
119 try
120 {
121 CClientTCPSocket my_sock;
122
123 bool last_thread_do_process = m_thread_do_process;
124
125 while (!m_thread_exit)
126 {
127 if (!m_thread_do_process)
128 {
129 if (my_sock.isConnected())
130 {
131 // Close connection:
132 try
133 {
134 my_sock.close();
135 }
136 catch (...)
137 {
138 }
139 }
140 else
141 {
142 // Nothing to be done... just wait
143 }
144
145 if (last_thread_do_process) // Let the waiting caller continue
146 // now.
147 m_sem_sock_closed.set_value();
148
149 last_thread_do_process = m_thread_do_process;
150 std::this_thread::sleep_for(100ms);
151 continue;
152 }
153
154 last_thread_do_process = m_thread_do_process;
155
156 // We have a mission to do here... is the channel already open??
157
158 if (!my_sock.isConnected())
159 {
160 TConnResult connect_res = connError;
161
162 std::vector<uint8_t> buf;
163 try
164 {
165 // Nope, it's the first time: get params and try open the
166 // connection:
167 stream_data.clear();
168
169 cout << mrpt::format(
170 "[CNTRIPClient] Trying to connect to %s:%i\n", m_args.server.c_str(), m_args.port);
171
172 my_sock.connect(m_args.server, m_args.port);
173 if (m_thread_exit) break;
174
175 // Prepare HTTP request:
176 // -------------------------------------------
177 string req = mrpt::format("GET /%s HTTP/1.0\r\n", m_args.mountpoint.c_str());
178
179 if (isalpha(m_args.server[0])) req += mrpt::format("Host: %s\r\n", m_args.server.c_str());
180
181 req += "User-Agent: NTRIP MRPT Library\r\n";
182 req += "Accept: */*\r\n";
183 req += "Connection: close\r\n";
184
185 // Implement HTTP Basic authentication:
186 // See:
187 // http://en.wikipedia.org/wiki/Basic_access_authentication
188 if (!m_args.user.empty())
189 {
190 string auth_str = m_args.user + string(":") + m_args.password;
191 std::vector<uint8_t> v(auth_str.size());
192 std::memcpy(&v[0], &auth_str[0], auth_str.size());
193
194 string encoded_str;
195 mrpt::system::encodeBase64(v, encoded_str);
196
197 req += "Authorization: Basic ";
198 req += encoded_str;
199 req += "\r\n";
200 }
201
202 // End:
203 req += "\r\n";
204 // cout << req;
205
206 // Send:
207 my_sock.sendString(req);
208
209 // Try to read the header of the response:
210 size_t to_read_now = 30;
211 buf.resize(to_read_now);
212 size_t len = my_sock.readAsync(&buf[0], to_read_now, 4000, 200);
213
214 buf.resize(len);
215
216 if ((len != 0) && my_sock.isConnected()) connect_res = connOk;
217 }
218 catch (std::exception&)
219 {
220 // cout << e.what() << endl;
221 connect_res = connError;
222 }
223
224 // We are not disconnected yet, it's a good thing... anyway,
225 // check the answer code:
226 if (!buf.empty())
227 {
228 string resp;
229 resp.resize(buf.size());
230 std::memcpy(&resp[0], &buf[0], buf.size());
231
232 if (resp.find(" 200 ") == string::npos)
233 {
234 // It's NOT a good response...
235 connect_res = connError;
236
237 // 401?
238 if (resp.find(" 401 ") != string::npos) connect_res = connUnauthorized;
239 }
240 }
241
242 // Signal my caller that the connection is established:
243 // ---------------------------------------------------------------
244 if (m_waiting_answer_connection)
245 {
246 m_waiting_answer_connection = false;
247
248 m_answer_connection = connect_res;
249 m_sem_first_connect_done.set_value();
250 }
251
252 if (connect_res != connOk) my_sock.close();
253 }
254
255 // Retry if it was a failed connection.
256 if (!my_sock.isConnected())
257 {
258 std::this_thread::sleep_for(500ms);
259 continue;
260 }
261
262 // Read data from the stream and accumulate it in a buffer:
263 // ----------------------------------------------------------------------
264 std::vector<uint8_t> buf;
265 size_t to_read_now = 1000;
266 buf.resize(to_read_now);
267 size_t len = my_sock.readAsync(&buf[0], to_read_now, 10, 5);
268
269 buf.resize(len);
270
271 if (my_sock.isConnected())
272 {
273 // Send data to main buffer:
274 if (stream_data.size() > 1024 * 8) stream_data.clear(); // It seems nobody's reading it...
275
276 stream_data.appendData(buf);
277 buf.clear();
278 }
279
280 // Send back data to the server, if so requested:
281 // ------------------------------------------
282 std::vector<uint8_t> upload_data;
283 m_upload_data.readAndClear(upload_data);
284 if (!upload_data.empty())
285 {
286 const size_t N = upload_data.size();
287 const size_t nWritten = my_sock.writeAsync(&upload_data[0], N, 1000);
288 if (nWritten != N)
289 cerr << "*ERROR*: Couldn't write back " << N << " bytes to NTRIP server!.\n";
290 }
291
292 std::this_thread::sleep_for(10ms);
293 } // end while
294
295 } // end try
296 catch (exception& e)
297 {
298 cerr << "[CNTRIPClient] Exception in working thread: " << endl << e.what() << endl;
299 }
300 catch (...)
301 {
302 cerr << "[CNTRIPClient] Runtime exception in working thread." << endl;
303 }
304
305 } // end working thread
306
307 /* --------------------------------------------------------
308 retrieveListOfMountpoints
309 -------------------------------------------------------- */
310 bool CNTRIPClient::retrieveListOfMountpoints(
311 TListMountPoints& out_list,
312 string& out_errmsg,
313 const string& server,
314 int port,
315 const string& auth_user,
316 const string& auth_pass)
317 {
318 string content;
319 net::HttpRequestOptions httpOptions;
320 net::HttpRequestOutput httpOut;
321
322 out_list.clear();
323
324 httpOptions.port = port;
325 httpOptions.auth_user = auth_user;
326 httpOptions.auth_pass = auth_pass;
327 httpOptions.timeout_ms = 6000;
328
329 net::http_errorcode ret =
330 net::http_get(string("http://") + server, content, httpOptions, httpOut);
331
332 out_errmsg = httpOut.errormsg;
333
334 // Parse contents:
335 if (ret != net::http_errorcode::Ok) return false;
336
337 std::stringstream ss(content);
338 string lin;
339 while (std::getline(ss, lin, '\n'))
340 {
341 if (lin.size() < 5) continue;
342 if (0 != ::strncmp("STR;", lin.c_str(), 4)) continue;
343
344 // ok, it's a stream:
345 deque<string> fields;
346 mrpt::system::tokenize(lin, ";", fields);
347
348 if (fields.size() < 13) continue;
349
350 TMountPoint mnt;
351
352 mnt.mountpoint_name = fields[1];
353 mnt.id = fields[2];
354 mnt.format = fields[3];
355 mnt.format_details = fields[4];
356 mnt.carrier = atoi(fields[5].c_str());
357 mnt.nav_system = fields[6];
358 mnt.network = fields[7];
359 mnt.country_code = fields[8];
360 mnt.latitude = atof(fields[9].c_str());
361 mnt.longitude = atof(fields[10].c_str());
362
363 // Longitude in range: -180,180
364 mnt.longitude = RAD2DEG(mrpt::math::wrapToPi(DEG2RAD(mnt.longitude)));
365
366 mnt.needs_nmea = atoi(fields[11].c_str()) != 0;
367 mnt.net_ref_stations = atoi(fields[12].c_str()) != 0;
368
369 if (fields.size() >= 19) mnt.extra_info = fields[18];
370
371 out_list.push_back(mnt);
372 }
373
374 return true;
375 }
376
377 /** Enqueues a string to be sent back to the NTRIP server (e.g. GGA frames) */
378 void CNTRIPClient::sendBackToServer(const std::string& data)
379 {
380 if (data.empty()) return;
381
382 std::vector<uint8_t> d(data.size());
383 std::memcpy(&d[0], &data[0], data.size());
384 m_upload_data.appendData(d);
385 }