Update nav2 library

Author: fred-labs

.github/workflows/test_build.yml

@@ -351,7 +351,7 @@ jobs:
           export -n CYCLONEDDS_URI
           export ROS_DOMAIN_ID=2
           # shellcheck disable=SC1083
-          scenario_batch_execution -i examples/example_external_method/scenarios -o test_example_external_method -- ros2 launch scenario_execution_ros scenario_launch.py scenario:={SCENARIO} output_dir:={OUTPUT_DIR}
+          scenario_batch_execution -i examples/example_external_method/scenarios -o test_example_external_method -- ros2 launch scenario_execution_ros scenario_launch.py scenario:={SCENARIO} output_dir:={OUTPUT_DIR} headless:=True
       - name: Upload result
         uses: actions/upload-artifact@0b2256b8c012f0828dc542b3febcab082c67f72b # v4.3.4
         if: always()
@@ -384,13 +384,59 @@ jobs:
           export ROS_DOMAIN_ID=2
           export IGN_PARTITION=${HOSTNAME}:${GITHUB_RUN_ID}
           # shellcheck disable=SC1083
-          scenario_batch_execution -i test/scenario_execution_gazebo_test/scenarios/ -o test_scenario_execution_gazebo -- ros2 launch tb4_sim_scenario sim_nav_scenario_launch.py scenario:={SCENARIO} output_dir:={OUTPUT_DIR}
+          scenario_batch_execution -i test/scenario_execution_gazebo_test/scenarios/ -o test_scenario_execution_gazebo -- ros2 launch tb4_sim_scenario sim_nav_scenario_launch.py scenario:={SCENARIO} output_dir:={OUTPUT_DIR} headless:=True
       - name: Upload result
         uses: actions/upload-artifact@0b2256b8c012f0828dc542b3febcab082c67f72b # v4.3.4
         if: always()
         with:
           name: test-scenario-execution-gazebo
           path: test_scenario_execution_gazebo/test.xml
+  test-scenario-execution-nav2:
+    needs: [build]
+    runs-on: intellabs-01
+    timeout-minutes: 30
+    container:
+      image: ghcr.io/intellabs/scenario-execution:${{ github.event.pull_request.base.ref }}
+      credentials:
+        username: ${{ github.repository_owner }}
+        password: ${{ secrets.GITHUB_TOKEN }}
+    steps:
+      - name: Restore cache
+        uses: actions/cache@0c45773b623bea8c8e75f6c82b208c3cf94ea4f9 # v4.0.2
+        with:
+          key: ${{ runner.os }}-build-${{ github.run_number }}
+          path: .
+      - name: Test Scenario Execution Nav2
+        shell: bash
+        run: |
+          source /opt/ros/${{ github.event.pull_request.base.ref == 'main' && 'humble' || github.event.pull_request.base.ref }}/setup.bash
+          source install/setup.bash
+          Xvfb :1 -screen 0 800x600x16 &
+          export DISPLAY=:1.0
+          export -n CYCLONEDDS_URI
+          export ROS_DOMAIN_ID=2
+          export IGN_PARTITION=${HOSTNAME}:${GITHUB_RUN_ID}
+          # shellcheck disable=SC1083
+          scenario_batch_execution -i test/scenario_execution_nav2_test/scenarios/ -o test_scenario_execution_nav2 -- ros2 launch tb4_sim_scenario sim_nav_scenario_launch.py scenario:={SCENARIO} output_dir:={OUTPUT_DIR} headless:=True
+      - name: Upload result
+        uses: actions/upload-artifact@0b2256b8c012f0828dc542b3febcab082c67f72b # v4.3.4
+        if: always()
+        with:
+          name: test-scenario-execution-nav2
+          path: test_scenario_execution_nav2/test.xml
+      - name: Convert video
+        if: always()
+        shell: bash
+        run: |
+          source /opt/ros/${{ github.event.pull_request.base.ref == 'main' && 'humble' || github.event.pull_request.base.ref }}/setup.bash
+          source /rosbag2_to_video/install/setup.bash
+          ros2 bag to_video  -t /static_camera/image_raw -o test_scenario_execution_nav2/test_scenario_execution_nav2/example_nav2.mp4  test_scenario_execution_nav2/test_scenario_execution_nav2/rosbag2_* --fps 5 --codec mp4v
+      - name: Upload video
+        uses: actions/upload-artifact@0b2256b8c012f0828dc542b3febcab082c67f72b # v4.3.4
+        if: always()
+        with:
+          name: test-scenario-execution-nav2-video
+          path: test_scenario_execution_nav2/test_scenario_execution_nav2/example_nav2.mp4
   test-scenario-execution-pybullet:
     needs: [build]
     runs-on: intellabs-01
