Go to the documentation of this file.
43 if ((getenv(
"MRPT_HWDRIVERS_VERBOSE") !=
nullptr) &&
44 atoi(getenv(
"MRPT_HWDRIVERS_VERBOSE")) != 0)
51 " MRPT C++ Library: %s - Sources timestamp: %s\n",
60 std::string INI_FILENAME(
argv[1]);
80 const std::string GLOBAL_SECT =
"global";
85 string rawlog_prefix =
"dataset";
86 int time_between_launches = 300;
87 bool use_sensoryframes =
false;
88 int GRABBER_PERIOD_MS = 1000;
89 int rawlog_GZ_compress_level = 1;
100 string rawlog_postfix =
"_";
106 "%04u-%02u-%02u_%02uh%02um%02us", (
unsigned int)parts.
year,
107 (
unsigned int)parts.
month, (
unsigned int)parts.
day,
108 (
unsigned int)parts.
hour, (
unsigned int)parts.
minute,
109 (
unsigned int)parts.
second);
122 rawlog_postfix += string(
".rawlog");
133 std::vector<std::string> sections;
136 vector<std::thread> lstThreads;
138 for (
auto& section : sections)
140 if (section == GLOBAL_SECT || section.empty() ||
146 std::this_thread::sleep_for(
147 std::chrono::milliseconds(time_between_launches));
166 auto lambdaProcessPending = [&]() {
168 copy_of_m_global_list_obs.clear();
178 if (use_sensoryframes)
190 lambdaProcessPending();
192 std::this_thread::sleep_for(
193 std::chrono::milliseconds(GRABBER_PERIOD_MS));
199 "[main thread] Ended due to other thread signal to exit "
204 lambdaProcessPending();
212 std::this_thread::sleep_for(300ms);
216 for (
auto& lstThread : lstThreads) lstThread.join();
227 catch (
const std::exception& e)
245 if (
auto gps = std::dynamic_pointer_cast<mrpt::obs::CObservationGPS>(o);
251 std::dynamic_pointer_cast<mrpt::obs::CObservationIMU>(o);
284 const auto fq =
static_cast<int>(
287 " GPS mode: " << fq <<
" label: " << o.
sensorLabel);
290 std::stringstream ss;
300 " IMU angles (degrees): "
301 "(yaw,pitch,roll)=(%.06f, %.06f, %.06f)",
314 std::string driver_name =
321 throw std::runtime_error(
322 std::string(
"Class name not recognized: ") + driver_name);
325 sensor->loadConfig(
params, sensor_label);
328 "[thread_" << sensor_label <<
"] Starting at "
329 << sensor->getProcessRate() <<
" Hz");
337 sensor->initialize();
340 rate.
setRate(sensor->getProcessRate());
349 sensor->getObservations(lstObjs);
366 catch (
const std::exception& e)
371 "Exception in SensorThread:\n"
394 for (
auto it = list_obs.begin(); it != list_obs.end(); ++it)
399 CAction::Ptr act = std::dynamic_pointer_cast<CAction>(it->second);
403 "Saved SF with " <<
m_curSF.
size() <<
" objects.");
410 (*m_out_arch_ptr) << acts;
415 std::dynamic_pointer_cast<CObservationOdometry>(it->second);
417 auto act = CActionRobotMovement2D::Create();
418 act->timestamp = odom->timestamp;
423 static bool last_odo_first =
true;
426 int64_t lticks_incr, rticks_incr;
430 last_odo_first =
false;
432 lticks_incr = rticks_incr = 0;
436 odo_incr = odom->odometry - last_odo.
odometry;
446 act->computeFromOdometry(odo_incr, odomOpts);
449 act->encoderLeftTicks = lticks_incr;
450 act->encoderRightTicks = rticks_incr;
452 act->hasVelocities =
true;
453 act->velocityLocal = odom->velocityLocal;
459 "Saved SF with " <<
m_curSF.
size() <<
" objects.");
466 (*m_out_arch_ptr) << acts;
472 std::dynamic_pointer_cast<CObservation>(it->second);
488 "Saved SF with " <<
m_curSF.
size() <<
" objects.");
497 "*** ERROR *** Class is not an action or an "
508 for (
auto& ob : list_obs)
510 auto& obj_ptr = ob.second;
511 (*m_out_arch_ptr) << *obj_ptr;
517 if (!list_obs.empty())
bool kbhit() noexcept
An OS-independent version of kbhit, which returns true if a key has been pushed.
std::shared_ptr< CObservation > Ptr
std::multimap< mrpt::system::TTimeStamp, mrpt::serialization::CSerializable::Ptr > TListObservations
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
mrpt::poses::CPose2D odometry
The absolute odometry measurement (IT IS NOT INCREMENTAL)
void dump_GPS_mode_info(const mrpt::obs::CObservationGPS &o) const
std::vector< double > rawMeasurements
The accelerometer and/or gyroscope measurements taken by the IMU at the given timestamp.
#define ASSERT_ABOVE_(__A, __B)
mrpt::config::CConfigFileMemory params
Populated in initialize().
std::shared_ptr< mrpt::obs ::CObservationOdometry > Ptr
#define MRPT_LOG_INFO_FMT(_FMT_STRING,...)
void getAllSections(std::vector< std::string > §ions) const override
Returns a list with all the section names.
void clear()
Clear all current observations.
std::shared_ptr< CSerializable > Ptr
#define MRPT_LOG_INFO(_STRING)
static time_point now() noexcept
Returns the current time using the currently selected Clock source.
A high-performance stopwatch, with typical resolution of nanoseconds.
#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...
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.
bool show_sensor_thread_exceptions
If enabled (default), exceptions in sensor threads will be reported to std::cerr.
T::Ptr getObservationByClass(size_t ith=0) const
Returns the i'th observation of a given class (or of a descendant class), or nullptr if there is no s...
bool has_GGA_datum() const
true if the corresponding field exists in messages.
@ IMU_PITCH
orientation pitch absolute value (global/navigation frame) (rad)
int32_t encoderRightTicks
#define IS_CLASS(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is of the given class.
std::mutex cs_m_global_list_obs
std::string m_rawlog_ext_imgs_dir
Directory where to save externally stored images, only for CCameraSensor's.
mrpt::hwdrivers::CGenericSensor::TListObservations TListObservations
MSG_CLASS & getMsgByClass()
Returns a reference to the message in the list CObservationGPS::messages of the requested class.
double second
Minute (0-59)
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
@ IMU_ROLL
orientation roll absolute value (global/navigation frame) (rad)
bool read_bool(const std::string §ion, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
Contains classes for various device interfaces.
Declares a class for storing a collection of robot actions.
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
mrpt::system::COutputLogger COutputLogger
TListObservations m_global_list_obs
#define THROW_EXCEPTION(msg)
This class stores measurements from an Inertial Measurement Unit (IMU) (attitude estimation,...
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
#define MRPT_LOG_DEBUG_FMT(_FMT_STRING,...)
Use: MRPT_LOG_DEBUG_FMT("i=%u", i);
static Ptr createSensorPtr(const std::string &className)
Just like createSensor, but returning a smart pointer to the newly created sensor object.
This namespace contains representation of robot actions and observations.
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 fileNameStripInvalidChars(const std::string &filename, const char replacement_to_invalid_chars='_')
Replace invalid filename chars by underscores ('_') or any other user-given char.
double Tac() noexcept
Stops the stopwatch.
An observation of the current (cumulative) odometry for a wheeled robot.
std::shared_ptr< mrpt::obs ::CObservationGPS > Ptr
size_t size() const
Returns the number of observations in the list.
std::string file_get_contents(const std::string &fileName)
Loads an entire text file and return its contents as a single std::string.
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.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
bool sleep()
Sleeps for some time, such as the return of this method is 1/rate (seconds) after the return of the p...
void dump_verbose_info(const mrpt::serialization::CSerializable::Ptr &o) const
std::string rawlog_filename
The generated .rawlog file.
void initialize(int argc, const char **argv)
Initializes the application from CLI parameters.
Saves data to a file and transparently compress the data using the given compression level.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
constexpr double RAD2DEG(const double x)
Radians to degrees.
void process_observations_for_sf(const TListObservations &list_obs)
bool hasEncodersInfo
"true" means that "encoderLeftTicks" and "encoderRightTicks" contain valid values.
#define MRPT_LOG_ERROR(_STRING)
void setContent(const std::vector< std::string > &stringList)
Changes the contents of the virtual "config file".
A class for calling sleep() in a loop, such that the amount of sleep time will be computed to make th...
int32_t encoderLeftTicks
For differential-driven robots: The ticks count for each wheel in ABSOLUTE VALUE (IT IS NOT INCREMENT...
std::atomic_bool allThreadsMustExit
double run_for_seconds
If >0, run() will return after this period (in seconds)
void setMinLoggingLevel(const VerbosityLevel level)
Set the minimum logging level for which the incoming logs are going to be taken into account.
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...
void setRate(const double rate_hz)
Changes the object loop rate (Hz)
const CObservation::Ptr & getObservationByIndex(size_t idx) const
Returns the i'th observation in the list (0=first).
void Tic() noexcept
Starts the stopwatch.
void process_observations_for_nonsf(const TListObservations &list_obs)
void run()
Runs with the current parameter set.
std::string MRPT_getCompilationDate()
Returns the MRPT source code timestamp, according to the Reproducible-Builds specifications: https://...
void SensorThread(std::string sensor_label)
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
Use: MRPT_LOG_DEBUG_STREAM("Var=" << value << " foo=" << foo_var);
void close()
Close the file.
#define MRPT_LOG_ERROR_STREAM(__CONTENTS)
CArchiveStreamBase< STREAM > archiveFrom(STREAM &s)
Helper function to create a templatized wrapper CArchive object for a: MRPT's CStream,...
This class stores messages from GNSS or GNSS+IMU devices, from consumer-grade inexpensive GPS receive...
void getDescriptionAsText(std::ostream &o) const override
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
LockHelper< T > lockHelper(T &t)
Syntactic sugar to easily create a locker to any kind of std::mutex.
std::atomic_bool m_isRunning
#define IS_DERIVED(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is an instance of the given ...
void insert(CAction &action)
Add a new object to the list.
std::shared_ptr< CAction > Ptr
Declares a class that represents any robot's observation.
uint8_t minute
Hour (0-23)
@ IMU_YAW
orientation yaw absolute value (global/navigation frame) (rad)
#define MRPT_LOG_INFO_STREAM(__CONTENTS)
VerbosityLevel getMinLoggingLevel() const
Declares a class for storing a robot action.
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...
mrpt::serialization::CArchive * m_out_arch_ptr
std::atomic_size_t rawlog_saved_objects
Counter of saved objects.
#define ASSERT_FILE_EXISTS_(FIL)
The parameter to be passed to "computeFromOdometry".
mrpt::obs::CSensoryFrame m_curSF
std::string MRPT_getVersion()
Returns a string describing the MRPT version.
void dump_IMU_info(const mrpt::obs::CObservationIMU &o) const
std::string std::string format(std::string_view fmt, ARGS &&... args)
Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 15 15:49:54 UTC 2020 | |