feat(component_state_monitor): add component state monitor

launch/tier4_system_launch/launch/system.launch.xml

@@ -36,6 +36,14 @@
       </include>
     </group>
 
+    <!-- Component State Monitor -->
+    <group>
+      <include file="$(find-pkg-share component_state_monitor)/launch/component_state_monitor.launch.py">
+        <arg name="mode" value="$(var run_mode)"/>
+        <arg name="file" value="$(find-pkg-share component_state_monitor)/config/topics.yaml"/>
+      </include>
+    </group>
+
     <!-- Error Monitor -->
     <group>
       <let name="config_file" value="$(var tier4_system_launch_param_path)/system_error_monitor/system_error_monitor.param.yaml" if="$(eval &quot;'$(var run_mode)'=='online'&quot;)"/>

system/component_state_monitor/CMakeLists.txt

@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 3.14)
+project(component_state_monitor)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/main.cpp
+)
+
+rclcpp_components_register_nodes(${PROJECT_NAME}
+  "component_state_monitor::StateMonitor"
+)
+
+ament_auto_package(INSTALL_TO_SHARE config launch)

system/component_state_monitor/config/topics.yaml

@@ -0,0 +1,182 @@
+- module: map
+  mode: [online, planning_simulation]
+  type: launch
+  args:
+    node_name_suffix: vector_map
+    topic: /map/vector_map
+    topic_type: autoware_auto_mapping_msgs/msg/HADMapBin
+    best_effort: false
+    transient_local: true
+    warn_rate: 0.0
+    error_rate: 0.0
+    timeout: 0.0
+
+- module: map
+  mode: [online]
+  type: launch
+  args:
+    node_name_suffix: pointcloud_map
+    topic: /map/pointcloud_map
+    topic_type: sensor_msgs/msg/PointCloud2
+    best_effort: false
+    transient_local: true
+    warn_rate: 0.0
+    error_rate: 0.0
+    timeout: 0.0
+
+- module: localization
+  mode: [online, planning_simulation]
+  type: autonomous
+  args:
+    node_name_suffix: initialpose3d
+    topic: /initialpose3d
+    topic_type: geometry_msgs/msg/PoseWithCovarianceStamped
+    best_effort: false
+    transient_local: false
+    warn_rate: 0.0
+    error_rate: 0.0
+    timeout: 0.0
+
+- module: localization
+  mode: [online]
+  type: autonomous
+  args:
+    node_name_suffix: pose_twist_fusion_filter_pose
+    topic: /localization/pose_twist_fusion_filter/pose
+    topic_type: geometry_msgs/msg/PoseStamped
+    best_effort: false
+    transient_local: false
+    warn_rate: 5.0
+    error_rate: 1.0
+    timeout: 1.0
+
+- module: perception
+  mode: [online]
+  type: launch
+  args:
+    node_name_suffix: obstacle_segmentation_pointcloud
+    topic: /perception/obstacle_segmentation/pointcloud
+    topic_type: sensor_msgs/msg/PointCloud2
+    best_effort: true
+    transient_local: false
+    warn_rate: 5.0
+    error_rate: 1.0
+    timeout: 1.0
+
+- module: perception
+  mode: [online, planning_simulation]
+  type: autonomous
+  args:
+    node_name_suffix: object_recognition_objects
+    topic: /perception/object_recognition/objects
+    topic_type: autoware_auto_perception_msgs/msg/PredictedObjects
+    best_effort: false
+    transient_local: false
+    warn_rate: 5.0
+    error_rate: 1.0
+    timeout: 1.0
+
+- module: planning
+  mode: [online, planning_simulation]
+  type: autonomous
+  args:
+    node_name_suffix: mission_planning_route
+    topic: /planning/mission_planning/route
+    topic_type: autoware_auto_planning_msgs/msg/HADMapRoute
+    best_effort: false
+    transient_local: true
+    warn_rate: 0.0
+    error_rate: 0.0
+    timeout: 0.0
+
+- module: planning
+  mode: [online, planning_simulation]
+  type: autonomous
+  args:
+    node_name_suffix: scenario_planning_trajectory
+    topic: /planning/scenario_planning/trajectory
+    topic_type: autoware_auto_planning_msgs/msg/Trajectory
+    best_effort: false
+    transient_local: false
+    warn_rate: 5.0
+    error_rate: 1.0
+    timeout: 1.0
+
+- module: control
+  mode: [online, planning_simulation]
+  type: autonomous
+  args:
+    node_name_suffix: trajectory_follower_control_cmd
+    topic: /control/trajectory_follower/control_cmd
+    topic_type: autoware_auto_control_msgs/msg/AckermannControlCommand
+    best_effort: false
+    transient_local: false
+    warn_rate: 5.0
+    error_rate: 1.0
+    timeout: 1.0
+
+- module: control
+  mode: [online, planning_simulation]
+  type: autonomous
+  args:
+    node_name_suffix: control_command_control_cmd
+    topic: /control/command/control_cmd
+    topic_type: autoware_auto_control_msgs/msg/AckermannControlCommand
+    best_effort: false
+    transient_local: false
+    warn_rate: 5.0
+    error_rate: 1.0
+    timeout: 1.0
+
+- module: vehicle
+  mode: [online, planning_simulation]
+  type: autonomous
+  args:
+    node_name_suffix: vehicle_status_velocity_status
+    topic: /vehicle/status/velocity_status
+    topic_type: autoware_auto_vehicle_msgs/msg/VelocityReport
+    best_effort: false
+    transient_local: false
+    warn_rate: 5.0
+    error_rate: 1.0
+    timeout: 1.0
+
+- module: vehicle
+  mode: [online, planning_simulation]
+  type: autonomous
+  args:
+    node_name_suffix: vehicle_status_steering_status
+    topic: /vehicle/status/steering_status
+    topic_type: autoware_auto_vehicle_msgs/msg/SteeringReport
+    best_effort: false
+    transient_local: false
+    warn_rate: 5.0
+    error_rate: 1.0
+    timeout: 1.0
+
+- module: system
+  mode: [online, planning_simulation]
+  type: launch
+  args:
+    node_name_suffix: system_emergency_control_cmd
+    topic: /system/emergency/control_cmd
+    topic_type: autoware_auto_control_msgs/msg/AckermannControlCommand
+    best_effort: false
+    transient_local: false
+    warn_rate: 5.0
+    error_rate: 1.0
+    timeout: 1.0
+
+- module: localization
+  mode: [online, planning_simulation]
+  type: autonomous
+  args:
+    node_name_suffix: transform_map_to_base_link
+    topic: /tf
+    frame_id: map
+    child_frame_id: base_link
+    best_effort: false
+    transient_local: false
+    warn_rate: 5.0
+    error_rate: 1.0
+    timeout: 1.0