@@ -434,6 +480,7 @@ jobs:
     - test-example-multirobot
     - test-example-external-method
     - test-scenario-execution-gazebo
+    - test-scenario-execution-nav2
     - test-scenario-execution-pybullet
     runs-on: intellabs-01
     if: ${{ always() }}
@@ -471,4 +518,5 @@ jobs:
             downloaded-artifacts/test-example-multirobot-result/test.xml
             downloaded-artifacts/test-example-external-method-result/test.xml
             downloaded-artifacts/test-scenario-execution-gazebo/test.xml
+            downloaded-artifacts/test-scenario-execution-nav2/test.xml
             downloaded-artifacts/test-scenario-execution-pybullet/test.xml

docs/libraries.rst

@@ -586,10 +586,10 @@ Use nav2 to navigate through poses.
      - ``string``
      - ``''``
      - If set, it's used as namespace (instead of the associated actor's namespace)
-   * - ``monitor_progress``
-     - ``bool``
-     - ``true``
-     - If yes, the action returns after the goal is reached or on failure. If no, the action returns after request.
+   * - ``action_topic``
+     - ``string``
+     - ``navigate_through_poses``
+     - Action name
 
 ``differential_drive_robot.nav_to_pose()``
 """"""""""""""""""""""""""""""""""""""""""
@@ -616,10 +616,6 @@ Use nav2 to navigate to goal pose.
      - ``string``
      - ``navigate_to_pose``
      - Action name
-   * - ``monitor_progress``
-     - ``bool``
-     - ``true``
-     - If yes, the action returns after the goal is reached or on failure. If no, the action returns after request.
 
 
 OS

libs/scenario_execution_nav2/scenario_execution_nav2/actions/nav_through_poses.py

@@ -14,117 +14,47 @@
 #
 # SPDX-License-Identifier: Apache-2.0
 
-from enum import Enum
-
-from rclpy.node import Node
 from rclpy.duration import Duration
-
-import py_trees
-
-from nav2_simple_commander.robot_navigator import TaskResult  # pylint: disable=import-error
-
-from .nav2_common import NamespaceAwareBasicNavigator
+from nav2_msgs.action import NavigateThroughPoses
 from scenario_execution_ros.actions.common import get_pose_stamped
-from scenario_execution.actions.base_action import BaseAction
-
-
-class NavThroughPosesState(Enum):
-    """
-    States for executing a nav-through-poses with nav2
-    """
-    IDLE = 1
-    NAV_TO_GOAL = 2
-    DONE = 3
+from scenario_execution_ros.actions.ros_action_call import RosActionCall, ActionCallActionState
 
 
-class NavThroughPoses(BaseAction):
+class NavThroughPoses(RosActionCall):
     """
     Class to navigate through poses
     """
 
-    def __init__(self, associated_actor, goal_poses: list, monitor_progress: bool, namespace_override: str):
-        super().__init__()
+    def __init__(self, associated_actor, goal_poses: list, action_topic: str, namespace_override: str):
         self.namespace = associated_actor["namespace"]
