Update nav2 example (#15)

Author: fred-labs

.github/workflows/test_build.yml

@@ -241,7 +241,7 @@ jobs:
           export IGN_PARTITION=${HOSTNAME}:${GITHUB_RUN_ID}
           sed -i 's/60s/600s/g' examples/example_nav2/example_nav2.osc
           # shellcheck disable=SC1083
-          scenario_batch_execution -i examples/example_nav2/ -o test_example_nav2 --ignore-process-return-value -- ros2 launch tb4_sim_scenario sim_nav_scenario_launch.py scenario:={SCENARIO} output_dir:={OUTPUT_DIR} headless:=True use_rviz:=False
+          scenario_batch_execution -i examples/example_nav2/ -o test_example_nav2 --ignore-process-return-value -- ros2 launch example_nav2 example_nav2_launch.py output_dir:={OUTPUT_DIR} headless:=True use_rviz:=False
       - name: Upload result
         uses: actions/upload-artifact@0b2256b8c012f0828dc542b3febcab082c67f72b # v4.3.4
         if: always()

docs/tutorials.rst

@@ -161,32 +161,26 @@ and control it with Nav2, can be found in :repo_link:`examples/example_nav2/exam
 This scenario files looks as follows:
 
 ::
-
+    import osc.helpers
     import osc.ros
+    import osc.nav2
 
-    scenario nav2_simulation_nav_to_pose:
+    scenario example_nav2:
+        timeout(60s)
         robot: differential_drive_robot
-        do parallel:
-            test_drive: serial:
-                robot.init_nav2(pose_3d(position_3d(x: 0.0m, y: 0.0m)))
-                robot.nav_to_pose(pose_3d(position_3d(x: 3.0m, y: -3.0m)))
-                robot.nav_to_pose(pose_3d(position_3d(x: 0.0m, y: 0.0m)))
-                emit end
-            time_out: serial:
-                wait elapsed(120s)
-                emit fail
+        do serial:
+            robot.init_nav2(pose_3d(position_3d(x: 0.0m, y: 0.0m)))
+            robot.nav_to_pose(pose_3d(position_3d(x: 3.0m, y: -3.0m)))
+
 
 Let’s break down the individual components of the scenario. The
 following snippet defines the turtlebot4 amr-object.
 
 .. code-block::
 
-   turtlebot4: differential_drive_robot:            # define turtlebot4 robot
+   robot: differential_drive_robot
 
-The ``do parallel`` runs the actual test drive and a time-out in
-parallel. In case something goes wrong, the time-out prevents the
-scenario from running indefinitely by canceling it after 2 minutes and
-marking it as failed.
+The ``do serial`` runs the actual test drive. A modifier is used to specify a timeout. If the scenario takes longer than 60 seconds, it will be marked as failed.
 
 
 Before being able to navigate, nav2 needs to be initialized. This
@@ -195,7 +189,7 @@ includes setting the initial pose of the Nav2 localization module
 
 .. code-block::
 
-   turtlebot4.init_nav2(pose_3d(position_3d(x: 0.0m, y: 0.0m)))                        # initialize Nav2
+   robot.init_nav2(pose_3d(position_3d(x: 0.0m, y: 0.0m)))
 
 Finally, the following snippet calls the Nav2 `NavigateToPose
 action <https://github.com/ros-planning/navigation2/blob/main/nav2_msgs/action/NavigateToPose.action>`__
@@ -204,59 +198,39 @@ starting position
 
 .. code-block::
 
-    turtlebot4.nav_to_pose(pose_3d(position_3d(x: 3.0m, y: -3.0m)))
-    turtlebot4.nav_to_pose(pose_3d(position_3d(x: 0.0m, y: 0.0m)))
+    robot.nav_to_pose(pose_3d(position_3d(x: 3.0m, y: -3.0m)))
 
-Once the robot reached the final goal pose ``emit end`` finishes the
-scenario and marks it as successful.
+Once the robot reached the final goal pose the scenario is marked as successful and the execution ends.
 
 To try this example, run
 
 .. code-block:: bash
 
-   ros2 launch tb4_sim_scenario sim_nav_scenario_launch.py scenario:=examples/example_nav2/example_nav2.osc headless:=False
-
-and you should see something like this
-
-.. figure:: images/tb4_scenario.gif
-   :alt: turtlebot4 nav2 scenario
+   ros2 launch example_nav2 example_nav2_launch.py
 
-   Turtlebot4 NAV2 scenario
 
 In case you want to run the navigation with SLAM instead of AMCL, update
 the above described scenario by setting the ``use_initial_pose`` to ``False``:
 
 ::
-
+    import osc.helpers
     import osc.ros
+    import osc.nav2
 
-    scenario nav2_simulation_nav_to_pose:
+    scenario example_nav2:
+        timeout(60s)
         robot: differential_drive_robot
-        do parallel:
-            test_drive: serial:
-                robot.init_nav2(
-                    initial_pose: pose_3d(position_3d(x: 0.0m, y: 0.0m)),
-                    use_initial_pose: false)
-                robot.nav_to_pose(pose_3d(position_3d(x: 3.0m, y: -3.0m)))
-                robot.nav_to_pose(pose_3d(position_3d(x: 0.0m, y: 0.0m)))
-                emit end
-            time_out: serial:
-                wait elapsed(120s)
-                emit fail
+        do serial:
+            robot.init_nav2(pose_3d(position_3d(x: 0.0m, y: 0.0m)), use_initial_pose: false)
+            robot.nav_to_pose(pose_3d(position_3d(x: 3.0m, y: -3.0m)))
 
 
 Then, run:
 
 .. code-block:: bash
 
-   ros2 launch tb4_sim_scenario sim_nav_scenario_launch.py scenario:=examples/example_nav2/example_nav2.osc headless:=false slam:=True
-
-and you should see something like this
-
-.. figure:: images/tb4_scenario_slam.PNG
-   :alt: turtlebot4 nav2 scenario SLAM
+   ros2 launch example_nav2 example_nav2_launch.py slam:=False
 
-   Turtlebot4 NAV2 scenario SLAM
 
 Create Navigation Scenario with Obstacle
 ----------------------------------------

examples/example_nav2/README.md

@@ -1,9 +1,9 @@
 # Example Navigation
 
-To run the Example Navigation 2 Scenario, first build the `tb4_sim_scenario` package:
+To run the Example Navigation 2 Scenario, first build the `example_nav2` package:
 
 ```bash
-colcon build --packages-up-to tb4_sim_scenario
+colcon build --packages-up-to example_nav2
 ```
 
 Source the workspace:
@@ -15,9 +15,9 @@ source install/setup.bash
 Now, run the following command to launch the scenario:
 
 ```bash
-ros2 launch tb4_sim_scenario sim_nav_scenario_launch.py scenario:=examples/example_nav2/example_nav2.osc
+ros2 launch example_nav2 example_nav2_launch.py
 ```
 
-A turtlebot is initialsed with nav2 which drives to a point and back.
+A turtlebot is initialsed with nav2 which drives to the scenario-specified goal.
 
 For a more detailed understanding of the code structure and scenario implementation please refer to the [tutorial documentation](https://intellabs.github.io/scenario_execution/tutorials.html).

examples/example_nav2/example_nav2/__init__.py

@@ -0,0 +1,15 @@
+# Copyright (C) 2024 Intel Corporation
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing,
+# software distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions
+# and limitations under the License.
+#
+# SPDX-License-Identifier: Apache-2.0

examples/example_nav2/launch/example_nav2_launch.py

@@ -0,0 +1,45 @@
+# Copyright (C) 2024 Intel Corporation
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing,
+# software distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions
+# and limitations under the License.
+#
+# SPDX-License-Identifier: Apache-2.0
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import PathJoinSubstitution, LaunchConfiguration
+
+
+def generate_launch_description():
+
+    example_nav2_dir = get_package_share_directory('example_nav2')
+    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
+    scenario_execution_ros_dir = get_package_share_directory('scenario_execution_ros')
+
+    scenario = LaunchConfiguration('scenario')
+    
+    return LaunchDescription([
+        DeclareLaunchArgument('scenario', description='Scenario file to execute', default_value=PathJoinSubstitution([example_nav2_dir, 'scenarios', 'example_nav2.osc'])),
+        
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([PathJoinSubstitution([nav2_bringup_dir, 'launch', 'tb4_simulation_launch.py'])])
+        ),
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([PathJoinSubstitution([scenario_execution_ros_dir, 'launch', 'scenario_launch.py'])]),
+            launch_arguments={'scenario': scenario}.items()
+        )
+    ])
+

