feat: add autoware_state_monitor package

system/autoware_state_monitor/CMakeLists.txt

@@ -0,0 +1,40 @@
+cmake_minimum_required(VERSION 3.5)
+project(autoware_state_monitor)
+
+### Compile options
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic -Werror)
+endif()
+
+### Dependencies
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+# Target
+## Set source files
+set(AUTOWARE_STATE_MONITOR_SRC
+  src/autoware_state_monitor_node/autoware_state_monitor_node.cpp
+  src/autoware_state_monitor_node/state_machine.cpp
+  src/autoware_state_monitor_node/diagnostics.cpp
+)
+
+## Add executables
+ament_auto_add_executable(autoware_state_monitor
+  src/autoware_state_monitor_node/main.cpp
+  ${AUTOWARE_STATE_MONITOR_SRC}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE
+  launch
+  config
+)

system/autoware_state_monitor/README.md

@@ -0,0 +1,46 @@
+# autoware_state_monitor
+
+## Purpose
+
+This node manages AutowareState transitions.
+
+## Inner-workings / Algorithms
+
+## Inputs / Outputs
+
+### Input
+
+| Name                               | Type                                                 | Description                                       |
+| ---------------------------------- | ---------------------------------------------------- | ------------------------------------------------- |
+| `/planning/mission_planning/route` | `autoware_auto_planning_msgs::msg::HADMapRoute`      | Subscribe route                                   |
+| `/localization/kinematic_state`    | `nav_msgs::msg::Odometry`                            | Used to decide whether vehicle is stopped or not  |
+| `/vehicle/state_report`            | `autoware_auto_vehicle_msgs::msg::ControlModeReport` | Used to check vehicle mode: autonomous or manual. |
+
+### Output
+
+| Name               | Type                                            | Description                                        |
+| ------------------ | ----------------------------------------------- | -------------------------------------------------- |
+| `/autoware/engage` | `autoware_auto_vehicle_msgs::msg::Engage`       | publish disengage flag on AutowareState transition |
+| `/autoware/state`  | `autoware_auto_system_msgs::msg::AutowareState` | publish AutowareState                              |
+
+## Parameters
+
+### Node Parameters
+
+| Name          | Type | Default Value | Explanation            |
+| ------------- | ---- | ------------- | ---------------------- |
+| `update_rate` | int  | `10`          | Timer callback period. |
+
+### Core Parameters
+
+| Name                      | Type   | Default Value | Explanation                                                                |
+| ------------------------- | ------ | ------------- | -------------------------------------------------------------------------- |
+| `th_arrived_distance_m`   | double | 1.0           | threshold distance to check if vehicle has arrived at the route's endpoint |
+| `th_stopped_time_sec`     | double | 1.0           | threshold time to check if vehicle is stopped                              |
+| `th_stopped_velocity_mps` | double | 0.01          | threshold velocity to check if vehicle is stopped                          |
+| `disengage_on_route`      | bool   | true          | send disengage flag or not when the route is subscribed                    |
+| `disengage_on_goal`       | bool   | true          | send disengage flag or not when the vehicle is arrived goal                |
+
+## Assumptions / Known limits
+
+TBD.

system/autoware_state_monitor/config/autoware_state_monitor.param.yaml