-        self.monitor_progress = monitor_progress
-        self.node = None
-        self.future = None
-        self.current_state = NavThroughPosesState.IDLE
-        self.nav = None
         if namespace_override:
             self.namespace = namespace_override
+        self.goal_poses = None
+        super().__init__(self.namespace + '/' + action_topic, "nav2_msgs.action.NavigateThroughPoses", "")
 
-        if not isinstance(goal_poses, list):
-            raise TypeError(f'goal_poses needs to be list of position_3d, got {type(goal_poses)}.')
-        else:
-            self.goal_poses = goal_poses
-
-    def setup(self, **kwargs):
-        """
-        Setup ROS2 node and service client
-
-        """
-        try:
-            self.node: Node = kwargs['node']
-        except KeyError as e:
-            error_message = "didn't find 'node' in setup's kwargs [{}][{}]".format(
-                self.name, self.__class__.__name__)
-            raise KeyError(error_message) from e
-
-        self.nav = NamespaceAwareBasicNavigator(
-            node_name="basic_nav_nav_through_poses", namespace=self.namespace)
-
-    def update(self) -> py_trees.common.Status:
-        """
-        Execute states
-        """
-        self.logger.debug(f"Current State {self.current_state}")
-        result = py_trees.common.Status.FAILURE
-        if self.current_state == NavThroughPosesState.IDLE:
-            self.current_state = NavThroughPosesState.NAV_TO_GOAL
-            goal_poses = []
-            for pose in self.goal_poses:
-                goal_poses.append(get_pose_stamped(self.nav.get_clock().now().to_msg(), pose))
-            self.feedback_message = "Execute navigation."  # pylint: disable= attribute-defined-outside-init
-            go_to_pose_result = self.nav.goThroughPoses(goal_poses)
-            if go_to_pose_result:
-                if self.monitor_progress:
-                    result = py_trees.common.Status.RUNNING
-                    self.current_state = NavThroughPosesState.NAV_TO_GOAL
-                else:
-                    result = py_trees.common.Status.SUCCESS
-                    self.current_state = NavThroughPosesState.DONE
-            else:
-                self.current_state = NavThroughPosesState.DONE
-                result = py_trees.common.Status.FAILURE
-        elif self.current_state == NavThroughPosesState.NAV_TO_GOAL:
-            if not self.nav.isTaskComplete():
-                feedback = self.nav.getFeedback()
-                if feedback:
-                    self.feedback_message = 'Estimated time of arrival: ' + '{0:.0f}'.format(  # pylint: disable= attribute-defined-outside-init
-                        Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9) + ' seconds.'
-
-                    if Duration.from_msg(feedback.navigation_time) > Duration(seconds=600):
-                        self.nav.cancelTask()
-                result = py_trees.common.Status.RUNNING
+    def execute(self, associated_actor, goal_poses: list, action_topic: str, namespace_override: str) -> None:  # pylint: disable=arguments-differ
+        self.namespace = associated_actor["namespace"]
+        if namespace_override:
+            self.namespace = namespace_override
+        self.goal_poses = goal_poses
+        super().execute(self.namespace + '/' + action_topic, "nav2_msgs.action.NavigateThroughPoses", "")
+
+    def get_goal_msg(self):
+        goal_msg = NavigateThroughPoses.Goal()
+        for pose in self.goal_poses:
+            goal_msg.poses.append(get_pose_stamped(self.node.get_clock().now().to_msg(), pose))
+        return goal_msg
+
+    def get_feedback_message(self, current_state):
+        feedback_message = super().get_feedback_message(current_state)
+
+        if self.current_state == ActionCallActionState.IDLE:
+            feedback_message = "Waiting for navigation"
+        elif self.current_state == ActionCallActionState.ACTION_CALLED:
+            if self.received_feedback:
+                feedback_message = f'Estimated time of arrival: {Duration.from_msg(self.received_feedback.estimated_time_remaining).nanoseconds / 1e9:0.0f}, poses left {self.received_feedback.number_of_poses_remaining}.'
             else:
