Go to the documentation of this file.
55 m_raw_dump_file_prefix(),
58 m_JAVAD_rtk_src_port(),
59 m_JAVAD_rtk_format(
"cmr")
69 const std::string& iniSection)
72 iniSection,
"parser",
m_parser,
false );
101 for (
int i = 1;
true; i++)
104 iniSection,
mrpt::format(
"setup_cmd%i", i), std::string());
106 if (sLine.empty())
break;
111 for (
int i = 1;
true; i++)
114 iniSection,
mrpt::format(
"shutdown_cmd%i", i), std::string());
116 if (sLine.empty())
break;
121 configSource.
read_float(iniSection,
"pose_x", 0),
122 configSource.
read_float(iniSection,
"pose_y", 0),
123 configSource.
read_float(iniSection,
"pose_z", 0),
150 const std::shared_ptr<mrpt::io::CStream>& external_stream,
151 const std::shared_ptr<std::mutex>& csOptionalExternalStream)
199 "Cannot change serial port name: an external stream has been "
200 "already bound manually.");
207 if (serial && serial->isOpen())
209 "Cannot change serial port name when it is already open");
228 m_data_stream = std::make_shared<mrpt::comms::CSerialPort>();
238 if (serial->isOpen())
return true;
241 cout <<
"[CGPSInterface] Opening " <<
m_COMname <<
" @ "
249 serial->setTimeouts(1, 0, 1, 1, 1);
259 catch (
const std::exception& e)
261 std::cerr <<
"[CGPSInterface::tryToOpenTheCOM] Error opening or "
262 "configuring serial port:"
294 const size_t to_read =
304 nRead = stream_tcpip->readAsync(buf, to_read, 100, 10);
306 else if (stream_serial)
308 nRead = stream_serial->Read(buf, to_read);
325 string sFilePostfix =
"_";
327 "%04u-%02u-%02u_%02uh%02um%02us", (
unsigned int)parts.
year,
328 (
unsigned int)parts.
month, (
unsigned int)parts.
day,
329 (
unsigned int)parts.
hour, (
unsigned int)parts.
minute,
330 (
unsigned int)parts.
second);
331 const string sFileName =
337 std::cout <<
"[CGPSInterface] Creating RAW dump file: `"
338 << sFileName <<
"`\n";
346 catch (std::exception&)
350 "[CGPSInterface::doProcess] Error reading stream of data: Closing "
355 stream_serial->close();
367 bool do_append_obs =
false;
377 cout <<
"[CGPSInterface] Initial timestamp: "
385 if (tdif >= 0 && tdif < 7500 )
390 cout <<
"[CGPSInterface] Warning: The initial timestamp "
391 "seems to be wrong! : "
399 if (time_diff < 0 || time_diff > 300)
406 cout <<
"[CGPSInterface ] Bad timestamp difference" << endl;
413 cout <<
"[CGPSInterface] WARNING: According to the "
414 "timestamps, we probably skipped one frame!"
472 throw std::runtime_error(
"[CGPSInterface] Unknown parser!");
480 if (!(*this.*parser_ptr)(min_bytes))
493 const std::list<CGPSInterface::ptr_parser_t>& all_parsers =
496 size_t global_min_bytes_max = 0;
499 bool all_parsers_want_to_skip =
true;
500 for (
auto all_parser : all_parsers)
502 parser_ptr = all_parser;
503 size_t this_parser_min_bytes;
504 if ((*this.*parser_ptr)(this_parser_min_bytes))
505 all_parsers_want_to_skip =
false;
525 const size_t len = strlen(str);
527 if (!stream_serial)
return;
533 written = stream_serial->Write(str, len);
536 if (
m_verbose) std::cout <<
"[CGPSInterface] TX: " << str;
539 throw std::runtime_error(
540 format(
"Error sending command: '%s'", str).c_str());
541 std::this_thread::sleep_for(5ms);
543 if (!waitForAnswer)
return;
545 std::this_thread::sleep_for(200ms);
550 while (bad_counter < 10)
555 written = stream_serial->Write(str, len);
556 nRead = stream_serial->Read(buf,
sizeof(buf));
559 if (
m_verbose) std::cout <<
"[CGPSInterface] RX: " << buf << std::endl;
562 throw std::runtime_error(
format(
563 "ERROR: Invalid response '%s' for command '%s'", buf, str));
565 if (nRead >= 3 && buf[0] ==
'R' && buf[1] ==
'E')
570 throw std::runtime_error(
571 format(
"ERROR: Invalid response '%s' for command '%s'", buf, str));
578 if (stream_serial && !stream_serial->isOpen())
return false;
584 cout <<
"[CGPSInterface] TX shutdown command: `" << m_shutdown_cmd
587 std::string sTx = m_shutdown_cmd;
599 std::this_thread::sleep_for(std::chrono::duration<double, std::milli>(
625 stream_serial->purgeBuffers();
635 cout <<
"[CGPSInterface] TX setup command: `" << m_setup_cmd
638 std::string sTx = m_setup_cmd;
646 catch (
const std::exception& e)
648 std::cerr <<
"[CGPSInterface::OnConnectionEstablished] Error "
649 "sending setup cmds: "
650 << e.what() << std::endl;
653 std::this_thread::sleep_for(std::chrono::duration<double, std::milli>(
656 std::this_thread::sleep_for(
669 std::this_thread::sleep_for(500ms);
671 std::this_thread::sleep_for(1000ms);
678 stream_serial->purgeBuffers();
702 "%%set,/par/cur/term/jps/0,{nscmd,37,n,\"\"}\r\n");
715 "%%%%set,/par/cur/term/jps/1,{cmr,-1,y,%s}\r\n",
720 "%%%%set,/par%s/imode,cmr\r\n",
728 "%%%%set,/par/cur/term/jps/1,{rtcm,-1,y,%s}\r\n",
733 "%%%%set,/par%s/imode,rtcm\r\n",
741 "%%%%set,/par/cur/term/jps/1,{rtcm3,-1,y,%s}\r\n",
746 "%%%%set,/par%s/imode,rtcm3\r\n",
752 cout <<
"Unknown RTK corrections format. Only supported: CMR, RTCM "
780 std::this_thread::sleep_for(500ms);
782 std::this_thread::sleep_for(1000ms);
789 stream_serial->purgeBuffers();
793 if (
m_verbose) cout <<
"[CGPSInterface] Configure RTK options" << endl;
797 const int elevation_mask = 5;
800 format(
"%%%%set,/par/lock/elm,%i\r\n", elevation_mask)
803 "%%set,/par/base/mode/,off\r\n");
813 "%%set,/par/pos/pd/qcheck,off\r\n");
815 "%%set,/par/pos/mode/cur,pd\r\n");
817 "%%set,/par/pos/pd/textr,10\r\n");
819 "%%set,/par/pos/pd/inuse,-1\r\n");
839 "%%%%set,/par%s/rate,%u\r\n",
847 "%%%%set,/par%s/imode,%s\r\n",
860 if (
m_verbose) cout <<
"[CGPSInterface] Using Advanced Input Mode";
862 if (
m_verbose) cout <<
"... done" << endl;
873 cout <<
"[CGPSInterface::OnConnectionEstablished] JAVAD/TopCon "
874 "commands sent successfully with AIM."
880 cout <<
"[CGPSInterface::OnConnectionEstablished] JAVAD/TopCon "
881 "commands sent successfully."
898 catch (
const std::exception& e)
900 std::cerr <<
"[CGPSInterface::sendCustomCommand] Error sending cmd: "
901 << e.what() << std::endl;
std::vector< std::string > m_setup_cmds
A TCP socket that can be connected to a TCP server, implementing MRPT's CStream interface for passing...
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
double timeDifference(const mrpt::system::TTimeStamp t_first, const mrpt::system::TTimeStamp t_later)
Returns the time difference from t1 to t2 (positive if t2 is posterior to t1), in seconds
static struct FontData data
mrpt::poses::CPose3D sensorPose
The sensor pose on the robot/vehicle.
bool m_data_stream_is_external
void flushParsedMessagesNow()
Queue out now the messages in m_just_parsed_messages, leaving it empty.
~CGPSInterface() override
Dtor.
size_t Write(const void *Buffer, size_t Count) override
Introduces a pure virtual method responsible for writing to the stream.
bool isGPS_connected()
Returns true if communications work, i.e.
bool open(const std::string &fileName, bool append=false)
Open the given file for write.
std::string getSerialPortName() const
Get the serial port to use (COM1, ttyUSB0, etc).
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
std::shared_ptr< std::mutex > m_data_stream_cs
double getSetupCommandsDelay() const
A class capable of reading GPS/GNSS/GNSS+IMU receiver data, from a serial port or from any input stre...
CGPSInterface()
Default ctor.
bool m_topcon_AIMConfigured
Indicates if the AIM has been properly set up.
bool strCmp(const std::string &s1, const std::string &s2)
Return true if the two strings are equal (case sensitive)
bool m_custom_cmds_append_CRLF
bool has_GGA_datum() const
true if the corresponding field exists in messages.
double read_double(const std::string §ion, const std::string &name, double defaultValue, bool failIfNotFound=false) const
void enableSetupCommandsAppendCRLF(const bool enable)
void setFromValues(const double x0, const double y0, const double z0, const double yaw=0, const double pitch=0, const double roll=0)
Set the pose from a 3D position (meters) and yaw/pitch/roll angles (radians) - This method recomputes...
void parseBuffer()
Process data in "m_buffer" to extract GPS messages, and remove them from the buffer.
std::string timeToString(const mrpt::system::TTimeStamp t)
Convert a timestamp into this textual form (UTC): HH:MM:SS.MMMMMM.
bool unsetJAVAD_AIM_mode()
Unset Advanced Input Mode for the primary port and use it only as a command port.
unsigned int m_JAVAD_rtk_src_baud
Only used when "m_JAVAD_rtk_src_port" is not empty.
bool setJAVAD_AIM_mode()
Set Advanced Input Mode for the primary port.
double second
Minute (0-59)
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
A communications serial port built as an implementation of a utils::CStream.
bool read_bool(const std::string §ion, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
Contains classes for various device interfaces.
Serial and networking devices and utilities.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
bool legacy_topcon_setup_commands()
message_list_t messages
The main piece of data in this class: a list of GNNS messages.
double m_topcon_data_period
The period in seconds which the data should be provided by the GPS.
bool tryToOpenTheCOM()
Returns true if the COM port is already open, or try to open it in other case.
#define THROW_EXCEPTION(msg)
#define ASSERT_(f)
Defines an assertion mechanism.
This namespace contains representation of robot actions and observations.
bool(CGPSInterface::*)(size_t &out_minimum_rx_buf_to_decide) ptr_parser_t
void setSerialPortName(const std::string &COM_port)
Set the serial port to use (COM1, ttyUSB0, etc).
mrpt::containers::circular_buffer< uint8_t > m_rx_buffer
Auxiliary buffer for readings.
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp.
void setParser(PARSERS parser)
Select the parser for incomming data, among the options enumerated in CGPSInterface.
const std::vector< std::string > & getShutdownCommands() const
std::string read_string(const std::string §ion, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
The parts of a date/time (it's like the standard 'tm' but with fractions of seconds).
std::string m_last_GGA
Used in getLastGGA()
static const TParsersRegistry & getInstance()
std::string fileNameStripInvalidChars(const std::string &filename, const char replacement_to_invalid_chars='_')
Replace invalid filename chars by underscores ('_') or any other user-given char.
std::vector< std::string > m_shutdown_cmds
std::string m_raw_dump_file_prefix
std::shared_ptr< mrpt::obs ::CObservationGPS > Ptr
int read_int(const std::string §ion, const std::string &name, int defaultValue, bool failIfNotFound=false) const
bool OnConnectionEstablished()
Implements custom messages to be sent to the GPS unit just after connection and before normal use.
bool m_topcon_useAIMMode
Use this mode for receive RTK corrections from a external source through the primary port.
void timestampToParts(TTimeStamp t, TTimeParts &p, bool localTime=false)
Gets the individual parts of a date/time (days, hours, minutes, seconds) - UTC time or local time.
void setSetupCommandsDelay(const double delay_secs)
PARSERS getParser() const
void bindStream(const std::shared_ptr< mrpt::io::CStream > &external_stream, const std::shared_ptr< std::mutex > &csOptionalExternalStream=std::shared_ptr< std::mutex >())
This enforces the use of a given user stream, instead of trying to open the serial port set in this c...
void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string &iniSection) override
See the class documentation at the top for expected parameters.
size_t size() const
Return the number of elements available for read ("pop") in the buffer (this is NOT the maximum size ...
void swap(CObservationGPS &o)
This class allows loading and storing values and vectors of different types from a configuration text...
void setSetupCommands(const std::vector< std::string > &cmds)
bool has_RMC_datum() const
true if the corresponding field exists in messages.
std::list< CGPSInterface::ptr_parser_t > all_parsers
double m_custom_cmds_delay
#define MRPT_LOG_ERROR(_STRING)
void JAVAD_sendMessage(const char *str, bool waitForAnswer=true)
Private auxiliary method.
mrpt::io::CFileOutputStream m_raw_output_file
bool OnConnectionShutdown()
Like OnConnectionEstablished() for sending optional shutdown commands.
poses::CPose3D m_sensorPose
bool fileOpenCorrectly() const
Returns true if the file was open without errors.
mrpt::Clock::time_point TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1,...
void appendObservation(const mrpt::serialization::CSerializable::Ptr &obj)
Like appendObservations() but for just one observation.
void keep_max(T &var, const K test_val)
If the second argument is above the first one, set the first argument to this higher value.
mrpt::obs::CObservationGPS m_just_parsed_messages
A private copy of the last received gps datum.
std::string m_sensorLabel
See CGenericSensor.
float read_float(const std::string §ion, const std::string &name, float defaultValue, bool failIfNotFound=false) const
std::shared_ptr< mrpt::io::CStream > m_data_stream
Typically a CSerialPort created by this class, but may be set externally.
ENUMTYPE read_enum(const std::string §ion, const std::string &name, const ENUMTYPE &defaultValue, bool failIfNotFound=false) const
Reads an "enum" value, where the value in the config file can be either a numerical value or the symb...
T pop()
Retrieve an element from the buffer.
constexpr double DEG2RAD(const double x)
Degrees to radians
bool implement_parser_NOVATEL_OEM6(size_t &out_minimum_rx_buf_to_decide)
void push_many(T *array_elements, size_t count)
Insert an array of elements in the buffer.
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
size_t available() const
The maximum number of elements that can be written ("push") without rising an overflow error.
Versatile class for consistent logging and management of output messages.
const std::vector< std::string > & getSetupCommands() const
bool m_sensorLabelAppendMsgType
bool sendCustomCommand(const void *data, const size_t datalen)
Send a custom data block to the GNSS device right now.
bool isEnabledSetupCommandsAppendCRLF() const
std::string m_JAVAD_rtk_src_port
If not empty, will send a cmd "set,/par/pos/pd/port,...".
void clear()
Empties this observation, clearing the container messages.
std::string getLastGGA(bool reset=true)
Gets the latest GGA command or an empty string if no newer GGA command was received since the last ca...
uint8_t minute
Hour (0-23)
std::string trim(const std::string &str)
Removes leading and trailing spaces.
bool implement_parser_NMEA(size_t &out_minimum_rx_buf_to_decide)
PARSERS
Read about parser selection in the documentation for CGPSInterface.
mrpt::system::TTimeStamp m_last_timestamp
int _strcmpi(const char *str1, const char *str2) noexcept
An OS-independent version of strcmpi.
std::string m_JAVAD_rtk_format
Only used when "m_JAVAD_rtk_src_port" is not empty: format of RTK corrections: "cmr",...
void setShutdownCommands(const std::vector< std::string > &cmds)
std::string std::string format(std::string_view fmt, ARGS &&... args)
void doProcess() override
This method will be invoked at a minimum rate of "process_rate" (Hz)
std::shared_ptr< std::mutex > m_data_stream_mine_cs
Page generated by Doxygen 1.8.18 for MRPT 2.0.4 at Thu Sep 24 07:14:18 UTC 2020 | |