Tutorial: Docking with Nova Carter Using Nav2 and FoundationPose in Sim
=======================================================================

Prerequisites
-------------
1. Finish the :doc:`Tutorial for FoundationPose with Isaac Sim </concepts/pose_estimation/foundationpose/tutorial_isaac_sim>`.

2. Download a pre-trained :ir_ngc:`SyntheticaDETR <teams/isaac/models/synthetica_detr>` model for AMR:

    .. code:: bash

       cd ${ISAAC_ROS_WS}/isaac_ros_assets/models/synthetica_detr &&  \
          wget 'https://api.ngc.nvidia.com/v2/models/nvidia/isaac/synthetica_detr/versions/1.0.0/files/sdetr_amr.etlt'

Instructions
------------

1. Set up Isaac Sim:

.. include:: _snippets/set_up_sim_w_dock.rst

2. Build and install the required packages:

.. include:: _snippets/build_docking_sim.rst

3. Continuing inside the Docker container, convert the encrypted model (``.etlt``) to a TensorRT engine plan:

   .. code:: bash

      /opt/nvidia/tao/tao-converter -k sdetr -t fp16 -e ${ISAAC_ROS_WS}/isaac_ros_assets/models/synthetica_detr/sdetr_amr.plan -p images,1x3x640x640,2x3x640x640,4x3x640x640 -p orig_target_sizes,1x2,2x2,4x2 ${ISAAC_ROS_WS}/isaac_ros_assets/models/synthetica_detr/sdetr_amr.etlt

4. Run the launch file:

   .. code:: bash

      ros2 launch nova_carter_docking docking_with_fp_sim.launch.py init_pose_x:=-2.0 init_pose_y:=14.0 init_pose_yaw:=3.14

   .. note::
   
      Verify that you see an ``RViz2`` window open. In ``RViz2``, change the ``Detection3DArray`` topic name to ``/tracking/output`` to display the 3D pose estimation bounding box overlaid on the input image.

5. Send a docking action:

    .. code:: bash

       ros2 action send_goal /dock_robot opennav_docking_msgs/action/DockRobot "
       {
           use_dock_id: false,
           dock_pose: {
               pose: {
               position: {x: -10.0, y: 14.0, z: 0.0},
               orientation: {x: 0.0, y: 0.0, z: 1.0, w: 0.0}
               },
               header: {
               frame_id: 'map'
               }
           },
           dock_type: 'nova_carter_dock',
           navigate_to_staging_pose: true
       }"

.. figure:: :ir_lfs:`<resources/isaac_ros_docs/robots/nova_carter/docking_with_fp_sim.gif>`
   :align: center
   :width: 800px

   Docking with Nova Carter in Isaac Sim Using FoundationPose

Troubleshooting
---------------

1. If the Nova Carter does not establish contact with the dock after the docking process is complete, consider
   adjusting the ``external_detection_translation_x`` parameter in
   ``/workspaces/isaac_ros-dev/src/nova_carter/nova_carter_docking/params/nova_carter_docking_fp.yaml``.
   The default value is ``-0.15``. You may increase this value to bring the Nova Carter closer to the dock.
   More details for docking parameters can be found on `GitHub <https://github.com/open-navigation/opennav_docking/tree/main?tab=readme-ov-file#configuration>`__.

2. If you encounter a situation where the robot cannot complete the docking goal using `FoundationPose`, 
   it may be due to the limited performance of your workstation. In our example, we are using a workstation with the following specifications:

   - CPU: Intel Core i9-14900K, 24 cores, 6.0 GHz
   - GPU: NVIDIA RTX 6000 Ada Generation
   - RAM: 128 GB DDR5

   We rely on pose estimation at the staging location and dock using the tracking results. As the robot gets closer to the dock, detection becomes unreliable due to the limited field of view. 
   To address this, you may need to increase the `reset_period` parameter in the launch file :ir_repo:`here <isaac_ros_pose_estimation> <isaac_ros_foundationpose/launch/isaac_ros_foundationpose_isaac_sim_tracking.launch.py>` 
   or consider launching the `fp_dock_pose_publisher` in the launch file :ir_repo:`here <nova_carter> <nova_carter_docking/launch/docking_with_fp_sim.launch.py>` separately at the staging location when the pose estimation is accurate.