-                self.current_state = NavThroughPosesState.DONE
-                result = self.nav.getResult()
-                if result == TaskResult.SUCCEEDED:
-                    self.feedback_message = 'Goal succeeded!'  # pylint: disable= attribute-defined-outside-init
-                    result = py_trees.common.Status.SUCCESS
-                elif result == TaskResult.CANCELED:
-                    self.feedback_message = 'Goal was canceled!'  # pylint: disable= attribute-defined-outside-init
-                    result = py_trees.common.Status.FAILURE
-                elif result == TaskResult.FAILED:
-                    self.feedback_message = 'Goal failed!'  # pylint: disable= attribute-defined-outside-init
-                    result = py_trees.common.Status.FAILURE
-        elif self.current_state == NavThroughPosesState.DONE:
-            self.logger.debug("Nothing to do!")
-        else:
-            self.logger.error(f"Invalid state {self.current_state}")
-
-        return result
-
-    def shutdown(self):
-        self.nav.cancelTask()
-        self.nav.destroy_node()
+                feedback_message = f"Executing navigation to ({self.goal_poses})."
+        elif current_state == ActionCallActionState.DONE:
+            feedback_message = f"Goal reached."
+        return feedback_message

libs/scenario_execution_nav2/scenario_execution_nav2/actions/nav_to_pose.py

