adding wait recovery integration test (#1685)

* adding recovery wait test

* adding copy rights

* fixing gaurds

Author: SteveMacenski

nav2_system_tests/CMakeLists.txt

@@ -57,7 +57,8 @@ if(BUILD_TESTING)
   add_subdirectory(src/system)
   add_subdirectory(src/updown)
   add_subdirectory(src/waypoint_follower)
-  add_subdirectory(src/recoveries)
+  add_subdirectory(src/recoveries/spin)
+  add_subdirectory(src/recoveries/wait)
   install(DIRECTORY maps DESTINATION share/${PROJECT_NAME})
 
 endif()

nav2_system_tests/src/recoveries/CMakeLists.txt renamed to nav2_system_tests/src/recoveries/spin/CMakeLists.txt

@@ -13,8 +13,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License. Reserved.
 
-#ifndef RECOVERIES__SPIN_RECOVERY_TESTER_HPP_
-#define RECOVERIES__SPIN_RECOVERY_TESTER_HPP_
+#ifndef RECOVERIES__SPIN__SPIN_RECOVERY_TESTER_HPP_
+#define RECOVERIES__SPIN__SPIN_RECOVERY_TESTER_HPP_
 
 #include <gtest/gtest.h>
 #include <memory>
@@ -86,4 +86,4 @@ class SpinRecoveryTester
 
 }  // namespace nav2_system_tests
 
-#endif  // RECOVERIES__SPIN_RECOVERY_TESTER_HPP_
+#endif  // RECOVERIES__SPIN__SPIN_RECOVERY_TESTER_HPP_

nav2_system_tests/src/recoveries/spin_recovery_tester.cpp renamed to nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp

@@ -1,3 +1,4 @@
+// Copyright (c) 2020 Samsung Research
 // Copyright (c) 2020 Sarthak Mittal
 //
 // Licensed under the Apache License, Version 2.0 (the "License");

nav2_system_tests/src/recoveries/spin_recovery_tester.hpp renamed to nav2_system_tests/src/recoveries/spin/spin_recovery_tester.hpp

@@ -0,0 +1,23 @@
+set(test_wait_recovery_exec test_wait_recovery_node)
+
+ament_add_gtest_executable(${test_wait_recovery_exec}
+  test_wait_recovery_node.cpp
+  wait_recovery_tester.cpp
+)
+
+ament_target_dependencies(${test_wait_recovery_exec}
+  ${dependencies}
+)
+
+ament_add_test(test_wait_recovery
+  GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_wait_recovery_launch.py"
+  WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  TIMEOUT 100
+  ENV
+    TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml
+    TEST_EXECUTABLE=$<TARGET_FILE:${test_wait_recovery_exec}>
+    TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world
+    GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
+    BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml
+)

nav2_system_tests/src/recoveries/test_spin_recovery_launch.py renamed to nav2_system_tests/src/recoveries/spin/test_spin_recovery_launch.py

@@ -0,0 +1,102 @@
+#! /usr/bin/env python3
+# Copyright (c) 2019 Samsung Research America
+#
+# 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.
+
+import os
+import sys
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch import LaunchService
+from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+from launch_testing.legacy import LaunchTestService
+
+from nav2_common.launch import RewrittenYaml
+
+
+def generate_launch_description():
+    map_yaml_file = os.getenv('TEST_MAP')
+    world = os.getenv('TEST_WORLD')
+
+    bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'),
+                                    'behavior_trees',
+                                    os.getenv('BT_NAVIGATOR_XML'))
+
+    bringup_dir = get_package_share_directory('nav2_bringup')
+    params_file = os.path.join(bringup_dir, 'params/nav2_params.yaml')
+
+    # Replace the `use_astar` setting on the params file
+    configured_params = RewrittenYaml(
+        source_file=params_file,
+        root_key='',
+        param_rewrites='',
+        convert_types=True)
+
+    return LaunchDescription([
+        SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1'),
+
+        # 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(
+            package='tf2_ros',
+            node_executable='static_transform_publisher',
+            output='screen',
+            arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']),
+
+        Node(
+            package='tf2_ros',
+            node_executable='static_transform_publisher',
+            output='screen',
+            arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']),
+
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(bringup_dir, 'launch', 'bringup_launch.py')),
+            launch_arguments={
+                              'map': map_yaml_file,
+                              'use_sim_time': 'True',
+                              'params_file': configured_params,
+                              'bt_xml_file': bt_navigator_xml,
+                              'autostart': 'True'}.items()),
+    ])
+
+
+def main(argv=sys.argv[1:]):
+    ld = generate_launch_description()
+
+    testExecutable = os.getenv('TEST_EXECUTABLE')
+
+    test1_action = ExecuteProcess(
+        cmd=[testExecutable],
+        name='test_wait_recovery_node',
+        output='screen')
+
+    lts = LaunchTestService()
+    lts.add_test_action(ld, test1_action)
+    ls = LaunchService(argv=argv)
+    ls.include_launch_description(ld)
+    return lts.run(ls)
+
+
+if __name__ == '__main__':
+    sys.exit(main())

nav2_system_tests/src/recoveries/test_spin_recovery_node.cpp renamed to nav2_system_tests/src/recoveries/spin/test_spin_recovery_node.cpp

@@ -0,0 +1,89 @@
+// Copyright (c) 2020 Samsung Research
+// Copyright (c) 2020 Sarthak Mittal
+//
+// 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. Reserved.
+
+#include <gtest/gtest.h>
+#include <cmath>
+#include <tuple>
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "wait_recovery_tester.hpp"
+
+using namespace std::chrono_literals;
+
+using nav2_system_tests::WaitRecoveryTester;
+
+class WaitRecoveryTestFixture
+  : public ::testing::TestWithParam<std::tuple<float, float>>
+{
+public:
+  static void SetUpTestCase()
+  {
+    wait_recovery_tester = new WaitRecoveryTester();
+    if (!wait_recovery_tester->isActive()) {
+      wait_recovery_tester->activate();
+    }
+  }
+
+  static void TearDownTestCase()
+  {
+    delete wait_recovery_tester;
+    wait_recovery_tester = nullptr;
+  }
+
+protected:
+  static WaitRecoveryTester * wait_recovery_tester;
+};
+
+WaitRecoveryTester * WaitRecoveryTestFixture::wait_recovery_tester = nullptr;
+
+TEST_P(WaitRecoveryTestFixture, testSWaitRecovery)
+{
+  float wait_time = std::get<0>(GetParam());
+
+  bool success = false;
+  int num_tries = 3;
+  for (int i = 0; i != num_tries; i++) {
+    success = success || wait_recovery_tester->recoveryTest(wait_time);
+    if (success) {
+      break;
+    }
+  }
+
+  EXPECT_EQ(true, success);
+}
+
+INSTANTIATE_TEST_CASE_P(
+  WaitRecoveryTests,
+  WaitRecoveryTestFixture,
+  ::testing::Values(
+    std::make_tuple(1.0, 0.0),
+    std::make_tuple(2.0, 0.0),
+    std::make_tuple(5.0, 0.0)), );
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  bool all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+
+  return all_successful;
+}