Add lrauv_system_tests package.

lrauv_system_tests/CMakeLists.txt

@@ -0,0 +1,46 @@
+#
+# Development of this module has been funded by the Monterey Bay Aquarium
+# Research Institute (MBARI) and the David and Lucile Packard Foundation
+#
+
+cmake_minimum_required(VERSION 3.5)
+
+project(lrauv_system_tests)
+
+find_package(ignition-gazebo7 REQUIRED)
+find_package(ignition-math7 REQUIRED)
+find_package(ignition-transport12 REQUIRED)
+find_package(lrauv_ignition_plugins REQUIRED)
+
+# Build-time constants
+set(PROJECT_BINARY_PATH ${CMAKE_CURRENT_BINARY_DIR})
+set(PROJECT_SOURCE_PATH ${CMAKE_CURRENT_SOURCE_DIR})
+configure_file(include/TestConstants.hh.in include/TestConstants.hh @ONLY)
+
+# Fetch and configure GTest
+include(FetchContent)
+FetchContent_Declare(
+  googletest
+  URL https://github.com/google/googletest/archive/609281088cfefc76f9d0ce82e1ff6c30cc3591e5.zip
+)
+set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
+FetchContent_MakeAvailable(googletest)
+include(GoogleTest)
+
+# Build test support library
+add_library(${PROJECT_NAME}_support
+  src/LRAUVController.cc
+  src/ModelManipulator.cc
+  src/ModelObserver.cc
+  src/Publisher.cc
+)
+target_include_directories(${PROJECT_NAME}_support PUBLIC
+  include ${CMAKE_CURRENT_BINARY_DIR}/include)
+target_link_libraries(${PROJECT_NAME}_support PUBLIC
+  ignition-gazebo7::ignition-gazebo7
+  ignition-math7::ignition-math7
+  ignition-transport12::ignition-transport12
+  lrauv_ignition_plugins::lrauv_command
+  lrauv_ignition_plugins::lrauv_state
+  gtest)
+target_compile_features(${PROJECT_NAME}_support PUBLIC cxx_std_17)

lrauv_system_tests/colcon.pkg

@@ -0,0 +1,3 @@
+{
+    "dependencies": ["lrauv_description", "lrauv_ignition_plugins"]
+}

lrauv_system_tests/include/TestConstants.hh.in

@@ -0,0 +1,8 @@
+#ifndef __TEST_CONSTANTS_HH__
+#define __TEST_CONSTANTS_HH__
+#define PROJECT_BINARY_PATH "@PROJECT_BINARY_PATH@"
+#define PROJECT_SOURCE_PATH "@PROJECT_SOURCE_PATH@"
+
+// Hardcode path, assume lrauv-application is a sibling of lrauv
+#define LRAUV_APP_PATH "@PROJECT_SOURCE_PATH@/../../lrauv-application"
+#endif

lrauv_system_tests/include/lrauv_system_tests/Client.hh

@@ -0,0 +1,106 @@
+/*
+ * Copyright (C) 2022 Open Source Robotics Foundation
+ *
+ * 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.
+ *
+ */
+
+/*
+ * Development of this module has been funded by the Monterey Bay Aquarium
+ * Research Institute (MBARI) and the David and Lucile Packard Foundation
+ */
+
+#ifndef LRAUV_SYSTEM_TESTS__CLIENT_HH
+#define LRAUV_SYSTEM_TESTS__CLIENT_HH
+
+#include <future>
+#include <mutex>
+#include <string>
+#include <unordered_map>
+
+#include <ignition/common/Console.hh>
+#include <ignition/transport/Node.hh>
+
+namespace lrauv_system_tests
+{
+
+/// Client for Ignition Transport topic-based services.
+/// \tparam RequestMessageT Request message type.
+/// \tparam ResponseMessageT Response message type.
+template<typename RequestMessageT, typename ResponseMessageT>
+class Client
+{
+  /// Constructor.
+  /// \param[in] _node Node to be used for topic advertisement and subscription.
+  /// \param[in] _ns Namespace for the ``requests`` and ``responses`` topics.
+  public: Client(ignition::transport::Node &_node, const std::string& _ns)
+    : requestsCounter(2)
+  {
+    const std::string responseTopicName = _ns + "/responses";
+    if (!_node.Subscribe(responseTopicName, &Client::OnResponse, this))
+    {
+      throw std::runtime_error("Failed to subscribe to " + responseTopicName);
+    }
+    this->requestsPublisher =
+        _node.Advertise<RequestMessageT>(_ns + "/requests");
+  }
+
+  /// Request a response from the remote server.
+  /// \param[in] _request Request message instance.
+  /// \returns a future response message, to be awaited.
+  public: std::future<ResponseMessageT> Request(RequestMessageT _request)
+  {
+    uint32_t request_id = this->requestsCounter++;
+    _request.set_req_id(request_id);
+
+    std::promise<ResponseMessageT> promise;
+    std::future<ResponseMessageT> future = promise.get_future();
+    {
+      std::lock_guard<std::mutex> lock(this->mutex);
+      this->responsePromises[request_id] = std::move(promise);
+    }
+    this->requestsPublisher.Publish(_request);
+    igndbg << "Published request" << "\n";
+    return future;
+  }
+
+  private: void OnResponse(const ResponseMessageT& response)
+  {
+    igndbg << "Received response\n";
+    std::lock_guard<std::mutex> lock(this->mutex);
+    auto it = this->responsePromises.find(response.req_id());
+    if (it == this->responsePromises.end())
+    {
+      ignwarn << "Received response with unknown request id: "
+              << response.req_id() << std::endl;
+      return;
+    }
+    it->second.set_value(response);
+    this->responsePromises.erase(it);
+  }
+
+  private: ignition::transport::Node node;
+
+  private: ignition::transport::Node::Publisher requestsPublisher;
+
+  private: std::unordered_map<
+    uint32_t, std::promise<ResponseMessageT>> responsePromises;
+
+  private: uint32_t requestsCounter;
+
+  private: std::mutex mutex;
+};
+
+}
+
+#endif  // LRAUV_SYSTEM_TESTS__CLIENT_HH

