Port system tests in nav2_system_tests (ros-navigation#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]>

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}')

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:/ros-navigation/navigation2/pull/3634
  add_subdirectory(src/localization)
- # add_subdirectory(src/system)
+ add_subdirectory(src/system)
+ # Uncomment after https:/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()
 

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>

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:/ros-navigation/navigation2/pull/3634 -->
  <exec_depend>navigation2</exec_depend>
  <exec_depend>lcov</exec_depend>

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__':

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

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 = 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()