feat: add autoware_control_center and autoware_node

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

common/autoware_control_center/CMakeLists.txt

@@ -0,0 +1,61 @@
+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/include/control_center_node.hpp
+)
+
+# In order to instantiate only a single instance, we have to use custom main function
+#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)
+#  add_smoke_test(${PROJECT_NAME} ${PROJECT_NAME}_node
+#    TEST_PARAM_FILENAMES "config/control_center.param.yaml"
+#  )
+
+  install(DIRECTORY
+    test/config/
+    DESTINATION share/${PROJECT_NAME}/test/config/
+  )
+
+  file(GLOB_RECURSE TEST_FILES test/*.cpp)
+
+  foreach(TEST_FILE ${TEST_FILES})
+    # Get the test name without directory and extension
+    get_filename_component(TEST_NAME ${TEST_FILE} NAME_WE)
+
+    # Add each test separately
+    ament_add_ros_isolated_gtest(${TEST_NAME} ${TEST_FILE} TIMEOUT 10)
+    target_include_directories(${TEST_NAME} PRIVATE src/include)
+    target_link_libraries(${TEST_NAME} ${PROJECT_NAME})
+    ament_target_dependencies(${TEST_NAME}
+      rclcpp
+      rclcpp_lifecycle
+      autoware_utils
+      autoware_control_center_msgs)
+  endforeach()
+
+#  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,99 @@
+# 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`
+
+### 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.
+
+This topic also contains the information about the node's status.
+
+- **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:
+    deadline_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,34 @@
+<?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>
+  <test_depend>ros_testing</test_depend>
+  <!--  <test_depend>autoware_testing</test_depend>-->
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

common/autoware_control_center/src/control_center_main.cpp

@@ -0,0 +1,122 @@
+// 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 <unistd.h>
+
+#include <algorithm>
+#include <memory>
+#include <string>
+
+const char * lock_file_path = "/tmp/autoware_control_center_node.lock";
+
+int open_lock_file()
+{
+  // Open or create the lock file with read/write permissions
+  int fd = open(lock_file_path, O_CREAT | O_RDWR, 0666);
+
+  // Check if the file descriptor is valid
+  if (fd == -1) {
+    RCLCPP_FATAL(rclcpp::get_logger(""), "Failed to open lock file");
+  }
+  return fd;
+}
+
+bool lock_file(int fd)
+{
+  struct flock fl = {F_WRLCK, SEEK_SET, 0, 0, 0};
+  // F_WRLCK: Write lock
+  // SEEK_SET: Base the lock offset from the beginning of the file
+  // 0, 0: Lock the entire file (start offset, length)
+
+  // Attempt to set the file lock using fcntl
+  if (fcntl(fd, F_SETLK, &fl) == -1) {
+    // Check if the file is already locked by another process
+    if (errno == EWOULDBLOCK) {
+      RCLCPP_FATAL(rclcpp::get_logger(""), "Another instance is already running");
+    } else {
+      // Handle other locking errors
+      RCLCPP_FATAL(rclcpp::get_logger(""), "Failed to lock file");
+    }
+    // Close the file descriptor on failure
+    close(fd);
+    return false;
+  }
+  return true;
+}
+
+void unlock_file(int fd)
+{
+  // Ensure the file descriptor is valid before closing
+  if (fd != -1) {
+    close(fd);
+  }
+}
+
+bool 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();
+}
+
+int main(int argc, char * argv[])
+{
+  bool use_lock_file = true;
+  for (int i = 1; i < argc; ++i) {
+    if (std::string(argv[i]) == "--dont-use-lock-file") {
+      use_lock_file = false;
+      break;
+    }
+  }
+  rclcpp::init(argc, argv);
+
+  // Lock file to prevent multiple instances on the same machine
+  int lock_fd = -1;
+  if (use_lock_file) {
+    lock_fd = open_lock_file();
+    if (lock_fd == -1 || !lock_file(lock_fd)) {
+      return 1;
+    }
+  }
+
+  // Check if node already exists to prevent multiple instances across the network
+  // It's a bit slow but better than nothing
+  const std::string node_name_with_namespace = "/autoware/control_center";
+  if (node_already_exists(node_name_with_namespace)) {
+    RCLCPP_FATAL(
+      rclcpp::get_logger(""), "Node %s already exists", node_name_with_namespace.c_str());
+    if (use_lock_file) {
+      unlock_file(lock_fd);
+    }
+    return 2;
+  }
+
+  // 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();
+
+  if (use_lock_file) {
+    unlock_file(lock_fd);
+  }
+  rclcpp::shutdown();
+  return 0;
+}