.. _isaac_sim_app_tutorial_ros2_navigation: =============================== ROS2 Navigation =============================== Learning Objectives ======================= In this ROS2 sample, we are demonstrating |isaac-sim| integrated with ROS2 Nav2. Getting Started =========================== **Prerequisite** - This ROS2 Navigation sample is only supported on ROS2 Foxy Fitzroy or later. See the ROS2 website for requirements and installation instructions: ``_ - The Nav2 project is required to run this sample. To install Nav2 refer to the `Nav2 installation page `_. - Enabled the omni.isaac.ros2_bridge extension from the extension manager menu `Window->Extensions`. - This tutorial requires ``carter_navigation``, ``carter_description``, and ``isaac_ros_navigation_goal`` ROS2 packages which are provided as part of your |isaac-sim| download. These ROS2 packages are located under the directory ``/src/navigation/``. They contain the required launch file, navigation parameters, and robot model. Complete :ref:`isaac_sim_app_install_ros`, make sure the ROS2 workspace environment is setup correctly. Nav2 Setup ====================== This block diagram shows the ROS2 messages required for Nav2. .. figure:: /content/images/isaac_sample_ros2_nav_1.png :align: center :width: 800 :alt: ROS2 Nav2 Overview Block Diagram As described above, the following topics and message types being published to Nav2 in this scenario are: =========== ====================== ROS2 Topic ROS2 Message Type =========== ====================== /tf tf2_msgs/TFMessage /odom nav_msgs/Odometry /map nav_msgs/OccupancyGrid /scan sensor_msgs/LaserScan =========== ====================== .. _isaac_sim_app_carter_ros2_omnigraph: Carter_ROS OmniGraph Nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Go to `Isaac Examples -> ROS -> Navigation` to load the warehouse scenario. 1. Open the main `ActionGraph` by expanding ``Carter_ROS``. Right click on ``ActionGraph`` and press `Open Graph`. The following ROS |omnigraph_short| nodes are setup to do the following: =============================== ================ Ominigraph Node Function =============================== ================ ros2_subscribe_twist Subscribes to the `/cmd_vel` topic and triggers the differential and articulation controllers to the move the robot ros2_publish_odometry Publishes odometry received from the ``isaac_compute_odometry_node`` ros2_publish_raw_transform_tree Publishes the transform between the `odom` frame and `base_link` frame ros2_publish_transform_tree Publishes the static transform between the `base_link` frame and `chassis_link` frame. Keep in mind that since the target prim is set as ``Carter_ROS``, the entire transform tree of the Carter robot (with chassis_link as root) will be published as children of the `base_link` frame ros2_publish_transform_tree_01 Publishes the static transform between the `chassis_link` frame and `carter_lidar` frame ros2_publish_laser_scan Publishes the 2D LaserScan received from the ``isaac_read_lidar_beams_node`` ros2_context Sets the ROS2 context with the default domain ID of 0 =============================== ================ 2. Open the `ROS_Cameras` graph by expanding ``Carter_ROS``. Right click on ``ROS_Cameras`` and press `Open Graph`. The following ROS Camera |omnigraph_short| nodes are setup to do the following: =============================== ================ Ominigraph Node Function =============================== ================ ros2_create_camera_left_info Auto-generates the CameraInfo publisher for the `/camera_info_left` topic. It automatically publishes since the ``enable_camera_left`` branch node is enabled by default ros2_create_camera_left_rgb Auto-generates the RGB Image publisher for the `/rgb_left` topic. It automatically publishes since the ``enable_camera_left`` and ``enable_camera_left_rgb`` branch nodes are enabled by default ros2_create_camera_left_depth Auto-generates the Depth (32FC1) Image publisher for the `/depth_left` topic. To start publishing, ensure ``enable_camera_left`` and ``enable_camera_left_depth`` branch nodes are enabled ros2_create_camera_right_info Auto-generates the CameraInfo publisher for the `/camera_info_right` topic. To start publishing, ensure the ``enable_camera_right`` branch node is enabled ros2_create_camera_right_rgb Auto-generates the RGB Image publisher for the `/rgb_right` topic. To start publishing, ensure ``enable_camera_right`` is enabled. The ``enable_camera_right_rgb`` branch node is already enabled by default ros2_create_camera_right_depth Auto-generates the Depth (32FC1) Image publisher for the `/depth_right` topic. To start publishing, ensure ``enable_camera_right`` and ``enable_camera_right_depth`` branch nodes are enabled ros2_context Sets the ROS2 context with the default domain ID of 0 =============================== ================ 3. Finally, to ensure all external ROS nodes reference simulation time, a ``ROS_Clock`` graph is added which contains a ``ros2_publish_clock`` node responsible for publishing the simulation time to the `/clock` topic. Occupancy Map ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ In this scenario, an occupancy map is required. For this sample, we are generating an occupancy map of the warehouse environment using the :ref:`Occupancy Map Generator extension ` within |isaac-sim|. #. Go to `Isaac Examples -> ROS -> Navigation` to load the warehouse scenario. #. At the upper left corner of the viewport, click on `Camera`. Select `Top` from the dropdown menu. #. Go to `Isaac Utils -> Occupancy Map`. #. In the Occupancy Map extension, ensure the Origin is set to ``X: 0.0, Y: 0.0, Z: 0.0``. For the lower bound, set ``Z: 0.1``. For the Upper Bound, set ``Z: 0.62``. Keep in mind, the upper bound Z distance has been set to 0.62 meters to match the vertical distance of the lidar onboard Carter with respect to the ground. #. Select the `warehouse_with_forklifts` prim in the stage. In the Occupancy Map extension, click on ``BOUND SELECTION``. The bounds of the occupancy map should be updated to incorporate the selected `warehouse_with_forklifts` prim. The map parameters should now look similar to the following: .. figure:: /content/images/isaac_sample_ros_nav_2.png :align: center :width: 600 :alt: Occupancy Map Properties UI Window A perimeter will be generated and it should resemble this Top View: .. figure:: /content/images/isaac_sample_ros_nav_3.png :align: center :width: 600 :alt: Top View of Warehouse with Occupancy Map #. Remove the `Carter_ROS` prim from the stage. #. Once the setup is complete, click on ``CALCULATE`` followed by ``VISUALIZE IMAGE``. A Visualization popup will appear. #. For `Rotate Image`, select 180 degrees and for `Coordinate Type` select ``ROS Occupancy Map Parameters File (YAML)``. Click ``RE-GENERATE IMAGE``. The ROS camera and Isaac Sim camera have different coordinates. #. Occupancy map parameters formatted to YAML will appear in the field below. Copy the full text. #. Create a YAML file for the occupancy map parameters called ``carter_warehouse_navigation.yaml`` and place it in the `maps` directory which is located in the sample ``carter_navigation`` ROS2 package (``carter_navigation/maps/carter_warehouse_navigation.yaml``). #. Insert the previously copied text in the ``carter_warehouse_navigation.yaml`` file. #. Back in the visualization tab in |isaac-sim|, click ``Save Image``. Name the image as ``carter_warehouse_navigation.png`` and choose to save in the same directory as the map parameters file. The final saved image will look like the following: .. figure:: /content/images/isaac_sample_ros_nav_warehouse_map.png :align: center :width: 500 :alt: Sample Occupancy Map generated from warehouse stage An occupancy map is now ready to be used with Nav2! .. _Running Nav2: Running Nav2 =========================== #. Go to `Isaac Examples -> ROS -> Navigation` to load the warehouse scenario. #. Click on ``Play`` to begin simulation. #. In a new terminal, run the ROS2 launch file to begin Nav2. .. code-block:: bash ros2 launch carter_navigation carter_navigation.launch.py RViz2 will open and begin loading the occupancy map. If a map does not appear, repeat the previous step. #. Load the the URDF model into the RViz2 window by clicking on `RobotModel -> Description File` and navigate to the location of the Carter URDF model which can be found at the `urdf` directory in the sample `carter_description` ROS2 package (``carter_description/urdf/carter.urdf``). .. raw:: html
#. Since the position of the robot is defined in the parameter file ``carter_navigation_params.yaml``, the robot should already be properly localized. If required, the ``2D Pose Estimate`` button can be used to re-set the position of the robot. #. Click on the ``Navigation2 Goal`` button and then click and drag at the desired location point in the map. Nav2 will now generate a trajectory and the robot will start moving towards its destination! .. raw:: html
Run ROS2 Navigation with RTX Lidar =================================== Open the scene located at `Isaac/Samples/ROS2/Scenario/rtx_lidar_carter_warehouse_navigation.usd` This scene contains the Carter robot with the RTX Lidar. Press play and follow from step 3 onwards above. Refer to :ref:`isaac_sim_app_tutorial_ros2_rtx_lidar` for using RTX Lidar with ROS2 .. _isaac_sim_app_tutorial_ros2_nav_goals: Sending Goals Programmatically =============================== The ``isaac_ros_navigation_goal`` ROS2 package can be used to set goal poses for the robot using a python node. It is able to randomly generate and send goal poses to Nav2. It is also able to send user-defined goal poses if needed. #. Make any changes to the parameters defined in the launch file found under ``isaac_ros_navigation_goal/launch`` as required. Make sure to re-build and source the package/workspace after modifying its contents. The parameters are described below: - **goal_generator_type**: Type of the goal generator. Use ``RandomGoalGenerator`` to randomly generate goals or use ``GoalReader`` for sending user-defined goals in a specific order. - **map_yaml_path**: The path to the occupancy map parameters yaml file. Example file is present at ``isaac_ros_navigation_goal/assets/carter_warehouse_navigation.yaml``. The map image is being used to identify the obstacles in the vicinity of a generated pose. **Required** if goal generator type is set as ``RandomGoalGenerator``. - **iteration_count**: Number of times goal is to be set. - **action_server_name**: Name of the action server. - **obstacle_search_distance_in_meters**: Distance in meters in which there should not be any obstacle of a generated pose. - **goal_text_file_path**: The path to the text file which contains user-defined static goals. Each line in the file has a single goal pose in the following format: ``pose.x pose.y orientation.x orientation.y orientation.z orientation.w``. Sample file is present at: ``isaac_ros_navigation_goal/assets/goals.txt``. **Required** if goal generator type is set as ``GoalReader``. - **initial_pose**: If initial_pose is set, it will be published to `/initialpose` topic and goal poses will be sent to action server after that. Format is ``[pose.x, pose.y, pose.z, orientation.x, orientation.y, orientation.z, orientation.w]``. #. To run the launch file, use the following command: .. code-block:: bash ros2 launch isaac_ros_navigation_goal isaac_ros_navigation_goal.launch.py .. note:: The package will stop processing (setting goals) once any of the below conditions are met: 1. Number of goals published till now >= iteration_count. 2. If ``GoalReader`` is being used then if all the goals from file are published, or if condition (1) is true. 3. A goal is rejected by the action server. 4. In case of ``RandomGoalGenerator``, if a goal was not generated even after running the maximum number of iterations, which is rare but could happen in very dense maps. To learn more about programmatically sending navigation goals to multiple robots simultaneously see :ref:`isaac_sim_app_tutorial_ros2_multi_nav_goals`. Summary ======== In this tutorial, we covered #. Occupancy map #. Running Isaac Sim with Nav2. #. Running the Isaac ROS2 Navigation Goal package to send nav goals programmatically. Next Steps ^^^^^^^^^^^^^^^^^^^^^^ Continue on to the next tutorial in our ROS2 Tutorials series, :ref:`isaac_sim_app_tutorial_ros2_multi_navigation` to move multiple navigating robots with ROS2. Further Learning ^^^^^^^^^^^^^^^^^^^^^^ - To learn more about Nav2, refer to the project website: ``_ - More about :ref:`ext_omni_isaac_occupancy_map`.