Go to the documentation of this file.
12 #include <mrpt/3rdparty/tclap/CmdLine.h>
46 if ((getenv(
"MRPT_HWDRIVERS_VERBOSE") !=
nullptr) &&
47 atoi(getenv(
"MRPT_HWDRIVERS_VERBOSE")) != 0)
54 " MRPT C++ Library: %s - Sources timestamp: %s\n",
62 TCLAP::UnlabeledValueArg<std::string> argConfigFile(
63 "config",
"Config file",
true,
"",
"<configFile.ini>",
cmd);
64 TCLAP::ValueArg<std::string> argPlugins(
66 "Load one or more plug-in modules (.so/.dll) with additional sensor "
67 "drivers (comma-separated list)",
68 false,
"",
"myModule.so",
cmd);
77 if (argPlugins.isSet())
95 const std::string GLOBAL_SECT =
"global";
100 string rawlog_prefix =
"dataset";
101 int time_between_launches = 300;
102 bool use_sensoryframes =
false;
103 int GRABBER_PERIOD_MS = 1000;
104 int rawlog_GZ_compress_level = 1;
115 string rawlog_postfix =
"_";
121 "%04u-%02u-%02u_%02uh%02um%02us", (
unsigned int)parts.
year,
122 (
unsigned int)parts.
month, (
unsigned int)parts.
day,
123 (
unsigned int)parts.
hour, (
unsigned int)parts.
minute,
124 (
unsigned int)parts.
second);
137 rawlog_postfix += string(
".rawlog");
150 std::vector<std::string> sections;
153 vector<std::thread> lstThreads;
155 for (
auto& section : sections)
157 if (section == GLOBAL_SECT || section.empty() ||
161 std::thread& newThread = lstThreads.emplace_back(
166 std::this_thread::sleep_for(
167 std::chrono::milliseconds(time_between_launches));
186 auto lambdaProcessPending = [&]() {
188 copy_of_m_global_list_obs.clear();
198 if (use_sensoryframes)
213 lambdaProcessPending();
215 std::this_thread::sleep_for(
216 std::chrono::milliseconds(GRABBER_PERIOD_MS));
222 "[main thread] Ended due to other thread signal to exit "
227 lambdaProcessPending();
235 std::this_thread::sleep_for(300ms);
239 for (
auto& lstThread : lstThreads) lstThread.join();
256 catch (
const std::exception& e)
276 if (
auto gps = std::dynamic_pointer_cast<mrpt::obs::CObservationGPS>(o);
282 std::dynamic_pointer_cast<mrpt::obs::CObservationIMU>(o);
315 const auto fq =
static_cast<int>(
318 " GPS mode: " << fq <<
" label: " << o.
sensorLabel);
321 std::stringstream ss;
331 " IMU angles (degrees): "
332 "(yaw,pitch,roll)=(%.06f, %.06f, %.06f)",
345 std::string driver_name =
352 throw std::runtime_error(
353 std::string(
"Class name not recognized: ") + driver_name);
356 sensor->loadConfig(
params, sensor_label);
359 "[thread_" << sensor_label <<
"] Starting at "
360 << sensor->getProcessRate() <<
" Hz");
368 sensor->initialize();
371 rate.
setRate(sensor->getProcessRate());
380 sensor->getObservations(lstObjs);
397 catch (
const std::exception& e)
402 "Exception in SensorThread:\n"
425 for (
auto it = list_obs.begin(); it != list_obs.end(); ++it)
430 CAction::Ptr act = std::dynamic_pointer_cast<CAction>(it->second);
434 "Saved SF with " <<
m_curSF.
size() <<
" objects.");
441 (*m_out_arch_ptr) << acts;
446 std::dynamic_pointer_cast<CObservationOdometry>(it->second);
448 auto act = CActionRobotMovement2D::Create();
449 act->timestamp = odom->timestamp;
454 static bool last_odo_first =
true;
457 int64_t lticks_incr, rticks_incr;
461 last_odo_first =
false;
463 lticks_incr = rticks_incr = 0;
467 odo_incr = odom->odometry - last_odo.
odometry;
477 act->computeFromOdometry(odo_incr, odomOpts);
480 act->encoderLeftTicks = lticks_incr;
481 act->encoderRightTicks = rticks_incr;
483 act->hasVelocities =
true;
484 act->velocityLocal = odom->velocityLocal;
489 "Saved SF with " <<
m_curSF.
size() <<
" objects.");
496 (*m_out_arch_ptr) << acts;
505 std::dynamic_pointer_cast<CObservation>(it->second);
524 "Saved SF with " <<
m_curSF.
size() <<
" objects.");
533 "*** ERROR *** Class is not an action or an "
544 for (
auto& ob : list_obs)
546 auto& obj_ptr = ob.second;
547 (*m_out_arch_ptr) << *obj_ptr;
556 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.
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
void thread_name(const std::string &name, std::thread &theThread)
Sets the name of the given thread; useful for debuggers.
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...
std::mutex m_isRunningMtx
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)
TCLAP::CmdLine cmd("system_control_rate_timer_example")
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)
std::mutex params_mtx
Acquire this mutex if changing parameters while the app is running.
#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...
std::size_t rawlog_saved_objects
Counter of saved objects.
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.
#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
#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.
std::mutex results_mtx
Acquire this mutex if reading these output variables while the app is running.
bool loadPluginModules(const std::string &moduleFileNames, mrpt::optional_ref< std::string > outErrorMsgs=std::nullopt)
Like loadPluginModule(), but loads a comma (,) separated list of "plug-in" modules.
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.4 at Sun Jul 19 15:15:43 UTC 2020 | |