Progress with docs and tutorial on localization-mode

MOLAorg · Dec 13, 2024 · 2a1cfae · 2a1cfae
1 parent b761576
commit 2a1cfae
Show file tree

Hide file tree

Showing 5 changed files with 161 additions and 44 deletions.
diff --git a/docs/source/building-maps.rst b/docs/source/building-maps.rst
@@ -357,6 +357,7 @@ and then visualize them with the GUI app :ref:`icp-log-viewer <app_icp-log-viewe
 ----------------------------------------
 Once you have a map, here are some next steps:
 
+- :ref:`This tutorial <tutorial-mola-lo-map-and-localize>` on how to save/load maps and re-localize using MOLA-LO ROS 2 nodes.
 - loop closure (Write me!)
 - :ref:`geo-referencing`.
 - :ref:`Use for localization <localization-only>`.

diff --git a/docs/source/index.rst b/docs/source/index.rst
@@ -187,13 +187,17 @@ How to install all MOLA modules:
     +=========================+===============+=====================+=====================+
     | ROS 2 Humble (u22.04)   |  |badgeHrel|  |   |badgeHrel_LO|    |   |badgeHrel_MP|    |
     +-------------------------+---------------+---------------------+---------------------+
-    | ROS 2 Iron (u22.04)     |  |badgeIrel|  |   |badgeIrel_LO|    |   |badgeIrel_MP|    |
-    +-------------------------+---------------+---------------------+---------------------+
     | ROS 2 Jazzy (u24.04)    |  |badgeJrel|  |   |badgeJrel_LO|    |   |badgeJrel_MP|    |
     +-------------------------+---------------+---------------------+---------------------+
     | ROS 2 Rolling (u24.04)  |  |badgeRrel|  |   |badgeRrel_LO|    |   |badgeRrel_MP|    |
     +-------------------------+---------------+---------------------+---------------------+
 
+    +-------------------------+---------------+---------------------+---------------------+
+    | EOL ROS distribution    |      MOLA     | mola_lidar_odometry |   mp2p_icp          |
+    +=========================+===============+=====================+=====================+
+    | ROS 2 Iron (u22.04)     |  |badgeIrel|  |   |badgeIrel_LO|    |   |badgeIrel_MP|    |
+    +-------------------------+---------------+---------------------+---------------------+
+
 
 .. dropdown:: Build from sources
     :icon: code-square

diff --git a/docs/source/localization.rst b/docs/source/localization.rst
@@ -26,6 +26,9 @@ ____________________________________________
 
 |
 
+.. _localization-only_common:
+
+
 1. Common aspects
 --------------------------------------
 Independently of the localization algorithm, the problem of finding the *rough* localization
@@ -46,11 +49,23 @@ pose in the environment:
 
 2. Localization with LiDAR odometry
 --------------------------------------
-Write me!
+See the step-by-step instructions in :ref:`this tutorial <tutorial-mola-lo-map-and-localize>`.
+As a summary, you need to:
 
+* Launch MOLA-LO in localization-only mode.
+* Load a local map into it. In MOLA-LO, a local map comprises two files, a ``*.mm`` metric map and the keyframes in a ``*.simplemap`` file.
+  (In fact: keyframes are only required if you want to extend the map, i.e. multi-session mapping).
+* Request relocalization via a service call or via the topic ``/initialpose``.
 
-|
+This video shows the steps in the tutorial:
 
+.. raw:: html
+
+    <div style="margin-top:10px;">
+      <iframe width="560" height="315" src="https://www.youtube.com/embed/xxxx" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture; web-share" referrerpolicy="strict-origin-when-cross-origin" allowfullscreen></iframe>
+    </div>
+
+|
 
 ----
 
@@ -70,8 +85,8 @@ To localize with a particle filter (PF) you will need:
    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
-================
+3.1. PF localization tutorial
+================================
 A complete demonstration has been put together on: https://github.com/MOLAorg/mola_warehouse_pf_tutorial
 
 .. raw:: html

diff --git a/docs/source/ros2api.rst b/docs/source/ros2api.rst
@@ -88,6 +88,8 @@ the corresponding checkbox in the
 
 ----
 
+.. _mola_ros2api_relocalization:
+
 3. Re-localization
 --------------------------------------
 Write me!
@@ -108,8 +110,7 @@ These frames of reference exist when using MOLA :ref:`geo-referenced <geo-refere
 
 .. note::
 
