Add sim tests

pmb2_gazebo/CMakeLists.txt

@@ -10,7 +10,20 @@ if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
 
   ament_lint_auto_find_test_dependencies()
+
+  ament_add_gtest_executable(test_pmb2_gazebo
+      test/test_pmb2_gazebo.cpp)
+
+  ament_target_dependencies(test_pmb2_gazebo
+    control_msgs
+    rclcpp
+  )
+add_launch_test(
+  test/test_gazebo.launch.py
+  TARGET "test_pmb2_gazebo"
+  ARGS "test_dir:=${CMAKE_CURRENT_BINARY_DIR}"
+)
+
 endif()
 
-# Media commented out
 ament_auto_package(INSTALL_TO_SHARE launch models worlds)

pmb2_gazebo/package.xml

@@ -24,8 +24,14 @@
   <exec_depend>simple_models_gazebo</exec_depend>
   -->
 
+  <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
+  <test_depend>control_msgs</test_depend>
+  <test_depend>sensor_msgs</test_depend>
+  <test_depend>launch_testing_ament_cmake</test_depend>
+  <test_depend>rclcpp</test_depend>
+  <test_depend>std_msgs</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

pmb2_gazebo/test/test_gazebo.launch.py

@@ -0,0 +1,67 @@
+# Copyright (c) 2021 PAL Robotics S.L.
+#
+# 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 launch_testing
+import unittest
+import os
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import PathJoinSubstitution, LaunchConfiguration
+from launch_ros.actions import Node
+from launch_pal.include_utils import include_launch_py_description
+
+
+def generate_test_description():
+    # This is necessary to get unbuffered output from the process under test
+    proc_env = os.environ.copy()
+    proc_env['PYTHONUNBUFFERED'] = '1'
+
+    test_dir = DeclareLaunchArgument('test_dir',
+                                     description='Path to test dir, where the gtest '
+                                     'executable is located')
+
+    # dut = device under test, aka the actual test
+    # The compiled gtest executable is not installed, wee need to pass the test_dir
+    # to get the path to the executable
+    dut_process = launch_testing.actions.GTest(
+        path=PathJoinSubstitution(
+            [LaunchConfiguration('test_dir'), 'test_pmb2_gazebo']),
+        output="both")
+
+    pmb2_gazebo_launch = include_launch_py_description('pmb2_gazebo',
+                                                       ['launch', 'pmb2_gazebo.launch.py'])
+
+    return LaunchDescription([
+        test_dir,
+        pmb2_gazebo_launch,
+        dut_process,
+
+        launch_testing.actions.ReadyToTest(),
+    ]), {'dut_process': dut_process}
+
+
+class TestDescriptionPublished(unittest.TestCase):
+
+    def test_wait_for_end(self, proc_output, proc_info, dut_process):
+        # Wait until process ends
+        proc_info.assertWaitForShutdown(process=dut_process, timeout=10)
+
+
+@ launch_testing.post_shutdown_test()
+class TestSuccessfulExit(unittest.TestCase):
+
+    def test_exit_code(self, proc_info, dut_process):
+        # Check that dut_process finishes with code 0
+        launch_testing.asserts.assertExitCodes(proc_info, process=dut_process)

pmb2_gazebo/test/test_pmb2_gazebo.cpp

@@ -0,0 +1,66 @@
+// Copyright 2021 PAL Robotics SL.
+//
+// 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.
+
+#include <rclcpp/rclcpp.hpp>
+#include <gtest/gtest.h>
+#include <std_msgs/msg/string.hpp>
+#include <sensor_msgs/msg/joint_state.hpp>
+
+template<typename MsgType>
+void waitForMessage(
+  const rclcpp::Node::SharedPtr & node, const std::string & topic_name,
+  const std::chrono::milliseconds & timeout)
+{
+  typename MsgType::SharedPtr msg;
+
+  auto sub = node->create_subscription<std_msgs::msg::String>(
+    topic_name, 1, [&msg](const typename MsgType::SharedPtr recv_msg)
+    {});
+
+  rclcpp::WaitSet wait_set;
+  wait_set.add_subscription(sub);
+  auto ret = wait_set.wait(timeout);
+  EXPECT_EQ(
+    ret.kind(),
+    rclcpp::WaitResultKind::Ready) << "Did not receive any message on " << topic_name;
+  if (ret.kind() == rclcpp::WaitResultKind::Ready) {
+    MsgType msg;
+    rclcpp::MessageInfo info;
+    auto ret_take = sub->take(msg, info);
+    EXPECT_TRUE(ret_take) << "Error retrieving message";
+  } else {
+    RCLCPP_INFO_STREAM(node->get_logger(), "Got " << topic_name);
+  }
+}
+
+TEST(TestGazeboPMB2, test_topics)
+{
+  std::shared_ptr<rclcpp::Executor> executor =
+    std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+
+  auto test_node = std::make_shared<rclcpp::Node>("test_pmb2_gazebo");
+
+  waitForMessage<std_msgs::msg::String>(test_node, "robot_description", std::chrono::seconds(10));
+//  test_node->get_clock()->
+    rclcpp::sleep_for(std::chrono::seconds(20));
+}
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  ::testing::InitGoogleTest(&argc, argv);
+  auto res = RUN_ALL_TESTS();
+  rclcpp::shutdown();
+  return res;
+}