@@ -26,13 +26,20 @@ class NavToPose(RosActionCall):
     Class to navigate to a pose
     """
 
-    def __init__(self, associated_actor, goal_pose: list, monitor_progress: bool, action_topic: str, namespace_override: str) -> None:
+    def __init__(self, associated_actor, goal_pose: list, action_topic: str, namespace_override: str) -> None:
         self.namespace = associated_actor["namespace"]
         if namespace_override:
             self.namespace = namespace_override
-        self.goal_pose = goal_pose
+        self.goal_pose = None
         super().__init__(self.namespace + '/' + action_topic, "nav2_msgs.action.NavigateToPose", "")
 
+    def execute(self, associated_actor, goal_pose: list, action_topic: str, namespace_override: str) -> None:  # pylint: disable=arguments-differ
+        self.namespace = associated_actor["namespace"]
+        if namespace_override:
+            self.namespace = namespace_override
+        self.goal_pose = goal_pose
+        super().execute(self.namespace + '/' + action_topic, "nav2_msgs.action.NavigateToPose", "")
+
     def get_goal_msg(self):
         goal_msg = NavigateToPose.Goal()
         goal_msg.pose = get_pose_stamped(self.node.get_clock().now().to_msg(), self.goal_pose)
@@ -45,11 +52,9 @@ def get_feedback_message(self, current_state):
             feedback_message = "Waiting for navigation"
         elif self.current_state == ActionCallActionState.ACTION_CALLED:
             if self.received_feedback:
-                feedback_message = 'Estimated time of arrival: ' + \
-                    '{0:.0f}'.format(Duration.from_msg(self.received_feedback.estimated_time_remaining).nanoseconds / 1e9) + ' seconds.'
+                feedback_message = f'Estimated time of arrival: {Duration.from_msg(self.received_feedback.estimated_time_remaining).nanoseconds / 1e9:0.0f}.'
             else:
-                goal_msg = self.get_goal_msg()
-                feedback_message = f"Executing navigation to ({goal_msg.pose.pose.position.x}, {goal_msg.pose.pose.position.y})."
+                feedback_message = f"Executing navigation to ({self.goal_pose})."
         elif current_state == ActionCallActionState.DONE:
             feedback_message = f"Goal reached."
         return feedback_message

libs/scenario_execution_nav2/scenario_execution_nav2/lib_osc/nav2.osc

@@ -13,11 +13,10 @@ action differential_drive_robot.nav_through_poses:
     # Use nav2 to navigate through poses.
     goal_poses: list of pose_3d          # goal poses to navigate through
     namespace_override: string = ''      # if set, it's used as namespace (instead of the associated actor's name)
-    monitor_progress: bool = true        # if true, the action returns after the goal is reached or on failure. If no, the action returns after request.
+    action_topic: string = 'navigate_through_poses' # Name of action
 
 action differential_drive_robot.nav_to_pose:
     # Nav2 to navigate to goal pose.
     goal_pose: pose_3d                   # goal pose to navigate to
     namespace_override: string = ''      # if set, it's used as namespace (instead of the associated actor's name)
-    action_topic: string = 'navigate_to_pose' # Name of action
-    monitor_progress: bool = true        # if yes, the action returns after the goal is reached or on failure. If no, the action returns after request.
+    action_topic: string = 'navigate_to_pose' # Name of action

scenario_execution_ros/scenario_execution_ros/actions/ros_action_call.py

@@ -48,15 +48,12 @@ def __init__(self, action_name: str, action_type: str, data: str):
         self.client = None
         self.send_goal_future = None
         self.goal_handle = None
-        self.action_type = action_type
+        self.action_type_string = action_type
+        self.action_type = None
         self.action_name = action_name
         self.received_feedback = None
-        if data:
-            try:
-                trimmed_data = data.encode('utf-8').decode('unicode_escape')
-                self.data = literal_eval(trimmed_data)
-            except Exception as e:  # pylint: disable=broad-except
-                raise ValueError(f"Error while parsing sevice call data:") from e
+        self.data = None
+        self.parse_data(data)
         self.current_state = ActionCallActionState.IDLE
         self.cb_group = ReentrantCallbackGroup()
 
@@ -72,7 +69,7 @@ def setup(self, **kwargs):
                 self.name, self.__class__.__name__)
             raise KeyError(error_message) from e
 
-        datatype_in_list = self.action_type.split(".")
+        datatype_in_list = self.action_type_string.split(".")
         try:
             self.action_type = getattr(
                 importlib.import_module(".".join(datatype_in_list[0:-1])),
@@ -82,6 +79,21 @@ def setup(self, **kwargs):
 
         self.client = ActionClient(self.node, self.action_type, self.action_name, callback_group=self.cb_group)
 
+    def execute(self, action_name: str, action_type: str, data: str):
+        if self.action_name != action_name or self.action_type_string != action_type:
+            raise ValueError(f"Updating action_name or action_type_string not supported.")
+
+        self.parse_data(data)
+        self.current_state = ActionCallActionState.IDLE
+
+    def parse_data(self, data):
+        if data:
+            try:
+                trimmed_data = data.encode('utf-8').decode('unicode_escape')
+                self.data = literal_eval(trimmed_data)
+            except Exception as e:  # pylint: disable=broad-except
+                raise ValueError(f"Error while parsing sevice call data:") from e
+
     def update(self) -> py_trees.common.Status:
         """
         Execute states

test/scenario_execution_nav2_test/README.md

@@ -0,0 +1,3 @@
+# Tests of Scenario Execution Library for Nav2
+
+The `scenario_execution_nav2_test` package provides test scenarios to test `scenario_execution_nav2` within the `tb4_sim_scenario` setup.

test/scenario_execution_nav2_test/package.xml

@@ -0,0 +1,24 @@
+<?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>scenario_execution_nav2_test</name>
+  <version>1.2.0</version>
+  <description>Tests for Scenario Execution library for Nav2</description>
+  <author email="[email protected]">Intel Labs</author>
+  <maintainer email="[email protected]">Intel Labs</maintainer>
+  <license file="../../LICENSE">Apache-2.0</license>
+  
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>scenario_execution_nav2</exec_depend>
+  <exec_depend>tb4_sim_scenario</exec_depend>
+  <exec_depend>gazebo_static_camera</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>