wip, ported only test_bt_navigator

Signed-off-by: stevedan <[email protected]>
ros-navigation · Jun 16, 2024 · a6a91ae · a6a91ae
1 parent 23ddd86
commit a6a91ae
Show file tree

Hide file tree

Showing 5 changed files with 181 additions and 96 deletions.
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()
 
@@ -115,7 +116,7 @@ if(BUILD_TESTING)
   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)
   # add_subdirectory(src/system_failure)
   # add_subdirectory(src/updown)
   # add_subdirectory(src/waypoint_follower)

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/system/CMakeLists.txt b/nav2_system_tests/src/system/CMakeLists.txt
@@ -15,90 +15,90 @@ ament_add_test(test_bt_navigator
     PLANNER=nav2_navfn_planner::NavfnPlanner
 )
 
-ament_add_test(test_bt_navigator_with_wrong_init_pose
-  GENERATE_RESULT_FOR_RETURN_CODE_ZERO
-  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_wrong_init_pose_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.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
-    CONTROLLER=nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController
-    PLANNER=nav2_navfn_planner::NavfnPlanner
-)
+# ament_add_test(test_bt_navigator_with_wrong_init_pose
+#   GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+#   COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_wrong_init_pose_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.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
+#     CONTROLLER=nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController
+#     PLANNER=nav2_navfn_planner::NavfnPlanner
+# )
 
-ament_add_test(test_bt_navigator_with_dijkstra
-  GENERATE_RESULT_FOR_RETURN_CODE_ZERO
-  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_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.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
-    CONTROLLER=dwb_core::DWBLocalPlanner
-    PLANNER=nav2_navfn_planner::NavfnPlanner
-)
+# ament_add_test(test_bt_navigator_with_dijkstra
+#   GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+#   COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_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.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
+#     CONTROLLER=dwb_core::DWBLocalPlanner
+#     PLANNER=nav2_navfn_planner::NavfnPlanner
+# )
 
-ament_add_test(test_bt_navigator_2
-  GENERATE_RESULT_FOR_RETURN_CODE_ZERO
-  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_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.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
-    CONTROLLER=dwb_core::DWBLocalPlanner
-    PLANNER=nav2_navfn_planner::NavfnPlanner
-)
+# ament_add_test(test_bt_navigator_2
+#   GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+#   COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_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.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
+#     CONTROLLER=dwb_core::DWBLocalPlanner
+#     PLANNER=nav2_navfn_planner::NavfnPlanner
+# )
 
-ament_add_test(test_dynamic_obstacle
-  GENERATE_RESULT_FOR_RETURN_CODE_ZERO
-  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_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
-    BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
-    TESTER=nav_to_pose_tester_node.py
-    ASTAR=False
-    CONTROLLER=dwb_core::DWBLocalPlanner
-    PLANNER=nav2_navfn_planner::NavfnPlanner
-)
+# ament_add_test(test_dynamic_obstacle
+#   GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+#   COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_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
+#     BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
+#     TESTER=nav_to_pose_tester_node.py
+#     ASTAR=False
+#     CONTROLLER=dwb_core::DWBLocalPlanner
+#     PLANNER=nav2_navfn_planner::NavfnPlanner
+# )
 
-ament_add_test(test_nav_through_poses
-  GENERATE_RESULT_FOR_RETURN_CODE_ZERO
-  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_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
-    BT_NAVIGATOR_XML=navigate_through_poses_w_replanning_and_recovery.xml
-    TESTER=nav_through_poses_tester_node.py
-    ASTAR=False
-    CONTROLLER=dwb_core::DWBLocalPlanner
-    PLANNER=nav2_navfn_planner::NavfnPlanner
-)
+# ament_add_test(test_nav_through_poses
+#   GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+#   COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_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
+#     BT_NAVIGATOR_XML=navigate_through_poses_w_replanning_and_recovery.xml
+#     TESTER=nav_through_poses_tester_node.py
+#     ASTAR=False
+#     CONTROLLER=dwb_core::DWBLocalPlanner
+#     PLANNER=nav2_navfn_planner::NavfnPlanner
+# )
 
 # ament_add_test(test_multi_robot
 #   GENERATE_RESULT_FOR_RETURN_CODE_ZERO

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,48 @@ 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/test_system_launch.py b/nav2_system_tests/src/system/test_system_launch.py
@@ -17,12 +17,13 @@
 
 import os
 import sys
-
+from pathlib import Path
 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 +37,16 @@
 
 
 def generate_launch_description():
-    map_yaml_file = os.getenv('TEST_MAP')
-    world = os.getenv('TEST_WORLD')
+    # 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'),
@@ -77,19 +86,47 @@ def generate_launch_description():
 
     return LaunchDescription(
         [
-            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',
+            # SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),
+            # SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'),
+            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(),
             ),
+            # # Launch gazebo server for simulation
+            # ExecuteProcess(
+            #     cmd=[
+            #         'gzserver',
+            #         '-s',
+            #         'libgazebo_ros_init.so',
+            #         '--minimal_comms',
+            #         world,
+            #     ],
+            #     output='screen',
+            # ),
             # TODO(orduno) Launch the robot state publisher instead
             #              using a local copy of TB3 urdf file
             Node(