Feature: basic safety manager

src/misc/safety_manager/CMakeLists.txt

@@ -0,0 +1,42 @@
+# Package:   safety_manager
+# Filename:  CMakeLists.txt
+# Author:    Joshua Williams
+# Email:     joshmackwilliams@protonmail.com
+# Copyright: 2021, Nova UTD
+# License:   MIT License
+
+cmake_minimum_required(VERSION 3.5)
+project(safety_manager)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+ament_auto_add_library(safety_manager_lib OBJECT
+  src/SafetyManagerNode.cpp)
+target_include_directories(safety_manager_lib PUBLIC include)
+
+add_executable(safety_manager src/safety_manager.cpp)
+target_link_libraries(safety_manager safety_manager_lib)
+
+install(TARGETS safety_manager DESTINATION lib/${PROJECT_NAME})
+
+# Tests
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+
+  ament_add_gtest(tests test/test_safety_manager.cpp)
+  ament_target_dependencies(tests rclcpp voltron_msgs voltron_test_utils)
+  target_link_libraries(tests gtest_main safety_manager_lib)
+endif()
+
+ament_auto_package()

src/misc/safety_manager/include/safety_manager/SafetyManagerNode.hpp

@@ -0,0 +1,32 @@
+/*
+ * Package:   safety_manager
+ * Filename:  SafetyManagerNode.cpp
+ * Author:    Joshua Williams
+ * Email:     joshmackwilliams@protonmail.com
+ * Copyright: 2021, Nova UTD
+ * License:   MIT License
+ */
+
+#pragma once
+
+#include "rclcpp/rclcpp.hpp"
+#include "voltron_msgs/srv/safety_event.hpp"
+
+namespace Nova {
+namespace SafetyManager {
+
+using voltron_msgs::srv::SafetyEvent;
+
+class SafetyManagerNode : public rclcpp::Node {
+public:
+  SafetyManagerNode();
+  virtual ~SafetyManagerNode();
+
+private:
+  void log_event(const SafetyEvent::Request::SharedPtr & request, SafetyEvent::Response::SharedPtr & response);
+
+  rclcpp::Service<SafetyEvent>::SharedPtr server;
+};
+
+}
+}

src/misc/safety_manager/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>safety_manager</name>
+  <version>0.0.0</version>
+  <description>Safety manager node for Nova</description>
+  <maintainer email="joshmackwilliams@protonmail.com">Joshua Williams</maintainer>
+  <license>MIT License</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>voltron_msgs</depend>
+
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>voltron_test_utils</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/misc/safety_manager/src/SafetyManagerNode.cpp

@@ -0,0 +1,49 @@
+/*
+ * Package:   safety_manager
+ * Filename:  SafetyManagerNode.cpp
+ * Author:    Joshua Williams
+ * Email:     joshmackwilliams@protonmail.com
+ * Copyright: 2021, Nova UTD
+ * License:   MIT License
+ */
+
+#include "safety_manager/SafetyManagerNode.hpp"
+
+#include <string>
+
+using namespace Nova::SafetyManager;
+using voltron_msgs::srv::SafetyEvent;
+
+SafetyManagerNode::SafetyManagerNode() : Node("safety_manager") {
+  this->server = this->create_service<SafetyEvent>("safety_events",
+    [&] (const SafetyEvent::Request::SharedPtr request, SafetyEvent::Response::SharedPtr response) {
+      this->log_event(request, response);
+    }
+  );
+}
+
+SafetyManagerNode::~SafetyManagerNode() {
+
+}
+
+void SafetyManagerNode::log_event(const SafetyEvent::Request::SharedPtr & request, SafetyEvent::Response::SharedPtr & response) {
+  std::string message;
+  switch(request->status) {
+  case SafetyEvent::Request::STATUS_WORKING:
+    message += "A node is working to resolve safety event: ";
+    break;
+  case SafetyEvent::Request::STATUS_UNRESOLVED:
+    message += "A safety event is unresolved: ";
+    break;
+  case SafetyEvent::Request::STATUS_RESOLVED:
+    message += "A safety event has been resolved: ";
+    break;
+  default:
+    message += "A safety event was raised with a bad status: ";
+    break;
+  }
+  message += request->description;
+  RCLCPP_INFO(this->get_logger(), message);
+
+  (void) response;
+}

