Imported upstream version '1.2.0' of 'upstream'

.github/workflows/build-ros.yml

@@ -62,6 +62,7 @@ jobs:
       - name: setup ROS environment
         uses: ros-tooling/setup-ros@v0.7
         with:
+          use-ros2-testing: true
           required-ros-distributions: ${{ matrix.ros_distribution }}
 
       - name: Install rosdep dependencies

docs/source/Doxyfile

@@ -704,7 +704,7 @@ LAYOUT_FILE            = doxygen-layout.xml
 # LATEX_BIB_STYLE. To use this feature you need bibtex and perl available in the
 # search path. See also \cite for info how to create references.
 
-CITE_BIB_FILES         =
+CITE_BIB_FILES         = refs.bib
 
 #---------------------------------------------------------------------------
 # Configuration options related to warning and progress messages

docs/source/building-maps.rst

@@ -90,6 +90,7 @@ or from the :ref:`UI controls <mola_lo_gui_common_parts>` in the ``mola_lidar_od
                 MOLA_LIDAR_TOPIC=/ouster/points \
                 MOLA_GENERATE_SIMPLEMAP=true \
                 MOLA_SIMPLEMAP_OUTPUT=myMap.simplemap \
+                MOLA_SIMPLEMAP_GENERATE_LAZY_LOAD=true \
                   mola-lo-gui-rosbag2 /path/to/your/dataset.mcap
 
             .. note::
@@ -99,6 +100,7 @@ or from the :ref:`UI controls <mola_lo_gui_common_parts>` in the ``mola_lidar_od
 
             .. code-block:: bash
 
+                MOLA_SIMPLEMAP_GENERATE_LAZY_LOAD=true \
                 mola-lidar-odometry-cli \
                   -c $(ros2 pkg prefix mola_lidar_odometry)/share/mola_lidar_odometry/pipelines/lidar3d-default.yaml \
                   --input-rosbag2 /path/to/your/dataset.mcap \
@@ -109,6 +111,25 @@ or from the :ref:`UI controls <mola_lo_gui_common_parts>` in the ``mola_lidar_od
             .. note::
                 Remember changing ``--lidar-sensor-label /ouster/points`` to your actual raw (unfiltered) LiDAR topic (``sensor_msgs/PointCloud2``).
 
+        .. tab-item:: From a rosbag2 (CLI) (Large datasets)
+
+            For maps large enough such as the final `.simplemap` does not fit in RAM, you can enable
+            lazy-load simplemap generation in the CLI with:
+
+            .. code-block:: bash
+
+                MOLA_GENERATE_SIMPLEMAP=true \
+                MOLA_SIMPLEMAP_GENERATE_LAZY_LOAD=true \
+                MOLA_SIMPLEMAP_OUTPUT=myMap.simplemap \
+                mola-lidar-odometry-cli \
+                  -c $(ros2 pkg prefix mola_lidar_odometry)/share/mola_lidar_odometry/pipelines/lidar3d-default.yaml \
+                  --input-rosbag2 /path/to/your/dataset.mcap \
+                  --lidar-sensor-label /ouster/points \
+                  --output-tum-path trajectory.tum
+
+            .. note::
+                Remember changing ``--lidar-sensor-label /ouster/points`` to your actual raw (unfiltered) LiDAR topic (``sensor_msgs/PointCloud2``).
+
 
 .. hint::
 
@@ -210,13 +231,16 @@ Afterwards, visualizing :ref:`metric map files <mp2p_icp_basics>` (``*.mm``) can
         .. tab-item:: Build an aggregated 3D point cloud
           :selected:
 
-            Download the example pipeline `sm2mm_pointcloud_voxelize.yaml <https://github.com/MOLAorg/mp2p_icp/raw/master/demos/sm2mm_pointcloud_voxelize.yaml>`_
+            Download the example pipeline `sm2mm_pointcloud_voxelize_no_deskew.yaml <https://github.com/MOLAorg/mp2p_icp/raw/master/demos/sm2mm_pointcloud_voxelize_no_deskew.yaml>`_
             and then run:
 
             .. code-block:: bash
 
                 # Build metric map (mm) from simplemap (sm):
