| 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 |
} |