src/misc/safety_manager/src/safety_manager.cpp

@@ -0,0 +1,21 @@
+/*
+ * Package:   safety_manager
+ * Filename:  src/safety_manager.cpp
+ * Author:    Joshua Williams
+ * Email:     joshmackwilliams@protonmail.com
+ * Copyright: 2021, Nova UTD
+ * License:   MIT License
+ */
+
+// The safety manager. Currently just a hello world, but that's going to change.
+
+#include <memory>
+#include "rclcpp/rclcpp.hpp"
+#include "safety_manager/SafetyManagerNode.hpp"
+
+int main(int argc, char ** argv) {
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<Nova::SafetyManager::SafetyManagerNode>());
+  rclcpp::shutdown();
+  return 0;
+}

src/misc/safety_manager/test/test_safety_manager.cpp

@@ -0,0 +1,57 @@
+/*
+ * Package:   safety_manager
+ * Filename:  test_safety_manager.cpp
+ * Author:    Joshua Williams
+ * Email:     joshmackwilliams@protonmail.com
+ * Copyright: 2021, Nova UTD
+ * License:   MIT License
+ */
+
+// Basic tests for the safety manager
+
+#include <gtest/gtest.h>
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+#include "voltron_msgs/srv/safety_event.hpp"
+
+#include "voltron_test_utils/TestClient.hpp"
+
+#include "safety_manager/SafetyManagerNode.hpp"
+
+using namespace Voltron::TestUtils;
+using namespace Nova::SafetyManager;
+
+class TestSafetyManager : public ::testing::Test {
+protected:
+  void SetUp() override {
+    rclcpp::init(0, nullptr);
+    my_safety_manager = std::make_shared<SafetyManagerNode>();
+    test_client = std::make_unique<TestClient<voltron_msgs::srv::SafetyEvent>>("safety_events");
+  }
+
+  void TearDown() override {
+    rclcpp::shutdown();
+  }
+
+  std::shared_ptr<SafetyManagerNode> my_safety_manager;
+  std::unique_ptr<TestClient<voltron_msgs::srv::SafetyEvent>> test_client;
+};
+
+TEST_F(TestSafetyManager, test_initializes) {
+  // All the ecessary code for this test is already written in the
+  // fixture
+}
+
+// Not much we can do here until actual logic is implemented
+TEST_F(TestSafetyManager, test_event) {
+  auto message_to_send = voltron_msgs::srv::SafetyEvent::Request();
+  message_to_send.event_uid = 4612894;
+  message_to_send.sequence_number = 0;
+  message_to_send.status = voltron_msgs::srv::SafetyEvent::Request::STATUS_RESOLVED;
+  message_to_send.description = "A test event";
+  message_to_send.additional_data = "";
+  test_client->send_request(message_to_send);
+  rclcpp::spin_some(my_safety_manager);
+  ASSERT_TRUE(test_client->request_complete());
+}

src/msg/voltron_msgs/CMakeLists.txt

@@ -11,6 +11,8 @@ ament_auto_find_build_dependencies()
 rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/Gear.msg"
   "msg/CanFrame.msg"
+  "srv/SafetyEvent.srv"
+  "srv/SafetyCommand.srv"
   DEPENDENCIES
     "builtin_interfaces"
     "geometry_msgs"

src/msg/voltron_msgs/srv/SafetyCommand.srv

@@ -0,0 +1,24 @@
+# Package:   voltron_msgs
+# Filename:  SafetyCommand.srv
+# Author:    Joshua Williams
+# Email:     joshmackwilliams@protonmail.com
+# Copyright: 2021, Nova UTD
+# License:   MIT License
+
+# The service the safety manager uses to assign recovery strategies to nodes
+# See https://github.com/Nova-UTD/navigator/wiki/Safety-Manager-Design
+
+# Which of the node's strategies to use
+# 0 is reserved for normal operation
+# 255 is reserved for termination
+uint8 strategy
+
+# Sequence number, to ensure messages can't be processed out of order
+uint32 sequence_number
+
+# JSON-formatted command-specific data
+string additional_data
+
+---
+
+# We send nothing back, but this is used as an acknowledgement

