Go to the documentation of this file.
32 constexpr
auto sect =
"MappingApplication";
48 " icp-slam - Part of the MRPT\n"
49 " MRPT C++ Library: %s - Sources timestamp: %s\n\n",
60 const std::string configFile = std::string(
argv[1]);
92 const string OUT_DIR_STD =
94 const char* OUT_DIR = OUT_DIR_STD.c_str();
97 const bool SAVE_POSE_LOG =
99 const bool SAVE_3D_SCENE =
101 const bool CAMERA_3DSCENE_FOLLOWS_ROBOT =
104 bool SHOW_PROGRESS_3D_REAL_TIME =
false;
105 int SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS = 0;
106 bool SHOW_LASER_SCANS_3D =
true;
111 SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS,
int,
params,
sect);
139 std::stringstream ss;
163 #if MRPT_HAS_WXWIDGETS
164 if (SHOW_PROGRESS_3D_REAL_TIME)
166 win3D = std::make_shared<CDisplayWindow3D>(
167 "ICP-SLAM @ MRPT C++ Library", 600, 500);
168 win3D->setCameraZoom(20);
169 win3D->setCameraAzimuthDeg(-45);
196 const bool isObsBasedRawlog = observation ? true :
false;
200 (!isObsBasedRawlog && !observations->empty() &&
201 *observations->begin() &&
205 isObsBasedRawlog ? observation->timestamp
206 : (*observations->begin())->timestamp;
209 std::vector<mrpt::obs::CObservation2DRangeScan::Ptr> lst_lidars;
212 if (isObsBasedRawlog)
215 static bool firstOdo =
true;
218 auto o = std::dynamic_pointer_cast<CObservationOdometry>(
220 if (!firstOdo) odoPose = odoPose + (o->odometry - lastOdo);
222 lastOdo = o->odometry;
229 action->getBestMovementEstimation();
230 if (act) odoPose = odoPose + act->poseChange->getMeanVal();
234 if (SHOW_LASER_SCANS_3D)
237 if (isObsBasedRawlog)
241 lst_lidars.push_back(
242 std::dynamic_pointer_cast<CObservation2DRangeScan>(
249 for (
size_t i = 0;; i++)
257 lst_lidars.push_back(new_obs);
265 if (isObsBasedRawlog)
269 t_exec = tictac.
Tac();
280 if (LOG_FREQUENCY > 0 && 0 == (step % LOG_FREQUENCY))
287 mrpt::format(
"%s/mapbuild_posepdf_%03u.txt", OUT_DIR, step);
289 "Saving pose log information to `%s`", str.c_str());
298 if ((LOG_FREQUENCY > 0 && 0 == (step % LOG_FREQUENCY)) ||
299 (SAVE_3D_SCENE || win3D))
307 view_map->setBorderSize(2);
308 view_map->setViewportPosition(0.01, 0.01, 0.35, 0.35);
309 view_map->setTransparent(
false);
323 groundPlane->setColor(0.4f, 0.4f, 0.4f);
324 view->insert(groundPlane);
328 if (CAMERA_3DSCENE_FOLLOWS_ROBOT)
330 scene->enableFollowCamera(
true);
348 pMap->getAs3DObject(ptsMap);
349 view_map->insert(ptsMap);
356 obj->setPose(robotPose);
361 obj->setPose(robotPose);
362 view_map->insert(obj);
366 if (SHOW_LASER_SCANS_3D)
368 for (
auto& lst_current_laser_scan : lst_lidars)
373 obj->setScan(*lst_current_laser_scan);
374 obj->setPose(robotPose);
375 obj->setSurfaceColor(1.0f, 0.0f, 0.0f, 0.5f);
382 if (LOG_FREQUENCY > 0 && 0 == (step % LOG_FREQUENCY) &&
386 mrpt::format(
"%s/buildingmap_%05u.3Dscene", OUT_DIR, step));
394 win3D->get3DSceneAndLock();
397 win3D->unlockAccess3DScene();
400 win3D->setCameraPointingToPoint(
401 robotPose.
x(), robotPose.
y(), robotPose.z());
404 win3D->forceRepaint();
406 std::this_thread::sleep_for(std::chrono::milliseconds(
407 SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS));
414 auto str =
mrpt::format(
"%s/log_MemoryUsage.txt", OUT_DIR);
423 "Memory usage:%.04f MiB", memUsage / (1024.0 * 1024.0));
428 "%i %f %f %f\n", step, robotPose.
x(), robotPose.
y(),
436 "%i %f %f %f\n", step, odoPose.
x(), odoPose.
y(), odoPose.
phi());
443 "----------- **END** (total time: %.03f sec) ---------",
450 auto str =
format(
"%s/_finalmap_.simplemap", OUT_DIR);
452 "Dumping final map in binary format to: %s\n", str.c_str());
459 auto str =
format(
"%s/_finalmaps_.txt", OUT_DIR);
464 if (win3D) win3D->waitForKey();
482 m_rawlogFileName = std::string(
argv[2]);
485 sect,
"rawlog_file", std::string(
"log.rawlog"),
true);
516 std::thread hSensorThread;
526 using namespace std::chrono_literals;
527 std::this_thread::sleep_for(2000ms);
529 if (m_allThreadsMustExit)
530 throw std::runtime_error(
531 "\n\n==== ABORTING: It seems that we could not connect to the "
532 "LIDAR. See reported errors. ==== \n");
547 if (m_allThreadsMustExit)
return false;
558 std::lock_guard<std::mutex> csl(m_cs_global_list_obs);
559 obs_copy = m_global_list_obs;
560 m_global_list_obs.clear();
563 for (
auto it = obs_copy.rbegin(); !new_obs && it != obs_copy.rend();
568 std::dynamic_pointer_cast<CObservation2DRangeScan>(
574 observation = std::move(new_obs);
579 using namespace std::chrono_literals;
580 std::this_thread::sleep_for(10ms);
597 std::string driver_name =
600 CGenericSensor::createSensorPtr(driver_name);
602 throw std::runtime_error(
603 std::string(
"***ERROR***: Class name not recognized: ") +
610 <<
" at " << sensor->getProcessRate() <<
" Hz" << std::endl;
613 sensor->getProcessRate() > 0,
614 "process_rate must be set to a valid value (>0 Hz).");
618 sensor->initialize();
619 while (!m_allThreadsMustExit)
624 sensor->getObservations(lstObjs);
626 std::lock_guard<std::mutex> lock(m_cs_global_list_obs);
627 m_global_list_obs.insert(lstObjs.begin(), lstObjs.end());
635 printf(
"[thread_%s] Closing...", tp.
section_name.c_str());
637 catch (
const std::exception& e)
639 std::cerr <<
"[SensorThread] Closing due to exception:\n"
641 m_allThreadsMustExit =
true;
645 std::cerr <<
"[SensorThread] Untyped exception! Closing." << std::endl;
646 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
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.3 at Fri May 15 15:49:54 UTC 2020 | |