feat: add autoware_control_center and autoware_node

Signed-off-by: M. Fatih Cırıt <[email protected]>
Co-Authored-by: Alexey Panferov <[email protected]>

common/autoware_control_center/CMakeLists.txt

@@ -0,0 +1,37 @@
+cmake_minimum_required(VERSION 3.8)
+project(autoware_control_center)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/control_center_node.cpp
+  src/node_registry.cpp
+  src/include/control_center_node.hpp
+  src/include/node_registry.hpp
+)
+
+#rclcpp_components_register_node(${PROJECT_NAME}
+#  PLUGIN "autoware::control_center::ControlCenter"
+#  EXECUTABLE ${PROJECT_NAME}_node
+#)
+
+ament_auto_add_executable(${PROJECT_NAME}_node
+  src/control_center_main.cpp
+)
+
+if(BUILD_TESTING)
+  file(GLOB_RECURSE TEST_SOURCES test/*.cpp)
+  ament_add_ros_isolated_gtest(test_autoware_control_center ${TEST_SOURCES})
+  target_include_directories(test_autoware_control_center PRIVATE src/include)
+  target_link_libraries(test_autoware_control_center ${PROJECT_NAME})
+  ament_target_dependencies(test_autoware_control_center
+    rclcpp
+    rclcpp_lifecycle
+    autoware_utils
+    autoware_control_center_msgs)
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE
+  launch
+  config)

common/autoware_control_center/README.md

@@ -0,0 +1,104 @@
+# Autoware Control Center
+
+## Abbreviations
+
+- **ACC:** Autoware Control Center
+- **AN:** Autoware Node
+
+## Overview
+
+The Autoware Control Center (ACC) is a core package within the Autoware system, designed to manage and monitor Autoware
+nodes (ANs).
+
+It provides services for the registration, de-registration, and error handling of ANs, as well as publishing reports on
+their status.
+
+ACC maintains an internal registry to keep track of registered ANs.
+
+## Interfaces
+
+### Services
+
+#### Register
+
+Registers an Autoware node to the ACC.
+
+- **Topic:** `/autoware/control_center/srv/register`
+- **Type:** `autoware_control_center_msgs::srv::Register`
+
+#### Deregister
+
+De-registers an Autoware node from the ACC.
+
+- **Topic:** `/autoware/control_center/srv/deregister`
+- **Type:** `autoware_control_center_msgs::srv::Deregister`
+
+#### ReportState
+
+Reports the state of an Autoware node to the ACC.
+
+- **Topic:** `/autoware/control_center/srv/report_state`
+- **Type:** `autoware_control_center_msgs::srv::ReportState`
+
+### Subscribers
+
+#### Heartbeat (**Source:** Autoware Node)
+
+Subscribes to the heartbeat topic of an Autoware node after its registration.
+ACC controls the liveness of Autoware nodes by monitoring this topic.
+
+- **Topic:** `/autoware_node_name/heartbeat`
+- **Type:** `autoware_control_center_msgs::msg::Heartbeat`
+- **Source:** An Autoware Node
+
+### Publishers
+
+#### NodeReports
+
+Publishes reports on the current status of registered Autoware nodes.
+
+- **Topic:** `/autoware/control_center/node_reports`
+- **Type:** `autoware_control_center_msgs::msg::NodeReport`
+
+## Parameters
+
+| Name                  | Type     | Default Value | Description                                                                         |
+| --------------------- | -------- | ------------- | ----------------------------------------------------------------------------------- |
+| `lease_duration_ms`   | `double` | `220.0`       | If not received another heartbeat within this duration, AN will be considered dead. |
+| `report_publish_rate` | `double` | `1.0`         | Publish NodeReports rate (hz)                                                       |
+
+## Singleton Constraint
+
+To ensure that only one instance of the ACC is running at any given time, two checks are performed in the main function:
+
+### 1. Lockfile Check
+
+This lockfile mechanism is fast and reliable for single-machine scenarios.
+It prevents multiple instances of ACC from running concurrently on the same machine.
+
+**Path:** `/tmp/autoware_control_center_node.lock`
+
+### 2. Network-Wide Node Name Check
+
+This involves gathering all node names and comparing them with the ACC node name (`/autoware/control_center`).
+While this method is slower and less reliable than the lockfile check,
+it is necessary for scenarios where the ACC is run across a network of machines.
+This ensures that no other instance of ACC is running on any other machine within the network.
+
+## Usage
+
+`ros2 launch autoware_control_center control_center.launch.xml`
+
+## Workflow
+
+When an Autoware Node starts, it registers itself with the ACC.
+
+The ACC then subscribes to the heartbeat topic of the Autoware node to monitor its liveness.
+
+If the ACC does not receive a heartbeat from the Autoware node within the specified lease duration,
+it considers the node to be dead.
+
+## Credits
+
+- Heartbeat functionality is based on ROS 2 [software_watchdogs](https://github.com/ros-safety/software_watchdogs)
+  package.

common/autoware_control_center/config/control_center.param.yaml

@@ -0,0 +1,4 @@
+/**:
+  ros__parameters:
+    lease_duration_ms: 220.0
+    report_publish_rate: 1.0

common/autoware_control_center/launch/control_center.launch.xml

@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <arg name="param_path" default="$(find-pkg-share autoware_control_center)/config/control_center.param.yaml"/>
+
+  <node pkg="autoware_control_center" exec="autoware_control_center_node" name="control_center" output="screen">
+    <param from="$(var param_path)"/>
+  </node>
+</launch>

common/autoware_control_center/package.xml

@@ -0,0 +1,32 @@
+<?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>autoware_control_center</name>
+  <version>0.0.0</version>
+  <description>
+    Autoware Control Center (ACC) is an Autoware.Core package designed to manage
+    and monitor Autoware nodes within a system.
+  </description>
+  <maintainer email="[email protected]">M. Fatih Cırıt</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>autoware_control_center_msgs</depend>
+  <depend>autoware_utils</depend>
+  <depend>lifecycle_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>rclcpp_lifecycle</depend>
+
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_cmake_ros</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+  <test_depend>autoware_utils</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

common/autoware_control_center/src/control_center_main.cpp

@@ -0,0 +1,93 @@
+// Copyright 2024 The Autoware Contributors
+//
+// 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 "include/control_center_node.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <fcntl.h>
+#include <sys/file.h>
+#include <unistd.h>
+
+#include <memory>
+
+const char * LOCK_FILE = "/tmp/autoware_control_center_node.lock";
+
+bool lock_file()
+{
+  int fd = open(LOCK_FILE, O_CREAT | O_RDWR, 0666);
+  if (fd == -1) {
+    RCLCPP_FATAL(rclcpp::get_logger(""), "Failed to open lock file");
+    return false;
+  }
+
+  if (flock(fd, LOCK_EX | LOCK_NB) == -1) {
+    if (errno == EWOULDBLOCK) {
+      RCLCPP_FATAL(rclcpp::get_logger(""), "Another instance is already running");
+    } else {
+      RCLCPP_FATAL(rclcpp::get_logger(""), "Failed to lock file");
+    }
+    close(fd);
+    return false;
+  }
+
+  // Keep the file descriptor open to maintain the lock
+  return true;
+}
+
+void unlock_file(int fd)
+{
+  if (fd != -1) {
+    flock(fd, LOCK_UN);
+    close(fd);
+  }
+}
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+
+  // Lock file to prevent multiple instances on the same machine
+  int lock_fd = open(LOCK_FILE, O_CREAT | O_RDWR, 0666);
+  if (!lock_file()) {
+    return 1;
+  }
+
+  // Check if node already exists to prevent multiple instances across the network
+  // It's a bit slow but better than nothing
+  const auto node_name_with_namespace = std::string("/autoware/control_center");
+
+  auto node_already_exists = [](const std::string & node_name) {
+    auto temp_node = rclcpp::Node::make_shared("temp_node");
+    auto all_nodes = temp_node->get_node_names();
+    return std::find(all_nodes.begin(), all_nodes.end(), node_name) != all_nodes.end();
+  };
+
+  if (node_already_exists(node_name_with_namespace)) {
+    RCLCPP_FATAL(
+      rclcpp::get_logger(""), "Node %s already exists", node_name_with_namespace.c_str());
+    unlock_file(lock_fd);
+    throw std::runtime_error("Node already exists");
+  }
+
+  // Instantiate the control center node, hopefully the only instance
+  auto control_center =
+    std::make_shared<autoware::control_center::ControlCenter>(rclcpp::NodeOptions());
+  rclcpp::executors::MultiThreadedExecutor exec;
+  exec.add_node(control_center->get_node_base_interface());
+  exec.spin();
+  unlock_file(lock_fd);
+  rclcpp::shutdown();
+  return 0;
+}