@@ -0,0 +1,155 @@
+# Autoware State Monitor Parameters
+/**:
+  ros__parameters:
+    # modules_names: string array
+    module_names: [
+      "map",
+      "sensing",
+      "perception",
+      "localization",
+      "planning",
+      "control",
+      "vehicle",
+      "system"
+    ]
+
+    # Topic Config
+    ## names: string array
+    ## configs: top level
+    ### names used to declare parameter group
+    #### module: string
+    #### timeout[s]: double (0: none)
+    #### warn_rate[hz]: double (0: none)
+    topic_configs:
+      names: [
+        "/map/vector_map",
+        "/map/pointcloud_map",
+        "/perception/obstacle_segmentation/pointcloud",
+        "/initialpose3d",
+        "/localization/pose_twist_fusion_filter/pose",
+        "/perception/object_recognition/objects",
+        "/planning/mission_planning/route",
+        "/planning/scenario_planning/trajectory",
+        "/control/trajectory_follower/control_cmd",
+        "/control/command/control_cmd",
+        "/vehicle/status/velocity_status",
+        "/vehicle/status/steering_status",
+        "/system/emergency/control_cmd"
+      ]
+
+      configs:
+        /map/vector_map:
+          module: "map"
+          timeout: 0.0
+          warn_rate: 0.0
+          type: "autoware_auto_mapping_msgs/msg/HADMapBin"
+          transient_local: True
+
+        /map/pointcloud_map:
+          module: "map"
+          timeout: 0.0
+          warn_rate: 0.0
+          type: "sensor_msgs/msg/PointCloud2"
+          transient_local: True
+
+        /perception/obstacle_segmentation/pointcloud:
+          module: "sensing"
+          timeout: 1.0
+          warn_rate: 5.0
+          type: "sensor_msgs/msg/PointCloud2"
+          best_effort: True
+
+        /initialpose3d:
+          module: "localization"
+          timeout: 0.0
+          warn_rate: 0.0
+          type: "geometry_msgs/msg/PoseWithCovarianceStamped"
+
+        /localization/pose_twist_fusion_filter/pose:
+          module: "localization"
+          timeout: 1.0
+          warn_rate: 5.0
+          type: "geometry_msgs/msg/PoseStamped"
+
+        # Can be both with feature array or without
+        # In prediction.launch.xml this is set to without
+        /perception/object_recognition/objects:
+          module: "perception"
+          timeout: 1.0
+          warn_rate: 5.0
+          type: "autoware_auto_perception_msgs/msg/PredictedObjects"
+          # This topic could have two different types depending on the launch flags used.
+          # The implementation of subscriptions in ROS2 does not allow for multiple types
+          # to be defined for a topic. By default this is set to not use have features.
+          # type: ["autoware_perception_msgs/msg/DynamicObjectArray", "autoware_perception_msgs/msg/DynamicObjectWithFeatureArray"]
+
+        /planning/mission_planning/route:
+          module: "planning"
+          timeout: 0.0
+          warn_rate: 0.0
+          type: "autoware_auto_planning_msgs/msg/HADMapRoute"
+          transient_local: True
+
+        /planning/scenario_planning/trajectory:
+          module: "planning"
+          timeout: 1.0
+          warn_rate: 5.0
+          type: "autoware_auto_planning_msgs/msg/Trajectory"
+
+        /control/trajectory_follower/control_cmd:
+          module: "control"
+          timeout: 1.0
+          warn_rate: 5.0
+          type: "autoware_auto_control_msgs/msg/AckermannControlCommand"
+
+        /control/command/control_cmd:
+          module: "control"
+          timeout: 1.0
+          warn_rate: 5.0
+          type: "autoware_auto_control_msgs/msg/AckermannControlCommand"
+
+        /vehicle/status/velocity_status:
+          module: "vehicle"
+          timeout: 1.0
+          warn_rate: 5.0
+          type: "autoware_auto_vehicle_msgs/msg/VelocityReport"
+
+        /vehicle/status/steering_status:
+          module: "vehicle"
+          timeout: 1.0
+          warn_rate: 5.0
+          type: "autoware_auto_vehicle_msgs/msg/SteeringReport"
+
+        /system/emergency/control_cmd:
+          module: "system"
+          timeout: 1.0
+          warn_rate: 5.0
+          type: "autoware_auto_control_msgs/msg/AckermannControlCommand"
+
+    # Param Config
+    ## names: string array
+    ## configs: top level
+    ### names used to declare parameter group
+    #### module: string
+    # param_configs:
+    #   names: ["/vehicle_info"]
+    #   configs:
+    #     /vehicle_info:
+    #       module: "vehicle"
+
+    # TF Config
+    ## names: string array
+    ## configs: top level
+    ### names used to declare parameter group
+    #### module: string
+    #### from: string
+    #### to: string
+    #### timeout[s]: double (0: none)
+    tf_configs:
+      names: ["map_to_base_link"]
+      configs:
+        map_to_base_link:
+          module: "localization"
+          from: "map"
+          to: "base_link"
+          timeout: 1.0

system/autoware_state_monitor/config/autoware_state_monitor.planning_simulation.param.yaml

@@ -0,0 +1,134 @@
+# Autoware State Monitor: Planning Simulator Parameters
+/**:
+  ros__parameters:
+    # modules_names: string array
+    module_names: [
+      "map",
+      "sensing",
+      "perception",
+      "localization",
+      "planning",
+      "control",
+      "vehicle",
+      "system"
+    ]
+
+# Topic Config
+    ## names: string array
+    ## configs: top level
+    ### names used to declare parameter group
+    #### module: string
+    #### timeout[s]: double (0: none)
+    #### warn_rate[hz]: double (0: none)
+    topic_configs:
+      names: [
+        "/map/vector_map",
+        "/map/pointcloud_map",
+        "/perception/object_recognition/objects",
+        "/initialpose2d",
+        "/planning/mission_planning/route",
+        "/planning/scenario_planning/trajectory",
+        "/control/trajectory_follower/control_cmd",
+        "/control/command/control_cmd",
+        "/vehicle/status/velocity_status",
+        "/vehicle/status/steering_status",
+        "/system/emergency/control_cmd"
+      ]
+
+      configs:
+        /map/vector_map:
+          module: "map"
+          timeout: 0.0
+          warn_rate: 0.0
+          type: "autoware_auto_mapping_msgs/msg/HADMapBin"
+          transient_local: True
+
+        /map/pointcloud_map:
+          module: "map"
+          timeout: 0.0
+          warn_rate: 0.0
+          type: "sensor_msgs/msg/PointCloud2"
+          transient_local: True
+
+        /perception/object_recognition/objects:
+          module: "perception"
+          timeout: 1.0
+          warn_rate: 5.0
+          type: "autoware_auto_perception_msgs/msg/PredictedObjects"
+
+        /initialpose2d:
+          module: "localization"
+          timeout: 0.0
+          warn_rate: 0.0
+          type: "geometry_msgs/msg/PoseWithCovarianceStamped"
+
+        /planning/mission_planning/route:
+          module: "planning"
+          timeout: 0.0
+          warn_rate: 0.0
+          type: "autoware_auto_planning_msgs/msg/HADMapRoute"
+          transient_local: True
+
+        /planning/scenario_planning/trajectory:
+          module: "planning"
+          timeout: 1.0
+          warn_rate: 5.0
+          type: "autoware_auto_planning_msgs/msg/Trajectory"
+
+        /control/trajectory_follower/control_cmd:
+          module: "control"
+          timeout: 1.0
+          warn_rate: 5.0
+          type: "autoware_auto_control_msgs/msg/AckermannControlCommand"
+
+        /control/command/control_cmd:
+          module: "control"
+          timeout: 1.0
+          warn_rate: 5.0
+          type: "autoware_auto_control_msgs/msg/AckermannControlCommand"
+
+        /vehicle/status/velocity_status:
+          module: "vehicle"
+          timeout: 1.0
+          warn_rate: 5.0
+          type: "autoware_auto_vehicle_msgs/msg/VelocityReport"
+
+        /vehicle/status/steering_status:
+          module: "vehicle"
+          timeout: 1.0
+          warn_rate: 5.0
+          type: "autoware_auto_vehicle_msgs/msg/SteeringReport"
+
+        /system/emergency/control_cmd:
+          module: "system"
+          timeout: 1.0
+          warn_rate: 5.0
+          type: "autoware_auto_control_msgs/msg/AckermannControlCommand"
+
+    # Param Config
+    ## names: string array
+    ## configs: top level
+    ### names used to declare parameter group
+    #### module: string
+    # param_configs:
+    #   names: ["/vehicle_info"]
+    #   configs:
+    #     /vehicle_info:
+    #       module: "vehicle"
+
+    # TF Config
+    ## names: string array
+    ## configs: top level
+    ### names used to declare parameter group
+    #### module: string
+    #### from: string
+    #### to: string
+    #### timeout[s]: double (0: none)
+    tf_configs:
+      names: ["map_to_base_link"]
+      configs:
+        map_to_base_link:
+          module: "localization"
+          from: "map"
+          to: "base_link"
+          timeout: 1.0