system/component_state_monitor/launch/component_state_monitor.launch.py

@@ -0,0 +1,86 @@
+# Copyright 2022 TIER IV, Inc.
+#
+# 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.
+
+
+from collections import defaultdict
+from pathlib import Path
+
+import launch
+from launch.actions import DeclareLaunchArgument
+from launch.actions import IncludeLaunchDescription
+from launch.actions import OpaqueFunction
+from launch.substitutions import LaunchConfiguration
+from launch.substitutions import PathJoinSubstitution
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
+from launch_ros.substitutions import FindPackageShare
+import yaml
+
+
+def create_diagnostic_name(row):
+    return "{}_topic_status".format(row["module"])
+
+
+def create_topic_monitor_name(row):
+    diag_name = create_diagnostic_name(row)
+    return "topic_state_monitor_{}: {}".format(row["args"]["node_name_suffix"], diag_name)
+
+
+def create_topic_monitor_node(row):
+    tf_mode = "" if "topic_type" in row["args"] else "_tf"
+    package = FindPackageShare("topic_state_monitor")
+    include = PathJoinSubstitution([package, f"launch/topic_state_monitor{tf_mode}.launch.xml"])
+    diag_name = create_diagnostic_name(row)
+    arguments = [("diag_name", diag_name)] + [(k, str(v)) for k, v in row["args"].items()]
+    return IncludeLaunchDescription(include, launch_arguments=arguments)
+
+
+def launch_setup(context, *args, **kwargs):
+    # create topic monitors
+    mode = LaunchConfiguration("mode").perform(context)
+    rows = yaml.safe_load(Path(LaunchConfiguration("file").perform(context)).read_text())
+    rows = [row for row in rows if mode in row["mode"]]
+    topic_monitor_nodes = [create_topic_monitor_node(row) for row in rows]
+    topic_monitor_names = [create_topic_monitor_name(row) for row in rows]
+    topic_monitor_param = defaultdict(lambda: defaultdict(list))
+    for row in rows:
+        topic_monitor_param[row["type"]][row["module"]].append(create_topic_monitor_name(row))
+    topic_monitor_param = {name: dict(module) for name, module in topic_monitor_param.items()}
+
+    # create component
+    component = ComposableNode(
+        namespace="component_state_monitor",
+        name="component",
+        package="component_state_monitor",
+        plugin="component_state_monitor::StateMonitor",
+        parameters=[{"topic_monitor_names": topic_monitor_names}, topic_monitor_param],
+    )
+    container = ComposableNodeContainer(
+        namespace="component_state_monitor",
+        name="container",
+        package="rclcpp_components",
+        executable="component_container",
+        composable_node_descriptions=[component],
+    )
+    return [container, *topic_monitor_nodes]
+
+
+def generate_launch_description():
+    return launch.LaunchDescription(
+        [
+            DeclareLaunchArgument("file"),
+            DeclareLaunchArgument("mode"),
+            OpaqueFunction(function=launch_setup),
+        ]
+    )

system/component_state_monitor/package.xml

@@ -0,0 +1,25 @@
+<?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>component_state_monitor</name>
+  <version>0.1.0</version>
+  <description>The component_state_monitor package</description>
+  <maintainer email="[email protected]">Takagi, Isamu</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <build_depend>autoware_cmake</build_depend>
+
+  <depend>diagnostic_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>tier4_system_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>