lrauv_system_tests/include/lrauv_system_tests/LRAUVController.hh

@@ -0,0 +1,75 @@
+/*
+ * Copyright (C) 2022 Open Source Robotics Foundation
+ *
+ * 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.
+ *
+ */
+
+/*
+ * Development of this module has been funded by the Monterey Bay Aquarium
+ * Research Institute (MBARI) and the David and Lucile Packard Foundation
+ */
+
+#ifndef LRAUV_SYSTEM_TESTS__LRAUVCONTROLLER_HH
+#define LRAUV_SYSTEM_TESTS__LRAUVCONTROLLER_HH
+
+#include <signal.h>
+
+#include <cstdio>
+#include <string>
+#include <thread>
+#include <vector>
+
+namespace lrauv_system_tests
+{
+
+/// Handler for an MBARI's LRAUV controller process.
+class LRAUVController
+{
+ public: ~LRAUVController();
+
+ /// Whether the controller process is still running or not.
+ /// \throws std::system_error if a system call fails.
+ public: bool IsRunning();
+
+ /// Kill controller process if running.
+ /// \param[in] signal Signal to be sent.
+ /// \return true if the signal was delivered, false otherwise.
+ /// \throws std::system_error if a system call fails.
+ public: bool Kill(int signal);
+
+ /// Wait for controller process to exit.
+ /// \return process exit status.
+ /// \throws std::system_error if a system call fails.
+ public: int Wait();
+
+ /// Start an LRAUV controller process to run `_commands`.
+ /// \param[in] _commands A sequence of LRAUV commands to execute.
+ /// \return handler instance for underlying controller process.
+ /// \throws std::system_error if a system call fails.
+ public: static LRAUVController Execute(const std::vector<std::string> &_commands);
+
+ // Constructor.
+ // \param[in] pid LRAUV controller process ID.
+ // \param[in] _stdout Read end of a pipe connected to
+ // the LRAUV controller process standard output.
+ private: LRAUVController(int _pid, FILE *_stdout);
+
+ private: std::thread backgroundThread;
+
+ private: int pid;
+};
+
+}
+
+#endif // LRAUV_SYSTEM_TESTS__LRAUVCONTROLLER_HH

lrauv_system_tests/include/lrauv_system_tests/ModelManipulator.hh

@@ -0,0 +1,59 @@
+/*
+ * Copyright (C) 2022 Open Source Robotics Foundation
+ *
+ * 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.
+ *
+ */
+
+/*
+ * Development of this module has been funded by the Monterey Bay Aquarium
+ * Research Institute (MBARI) and the David and Lucile Packard Foundation
+ */
+
+#ifndef LRAUV_SYSTEM_TESTS__MODEL_MANIPULATOR_HH
+#define LRAUV_SYSTEM_TESTS__MODEL_MANIPULATOR_HH
+
+#include <optional>
+#include <string>
+
+#include <ignition/gazebo/EntityComponentManager.hh>
+
+#include <ignition/math/Pose3.hh>
+#include <ignition/math/Quaternion.hh>
+
+namespace lrauv_system_tests {
+
+/// Helper class to manipulate a model in an Ignition Gazebo simulation.
+class ModelManipulator
+{
+  /// Constructor.
+  /// \param[in] _modelName Name of the model to manipulate.
+  public: ModelManipulator(const std::string &_modelName);
+
+  /// Update simulation state as necessary.
+  /// \note To be called on world pre-update.
+  /// \param[in] _ecm Entity component manager to update.
+  public: void Update(ignition::gazebo::EntityComponentManager &_ecm);
+
+  /// Set model orientation in simulation.
+  /// \param[in] _orientation Model orientation in the world frame.
+  public: void SetOrientation(const ignition::math::Quaterniond &_orientation);
+
+  private: std::string modelName;
+
+  private: std::optional<ignition::math::Pose3d> poseRequest;
+};
+
+}
+
+#endif // LRAUV_SYSTEM_TESTS__MODEL_MANIPULATOR_HH