-                sm2mm -i mvsim-warehouse01.simplemap -o mvsim-warehouse01.mm -p sm2mm_pointcloud_voxelize.yaml
+                sm2mm \
+                 -i mvsim-warehouse01.simplemap \
+                 -o mvsim-warehouse01.mm \
+                 -p sm2mm_pointcloud_voxelize_no_deskew.yaml
 
                 # View mm:
                 mm-viewer mvsim-warehouse01.mm
@@ -225,13 +249,16 @@ Afterwards, visualizing :ref:`metric map files <mp2p_icp_basics>` (``*.mm``) can
 
         .. tab-item:: Build a voxel map + 2D grid map
 
-            Download the example pipeline `sm2mm_bonxai_voxelmap_gridmap.yaml <https://github.com/MOLAorg/mp2p_icp/raw/master/demos/sm2mm_pointcloud_voxelize.yaml>`_
+            Download the example pipeline `sm2mm_bonxai_voxelmap_gridmap_no_deskew.yaml <https://github.com/MOLAorg/mp2p_icp/raw/master/demos/sm2mm_bonxai_voxelmap_gridmap_no_deskew.yaml>`_
             and then run:
 
             .. code-block:: bash
 
                 # Build metric map (mm) from simplemap (sm):
-                sm2mm -i mvsim-warehouse01.simplemap -o mvsim-warehouse01.mm -p sm2mm_bonxai_voxelmap_gridmap.yaml
+                sm2mm \
+                  -i mvsim-warehouse01.simplemap \
+                  -o mvsim-warehouse01.mm \
+                  -p sm2mm_bonxai_voxelmap_gridmap_no_deskew.yaml
 
                 # View mm:
                 mm-viewer mvsim-warehouse01.mm
@@ -273,16 +300,43 @@ Please, read carefully `its documentation <https://github.com/mrpt-ros-pkg/mrpt_
 
   .. image:: https://mrpt.github.io/imgs/screenshot-rviz2-mrpt-map-server-demo-warehouse.png
 
+|
+
+6. See the inner workings of ICP
+----------------------------------------
+If you are interested in learning about the internal workings of each ICP optimization,
+or if there is something wrong at some particular timestamp and want to **debug it**,
+you can enable the generation of :ref:`ICP log files <pipeline_icp_log_files>`
+and then visualize them with the GUI app :ref:`icp-log-viewer <app_icp-log-viewer>`.
+
+.. dropdown:: Debug ICP
+
+  First, re-run MOLA LO enabling the generation of ICP log files (see :ref:`all the available options <pipeline_icp_log_files>`):
+
+  .. code-block:: bash
+
+      # Generate ICP log files:
+      MP2P_ICP_GENERATE_DEBUG_FILES=1 \
+      MP2P_ICP_LOG_FILES_DECIMATION=1 \
+      mola-lo-gui-rosbag  [...]  # the rest remains the same
+
+  You should now have a directory ``icp-logs`` with as many files as times ICP invocations.
+  Then, visualize the logs in the GUI with:
+
+  .. code-block:: bash
+
+      # Open the logs:
+      icp-log-viewer -d icp-logs/ -l libmola_metric_maps.so
+
 
 |
 
 
-6. What's next?
+7. What's next?
 ----------------------------------------
+Once you have a map, here are some next steps:
 
-Write me:
-
-- georeferencing
-- loop closure
-- Use for localization
+- loop closure (Write me!)
+- :ref:`geo-referencing`.
+- :ref:`Use for localization <localization-only>`.
 

docs/source/geo-referencing.rst

@@ -8,18 +8,51 @@ Georeferencing trajectories and metric maps is implemented in the :ref:`mola_sm_
 The concept of using simple-maps as intermediary map format together with the layered metric map format (see :cite:`blanco2024mola_lo`)
 enables embedding georeferenced coordinates to any kind of map typically used in robotics: grid maps, voxel maps, point clouds, etc.
 
-.. image:: https://mrpt.github.io/imgs/kaist01_georef_sample.png
-
 |
 
+.. image:: https://mrpt.github.io/imgs/kaist01_georef_sample.png
+   :width: 250
+   :align: right
+
 .. contents:: Table of Contents
    :depth: 1
    :local:
    :backlinks: none
 
 |
 
