Port system tests in nav2_system_tests (#4440)

* wip, ported only test_bt_navigator Signed-off-by: stevedan <[email protected]> * include test_bt_navigator_with dijlstra and test_bt_navigator_2 Signed-off-by: stevedan <[email protected]> * uncomment some lines Signed-off-by: stevedan <[email protected]> * More tests Signed-off-by: stevedan <[email protected]> * Include end of line Signed-off-by: stevedan <[email protected]> * move gz_sim cleanup process to utils nav2_simple_commander Signed-off-by: stevedan <[email protected]> * fix linter Signed-off-by: stevedan <[email protected]> * cleanup Signed-off-by: stevedan <[email protected]> * removed unused path Signed-off-by: stevedan <[email protected]> * cleanup Signed-off-by: stevedan <[email protected]> * more cleanup Signed-off-by: stevedan <[email protected]> * reduce set initial pose time Signed-off-by: stevedan <[email protected]> * remove repeated variable Signed-off-by: stevedan <[email protected]> * Remove log Signed-off-by: stevedan <[email protected]> * Remove todo Signed-off-by: stevedan <[email protected]> * use robot publisher Signed-off-by: stevedan <[email protected]> * use robot publisher Signed-off-by: stevedan <[email protected]> * include copyright Signed-off-by: stevedan <[email protected]> * correct year Signed-off-by: stevedan <[email protected]> --------- Signed-off-by: stevedan <[email protected]> Signed-off-by: Stevedan Ogochukwu Omodolor <[email protected]>
ros-navigation · Jun 20, 2024 · 9057456 · 9057456
1 parent 43b2388
commit 9057456
Show file tree

Hide file tree

Showing 18 changed files with 451 additions and 2,234 deletions.
diff --git a/nav2_simple_commander/nav2_simple_commander/utils.py b/nav2_simple_commander/nav2_simple_commander/utils.py
@@ -0,0 +1,55 @@
+#! /usr/bin/env python3
+# Copyright 2024 Open Navigation LLC
+# Copyright 2024 Stevedan Ogochukwu Omodolor Omodia
+#
+# 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.
+
+"""General utility functions."""
+
+import os
+import signal
+import subprocess
+
+
+def find_os_processes(name):
+    """Find all the processes that are running gz sim."""
+    ps_output = subprocess.check_output(['ps', 'aux'], text=True)
+    ps_lines = ps_output.split('\n')
+    gz_sim_processes = []
+    for line in ps_lines:
+        if name in line:
+            columns = line.split()
+            pid = columns[1]
+            command = ' '.join(columns[10:])
+            if command.startswith(name):
+                gz_sim_processes.append((pid, command))
+    return gz_sim_processes
+
+
+def kill_process(pid):
+    """Kill a process with a given 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 kill_os_processes(name):
+    """Kill all processes that are running with name."""
+    processes = find_os_processes(name)
+    if processes:
+        for pid, _ in processes:
+            kill_process(pid)
+    else:
+        print(f'No processes found starting with {name}')
diff --git a/nav2_system_tests/CMakeLists.txt b/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)
@@ -127,7 +128,7 @@ if(BUILD_TESTING)
   # add_subdirectory(src/behaviors/assisted_teleop)
   # add_subdirectory(src/costmap_filters)
   add_subdirectory(src/error_codes)
-  install(DIRECTORY maps DESTINATION share/${PROJECT_NAME})
+  install(DIRECTORY maps models DESTINATION share/${PROJECT_NAME})
 
 endif()
 

diff --git a/nav2_system_tests/models/cardboard_box.sdf b/nav2_system_tests/models/cardboard_box.sdf
@@ -0,0 +1,9 @@
+<sdf version="1.6">
+     <model name='cardboard_box'>
+      <include>
+      <uri>
+      https://fuel.gazebosim.org/1.0/OpenRobotics/models/Cardboard box
+      </uri>
+      </include>
+    </model>
+</sdf>
diff --git a/nav2_system_tests/package.xml b/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>

diff --git a/nav2_system_tests/src/localization/test_localization_launch.py b/nav2_system_tests/src/localization/test_localization_launch.py
@@ -15,6 +15,7 @@
 # limitations under the License.
 
 import os
+from pathlib import Path
 import sys
 
 from ament_index_python.packages import get_package_share_directory
@@ -26,6 +27,8 @@
 import launch_ros.actions
 from launch_testing.legacy import LaunchTestService
 
+from nav2_simple_commander.utils import kill_os_processes
+
 
 def main(argv=sys.argv[1:]):
     testExecutable = os.getenv('TEST_EXECUTABLE')
@@ -36,11 +39,19 @@ def main(argv=sys.argv[1:]):
     world_sdf_xacro = os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro')
     robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf')
 
+    urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf')
+    with open(urdf, 'r') as infp:
+        robot_description = infp.read()
+
     map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'tb3_sandbox.yaml')
 
     set_env_vars_resources = AppendEnvironmentVariable(
         'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models')
     )
+    set_env_vars_resources2 = AppendEnvironmentVariable(
+        'GZ_SIM_RESOURCE_PATH',
+        str(Path(os.path.join(sim_dir)).parent.resolve())
+    )
 
     start_gazebo_server = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
@@ -64,18 +75,14 @@ def main(argv=sys.argv[1:]):
             'yaw': '0.0',
         }.items(),
     )
-
-    link_footprint = launch_ros.actions.Node(
-        package='tf2_ros',
-        executable='static_transform_publisher',
-        output='screen',
-        arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'],
-    )
-    footprint_scan = launch_ros.actions.Node(
-        package='tf2_ros',
-        executable='static_transform_publisher',
+    run_robot_state_publisher = launch_ros.actions.Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        name='robot_state_publisher',
         output='screen',
-        arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'],
+        parameters=[
+            {'use_sim_time': True, 'robot_description': robot_description}
+        ],
     )
     run_map_server = launch_ros.actions.Node(
         package='nav2_map_server',
@@ -97,10 +104,10 @@ def main(argv=sys.argv[1:]):
     ld = LaunchDescription(
         [
             set_env_vars_resources,
+            set_env_vars_resources2,
             start_gazebo_server,
             spawn_robot,
-            link_footprint,
-            footprint_scan,
+            run_robot_state_publisher,
             run_map_server,
             run_amcl,
             run_lifecycle_manager,
@@ -115,7 +122,9 @@ 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)
+    kill_os_processes('gz sim')
+    return return_code
 
 
 if __name__ == '__main__':

diff --git a/nav2_system_tests/src/system/CMakeLists.txt b/nav2_system_tests/src/system/CMakeLists.txt
@@ -5,9 +5,6 @@ ament_add_test(test_bt_navigator
   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
@@ -22,9 +19,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 +33,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 +47,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 +56,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 +71,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

diff --git a/nav2_system_tests/src/system/nav2_system_params.yaml b/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
+
diff --git a/nav2_system_tests/src/system/nav_to_pose_tester_node.py b/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 = 15
         self.action_client = ActionClient(self, NavigateToPose, 'navigate_to_pose')
 
     def info_msg(self, msg: str):
@@ -212,11 +213,21 @@ 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
+            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 +242,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()