Port system tests in nav2_system_tests

nav2_system_tests/CMakeLists.txt

@@ -28,6 +28,7 @@ find_package(navigation2)
 find_package(angles REQUIRED)
 find_package(behaviortree_cpp REQUIRED)
 find_package(pluginlib REQUIRED)
+find_package(nav2_minimal_tb3_sim REQUIRED)
 
 nav2_package()
 
@@ -113,9 +114,9 @@ if(BUILD_TESTING)
 
   add_subdirectory(src/behavior_tree)
   add_subdirectory(src/planning)
-  # Uncomment after https://github.com/ros-navigation/navigation2/pull/3634
   add_subdirectory(src/localization)
-  # add_subdirectory(src/system)
+  add_subdirectory(src/system)
+  # Uncomment after https://github.com/ros-navigation/navigation2/pull/3634
   # add_subdirectory(src/system_failure)
   # add_subdirectory(src/updown)
   # add_subdirectory(src/waypoint_follower)

nav2_system_tests/package.xml

@@ -30,6 +30,7 @@
   <build_depend>launch_ros</build_depend>
   <build_depend>launch_testing</build_depend>
   <build_depend>nav2_planner</build_depend>
+  <build_depend>nav2_minimal_tb3_sim</build_depend>
 
   <exec_depend>launch_ros</exec_depend>
   <exec_depend>launch_testing</exec_depend>
@@ -48,6 +49,7 @@
   <exec_depend>nav2_amcl</exec_depend>
   <exec_depend>std_msgs</exec_depend>
   <exec_depend>tf2_geometry_msgs</exec_depend>
+  <exec_depend>nav2_minimal_tb3_sim</exec_depend>
   <!-- <exec_depend>gazebo_ros_pkgs</exec_depend> Remove/Replace after https://github.com/ros-navigation/navigation2/pull/3634 -->
   <exec_depend>navigation2</exec_depend>
   <exec_depend>lcov</exec_depend>

nav2_system_tests/src/system/CMakeLists.txt