-   For non geo-referenced maps, the meaning of all frames are the same but ``utm`` and ``enu``
-   will not be present.
+   For non geo-referenced maps, all frames remain the same but ``utm`` and ``enu`` will not exist.
 
 These are the existing frames:
 

diff --git a/docs/source/tutorial-mola-lo-map-and-localize.rst b/docs/source/tutorial-mola-lo-map-and-localize.rst
@@ -14,78 +14,174 @@ and how to load that map to use the LO localization mode.
 
 |
 
-MOLA installation
+This video shows the steps in the tutorial:
+
+.. raw:: html
+
+    <div style="margin-top:10px;">
+      <iframe width="560" height="315" src="https://www.youtube.com/embed/xxxx" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture; web-share" referrerpolicy="strict-origin-when-cross-origin" allowfullscreen></iframe>
+    </div>
+
+|
+
+Prerequisites: MOLA installation
 ----------------------------------
-This tutorial requires the installation of these MOLA packages: ``mola_demos``, ``mola_viz``.
+This tutorial requires the installation of these packages: ``mola_lidar_odometry``, ``mola_viz``, ``mvsim``.
+The MVSim simulator is used as an example, but a live robot or LiDAR sensor can be used instead.
 
 Following the default :ref:`installation instructions <installing>` is enough.
 
-It also uses the MVSim simulator, although a live robot or LiDAR sensor can be used instead.
 
 |
 
-Create a map and save it
+1. Create a map and save it
 ----------------------------------
 
-Open three (3) terminals, and run these commands in each one:
+Open **three terminals**, and run these commands in each one:
+
+.. tab-set::
+
+    .. tab-item:: #1: Simulator
+      :selected:
+
+      Launch the simulator or, if you are on a real robot, make sure to have the system up
+      and LiDAR data coming out:
+
+      .. code-block:: bash
+
+          ros2 launch mvsim demo_warehouse.launch.py \
+            do_fake_localization:=False \
+            use_rviz:=False
+
+
+    .. tab-item:: #2: MOLA-LO
+
+        In terminal #2, launch MOLA-LO, enabling saving the map in simple-map format:
+
+        .. code-block:: bash
+
+            MOLA_GENERATE_SIMPLEMAP=true \
+            ros2 launch mola_lidar_odometry ros2-lidar-odometry.launch.py \
+              lidar_topic_name:=/lidar1_points
+
+        .. note::
+
+          Remember replacing ``/lidar1_points`` with your actual PointCloud2 topic with raw LiDAR data.
+
+        Next, **move the robot around**.
+        In the simulator, you can click on the MVSim GUI and use keys ``a/d``+ ``s/w`` to drive around,
+        or `use a gamepad to teleoperate it <https://mvsimulator.readthedocs.io/en/latest/teleoperation.html>`_.
+
+    .. tab-item:: #3: Save the map
+
+        Once the map looks OK in the mola_viz GUI, let's save it.
+        In terminal #3, run:
 
-In terminal #1, launch the simulator (or your custom launch for your real sensor):
+        .. code-block:: bash
 
-.. code-block:: bash
+            ros2 service call /map_save mola_msgs/srv/MapSave "map_path: '/tmp/my_map'"
+
+        Watch the response to check that ``success`` is ``true``. Your map is now
+        stored as file(s) named ``/tmp/my_map*``.
 
-    ros2 launch mvsim demo_warehouse.launch.py \
-      do_fake_localization:=False \
-      use_rviz:=False
 
 |
 
-In terminal #2, launch MOLA-LO, enabling saving the map in simple-map format:
 
-.. code-block:: bash
+2. Load a prebuilt map in localize-only mode
+---------------------------------------------
+
+..  note::
+
+  Make sure of closing the former instance of MOLA-LO in terminal #2 used to build the map
+  before going on with this part.
+
+Again, we will use **three terminals**:
+
+.. tab-set::
+
+    .. tab-item:: #1: Simulator
+
+      You can keep the former instance of the MVSim simulator running from the
+      former step, or launch it again and move to a different pose, it is up to you!
+
+    .. tab-item:: #2: MOLA-LO in localization mode
+      :selected:
+
+      Let's launch MOLA-LO in **non-mapping (localization only) mode** with:
+
+      .. code-block:: bash
 
