MRPT  1.9.9
RawlogGrabberApp.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, 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 "apps-precomp.h" // Precompiled headers
11 
14 #include <mrpt/core/round.h>
16 #include <mrpt/img/CImage.h>
24 #include <mrpt/obs/CSensoryFrame.h>
26 #include <mrpt/system/CRateTimer.h>
27 #include <mrpt/system/filesystem.h>
28 #include <mrpt/system/os.h>
29 #include <thread>
30 
31 using namespace mrpt::apps;
32 
34  : mrpt::system::COutputLogger("RawlogGrabberApp")
35 {
36 }
37 
38 void RawlogGrabberApp::initialize(int argc, const char** argv)
39 {
41 
42  if ((getenv("MRPT_HWDRIVERS_VERBOSE") != nullptr) &&
43  atoi(getenv("MRPT_HWDRIVERS_VERBOSE")) != 0)
44  {
46  }
47 
48  MRPT_LOG_INFO(" rawlog-grabber - Part of the MRPT");
50  " MRPT C++ Library: %s - Sources timestamp: %s\n",
53 
54  // Process arguments:
55  if (argc < 2)
56  THROW_EXCEPTION_FMT("Usage: %s <config_file.ini>\n\n", argv[0]);
57 
58  {
59  std::string INI_FILENAME(argv[1]);
60  ASSERT_FILE_EXISTS_(INI_FILENAME);
61  std::vector<std::string> cfgLines;
62  mrpt::io::loadTextFile(cfgLines, INI_FILENAME);
63  params.setContent(cfgLines);
64  }
65 
66  MRPT_END
67 }
68 
70 {
72 
73  using namespace mrpt;
74  using namespace mrpt::system;
75  using namespace mrpt::hwdrivers;
76  using namespace mrpt::config;
77  using namespace mrpt::serialization;
78  using namespace mrpt::img;
79  using namespace mrpt::obs;
80  using namespace mrpt::poses;
81  using namespace std;
82 
83  const std::string GLOBAL_SECT = "global";
84 
85  // ------------------------------------------
86  // Load config from file:
87  // ------------------------------------------
88  string rawlog_prefix = "dataset";
89  int time_between_launches = 300;
90  bool use_sensoryframes = false;
91  int GRABBER_PERIOD_MS = 1000;
92  int rawlog_GZ_compress_level = 1; // 0: No compress, 1-9: compress level
93 
94  MRPT_LOAD_CONFIG_VAR(rawlog_prefix, string, params, GLOBAL_SECT);
95  MRPT_LOAD_CONFIG_VAR(time_between_launches, int, params, GLOBAL_SECT);
96  MRPT_LOAD_CONFIG_VAR(SF_max_time_span, float, params, GLOBAL_SECT);
97  MRPT_LOAD_CONFIG_VAR(use_sensoryframes, bool, params, GLOBAL_SECT);
98  MRPT_LOAD_CONFIG_VAR(GRABBER_PERIOD_MS, int, params, GLOBAL_SECT);
99 
100  MRPT_LOAD_CONFIG_VAR(rawlog_GZ_compress_level, int, params, GLOBAL_SECT);
101 
102  // Build full rawlog file name:
103  string rawlog_postfix = "_";
104 
105  // rawlog_postfix += dateTimeToString( now() );
107  mrpt::system::timestampToParts(now(), parts, true);
108  rawlog_postfix += format(
109  "%04u-%02u-%02u_%02uh%02um%02us", (unsigned int)parts.year,
110  (unsigned int)parts.month, (unsigned int)parts.day,
111  (unsigned int)parts.hour, (unsigned int)parts.minute,
112  (unsigned int)parts.second);
113 
114  rawlog_postfix = mrpt::system::fileNameStripInvalidChars(rawlog_postfix);
115 
116  // Only set this if we want externally stored images:
118  rawlog_prefix +
119  fileNameStripInvalidChars(rawlog_postfix + string("_Images"));
120 
121  // Also, set the path in CImage to enable online visualization in a GUI
122  // window:
123  CImage::setImagesPathBase(m_rawlog_ext_imgs_dir);
124 
125  rawlog_postfix += string(".rawlog");
126  rawlog_postfix = fileNameStripInvalidChars(rawlog_postfix);
127 
128  rawlog_filename = rawlog_prefix + rawlog_postfix;
129 
130  MRPT_LOG_INFO_STREAM("Output rawlog filename: " << rawlog_filename);
131  MRPT_LOG_INFO_STREAM("External image storage: " << m_rawlog_ext_imgs_dir);
132 
133  // ----------------------------------------------
134  // Launch threads:
135  // ----------------------------------------------
136  std::vector<std::string> sections;
137  params.getAllSections(sections);
138 
139  vector<std::thread> lstThreads;
140 
141  for (auto& section : sections)
142  {
143  if (section == GLOBAL_SECT || section.empty() ||
144  params.read_bool(section, "rawlog-grabber-ignore", false, false))
145  continue; // This is not a sensor:
146 
147  lstThreads.emplace_back(&RawlogGrabberApp::SensorThread, this, section);
148 
149  std::this_thread::sleep_for(
150  std::chrono::milliseconds(time_between_launches));
151  }
152 
153  // ----------------------------------------------
154  // Run:
155  // ----------------------------------------------
157  auto out_arch_obj = archiveFrom(out_file);
158  m_out_arch_ptr = &out_arch_obj;
159 
160  out_file.open(rawlog_filename, rawlog_GZ_compress_level);
161 
162  CGenericSensor::TListObservations copy_of_m_global_list_obs;
163 
164  MRPT_LOG_INFO_STREAM("Press any key to exit program");
165 
166  mrpt::system::CTicTac run_timer;
167  run_timer.Tic();
168 
169  while (!os::kbhit() && !allThreadsMustExit)
170  {
171  // Check "run for X seconds" flag:
172  if (run_for_seconds > 0 && run_timer.Tac() > run_for_seconds) break;
173 
174  // See if we have observations and process them:
175  {
176  std::lock_guard<std::mutex> lock(cs_m_global_list_obs);
177  copy_of_m_global_list_obs.clear();
178 
179  if (!m_global_list_obs.empty())
180  {
181  auto itEnd = m_global_list_obs.begin();
182  std::advance(itEnd, m_global_list_obs.size() / 2);
183  copy_of_m_global_list_obs.insert(
184  m_global_list_obs.begin(), itEnd);
185  m_global_list_obs.erase(m_global_list_obs.begin(), itEnd);
186  }
187  } // End of cs lock
188 
189  if (use_sensoryframes)
190  {
191  process_observations_for_sf(copy_of_m_global_list_obs);
192  }
193  else
194  {
195  process_observations_for_nonsf(copy_of_m_global_list_obs);
196  }
197 
198  std::this_thread::sleep_for(
199  std::chrono::milliseconds(GRABBER_PERIOD_MS));
200  }
201 
202  if (allThreadsMustExit)
203  {
205  "[main thread] Ended due to other thread signal to exit "
206  "application.");
207  }
208 
209  // Flush file to disk:
210  out_file.close();
211 
212  // Wait all threads:
213  // ----------------------------
214  allThreadsMustExit = true;
215  std::this_thread::sleep_for(300ms);
216 
217  MRPT_LOG_INFO("Waiting for all threads to close...");
218 
219  for (auto& lstThread : lstThreads) lstThread.join();
220 
221  MRPT_END
222 }
223 
226 {
227  // Show GPS mode:
229 
230  static auto last_t = mrpt::Clock::now();
231  const auto t_now = mrpt::Clock::now();
232  if (mrpt::system::timeDifference(last_t, t_now) < 1.0) return;
233  last_t = t_now;
234 
235  if (auto gps = std::dynamic_pointer_cast<mrpt::obs::CObservationGPS>(o);
236  gps)
237  {
238  dump_GPS_mode_info(*gps);
239  }
240  else if (auto imu =
241  std::dynamic_pointer_cast<mrpt::obs::CObservationIMU>(o);
242  imu)
243  {
244  dump_IMU_info(*imu);
245  }
246 }
247 
249  const mrpt::obs::CSensoryFrame& sf) const
250 {
252 
253  // Show GPS mode:
255  std::size_t idx = 0;
256  do
257  {
259  if (gps) dump_GPS_mode_info(*gps);
260  } while (gps);
261 
262  // Show IMU angles:
264  if (imu) dump_IMU_info(*imu);
265 }
266 
268  const mrpt::obs::CObservationGPS& o) const
269 {
270  if (o.has_GGA_datum())
271  {
273 
274  const auto fq = static_cast<int>(
275  o.getMsgByClass<Message_NMEA_GGA>().fields.fix_quality);
277  " GPS mode: " << fq << " label: " << o.sensorLabel);
278  }
279  {
280  std::stringstream ss;
281  o.getDescriptionAsText(ss);
282  MRPT_LOG_DEBUG_STREAM(ss.str());
283  }
284 }
285 
287 {
288  // Show IMU angles:
290  " IMU angles (degrees): "
291  "(yaw,pitch,roll)=(%.06f, %.06f, %.06f)",
295 }
296 
297 // ------------------------------------------------------
298 // SensorThread
299 // ------------------------------------------------------
301 {
302  try
303  {
304  std::string driver_name =
305  params.read_string(sensor_label, "driver", "", true);
306 
307  auto sensor =
309 
310  if (!sensor)
311  throw std::runtime_error(
312  std::string("Class name not recognized: ") + driver_name);
313 
314  // Load common & sensor specific parameters:
315  sensor->loadConfig(params, sensor_label);
316 
318  "[thread_" << sensor_label << "] Starting at "
319  << sensor->getProcessRate() << " Hz");
320 
321  ASSERT_ABOVE_(sensor->getProcessRate(), 0);
322 
323  // For imaging sensors, set external storage directory:
324  sensor->setPathForExternalImages(m_rawlog_ext_imgs_dir);
325 
326  // Init device:
327  sensor->initialize();
328 
330  rate.setRate(sensor->getProcessRate());
331 
332  while (!allThreadsMustExit)
333  {
334  // Process
335  sensor->doProcess();
336 
337  // Get new observations
339  sensor->getObservations(lstObjs);
340 
341  {
342  std::lock_guard<std::mutex> lock(cs_m_global_list_obs);
343  m_global_list_obs.insert(lstObjs.begin(), lstObjs.end());
344  }
345 
346  lstObjs.clear();
347 
348  // wait for the process period:
349  rate.sleep();
350  }
351 
352  sensor.reset();
353 
354  MRPT_LOG_INFO_FMT("[thread_%s] Closing...", sensor_label.c_str());
355  }
356  catch (const std::exception& e)
357  {
359  {
361  "Exception in SensorThread:\n"
362  << mrpt::exception_to_str(e));
363  }
364  allThreadsMustExit = true;
365  }
366  catch (...)
367  {
369  {
370  MRPT_LOG_ERROR("Untyped exception in SensorThread.");
371  }
372  allThreadsMustExit = true;
373  }
374 }
375 
377  const RawlogGrabberApp::TListObservations& list_obs)
378 {
379  using namespace mrpt::obs;
380 
381  // -----------------------
382  // USE SENSORY-FRAMES
383  // -----------------------
384  for (auto it = list_obs.begin(); it != list_obs.end(); ++it)
385  {
386  // If we have an action, save the SF and start a new one:
387  if (IS_DERIVED(*it->second, CAction))
388  {
389  CAction::Ptr act = std::dynamic_pointer_cast<CAction>(it->second);
390 
391  (*m_out_arch_ptr) << m_curSF;
393  "Saved SF with " << m_curSF.size() << " objects.");
394  m_curSF.clear();
395 
396  CActionCollection acts;
397  acts.insert(*act);
398  act.reset();
399 
400  (*m_out_arch_ptr) << acts;
401  }
402  else if (IS_CLASS(*it->second, CObservationOdometry))
403  {
405  std::dynamic_pointer_cast<CObservationOdometry>(it->second);
406 
407  auto act = CActionRobotMovement2D::Create();
408  act->timestamp = odom->timestamp;
409 
410  // Compute the increment since the last reading:
412  static CObservationOdometry last_odo;
413  static bool last_odo_first = true;
414 
415  mrpt::poses::CPose2D odo_incr;
416  int64_t lticks_incr, rticks_incr;
417 
418  if (last_odo_first)
419  {
420  last_odo_first = false;
421  odo_incr = mrpt::poses::CPose2D(0, 0, 0);
422  lticks_incr = rticks_incr = 0;
423  }
424  else
425  {
426  odo_incr = odom->odometry - last_odo.odometry;
427  lticks_incr =
428  odom->encoderLeftTicks - last_odo.encoderLeftTicks;
429  rticks_incr =
430  odom->encoderRightTicks - last_odo.encoderRightTicks;
431 
432  last_odo = *odom;
433  }
434 
435  // Save as action & dump to file:
436  act->computeFromOdometry(odo_incr, odomOpts);
437 
438  act->hasEncodersInfo = true;
439  act->encoderLeftTicks = lticks_incr;
440  act->encoderRightTicks = rticks_incr;
441 
442  act->hasVelocities = true;
443  act->velocityLocal = odom->velocityLocal;
444 
445  (*m_out_arch_ptr) << m_curSF;
447 
449  "Saved SF with " << m_curSF.size() << " objects.");
450  m_curSF.clear();
451 
452  CActionCollection acts;
453  acts.insert(*act);
454  act.reset();
455 
456  (*m_out_arch_ptr) << acts;
458  }
459  else if (IS_DERIVED(*it->second, CObservation))
460  {
461  CObservation::Ptr obs =
462  std::dynamic_pointer_cast<CObservation>(it->second);
463 
464  // First, check if inserting this OBS into the SF would
465  // overflow "SF_max_time_span":
466  if (m_curSF.size() != 0 &&
468  m_curSF.getObservationByIndex(0)->timestamp,
469  obs->timestamp) > SF_max_time_span)
470  {
472 
473  // Save and start a new one:
474  (*m_out_arch_ptr) << m_curSF;
476 
478  "Saved SF with " << m_curSF.size() << " objects.");
479  m_curSF.clear();
480  }
481 
482  // Now, insert the observation in the SF:
483  m_curSF.insert(obs);
484  }
485  else
487  "*** ERROR *** Class is not an action or an "
488  "observation");
489  }
490 }
491 
493  const RawlogGrabberApp::TListObservations& list_obs)
494 {
495  // ---------------------------
496  // DO NOT USE SENSORY-FRAMES
497  // ---------------------------
498  for (auto& ob : list_obs)
499  {
500  auto& obj_ptr = ob.second;
501  (*m_out_arch_ptr) << *obj_ptr;
503 
504  dump_verbose_info(obj_ptr);
505  }
506 
507  if (!list_obs.empty())
508  {
509  MRPT_LOG_INFO_STREAM("Saved " << list_obs.size() << " objects.");
510  }
511 }
void clear()
Clear all current observations.
double Tac() noexcept
Stops the stopwatch.
Definition: CTicTac.cpp:86
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...
Definition: datetime.cpp:50
bool has_GGA_datum() const
true if the corresponding field exists in messages.
A class for calling sleep() in a loop, such that the amount of sleep time will be computed to make th...
void insert(const CObservation::Ptr &obs)
Inserts a new observation to the list: The pointer to the objects is copied, thus DO NOT delete the p...
mrpt::serialization::CArchive * m_out_arch_ptr
#define MRPT_START
Definition: exceptions.h:241
std::vector< double > rawMeasurements
The accelerometer and/or gyroscope measurements taken by the IMU at the given timestamp.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
bool open(const std::string &fileName, int compress_level=1, mrpt::optional_ref< std::string > error_msg=std::nullopt)
Open a file for write, choosing the compression level.
mrpt::poses::CPose2D odometry
The absolute odometry measurement (IT IS NOT INCREMENTAL)
TListObservations m_global_list_obs
bool sleep()
Sleeps for some time, such as the return of this method is 1/rate (seconds) after the return of the p...
Definition: CRateTimer.cpp:25
orientation pitch absolute value (global/navigation frame) (rad)
std::string m_rawlog_ext_imgs_dir
Directory where to save externally stored images, only for CCameraSensor&#39;s.
A high-performance stopwatch, with typical resolution of nanoseconds.
void setMinLoggingLevel(const VerbosityLevel level)
Set the minimum logging level for which the incoming logs are going to be taken into account...
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:86
Contains classes for various device interfaces.
STL namespace.
std::string MRPT_getCompilationDate()
Returns the MRPT source code timestamp, according to the Reproducible-Builds specifications: https://...
Definition: os.cpp:154
std::string fileNameStripInvalidChars(const std::string &filename, const char replacement_to_invalid_chars='_')
Replace invalid filename chars by underscores (&#39;_&#39;) or any other user-given char. ...
Definition: filesystem.cpp:329
This class stores measurements from an Inertial Measurement Unit (IMU) (attitude estimation, raw gyroscope and accelerometer values), altimeters or magnetometers.
mrpt::system::COutputLogger COutputLogger
The parameter to be passed to "computeFromOdometry".
static time_point now() noexcept
Returns the current time, with the highest resolution available.
Definition: Clock.cpp:46
mrpt::hwdrivers::CGenericSensor::TListObservations TListObservations
Declares a class for storing a collection of robot actions.
bool show_sensor_thread_exceptions
If enabled (default), exceptions in sensor threads will be reported to std::cerr. ...
void dump_verbose_info(const mrpt::serialization::CSerializable::Ptr &o) const
CArchiveStreamBase< STREAM > archiveFrom(STREAM &s)
Helper function to create a templatized wrapper CArchive object for a: MRPT&#39;s CStream, std::istream, std::ostream, std::stringstream.
Definition: CArchive.h:591
VerbosityLevel getMinLoggingLevel() const
bool loadTextFile(std::vector< std::string > &o, const std::string &fileName)
Loads a text file as a vector of string lines.
void getDescriptionAsText(std::ostream &o) const override
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
#define MRPT_LOG_DEBUG_FMT(_FMT_STRING,...)
Use: MRPT_LOG_DEBUG_FMT("i=%u", i);
void initialize(int argc, const char **argv)
Initializes the application from CLI parameters.
The parts of a date/time (it&#39;s like the standard &#39;tm&#39; but with fractions of seconds).
Definition: datetime.h:49
static Ptr createSensorPtr(const std::string &className)
Just like createSensor, but returning a smart pointer to the newly created sensor object...
std::string rawlog_filename
The generated .rawlog file.
void run()
Runs with the current parameter set.
#define IS_DERIVED(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is an instance of the given ...
Definition: CObject.h:151
uint8_t day
Month (1-12)
Definition: datetime.h:53
This namespace contains representation of robot actions and observations.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:51
#define IS_CLASS(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is of the given class...
Definition: CObject.h:146
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
Use: MRPT_LOG_DEBUG_STREAM("Var=" << value << " foo=" << foo_var);
void SensorThread(std::string sensor_label)
GLsizei const GLchar ** string
Definition: glext.h:4116
MSG_CLASS & getMsgByClass()
Returns a reference to the message in the list CObservationGPS::messages of the requested class...
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
void process_observations_for_nonsf(const TListObservations &list_obs)
Declares a class for storing a robot action.
Definition: CAction.h:24
double second
Minute (0-59)
Definition: datetime.h:56
#define MRPT_LOAD_CONFIG_VAR( variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
void setRate(const double rate_hz)
Changes the object loop rate (Hz)
Definition: CRateTimer.cpp:20
#define MRPT_LOG_INFO_STREAM(__CONTENTS)
double run_for_seconds
If >0, run() will return after this period (in seconds)
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
Definition: CObservation.h:62
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
const char * argv[]
int32_t encoderLeftTicks
For differential-driven robots: The ticks count for each wheel in ABSOLUTE VALUE (IT IS NOT INCREMENT...
uint8_t minute
Hour (0-23)
Definition: datetime.h:55
std::multimap< mrpt::system::TTimeStamp, mrpt::serialization::CSerializable::Ptr > TListObservations
void process_observations_for_sf(const TListObservations &list_obs)
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:43
#define MRPT_LOG_ERROR(_STRING)
std::size_t rawlog_saved_objects
Counter of saved objects.
constexpr double RAD2DEG(const double x)
Radians to degrees.
#define ASSERT_ABOVE_(__A, __B)
Definition: exceptions.h:155
const CObservation::Ptr & getObservationByIndex(const size_t &idx) const
Returns the i&#39;th observation in the list (0=first).
#define MRPT_END
Definition: exceptions.h:245
bool kbhit() noexcept
An OS-independent version of kbhit, which returns true if a key has been pushed.
Definition: os.cpp:394
uint8_t month
The year.
Definition: datetime.h:52
std::string exception_to_str(const std::exception &e)
Builds a nice textual representation of a nested exception, which if generated using MRPT macros (THR...
Definition: exceptions.cpp:59
uint8_t hour
Day (1-31)
Definition: datetime.h:54
const int argc
orientation yaw absolute value (global/navigation frame) (rad)
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...
Definition: datetime.h:123
orientation roll absolute value (global/navigation frame) (rad)
std::string MRPT_getVersion()
Returns a string describing the MRPT version.
Definition: os.cpp:187
An observation of the current (cumulative) odometry for a wheeled robot.
Saves data to a file and transparently compress the data using the given compression level...
void Tic() noexcept
Starts the stopwatch.
Definition: CTicTac.cpp:75
#define ASSERT_FILE_EXISTS_(FIL)
Definition: filesystem.h:22
void dump_GPS_mode_info(const mrpt::obs::CObservationGPS &o) const
__int64 int64_t
Definition: glext.h:3456
T::Ptr getObservationByClass(const size_t &ith=0) const
Returns the i&#39;th observation of a given class (or of a descendant class), or nullptr if there is no s...
This class stores messages from GNSS or GNSS+IMU devices, from consumer-grade inexpensive GPS receive...
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
#define MRPT_LOG_ERROR_STREAM(__CONTENTS)
size_t size() const
Returns the number of observations in the list.
GLenum const GLfloat * params
Definition: glext.h:3538
void dump_IMU_info(const mrpt::obs::CObservationIMU &o) const
#define MRPT_LOG_INFO_FMT(_FMT_STRING,...)
#define MRPT_LOG_INFO(_STRING)
mrpt::obs::CSensoryFrame m_curSF
void insert(CAction &action)
Add a new object to the list.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 836b840ab Mon Nov 18 00:58:29 2019 +0100 at lun nov 18 01:00:14 CET 2019