@@ -22,9 +22,6 @@ ament_add_test(test_bt_navigator_with_wrong_init_pose
   TIMEOUT 180
   ENV
     TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
-    TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml
-    TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world
-    GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
     BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
     TESTER=nav_to_pose_tester_node.py
     ASTAR=True
@@ -39,9 +36,6 @@ ament_add_test(test_bt_navigator_with_dijkstra
   TIMEOUT 180
   ENV
     TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
-    TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml
-    TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world
-    GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
     BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
     TESTER=nav_to_pose_tester_node.py
     ASTAR=False
@@ -56,9 +50,6 @@ ament_add_test(test_bt_navigator_2
   TIMEOUT 180
   ENV
     TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
-    TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml
-    TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world
-    GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
     BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
     TESTER=nav_to_pose_tester_node.py
     ASTAR=False
@@ -68,14 +59,12 @@ ament_add_test(test_bt_navigator_2
 
 ament_add_test(test_dynamic_obstacle
   GENERATE_RESULT_FOR_RETURN_CODE_ZERO
-  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py"
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_with_obstacle_launch.py"
   WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
   TIMEOUT 180
   ENV
     TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
-    TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml
-    TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo_obstacle.world
-    GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
+    ADD_OBSTACLE=True
     BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
     TESTER=nav_to_pose_tester_node.py
     ASTAR=False
@@ -85,14 +74,12 @@ ament_add_test(test_dynamic_obstacle
 
 ament_add_test(test_nav_through_poses
   GENERATE_RESULT_FOR_RETURN_CODE_ZERO
-  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py"
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_with_obstacle_launch.py"
   WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
   TIMEOUT 180
   ENV
     TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
-    TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml
-    TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo_obstacle.world
-    GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
+    ADD_OBSTACLE=True
     BT_NAVIGATOR_XML=navigate_through_poses_w_replanning_and_recovery.xml
     TESTER=nav_through_poses_tester_node.py
     ASTAR=False

nav2_system_tests/src/system/nav2_system_params.yaml

@@ -317,3 +317,49 @@ collision_monitor:
       min_height: 0.15
       max_height: 2.0
       enabled: True
+
+docking_server:
+  ros__parameters:
+    controller_frequency: 50.0
+    initial_perception_timeout: 5.0
+    wait_charge_timeout: 5.0
+    dock_approach_timeout: 30.0
+    undock_linear_tolerance: 0.05
+    undock_angular_tolerance: 0.1
+    max_retries: 3
+    base_frame: "base_link"
+    fixed_frame: "odom"
+    dock_backwards: false
+    dock_prestaging_tolerance: 0.5
+
+    # Types of docks
+    dock_plugins: ['simple_charging_dock']
+    simple_charging_dock:
+      plugin: 'opennav_docking::SimpleChargingDock'
+      docking_threshold: 0.05
+      staging_x_offset: -0.7
+      use_external_detection_pose: true
+      use_battery_status: false # true
+      use_stall_detection: false # true
+
+      external_detection_timeout: 1.0
+      external_detection_translation_x: -0.18
+      external_detection_translation_y: 0.0
+      external_detection_rotation_roll: -1.57
+      external_detection_rotation_pitch: -1.57
+      external_detection_rotation_yaw: 0.0
+      filter_coef: 0.1
+
+    # Dock instances
+    docks: ['home_dock']  # Input your docks here
+    home_dock:
+      type: 'simple_charging_dock'
+      frame: map
+      pose: [0.0, 0.0, 0.0]
+
+    controller:
+      k_phi: 3.0
+      k_delta: 2.0
+      v_linear_min: 0.15
+      v_linear_max: 0.15
+

nav2_system_tests/src/system/nav_to_pose_tester_node.py

@@ -59,6 +59,7 @@ def __init__(self, initial_pose: Pose, goal_pose: Pose, namespace: str = ''):
         self.initial_pose_received = False
         self.initial_pose = initial_pose
         self.goal_pose = goal_pose
+        self.set_initial_pose_timeout = 100
         self.action_client = ActionClient(self, NavigateToPose, 'navigate_to_pose')
 
     def info_msg(self, msg: str):
@@ -212,11 +213,22 @@ def shutdown(self):
 
     def wait_for_initial_pose(self):
         self.initial_pose_received = False
+        # If the initial pose is not received within 100 seconds, return False
+        # this is because when setting a wrong initial pose, amcl_pose is not received
+        # and the test will hang indefinitely
+        start_time = time.time()
+        duration = 0        
         while not self.initial_pose_received:
             self.info_msg('Setting initial pose')
             self.setInitialPose()
             self.info_msg('Waiting for amcl_pose to be received')
+            duration = time.time() - start_time
+            print("waited ", duration, " for initial pose to be set")
+            if duration > self.set_initial_pose_timeout:
+                self.error_msg('Timeout waiting for initial pose to be set')
+                return False
             rclpy.spin_once(self, timeout_sec=1)
+        return True
 
 
 def test_RobotMovesToGoal(robot_tester):
@@ -231,7 +243,8 @@ def run_all_tests(robot_tester):
     result = True
     if result:
         robot_tester.wait_for_node_active('amcl')
-        robot_tester.wait_for_initial_pose()
+        result = robot_tester.wait_for_initial_pose()
+    if result:
         robot_tester.wait_for_node_active('bt_navigator')
         result = robot_tester.runNavigateAction()
 

nav2_system_tests/src/system/test_system_launch.py

@@ -16,13 +16,16 @@
 # limitations under the License.
 
 import os
+from pathlib import Path
+import signal
+import subprocess
 import sys
-
 from ament_index_python.packages import get_package_share_directory
 
 from launch import LaunchDescription
 from launch import LaunchService
 from launch.actions import (
+    AppendEnvironmentVariable,
     ExecuteProcess,
     IncludeLaunchDescription,
     SetEnvironmentVariable,
@@ -36,8 +39,14 @@
 
 
 def generate_launch_description():
-    map_yaml_file = os.getenv('TEST_MAP')
-    world = os.getenv('TEST_WORLD')
+    sim_dir = get_package_share_directory('nav2_minimal_tb3_sim')
+    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
+    ros_gz_sim_dir = get_package_share_directory('ros_gz_sim')
+
+    world_sdf_xacro = os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro')
+    robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf')
+
+    map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'tb3_sandbox.yaml')
 
     bt_navigator_xml = os.path.join(
         get_package_share_directory('nav2_bt_navigator'),
@@ -79,16 +88,33 @@ def generate_launch_description():
         [
             SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),
             SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'),
-            # Launch gazebo server for simulation
-            ExecuteProcess(
-                cmd=[
-                    'gzserver',
-                    '-s',
-                    'libgazebo_ros_init.so',
-                    '--minimal_comms',
-                    world,
-                ],
-                output='screen',
+            AppendEnvironmentVariable(
+                'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models')
+            ),
+            AppendEnvironmentVariable(
+            'GZ_SIM_RESOURCE_PATH',
+                str(Path(os.path.join(sim_dir)).parent.resolve())
+            ),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    os.path.join(ros_gz_sim_dir, 'launch', 'gz_sim.launch.py')
+                ),
+                launch_arguments={'gz_args': ['-r -s ', world_sdf_xacro]}.items(),
+            ),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    os.path.join(sim_dir, 'launch', 'spawn_tb3.launch.py')
+                ),
+                launch_arguments={
+                    'use_sim_time': 'True',
+                    'robot_sdf': robot_sdf,
+                    'x_pose': '-2.0',
+                    'y_pose': '-0.5',
+                    'z_pose': '0.01',
+                    'roll': '0.0',
+                    'pitch': '0.0',
+                    'yaw': '0.0',
+                }.items(),
             ),
             # TODO(orduno) Launch the robot state publisher instead
             #              using a local copy of TB3 urdf file
@@ -122,6 +148,26 @@ def generate_launch_description():
         ]
     )
 
+def find_gz_sim_processes():
+    ps_output = subprocess.check_output(['ps', 'aux'], text=True)
+    ps_lines = ps_output.split('\n')
+    gz_sim_processes = []
+    for line in ps_lines:
+        if 'gz sim' in line:
+            columns = line.split()
+            pid = columns[1]
+            command = ' '.join(columns[10:])
+            if command.startswith('gz sim'):
+                gz_sim_processes.append((pid, command))
+
+    return gz_sim_processes
+
+def kill_process(pid):
+    try:
+        os.kill(int(pid), signal.SIGKILL)
+        print(f"Successfully killed process with PID: {pid}")
+    except Exception as e:
+        print(f"Failed to kill process with PID: {pid}. Error: {e}")
 
 def main(argv=sys.argv[1:]):
     ld = generate_launch_description()
@@ -145,7 +191,22 @@ def main(argv=sys.argv[1:]):
     lts.add_test_action(ld, test1_action)
     ls = LaunchService(argv=argv)
     ls.include_launch_description(ld)
-    return lts.run(ls)
+    return_code =  lts.run(ls)
+    # (TODO ) This is a workaround to kill the gz server after the test
+    # We noticed that the gz server is not killed after the test
+    # and it is still running in the background. This affects
+    # the next test run. This is a temporary fix until we find
+    # a better way of killing the gz server after the test.
+    processes = find_gz_sim_processes()
+    if processes:
+        print("Found the following processes starting with 'gz_sim':")
+        for pid, command in processes:
+            print(f"PID: {pid}, Command: {command}")
+            kill_process(pid)
+    else:
+        print("No processes found starting with 'gz_sim'")
+
+    return return_code
 
 
 if __name__ == '__main__':