-    MOLA_GENERATE_SIMPLEMAP=true \
-    ros2 launch mola_lidar_odometry ros2-lidar-odometry.launch.py \
-      lidar_topic_name:=/lidar1_points
+          MOLA_MAPPING_ENABLED=false \
+          ros2 launch mola_lidar_odometry ros2-lidar-odometry.launch.py \
+            lidar_topic_name:=/lidar1_points
 
-.. note::
+      .. note::
 
-  Remember replacing ``/lidar1_points`` with your actual PointCloud2 topic with raw LiDAR data.
+        Remember replacing ``/lidar1_points`` with your actual PointCloud2 topic with raw LiDAR data.
 
-Next, move the robot around. In the simulator, you can click on the MVSim GUI and use keys ``asdw`` to drive around, or use a gamepad.
+    .. tab-item:: #3: Load the map
+
+        Next, in terminal #3, let's order MOLA-LO to **load our former map** from disk:
+
+        .. code-block:: bash
+
+            ros2 service call /map_load mola_msgs/srv/MapLoad "map_path: '/tmp/my_map'"
+
+
+
+        Note that it is also possible to directly launch MOLA-LO with a map loaded from disk
+        from the beginning, but it implies passing the two 
 
 |
 
-Once the map looks OK in the mola_viz GUI, let's save it.
-In terminal #3, run:
+.. dropdown:: Directly loading the map from MOLA-LO start up
+
+    Instead of first invoking MOLA-LO and then requesting to load the map via a ROS 2 service,
+    it is possible to instruct MOLA-LO to start loading the map straightaway as it starts up
+    by specifying the path to **both** map files, instead of the **map prefix** used
+    in the ROS 2 service:
 
-.. code-block:: bash
+    .. code-block:: bash
 
-    ros2 service call /map_save mola_msgs/srv/MapSave "map_path: '/tmp/my_map'"
+        MOLA_MAPPING_ENABLED=false \
+        MOLA_LOAD_MM=/tmp/my_map.mm \
+        MOLA_LOAD_SM=/tmp/my_map.simplemap \
+        ros2 launch mola_lidar_odometry ros2-lidar-odometry.launch.py \
+          lidar_topic_name:=/lidar1_points
 
+    Of course, the ROS 2 service offers a greater flexibility to switch
+    between maps at run-time.
 
-Use a prebuilt map in localize-only mode
--------------------------------------------
 
-Keeping terminal #1 open as explained above with the simulator running,
-go to terminal #2 and launch MOLA-LO in **non-mapping (localization only) mode** with:
+|
+
 
-.. code-block:: bash
+3. Invoke relocalization
+---------------------------------------------
 
-    MOLA_MAPPING_ENABLED=false \
-    ros2 launch mola_lidar_odometry ros2-lidar-odometry.launch.py \
-      lidar_topic_name:=/lidar1_points
+As explained :ref:`here <localization-only_common>`, initial localization is a hard problem on its own
+and can be handled in different ways.
 
-.. note::
+Here we will show the common situation of wanting to re-localize the robot in a prebuilt map,
+given we already know **a rough estimation of its actual pose**, including the orientation.
+Check out :ref:`all the details <mola_ros2api_relocalization>` about requesting
+relocalization via ROS 2 API.
 
-  Remember replacing ``/lidar1_points`` with your actual PointCloud2 topic with raw LiDAR data.
 
-Next, in terminal #3, let's order MOLA-LO to **load our former map** from disk:
+.. tab-set::
 
-.. code-block:: bash
+    .. tab-item:: Re-localize with a topic
+      :selected:
 
-    ros2 service call /map_load mola_msgs/srv/MapLoad "map_path: '/tmp/my_map'"
+      Just use the RViz2's button ``2D pose estimate`` or FoxGlove's "Pose Estimate"
+      to pick a pose and MOLA-LO will try to re-localize the vehicle in the given area.
 
+    .. tab-item:: Re-localize with a service
 
+      There is also a ROS 2 service for programmatically request a relocalization, and
+      obtaining feedback about whether the request was received or not by a running MOLA module:
 
-Re-localization modes: Write me!
+      - Service default name: ``/relocalize_near_pose``
+      - Service interface: `mola_msgs::srv::RelocalizeNearPose <https://docs.ros.org/en/rolling/p/mola_msgs/interfaces/srv/RelocalizeNearPose.html>`_