examples/example_nav2/package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>example_nav2</name>
+  <version>1.2.0</version>
+  <description>Scenario Execution Example for Navigation2</description>
+  <author>Intel Labs</author>
+  <author email="[email protected]">Frederik Pasch</author>
+  <author email="[email protected]">Florian Mirus</author>
+  <maintainer email="[email protected]">Frederik Pasch</maintainer>
+  <maintainer email="[email protected]">Florian Mirus</maintainer>
+  <license>Apache-2.0</license>
+
+  <depend>scenario_execution_ros</depend>
+  <depend>scenario_execution_nav2</depend>
+  <depend>nav2_bringup</depend>
+
+  <exec_depend>rclpy</exec_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

examples/example_nav2/resource/example_nav2

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/example_nav2
+[install]
+install_scripts=$base/lib/example_nav2

examples/example_nav2/example_nav2.osc examples/example_nav2/scenarios/example_nav2.osc

@@ -0,0 +1,44 @@
+# Copyright (C) 2024 Intel Corporation
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing,
+# software distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions
+# and limitations under the License.
+#
+# SPDX-License-Identifier: Apache-2.0
+
+""" Setup file for package example_nav2 """
+from glob import glob
+import os
+from setuptools import setup
+
+PACKAGE_NAME = 'example_nav2'
+
+setup(
+    name=PACKAGE_NAME,
+    version='1.2.0',
+    packages=[PACKAGE_NAME],
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + PACKAGE_NAME]),
+        ('share/' + PACKAGE_NAME, ['package.xml']),
+        (os.path.join('share', PACKAGE_NAME, 'scenarios'), glob('scenarios/*.osc')),
+        (os.path.join('share', PACKAGE_NAME, 'launch'), glob('launch/*launch.py'))
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='Intel Labs',
+    maintainer_email='[email protected]',
+    description='Scenario Execution Example for Navigation2',
+    license='Apache License 2.0',
+    tests_require=['pytest'],
+    entry_points={
+    },
+)