Tutorial for Visual SLAM with Isaac Sim
=======================================

.. figure:: :ir_lfs:`<resources/isaac_ros_docs/concepts/visual_slam/cuvslam/After_localization.png>`
    :align: center
    :alt: After Localization
    :width: 600px

Overview
------------

This tutorial walks you through a graph to estimate 3D pose of the
camera with :ir_repo:`Visual SLAM <isaac_ros_visual_slam>`
using images from Isaac Sim.

Tutorial Walkthrough
--------------------

1.  Complete the :ref:`quickstart <repositories_and_packages/isaac_ros_visual_slam/isaac_ros_visual_slam/index:quickstart>`.

2.  Launch the Docker container using the ``run_dev.sh`` script:

    .. code:: bash

       cd ${ISAAC_ROS_WS}/src/isaac_ros_common && \
         ./scripts/run_dev.sh

3.  Install and launch Isaac Sim following the steps in the :doc:`Isaac ROS Isaac Sim Setup Guide </getting_started/isaac_sim/index>`

4.  Press **Play** to start publishing data from the Isaac Sim.

    .. figure:: :ir_lfs:`<resources/isaac_ros_docs/getting_started/isaac_sim_sample_scene.png>`
        :align: center
        :width: 600px

5.  In a separate terminal, start ``isaac_ros_visual_slam`` using the
    launch files:

    .. code:: bash

       ros2 launch isaac_ros_visual_slam isaac_ros_visual_slam_isaac_sim.launch.py

7.  In a separate terminal, send the signal to move the robot about as follows:

    .. code:: bash

       ros2 topic pub --once /cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.2, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.2}}"

7.  In a separate terminal, spin up RViz with default configuration file
    to see the rich visualizations as the robot moves.

    .. code:: bash
    
       rviz2 -d $(ros2 pkg prefix isaac_ros_visual_slam --share)/rviz/isaac_sim.cfg.rviz

    .. figure:: :ir_lfs:`<resources/isaac_ros_docs/concepts/visual_slam/cuvslam/Rviz_isaac_sim.png>`
        :align: center
        :width: 600px

8.  To see the odometry messages, in a separate terminal echo the contents of the ``/visual_slam/tracking/odometry`` topic with the
    following command:

    .. code:: bash

       ros2 topic echo /visual_slam/tracking/odometry

    .. figure:: :ir_lfs:`<resources/isaac_ros_docs/concepts/visual_slam/cuvslam/Terminal_output.png>`
        :align: center
        :width: 600px

Saving and Using the Map
------------------------

As soon as you start the visual SLAM node, it starts storing the
landmarks and the pose graph. 

To save them in a map and store the
map onto a disk:

1. Make a call to the ``SaveMap`` ROS 2 service with the
following command:

  .. note::

    ``/path/to/save/the/map`` must be a new empty directory
    every time you call this service because this service overwrites the
    existing contents.

.. code:: bash

   ros2 service call /visual_slam/save_map isaac_ros_visual_slam_interfaces/srv/FilePath "{file_path: /path/to/save/the/map}"


.. figure:: :ir_lfs:`<resources/isaac_ros_docs/concepts/visual_slam/cuvslam/Save_map.png>`
    :align: center
    :width: 600px

.. figure:: :ir_lfs:`<resources/isaac_ros_docs/concepts/visual_slam/cuvslam/Rviz_isaac_sim_mapping.png>`
    :align: center
    :width: 600px

2. To verify, try to load and localize in the previously saved map.
Stop the ``visual_slam`` node launched for creating and saving the map, then relaunch it.

3. Use the following command to load the map from the disk and provide an
approximate start location (prior). The orientation is currently ignored.

.. code:: bash

   ros2 service call /visual_slam/localize_in_map isaac_ros_visual_slam_interfaces/srv/LocalizeInMap "
     map_folder_path: '/path/to/save/the/map'
     pose_hint:
       position:
         x: x-position
         y: y-position
         z: z-position
       orientation:
         x: 0.0
         y: 0.0
         z: 0.0
         w: 1.0"


.. figure:: :ir_lfs:`<resources/isaac_ros_docs/concepts/visual_slam/cuvslam/Load_and_localize.png>`
    :align: center
    :width: 600px

After the above step returns success, you have successfully loaded and
localized your robot in the map. 

If it results in failure, you might have current landmarks from the approximate start
location that are not matching with stored landmarks and you need to provide
another valid value.

.. figure:: :ir_lfs:`<resources/isaac_ros_docs/concepts/visual_slam/cuvslam/Before_localization.png>`
    :align: center
    :alt: Before Localization
    :width: 600px

.. figure:: :ir_lfs:`<resources/isaac_ros_docs/concepts/visual_slam/cuvslam/After_localization.png>`
    :align: center
    :alt: After Localization
    :width: 600px