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);
62  }
63 
64  MRPT_END
65 }
66 
68 {
70 
71  using namespace mrpt;
72  using namespace mrpt::system;
73  using namespace mrpt::hwdrivers;
74  using namespace mrpt::config;
75  using namespace mrpt::serialization;
76  using namespace mrpt::img;
77  using namespace mrpt::obs;
78  using namespace mrpt::poses;
79  using namespace std;
80 
81  const std::string GLOBAL_SECT = "global";
82 
83  // ------------------------------------------
84  // Load config from file:
85  // ------------------------------------------
86  string rawlog_prefix = "dataset";
87  int time_between_launches = 300;
88  bool use_sensoryframes = false;
89  int GRABBER_PERIOD_MS = 1000;
90  int rawlog_GZ_compress_level = 1; // 0: No compress, 1-9: compress level
91 
92  MRPT_LOAD_CONFIG_VAR(rawlog_prefix, string, params, GLOBAL_SECT);
93  MRPT_LOAD_CONFIG_VAR(time_between_launches, int, params, GLOBAL_SECT);
94  MRPT_LOAD_CONFIG_VAR(SF_max_time_span, float, params, GLOBAL_SECT);
95  MRPT_LOAD_CONFIG_VAR(use_sensoryframes, bool, params, GLOBAL_SECT);
96  MRPT_LOAD_CONFIG_VAR(GRABBER_PERIOD_MS, int, params, GLOBAL_SECT);
97 
98  MRPT_LOAD_CONFIG_VAR(rawlog_GZ_compress_level, int, params, GLOBAL_SECT);
99 
100  // Build full rawlog file name:
101  string rawlog_postfix = "_";
102 
103  // rawlog_postfix += dateTimeToString( now() );
105  mrpt::system::timestampToParts(now(), parts, true);
106  rawlog_postfix += format(
107  "%04u-%02u-%02u_%02uh%02um%02us", (unsigned int)parts.year,
108  (unsigned int)parts.month, (unsigned int)parts.day,
109  (unsigned int)parts.hour, (unsigned int)parts.minute,
110  (unsigned int)parts.second);
111 
112  rawlog_postfix = mrpt::system::fileNameStripInvalidChars(rawlog_postfix);
113 
114  // Only set this if we want externally stored images:
116  rawlog_prefix +
117  fileNameStripInvalidChars(rawlog_postfix + string("_Images"));
118 
119  // Also, set the path in CImage to enable online visualization in a GUI
120  // window:
121  CImage::setImagesPathBase(m_rawlog_ext_imgs_dir);
122 
123  rawlog_postfix += string(".rawlog");
124  rawlog_postfix = fileNameStripInvalidChars(rawlog_postfix);
125 
126  rawlog_filename = rawlog_prefix + rawlog_postfix;
127 
128  MRPT_LOG_INFO_STREAM("Output rawlog filename: " << rawlog_filename);
129  MRPT_LOG_INFO_STREAM("External image storage: " << m_rawlog_ext_imgs_dir);
130 
131  // ----------------------------------------------
132  // Launch threads:
133  // ----------------------------------------------
134  std::vector<std::string> sections;
135  params.getAllSections(sections);
136 
137  vector<std::thread> lstThreads;
138 
139  for (auto& section : sections)
140  {
141  if (section == GLOBAL_SECT || section.empty() ||
142  params.read_bool(section, "rawlog-grabber-ignore", false, false))
143  continue; // This is not a sensor:
144 
145  lstThreads.emplace_back(&RawlogGrabberApp::SensorThread, this, section);
146 
147  std::this_thread::sleep_for(
148  std::chrono::milliseconds(time_between_launches));
149  }
150 
151  // ----------------------------------------------
152  // Run:
153  // ----------------------------------------------
155  auto out_arch_obj = archiveFrom(out_file);
156  m_out_arch_ptr = &out_arch_obj;
157 
158  out_file.open(rawlog_filename, rawlog_GZ_compress_level);
159 
160  CGenericSensor::TListObservations copy_of_m_global_list_obs;
161 
162  MRPT_LOG_INFO_STREAM("Press any key to exit program");
163 
164  mrpt::system::CTicTac run_timer;
165  run_timer.Tic();
166 
167  while (!os::kbhit() && !allThreadsMustExit)
168  {
169  // Check "run for X seconds" flag:
170  if (run_for_seconds > 0 && run_timer.Tac() > run_for_seconds) break;
171 
172  // See if we have observations and process them:
173  {
174  std::lock_guard<std::mutex> lock(cs_m_global_list_obs);
175  copy_of_m_global_list_obs.clear();
176 
177  if (!m_global_list_obs.empty())
178  {
179  auto itEnd = m_global_list_obs.begin();
180  std::advance(itEnd, m_global_list_obs.size() / 2);
181  copy_of_m_global_list_obs.insert(
182  m_global_list_obs.begin(), itEnd);
183  m_global_list_obs.erase(m_global_list_obs.begin(), itEnd);
184  }
185  } // End of cs lock
186 
187  if (use_sensoryframes)
188  {
189  process_observations_for_sf(copy_of_m_global_list_obs);
190  }
191  else
192  {
193  process_observations_for_nonsf(copy_of_m_global_list_obs);
194  }
195 
196  std::this_thread::sleep_for(
197  std::chrono::milliseconds(GRABBER_PERIOD_MS));
198  }
199 
200  if (allThreadsMustExit)
201  {
203  "[main thread] Ended due to other thread signal to exit "
204  "application.");
205  }
206 
207  // Flush file to disk:
208  out_file.close();
209 
210  // Wait all threads:
211  // ----------------------------
212  allThreadsMustExit = true;
213  std::this_thread::sleep_for(300ms);
214 
215  MRPT_LOG_INFO("Waiting for all threads to close...");
216 
217  for (auto& lstThread : lstThreads) lstThread.join();
218 
219  MRPT_END
220 }
221 
224 {
225  // Show GPS mode:
227 
228  static auto last_t = mrpt::Clock::now();
229  const auto t_now = mrpt::Clock::now();
230  if (mrpt::system::timeDifference(last_t, t_now) < 1.0) return;
231  last_t = t_now;
232 
233  if (auto gps = std::dynamic_pointer_cast<mrpt::obs::CObservationGPS>(o);
234  gps)
235  {
236  dump_GPS_mode_info(*gps);
237  }
238  else if (auto imu =
239  std::dynamic_pointer_cast<mrpt::obs::CObservationIMU>(o);
240  imu)
241  {
242  dump_IMU_info(*imu);
243  }
244 }
245 
247  const mrpt::obs::CSensoryFrame& sf) const
248 {
250 
251  // Show GPS mode:
253  std::size_t idx = 0;
254  do
255  {
257  if (gps) dump_GPS_mode_info(*gps);
258  } while (gps);
259 
260  // Show IMU angles:
262  if (imu) dump_IMU_info(*imu);
263 }
264 
266  const mrpt::obs::CObservationGPS& o) const
267 {
268  if (o.has_GGA_datum())
269  {
271 
272  const auto fq = static_cast<int>(
273  o.getMsgByClass<Message_NMEA_GGA>().fields.fix_quality);
275  " GPS mode: " << fq << " label: " << o.sensorLabel);
276  }
277  {
278  std::stringstream ss;
279  o.getDescriptionAsText(ss);
280  MRPT_LOG_DEBUG_STREAM(ss.str());
281  }
282 }
283 
285 {
286  // Show IMU angles:
288  " IMU angles (degrees): "
289  "(yaw,pitch,roll)=(%.06f, %.06f, %.06f)",
293 }
294 
295 // ------------------------------------------------------
296 // SensorThread
297 // ------------------------------------------------------
298 void RawlogGrabberApp::SensorThread(std::string sensor_label)
299 {
300  try
301  {
302  std::string driver_name =
303  params.read_string(sensor_label, "driver", "", true);
304 
305  auto sensor =
307 
308  if (!sensor)
309  throw std::runtime_error(
310  std::string("Class name not recognized: ") + driver_name);
311 
312  // Load common & sensor specific parameters:
313  sensor->loadConfig(params, sensor_label);
314 
316  "[thread_" << sensor_label << "] Starting at "
317  << sensor->getProcessRate() << " Hz");
318 
319  ASSERT_ABOVE_(sensor->getProcessRate(), 0);
320 
321  // For imaging sensors, set external storage directory:
322  sensor->setPathForExternalImages(m_rawlog_ext_imgs_dir);
323 
324  // Init device:
325  sensor->initialize();
326 
328  rate.setRate(sensor->getProcessRate());
329 
330  while (!allThreadsMustExit)
331  {
332  // Process
333  sensor->doProcess();
334 
335  // Get new observations
337  sensor->getObservations(lstObjs);
338 
339  {
340  std::lock_guard<std::mutex> lock(cs_m_global_list_obs);
341  m_global_list_obs.insert(lstObjs.begin(), lstObjs.end());
342  }
343 
344  lstObjs.clear();
345 
346  // wait for the process period:
347  rate.sleep();
348  }
349 
350  sensor.reset();
351 
352  MRPT_LOG_INFO_FMT("[thread_%s] Closing...", sensor_label.c_str());
353  }
354  catch (const std::exception& e)
355  {
357  {
359  "Exception in SensorThread:\n"
360  << mrpt::exception_to_str(e));
361  }
362  allThreadsMustExit = true;
363  }
364  catch (...)
365  {
367  {
368  MRPT_LOG_ERROR("Untyped exception in SensorThread.");
369  }
370  allThreadsMustExit = true;
371  }
372 }
373 
375  const RawlogGrabberApp::TListObservations& list_obs)
376 {
377  using namespace mrpt::obs;
378 
379  // -----------------------
380  // USE SENSORY-FRAMES
381  // -----------------------
382  for (auto it = list_obs.begin(); it != list_obs.end(); ++it)
383  {
384  // If we have an action, save the SF and start a new one:
385  if (IS_DERIVED(*it->second, CAction))
386  {
387  CAction::Ptr act = std::dynamic_pointer_cast<CAction>(it->second);
388 
389  (*m_out_arch_ptr) << m_curSF;
391  "Saved SF with " << m_curSF.size() << " objects.");
392  m_curSF.clear();
393 
394  CActionCollection acts;
395  acts.insert(*act);
396  act.reset();
397 
398  (*m_out_arch_ptr) << acts;
399  }
400  else if (IS_CLASS(*it->second, CObservationOdometry))
401  {
403  std::dynamic_pointer_cast<CObservationOdometry>(it->second);
404 
405  auto act = CActionRobotMovement2D::Create();
406  act->timestamp = odom->timestamp;
407 
408  // Compute the increment since the last reading:
410  static CObservationOdometry last_odo;
411  static bool last_odo_first = true;
412 
413  mrpt::poses::CPose2D odo_incr;
414  int64_t lticks_incr, rticks_incr;
415 
416  if (last_odo_first)
417  {
418  last_odo_first = false;
419  odo_incr = mrpt::poses::CPose2D(0, 0, 0);
420  lticks_incr = rticks_incr = 0;
421  }
422  else
423  {
424  odo_incr = odom->odometry - last_odo.odometry;
425  lticks_incr =
426  odom->encoderLeftTicks - last_odo.encoderLeftTicks;
427  rticks_incr =
428  odom->encoderRightTicks - last_odo.encoderRightTicks;
429 
430  last_odo = *odom;
431  }
432 
433  // Save as action & dump to file:
434  act->computeFromOdometry(odo_incr, odomOpts);
435 
436  act->hasEncodersInfo = true;
437  act->encoderLeftTicks = lticks_incr;
438  act->encoderRightTicks = rticks_incr;
439 
440  act->hasVelocities = true;
441  act->velocityLocal = odom->velocityLocal;
442 
443  (*m_out_arch_ptr) << m_curSF;
445 
447  "Saved SF with " << m_curSF.size() << " objects.");
448  m_curSF.clear();
449 
450  CActionCollection acts;
451  acts.insert(*act);
452  act.reset();
453 
454  (*m_out_arch_ptr) << acts;
456  }
457  else if (IS_DERIVED(*it->second, CObservation))
458  {
459  CObservation::Ptr obs =
460  std::dynamic_pointer_cast<CObservation>(it->second);
461 
462  // First, check if inserting this OBS into the SF would
463  // overflow "SF_max_time_span":
464  if (m_curSF.size() != 0 &&
466  m_curSF.getObservationByIndex(0)->timestamp,
467  obs->timestamp) > SF_max_time_span)
468  {
470 
471  // Save and start a new one:
472  (*m_out_arch_ptr) << m_curSF;
474 
476  "Saved SF with " << m_curSF.size() << " objects.");
477  m_curSF.clear();
478  }
479 
480  // Now, insert the observation in the SF:
481  m_curSF.insert(obs);
482  }
483  else
485  "*** ERROR *** Class is not an action or an "
486  "observation");
487  }
488 }
489 
491  const RawlogGrabberApp::TListObservations& list_obs)
492 {
493  // ---------------------------
494  // DO NOT USE SENSORY-FRAMES
495  // ---------------------------
496  for (auto& ob : list_obs)
497  {
498  auto& obj_ptr = ob.second;
499  (*m_out_arch_ptr) << *obj_ptr;
501 
502  dump_verbose_info(obj_ptr);
503  }
504 
505  if (!list_obs.empty())
506  {
507  MRPT_LOG_INFO_STREAM("Saved " << list_obs.size() << " objects.");
508  }
509 }
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::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
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.
T::Ptr getObservationByClass(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...
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
const CObservation::Ptr & getObservationByIndex(size_t idx) const
Returns the i&#39;th observation in the list (0=first).
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)
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 getAllSections(std::vector< std::string > &sections) const override
Returns a list with all the section names.
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.
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
void setContent(const std::vector< std::string > &stringList)
Changes the contents of the virtual "config file".
constexpr double RAD2DEG(const double x)
Radians to degrees.
#define ASSERT_ABOVE_(__A, __B)
Definition: exceptions.h:155
#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
std::string file_get_contents(const std::string &fileName)
Loads an entire text file and return its contents as a single std::string.
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
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.
mrpt::config::CConfigFileMemory params
Populated in initialize().
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: 24b95e159 Thu Jan 23 01:15:46 2020 +0100 at jue ene 23 01:30:10 CET 2020