Go to the documentation of this file.
33 constexpr
auto sect =
"MappingApplication";
49 " icp-slam - Part of the MRPT\n"
50 " MRPT C++ Library: %s - Sources timestamp: %s\n\n",
61 const std::string configFile = std::string(
argv[1]);
93 const string OUT_DIR_STD =
95 const char* OUT_DIR = OUT_DIR_STD.c_str();
98 const bool SAVE_POSE_LOG =
100 const bool SAVE_3D_SCENE =
102 const bool CAMERA_3DSCENE_FOLLOWS_ROBOT =
105 bool SHOW_PROGRESS_3D_REAL_TIME =
false;
106 int SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS = 0;
107 bool SHOW_LASER_SCANS_3D =
true;
112 SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS,
int,
params,
sect);
140 std::stringstream ss;
164 #if MRPT_HAS_WXWIDGETS
165 if (SHOW_PROGRESS_3D_REAL_TIME)
167 win3D = std::make_shared<CDisplayWindow3D>(
168 "ICP-SLAM @ MRPT C++ Library", 600, 500);
169 win3D->setCameraZoom(20);
170 win3D->setCameraAzimuthDeg(-45);
197 const bool isObsBasedRawlog = observation ? true :
false;
201 (!isObsBasedRawlog && !observations->empty() &&
202 *observations->begin() &&
206 isObsBasedRawlog ? observation->timestamp
207 : (*observations->begin())->timestamp;
210 std::vector<mrpt::obs::CObservation2DRangeScan::Ptr> lst_lidars;
213 if (isObsBasedRawlog)
216 static bool firstOdo =
true;
219 auto o = std::dynamic_pointer_cast<CObservationOdometry>(
221 if (!firstOdo) odoPose = odoPose + (o->odometry - lastOdo);
223 lastOdo = o->odometry;
230 action->getBestMovementEstimation();
231 if (act) odoPose = odoPose + act->poseChange->getMeanVal();
235 if (SHOW_LASER_SCANS_3D)
238 if (isObsBasedRawlog)
242 lst_lidars.push_back(
243 std::dynamic_pointer_cast<CObservation2DRangeScan>(
250 for (
size_t i = 0;; i++)
258 lst_lidars.push_back(new_obs);
266 if (isObsBasedRawlog)
270 t_exec = tictac.
Tac();
281 if (LOG_FREQUENCY > 0 && 0 == (step % LOG_FREQUENCY))
288 mrpt::format(
"%s/mapbuild_posepdf_%03u.txt", OUT_DIR, step);
290 "Saving pose log information to `%s`", str.c_str());
299 if ((LOG_FREQUENCY > 0 && 0 == (step % LOG_FREQUENCY)) ||
300 (SAVE_3D_SCENE || win3D))
308 view_map->setBorderSize(2);
309 view_map->setViewportPosition(0.01, 0.01, 0.35, 0.35);
310 view_map->setTransparent(
false);
324 groundPlane->setColor(0.4f, 0.4f, 0.4f);
325 view->insert(groundPlane);
329 if (CAMERA_3DSCENE_FOLLOWS_ROBOT)
331 scene->enableFollowCamera(
true);
349 pMap->getAs3DObject(ptsMap);
350 view_map->insert(ptsMap);
357 obj->setPose(robotPose);
362 obj->setPose(robotPose);
363 view_map->insert(obj);
367 if (SHOW_LASER_SCANS_3D)
369 for (
auto& lst_current_laser_scan : lst_lidars)
374 obj->setScan(*lst_current_laser_scan);
375 obj->setPose(robotPose);
376 obj->setSurfaceColor(1.0f, 0.0f, 0.0f, 0.5f);
383 if (LOG_FREQUENCY > 0 && 0 == (step % LOG_FREQUENCY) &&
387 mrpt::format(
"%s/buildingmap_%05u.3Dscene", OUT_DIR, step));
395 win3D->get3DSceneAndLock();
398 win3D->unlockAccess3DScene();
401 win3D->setCameraPointingToPoint(
402 robotPose.
x(), robotPose.
y(), robotPose.z());
405 win3D->forceRepaint();
407 std::this_thread::sleep_for(std::chrono::milliseconds(
408 SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS));
415 auto str =
mrpt::format(
"%s/log_MemoryUsage.txt", OUT_DIR);
424 "Memory usage:%.04f MiB", memUsage / (1024.0 * 1024.0));
429 "%i %f %f %f\n", step, robotPose.
x(), robotPose.
y(),
437 "%i %f %f %f\n", step, odoPose.
x(), odoPose.
y(), odoPose.
phi());
444 "----------- **END** (total time: %.03f sec) ---------",
451 auto str =
format(
"%s/_finalmap_.simplemap", OUT_DIR);
453 "Dumping final map in binary format to: %s\n", str.c_str());
460 auto str =
format(
"%s/_finalmaps_.txt", OUT_DIR);
465 if (win3D) win3D->waitForKey();
483 m_rawlogFileName = std::string(
argv[2]);
486 sect,
"rawlog_file", std::string(
"log.rawlog"),
true);
517 std::thread hSensorThread;
529 using namespace std::chrono_literals;
530 std::this_thread::sleep_for(2000ms);
532 if (m_allThreadsMustExit)
533 throw std::runtime_error(
534 "\n\n==== ABORTING: It seems that we could not connect to the "
535 "LIDAR. See reported errors. ==== \n");
550 if (m_allThreadsMustExit)
return false;
561 std::lock_guard<std::mutex> csl(m_cs_global_list_obs);
562 obs_copy = m_global_list_obs;
563 m_global_list_obs.clear();
566 for (
auto it = obs_copy.rbegin(); !new_obs && it != obs_copy.rend();
571 std::dynamic_pointer_cast<CObservation2DRangeScan>(
577 observation = std::move(new_obs);
582 using namespace std::chrono_literals;
583 std::this_thread::sleep_for(10ms);
600 std::string driver_name =
603 CGenericSensor::createSensorPtr(driver_name);
605 throw std::runtime_error(
606 std::string(
"***ERROR***: Class name not recognized: ") +
613 <<
" at " << sensor->getProcessRate() <<
" Hz" << std::endl;
616 sensor->getProcessRate() > 0,
617 "process_rate must be set to a valid value (>0 Hz).");
621 sensor->initialize();
622 while (!m_allThreadsMustExit)
627 sensor->getObservations(lstObjs);
629 std::lock_guard<std::mutex> lock(m_cs_global_list_obs);
630 m_global_list_obs.insert(lstObjs.begin(), lstObjs.end());
638 printf(
"[thread_%s] Closing...", tp.
section_name.c_str());
640 catch (
const std::exception& e)
642 std::cerr <<
"[SensorThread] Closing due to exception:\n"
644 m_allThreadsMustExit =
true;
648 std::cerr <<
"[SensorThread] Untyped exception! Closing." << std::endl;
649 m_allThreadsMustExit =
true;
virtual ~ICP_SLAM_App_Live() override
mrpt::poses::CPose3DPDF::Ptr getCurrentPoseEstimation() const override
Returns a copy of the current best pose estimation as a pose PDF.
bool kbhit() noexcept
An OS-independent version of kbhit, which returns true if a key has been pushed.
unsigned long getMemoryUsage()
Returns the memory occupied by this process, in bytes.
std::shared_ptr< CObservation > Ptr
std::multimap< mrpt::system::TTimeStamp, mrpt::serialization::CSerializable::Ptr > TListObservations
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
std::shared_ptr< CGenericSensor > Ptr
void processActionObservation(mrpt::obs::CActionCollection &action, mrpt::obs::CSensoryFrame &in_SF) override
Appends a new action and observations to update this map: See the description of the class at the top...
#define MRPT_LOG_INFO_FMT(_FMT_STRING,...)
void initialize(const mrpt::maps::CSimpleMap &initialMap=mrpt::maps::CSimpleMap(), const mrpt::poses::CPosePDF *x0=nullptr) override
Initialize the method, starting with a known location PDF "x0"(if supplied, set to nullptr to left un...
void setAzimuthDegrees(float ang)
int void fclose(FILE *f)
An OS-independent version of fclose.
static Ptr Create(Args &&... args)
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
T::Ptr mapByClass(size_t ith=0) const
Returns the i'th map of a given class (or of a derived class), or empty smart pointer if there is no ...
#define MRPT_LOG_INFO(_STRING)
mrpt::config::CConfigFileBase * cfgFile
bool keyExists(const std::string §ion, const std::string &key) const
Checks if a given key exists inside a section (case insensitive)
void processObservation(const mrpt::obs::CObservation::Ptr &obs)
The main method of this class: Process one odometry or sensor observation.
void setVerbosityLevel(const VerbosityLevel level)
alias of setMinLoggingLevel()
static time_point now() noexcept
Returns the current time using the currently selected Clock source.
std::shared_ptr< mrpt::opengl ::CGridPlaneXY > Ptr
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...
std::shared_ptr< mrpt::obs ::CSensoryFrame > Ptr
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
double phi() const
Get the phi angle of the 2D pose (in radians)
void SensorThread(TThreadParams params)
double x() const
Common members of all points & poses classes.
CICP::TConfigParams ICP_params
Options for the ICP algorithm itself.
#define IS_CLASS(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is of the given class.
bool impl_get_next_observations(mrpt::obs::CActionCollection::Ptr &action, mrpt::obs::CSensoryFrame::Ptr &observations, mrpt::obs::CObservation::Ptr &observation) override
Get next sensory data.
A class for very simple 2D SLAM based on ICP.
void run()
Runs with the current parameter set.
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors.
bool read_bool(const std::string §ion, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
Contains classes for various device interfaces.
std::shared_ptr< mrpt::obs ::CActionCollection > Ptr
std::shared_ptr< CRenderizable > Ptr
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void setPointingAt(float x, float y, float z)
#define ASSERT_(f)
Defines an assertion mechanism.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
std::shared_ptr< mrpt::obs ::CActionRobotMovement2D > Ptr
This namespace contains representation of robot actions and observations.
std::map< mrpt::system::TTimeStamp, mrpt::math::TPose3D > out_estimated_path
void getCurrentlyBuiltMap(mrpt::maps::CSimpleMap &out_map) const override
Fills "out_map" with the set of "poses"-"sensory-frames", thus the so far built map.
std::string read_string(const std::string §ion, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
std::shared_ptr< mrpt::obs ::CObservation2DRangeScan > Ptr
double Tac() noexcept
Stops the stopwatch.
An observation of the current (cumulative) odometry for a wheeled robot.
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form,...
int read_int(const std::string §ion, const std::string &name, int defaultValue, bool failIfNotFound=false) const
void thread_name(const std::string &name, std::thread &theThread)
Sets the name of the given thread; useful for debuggers.
virtual void impl_initialize(int argc, const char **argv)=0
std::string file_get_contents(const std::string &fileName)
Loads an entire text file and return its contents as a single std::string.
const mrpt::maps::CMultiMetricMap * getCurrentlyBuiltMetricMap() const override
Returns the map built so far.
void initialize(int argc, const char **argv)
Initializes the application from CLI parameters.
bool quits_with_esc_key
If enabled (default), stdin will be watched and application quits if ESC is pressed.
mrpt::config::CConfigFileMemory params
Populated in initialize().
bool sleep()
Sleeps for some time, such as the return of this method is 1/rate (seconds) after the return of the p...
std::shared_ptr< mrpt::opengl ::COpenGLViewport > Ptr
VerbosityLevel
Enumeration of available verbosity levels.
static Ptr Create(Args &&... args)
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string §ion) override
This method load the options from a ".ini"-like file or memory-stored string list.
void fromString(const std::string &s)
Builds from a string representation of the list, for example: "CPose2D, CObservation,...
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.
This class stores any customizable set of metric maps.
#define MRPT_LOG_ERROR(_STRING)
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
void setContent(const std::vector< std::string > &stringList)
Changes the contents of the virtual "config file".
mrpt::math::TPose3D asTPose() const
A class for calling sleep() in a loop, such that the amount of sleep time will be computed to make th...
A camera: if added to a scene, the viewpoint defined by this camera will be used instead of the camer...
std::shared_ptr< CDisplayWindow3D > Ptr
void setMinLoggingLevel(const VerbosityLevel level)
Set the minimum logging level for which the incoming logs are going to be taken into account.
mrpt::Clock::time_point TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1,...
void setZoomDistance(float z)
void Tic() noexcept
Starts the stopwatch.
unsigned int getCurrentlyBuiltMapSize() override
Returns just how many sensory-frames are stored in the currently build map.
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...
std::string MRPT_getCompilationDate()
Returns the MRPT source code timestamp, according to the Reproducible-Builds specifications: https://...
Classes for creating GUI windows for 2D and 3D visualization.
virtual std::string impl_get_usage() const =0
bool deleteFilesInDirectory(const std::string &s, bool deleteDirectoryAsWell=false)
Delete all the files in a given directory (nothing done if directory does not exists,...
This CStream derived class allow using a file as a write-only, binary stream.
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
static Ptr Create(Args &&... args)
void impl_initialize(int argc, const char **argv) override
virtual bool impl_get_next_observations(mrpt::obs::CActionCollection::Ptr &action, mrpt::obs::CSensoryFrame::Ptr &observations, mrpt::obs::CObservation::Ptr &observation)=0
Get next sensory data.
std::shared_ptr< mrpt::opengl ::COpenGLScene > Ptr
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const override
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >,...
CSetOfObjects::Ptr RobotPioneer()
Returns a representation of a Pioneer II mobile base.
CArchiveStreamBase< STREAM > archiveFrom(STREAM &s)
Helper function to create a templatized wrapper CArchive object for a: MRPT's CStream,...
static Ptr Create(Args &&... args)
void impl_initialize(int argc, const char **argv) override
double yaw() const
Get the YAW angle (in radians)
void setElevationDegrees(float ang)
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
int getch() noexcept
An OS-independent version of getch, which waits until a key is pushed.
This base provides a set of functions for maths stuff.
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
VerbosityLevel getMinLoggingLevel() const
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map.
TConfigParams ICP_options
Options for the ICP-SLAM application.
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string §ion) override
This method load the options from a ".ini"-like file or memory-stored string list.
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...
void setLoggerName(const std::string &name)
Set the name of the COutputLogger instance.
bool createDirectory(const std::string &dirName)
Creates a directory.
#define ASSERT_FILE_EXISTS_(FIL)
The namespace for 3D scene representation and rendering.
void saveCurrentMapToFile(const std::string &fileName, bool compressGZ=true) const
Save map (mrpt::maps::CSimpleMap) to a ".simplemap" file.
virtual void dumpToTextStream(std::ostream &out) const
This method should clearly display all the contents of the structure in textual form,...
std::string MRPT_getVersion()
Returns a string describing the MRPT version.
std::string std::string format(std::string_view fmt, ARGS &&... args)
mrpt::rtti::CListOfClasses alwaysInsertByClass
A list of observation classes (derived from mrpt::obs::CObservation) which will be always inserted in...
Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Fri Jul 17 08:43:33 UTC 2020 | |