src/msg/voltron_msgs/srv/SafetyEvent.srv

@@ -0,0 +1,34 @@
+# Package:   voltron_msgs
+# Filename:  SafetyEvent.srv
+# Author:    Joshua Williams
+# Email:     joshmackwilliams@protonmail.com
+# Copyright: 2021, Nova UTD
+# License:   MIT License
+
+# The service nodes use to notify the safety manager of an issue
+# See https://github.com/Nova-UTD/navigator/wiki/Safety-Manager-Design
+
+# A unique identifier for the type of this event
+uint64 event_uid
+
+# Sequence number, to ensure messages can't be processed out of order
+uint32 sequence_number
+
+# The current status - see below for values
+uint8 status
+# The node is working on assessing or resolving the event locally
+uint8 STATUS_WORKING = 0
+# The node cannot resolve the event locally
+uint8 STATUS_UNRESOLVED = 1
+# The event is resolved
+uint8 STATUS_RESOLVED = 2
+
+# A human-readable description, for debugging and visualization purposes only
+string description
+
+# JSON-formatted event-specific data
+string additional_data
+
+---
+
+# We send nothing back, but this is used as an acknowledgement

src/tools/voltron_test_utils/CMakeLists.txt

@@ -25,8 +25,10 @@ if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
 
-  ament_add_gtest(tests test/test_test_publisher_subscriber.cpp)
-  ament_target_dependencies(tests rclcpp std_msgs)
+  ament_add_gtest(tests
+    test/test_test_publisher_subscriber.cpp
+    test/test_test_client_server.cpp)
+  ament_target_dependencies(tests rclcpp std_msgs std_srvs)
   target_include_directories(tests PRIVATE include)
   target_link_libraries(tests gtest_main)
 endif()

src/tools/voltron_test_utils/include/voltron_test_utils/TestClient.hpp

@@ -0,0 +1,72 @@
+/*
+ * Package:   voltron_test_utils
+ * Filename:  TestClient.hpp
+ * Author:    Joshua Williams
+ * Email:     joshmackwilliams@protonmail.com
+ * Copyright: 2021, Nova UTD
+ * License:   MIT License
+ */
+
+#pragma once
+
+#include <chrono>
+#include <future>
+#include <memory>
+#include "rclcpp/rclcpp.hpp"
+#include <string>
+#include <unistd.h>
+
+using namespace std::chrono_literals;
+
+namespace Voltron {
+namespace TestUtils {
+
+template <typename ServiceType> class TestClient {
+public:
+  typedef typename ServiceType::Request RequestType;
+  typedef typename ServiceType::Response ResponseType;
+
+  TestClient(std::string topic) {
+    this->node = std::make_shared<TestClientNode>(topic);
+  }
+
+  void send_request(RequestType request) {
+    this->current_response = this->node->send_request(request);
+    rclcpp::spin_some(this->node);
+  }
+
+  bool request_complete() {
+    if(! this->current_response.valid()) return false;
+    usleep(50);
+    rclcpp::spin_some(this->node);
+    return this->current_response.wait_for(0s) == std::future_status::ready;
+  }
+
+  ResponseType get_response() {
+    rclcpp::spin_until_future_complete(this->node, this->current_response);
+    return *(this->current_response.get());
+  }
+
+private:
+  typedef rclcpp::Client<ServiceType> ClientType;
+
+  class TestClientNode : public rclcpp::Node {
+  public:
+    TestClientNode(std::string topic) : Node("test_client_node_" + topic) {
+      this->client = this->create_client<ServiceType>(topic);
+    }
+
+    typename ClientType::SharedFuture send_request(RequestType request) {
+      return this->client->async_send_request(std::make_shared<RequestType>(request));
+    }
+
+  private:
+    std::shared_ptr<ClientType> client;
+  };
+
+  typename ClientType::SharedFuture current_response;
+  std::shared_ptr<TestClientNode> node;
+};
+
+}
+}