-1. How to build a georeferenced map
+1. Frames for geo-referenced maps
+--------------------------------------------
+When working with ROS ``tf`` (transformations), MOLA packages use the following frame convention, 
+which extends the standard `REP-105 <https://www.ros.org/reps/rep-0105.html>`_ with additional
+``enu`` and ``utm`` frames:
+
+.. figure:: https://mrpt.github.io/imgs/mola_mrpt_ros_geo_referenced_utm_frames.png
+   :width: 500
+   :align: center
+
+These are the existing frames:
+
+- ``base_link``: The robot reference frame. For ground vehicles, normally placed at the
+  center of the rear axle.
+- ``odom``: The arbitrary origin for odometry measurements.
+- ``map``: The origin of the reference metric map used for localization.
+- ``enu``: For geo-referenced maps, the North (``y`` axis), East (``x`` axis), Up (``z`` axis) frame for which
+  we have reference geodetic coordinates (latitude and longitude). Different maps built in the same zone
+  will surely have different ``enu`` frames, since it is defined by collected GNSS measurements.
+- ``utm``: The origin of the `UTM zone <https://en.wikipedia.org/wiki/Universal_Transverse_Mercator_coordinate_system>`_
+  in which ``enu`` falls. Unlike ``enu``, it is **independent** of the trajectory followed while building the map.
+
+And this is who is responsible of publishing each transformation:
+
+- ``odom → base_link``: Odometry module. High-frequency, accurate in the short term, but drifts in the long term.
+- ``map → odom``: :ref:`Localization <localization>` module, which corrects the odometry drift.
+- ``enu → {map, utm}``: Published by ``mrpt_map_server`` (`github <https://github.com/mrpt-ros-pkg/mrpt_navigation/tree/ros2/mrpt_map_server/>`_),
+  if fed with a geo-referenced metric map (``.mm``) file.
+
+|
+
+2. How to build a georeferenced map
 --------------------------------------------
 First, build a simple-map from a dataset or a live robot as described in :ref:`building-maps`.
 Make sure of having a GPS (GNSS) sensor source emitting observations, and that they were captured
@@ -41,6 +74,15 @@ map file, use:
       # georeference it:
       mola-sm-georeferencing -i datasetWithGPS.simplemap --write-into myMap.mm
 
+Alternatively, the georeferenciation metadata can be also stored, independently of a metric map,
+in an independent file with:
+
+.. code-block:: bash
+
+      # georeference it:
+      mola-sm-georeferencing -i datasetWithGPS.simplemap --output myMap.georef
+
+
 
 .. dropdown:: Full CLI reference
    :icon: code-review
@@ -49,9 +91,10 @@ map file, use:
 
       USAGE:
 
-         mola-sm-georeferencing  [-v <INFO>] [-l <foobar.so>] [--write-into
-                                    <map.mm>] -i <map.simplemap> [--]
-                                    [--version] [-h]
+         mola-sm-georeferencing  [-v <INFO>] [-l <foobar.so>]
+                                 [--horizontality-sigma <1.0>] [-o <map.georef>]
+                                 [--write-into <map.mm>] -i <map.simplemap> [--]
+                                 [--version] [-h]
 
 
       Where: 
@@ -63,6 +106,13 @@ map file, use:
          One or more (comma separated) *.so files to load as plugins, e.g.
          defining new CMetricMap classes
 
+         --horizontality-sigma <1.0>
+         For short trajectories (not >10x the GPS uncertainty), this helps to
+         avoid degeneracy.
+
+         -o <map.georef>,  --output <map.georef>
+         Write the obtained georeferencing metadata to a .georef file
+
          --write-into <map.mm>
          An existing .mm file in which to write the georeferencing metadata
 
@@ -78,9 +128,10 @@ map file, use:
          -h,  --help
          Displays usage information and exits.
 
+
 |
 
-2. Georeferenced trajectories
+3. Georeferenced trajectories
 --------------------------------------
 Once you already have a **trajectory** file in the **local map frame of reference**,
 for example, as generated by MOLA-LO in TUM format,
@@ -95,8 +146,9 @@ for example in KML format suitable for visualization in Google Earth.
 
       USAGE:
 
-         mola-trajectory-georef  -o <path.kml> -t <traj.tum> -m <map.mm> [--]
-                                    [--version] [-h]
+         mola-trajectory-georef  -o <path.kml> -t <traj.tum> [-g <map.georef>]
+                                 [-m <map.mm>] [--] [--version] [-h]
+
 
       Where: 
 
@@ -106,8 +158,11 @@ for example in KML format suitable for visualization in Google Earth.
          -t <traj.tum>,  --trajectory <traj.tum>
          (required)  Input .tum trajectory, in map local coordinates
 
+         -g <map.georef>,  --geo-ref <map.georef>
+         Input .georef file with georef info
+
          -m <map.mm>,  --map <map.mm>
-         (required)  Input .mm map with georef info
+         Input .mm map with georef info
 
          --,  --ignore_rest
          Ignores the rest of the labeled arguments following this flag.
@@ -121,7 +176,7 @@ for example in KML format suitable for visualization in Google Earth.
 
 |
 
-3. Georeferenced maps in mm-viewer
+4. Georeferenced maps in mm-viewer
 ----------------------------------------
 Write me!
 

docs/source/index.rst

@@ -26,6 +26,7 @@ MOLA
   mola_lidar_odometry
   mola_lo_apps
   mola_lo_pipelines
+  wrappers_3rd_party
 
 .. toctree::
   :maxdepth: 2
@@ -241,7 +242,7 @@ How to install all MOLA modules:
     .. code-block:: bash
 
         cd ~/ros2_mola_ws/
-        colcon  build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release
+        colcon  build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo
 
 
     Next, activate the new environment and check if all new modules are visible:

docs/source/localization.rst

@@ -42,6 +42,7 @@ pose in the environment:
 
 |
 
+----
 
 2. Localization with LiDAR odometry
 --------------------------------------
@@ -51,6 +52,62 @@ Write me!
 |
 
 
+----
+
 3. Localization with particle filtering
 ----------------------------------------
-Write me!
+To localize with a particle filter (PF) you will need:
+
+1. A metric map (the ``.mm`` file) of the environment. Built as described in: :ref:`building-maps`.
+2. To publish that map using `mrpt_map_server <https://github.com/mrpt-ros-pkg/mrpt_navigation/tree/ros2/mrpt_map_server/>`_.
+3. Launch `mrpt_pf_localization <https://github.com/mrpt-ros-pkg/mrpt_navigation/tree/ros2/mrpt_pf_localization>`_ and configure it
+   to read from wheels odometry (if they exist), the input sensors (e.g. LiDAR), GPS (GNSS) if present. This node will subscribe to
+   the published metric map and use the sensors to publish updates on the robot pose in the `map` frame.
+
+.. note::
+
+   Do **NOT** set the raw LiDAR pointcloud as input for the PF. It is just too large and it must be subsampled first.
+   We can use `mrpt_pointcloud_pipeline <https://github.com/mrpt-ros-pkg/mrpt_navigation/tree/ros2/mrpt_pointcloud_pipeline>`_
+   for such task, as can be seen in the tutorial below.
+
+3.1. Tutorial
+================
+A complete demonstration has been put together on: https://github.com/MOLAorg/mola_warehouse_pf_tutorial
+
+.. raw:: html
+
+   <div style="width: 100%; overflow: hidden;">
+     <video controls autoplay loop muted style="width: 512px;">
+       <source src="https://mrpt.github.io/videos/mola-localization-pf-demo-warehouse.mp4" type="video/mp4">
+     </video>
+   </div>
+
+Key points of this tutorial:
+
+- The ROS 2 `launch file <https://github.com/MOLAorg/mola_warehouse_pf_tutorial/blob/develop/launch/tutorial_launch.py>`_.
+- A `custom sm2mm pipeline file <https://github.com/MOLAorg/mola_warehouse_pf_tutorial/tree/develop/sm2mm-config>`_ to create
+  a reference point cloud map sparse enough so localizing with a PF is not too slow.
+- A `pipeline for mrpt_pointcloud_pipeline <https://github.com/MOLAorg/mola_warehouse_pf_tutorial/blob/develop/params/point-cloud-pipeline.yaml>`_
+  to decimate the input raw 3D scan as input to the PF.
+
+
+.. dropdown:: How to run the tutorial
+
+   Clone the tutorial package (which already includes a prebuilt ``.mm``), make sure of having all dependencies,
+   build and run it:
+
+   .. code-block:: bash
+
+         cd ~/ros2_ws/src
+         git clone https://github.com/MOLAorg/mola_warehouse_pf_tutorial.git
+
+         cd ~/ros2_ws
+         rosdep install --from-paths src --ignore-src -r -y
+
+         colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo
+         . install/setup.bash
+
+         ros2 launch mola_warehouse_pf_tutorial tutorial_launch.py
+
+
+