Initial import of FaRui Campus ADS v3.2

slave/build.sh

@@ -0,0 +1,37 @@
+#!/usr/bin/env bash
+# ---------------------------------------------------------
+# Autoware 编译脚本
+# ---------------------------------------------------------
+set -euo pipefail
+
+readonly RED='\033[0;31m'
+readonly GREEN='\033[0;32m'
+readonly NC='\033[0m'
+
+log()  { echo -e "${GREEN}[INFO]${NC} $*"; }
+warn() { echo -e "${RED}[WARN]${NC} $*" >&2; }
+
+BUILD_ARGS=(
+  colcon build
+  --symlink-install
+  --cmake-args -DCMAKE_BUILD_TYPE=Release
+)
+
+# 如需部分包编译，把 xxx 换成包名后取消下一行注释
+#BUILD_ARGS+=(--packages-select pointcloud_process timoo_ros2_driver ros2_socketcan ecar_can_driver)
+
+log "Starting colcon build..."
+if AUTOWARE_COMPILE_WITH_CUDA=1 "${BUILD_ARGS[@]}"; then
+  log "Build succeeded. Sourcing workspace..."
+
+  # 临时关闭 -u，避免 COLCON_TRACE 未定义报错
+  set +u
+  source install/setup.bash
+  set -u
+
+  log "Done."
+else
+  warn "Build failed, skip sourcing."
+fi
+
+

slave/calibrate_accel_brake_map.sh

@@ -0,0 +1,275 @@
+#!/bin/bash
+#===============================================================================
+# 加减速映射表标定启动脚本
+# 位置: /home/nvidia/FaRui/v3.1/calibrate_accel_brake_map.sh
+#===============================================================================
+
+set -e  # 遇到错误立即退出
+
+# 颜色定义
+RED='\033[0;31m'
+GREEN='\033[0;32m'
+YELLOW='\033[1;33m'
+BLUE='\033[0;34m'
+NC='\033[0m' # No Color
+
+# 默认参数
+VEHICLE_MODEL="ecar_vehicle"
+USE_SIM_TIME="false"
+RVIZ="true"
+RECORD_BAG="false"
+UPDATE_METHOD="update_offset_four_cell_around"
+
+# 帮助信息
+show_help() {
+    echo "============================================================"
+    echo "        加减速映射表标定工具"
+    echo "============================================================"
+    echo ""
+    echo "用法: ./calibrate_accel_brake_map.sh [选项]"
+    echo ""
+    echo "选项:"
+    echo "  -m, --model <车型>       车辆模型名称 (默认: ecar404)"
+    echo "  -s, --sim-time          使用仿真时间 (rosbag回放时使用)"
+    echo "  -n, --no-rviz           不启动RViz"
+    echo "  -r, --record            录制数据到rosbag"
+    echo "  -c, --cell              使用逐网格标定算法 (默认: 四邻域)"
+    echo "  -h, --help              显示此帮助信息"
+    echo ""
+    echo "标定模式:"
+    echo "  1. 实车标定:     ./calibrate_accel_brake_map.sh"
+    echo "  2. rosbag标定:   ./calibrate_accel_brake_map.sh -s"
+    echo "  3. 高精度标定:   ./calibrate_accel_brake_map.sh -c"
+    echo ""
+    echo "============================================================"
+}
+
+# 解析参数
+while [[ $# -gt 0 ]]; do
+    case $1 in
+        -m|--model)
+            VEHICLE_MODEL="$2"
+            shift 2
+            ;;
+        -s|--sim-time)
+            USE_SIM_TIME="true"
+            shift
+            ;;
+        -n|--no-rviz)
+            RVIZ="false"
+            shift
+            ;;
+        -r|--record)
+            RECORD_BAG="true"
+            shift
+            ;;
+        -c|--cell)
+            UPDATE_METHOD="update_offset_each_cell"
+            shift
+            ;;
+        -h|--help)
+            show_help
+            exit 0
+            ;;
+        *)
+            echo -e "${RED}错误: 未知参数 $1${NC}"
+            show_help
+            exit 1
+            ;;
+    esac
+done
+
+# 获取脚本所在目录
+SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+cd "$SCRIPT_DIR"
+
+echo "============================================================"
+echo -e "${BLUE}        加减速映射表标定工具${NC}"
+echo "============================================================"
+echo ""
+
+# 检查工作空间
+if [ ! -f "install/setup.bash" ]; then
+    echo -e "${RED}错误: 未找到 install/setup.bash${NC}"
+    echo "请先编译工作空间: colcon build"
+    exit 1
+fi
+
+# Source 环境
+echo -e "${BLUE}[1/6] 加载 ROS2 环境...${NC}"
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+
+# 检查 launch 文件是否存在
+LAUNCH_FILE="src/Launcher/farui_launch/launch/calibration/accel_brake_map_calibration.launch.xml"
+if [ ! -f "$LAUNCH_FILE" ]; then
+    echo -e "${RED}错误: 未找到标定 launch 文件${NC}"
+    echo "路径: $LAUNCH_FILE"
+    exit 1
+fi
+
+# 创建标定结果目录
+CALIBRATION_DIR="$SCRIPT_DIR/src/Launcher/farui_launch/config/vehicle/calibrated_maps"
+mkdir -p "$CALIBRATION_DIR"
+
+echo -e "${GREEN}✓ 环境加载完成${NC}"
+echo ""
+
+# 显示配置信息
+echo -e "${BLUE}[2/6] 标定配置信息:${NC}"
+echo "  车辆模型:     $VEHICLE_MODEL"
+echo "  仿真时间:     $USE_SIM_TIME"
+echo "  RViz可视化:   $RVIZ"
+echo "  录制数据:     $RECORD_BAG"
+echo "  标定算法:     $UPDATE_METHOD"
+echo "  结果保存路径: $CALIBRATION_DIR"
+echo ""
+
+# 检查车辆模型是否存在
+VEHICLE_PKG="${VEHICLE_MODEL}_description"
+if ! ros2 pkg list | grep "^$VEHICLE_PKG$" > /dev/null 2>&1; then
+    echo -e "${YELLOW}警告: 未找到车辆模型包 $VEHICLE_PKG${NC}"
+    echo "可用的车辆模型:"
+    ros2 pkg list | grep "_description$" | sed 's/_description$//' | head -10
+    echo ""
+    read -p "是否继续? (y/n) " -n 1 -r
+    echo
+    if [[ ! $REPLY =~ ^[Yy]$ ]]; then
+        exit 1
+    fi
+fi
+
+# 如果录制数据，创建bag目录
+if [ "$RECORD_BAG" = "true" ]; then
+    BAG_DIR="$SCRIPT_DIR/calibration_bags"
+    mkdir -p "$BAG_DIR"
+    BAG_NAME="calibration_$(date +%Y%m%d_%H%M%S)"
+fi
+
+# 启动标定
+echo -e "${BLUE}[3/6] 启动标定节点...${NC}"
+echo ""
+echo -e "${YELLOW}提示: 标定过程中请进行以下操作:${NC}"
+echo "  1. 确保车辆在安全开阔场地"
+echo "  2. 使用遥控器控制车辆"
+echo "  3. 平稳加速、减速，覆盖各种速度"
+echo "  4. 观察RViz中单元格变红表示数据充足"
+echo "  5. 重点关注 0-6m/s 低速区间"
+echo ""
+read -p "按回车键开始标定，或按 Ctrl+C 取消..."
+echo ""
+
+# 构建 launch 参数
+LAUNCH_ARGS="vehicle_model:=$VEHICLE_MODEL"
+LAUNCH_ARGS="$LAUNCH_ARGS use_sim_time:=$USE_SIM_TIME"
+LAUNCH_ARGS="$LAUNCH_ARGS rviz:=$RVIZ"
+LAUNCH_ARGS="$LAUNCH_ARGS update_method:=$UPDATE_METHOD"
+LAUNCH_ARGS="$LAUNCH_ARGS progress_file_output:=true"
+LAUNCH_ARGS="$LAUNCH_ARGS pedal_accel_graph_output:=true"
+
+# 启动 rosbag 录制（后台）
+if [ "$RECORD_BAG" = "true" ]; then
+    echo -e "${BLUE}[4/6] 启动数据录制...${NC}"
+    ros2 bag record -o "$BAG_DIR/$BAG_NAME" \
+        /vehicle/status/velocity_status \
+        /vehicle/status/steering_status \
+        /vehicle/status/actuation_status \
+        /control/command/actuation_cmd \
+        /tf \
+        /tf_static \
+        &
+    BAG_PID=$!
+    echo -e "${GREEN}✓ 数据录制已启动，PID: $BAG_PID${NC}"
+    echo "  保存路径: $BAG_DIR/$BAG_NAME"
+    echo ""
+fi
+
+# 启动标定
+echo -e "${BLUE}[5/6] 启动标定器...${NC}"
+echo "  命令: ros2 launch farui_launch accel_brake_map_calibration.launch.xml $LAUNCH_ARGS"
+echo ""
+
+# 捕获 Ctrl+C 信号
+cleanup() {
+    echo ""
+    echo -e "${YELLOW}接收到中断信号，正在清理...${NC}"
+    if [ "$RECORD_BAG" = "true" ] && [ -n "$BAG_PID" ]; then
+        echo "停止数据录制..."
+        kill $BAG_PID 2>/dev/null || true
+        wait $BAG_PID 2>/dev/null || true
+    fi
+    echo -e "${GREEN}✓ 已清理${NC}"
+    exit 0
+}
+trap cleanup SIGINT SIGTERM
+
+# 启动标定节点
+ros2 launch farui_launch accel_brake_map_calibration.launch.xml $LAUNCH_ARGS &
+LAUNCH_PID=$!
+
+# 等待标定节点启动
+sleep 3
+
+# 检查节点是否正常运行
+if ! ps -p $LAUNCH_PID > /dev/null; then
+    echo -e "${RED}错误: 标定节点启动失败${NC}"
+    exit 1
+fi
+
+echo -e "${GREEN}✓ 标定节点已启动${NC}"
+echo ""
+
+# 显示操作提示
+echo "============================================================"
+echo -e "${BLUE}                    标定操作指南${NC}"
+echo "============================================================"
+echo ""
+echo "1. ${YELLOW}驾驶车辆${NC}: 使用遥控器控制车辆"
+echo "   - 平稳加速、减速"
+echo "   - 覆盖 0-最大车速 范围"
+echo "   - 覆盖 0-最大踏板开度 范围"
+echo ""
+echo "2. ${YELLOW}观察进度${NC}: 查看RViz可视化"
+echo "   - 灰色: 无数据"
+echo "   - 蓝色→红色: 数据逐渐充足"
+echo "   - 目标: 所有单元格变红"
+echo ""
+echo "3. ${YELLOW}保存结果${NC}: 新开终端执行"
+echo "   ros2 service call /accel_brake_map_calibrator/update_map_dir \\"
+echo "     tier4_vehicle_msgs/srv/UpdateAccelBrakeMap \\"
+echo "     \"path: '$CALIBRATION_DIR'\""
+echo ""
+echo "4. ${YELLOW}查看误差${NC}:"
+echo "   ros2 topic echo /accel_brake_map_calibrator/output/map_error_ratio"
+echo "   (比值 < 0.7 表示标定有效)"
+echo ""
+echo "5. ${YELLOW}结束标定${NC}: 在此窗口按 Ctrl+C"
+echo ""
+echo "============================================================"
+echo ""
+
+# 等待用户结束
+wait $LAUNCH_PID
+
+# 标定结束后的提示
+echo ""
+echo "============================================================"
+echo -e "${BLUE}                    标定已结束${NC}"
+echo "============================================================"
+echo ""
+echo -e "${GREEN}标定结果保存在:${NC}"
+echo "  $CALIBRATION_DIR/"
+echo ""
+echo "生成的文件:"
+ls -lh "$CALIBRATION_DIR/" 2>/dev/null || echo "  (目录为空，请检查是否已调用保存服务)"
+echo ""
+echo "下一步:"
+echo "  1. 检查 accel_map.csv 和 brake_map.csv 是否生成"
+echo "  2. 修改 raw_vehicle_cmd_converter 配置使用新表"
+echo "  3. 重启 Autoware 验证效果"
+echo ""
+echo "============================================================"
+
+
+# ros2 service call /accel_brake_map_calibrator/input/update_map_dir   tier4_vehicle_msgs/srv/UpdateAccelBrakeMap   "path: '/home/nvidia/FaRui-test/v3.2/src/Launcher/farui_launch/config/vehicle/calibrated_maps'"
+

slave/cleanup_master_autoware.sh

@@ -0,0 +1,75 @@
+#!/usr/bin/env bash
+# Clean leftover master-board Autoware/debug processes.
+#
+# 用法:
+#   ./cleanup_master_autoware.sh
+#
+# 说明:
+#   这个脚本用于主板调试后清理残留进程。
+#   如果你在同一块板上故意运行从板底盘驱动，不要执行本脚本，
+#   因为它会停止 ecar_can/socket_can 相关节点。
+
+set -euo pipefail
+
+GREEN='\033[0;32m'
+YELLOW='\033[1;33m'
+NC='\033[0m'
+
+log_info() { echo -e "${GREEN}[INFO]${NC} $*"; }
+log_warn() { echo -e "${YELLOW}[WARN]${NC} $*"; }
+
+PATTERNS=(
+  "run_ecar_master_debug.sh"
+  "run_preset_route_real_vehicle.sh"
+  "ros2 launch.*ecar_master"
+  "ros2 launch.*farui_launch"
+  "launch_ros_[0-9]+"
+  "launch_ros"
+  "component_container"
+  "component_container_mt"
+  "timoo_ros"
+  "pointcloud_process"
+  "map_hash_generator"
+  "imu_corrector"
+  "gyro_bias_validator"
+  "hipnuc"
+  "imu_sub"
+  "rqt_gui"
+  "rqt_gui_py_node"
+  "rviz2"
+  "ecar_can"
+  "ecar_can_receiver"
+  "ecar_can_sender"
+  "socket_can"
+  "socket_can_receiver"
+  "socket_can_sender"
+)
+
+terminate_patterns() {
+  local signal="$1"
+  local pattern
+
+  for pattern in "${PATTERNS[@]}"; do
+    pkill "-${signal}" -f "${pattern}" 2>/dev/null || true
+  done
+}
+
+log_info "Sending TERM to leftover master Autoware processes."
+terminate_patterns TERM
+sleep 3
+
+if pgrep -f "component_container|component_container_mt|timoo_ros|map_hash_generator|imu_corrector|gyro_bias_validator|ecar_can|socket_can|rviz2|rqt_gui|launch_ros|ecar_can_receiver|ecar_can_sender|socket_can_receiver|socket_can_sender" >/dev/null 2>&1; then
+  log_warn "Some processes are still alive; sending KILL."
+  terminate_patterns KILL
+  sleep 1
+fi
+
+log_info "Restarting ROS daemon to refresh node discovery."
+ros2 daemon stop >/dev/null 2>&1 || true
+ros2 daemon start >/dev/null 2>&1 || true
+
+log_info "Remaining matching processes:"
+ps -ef | grep -E 'autoware|component_container|rviz2|timoo|ros2 launch|launch_ros|preset_route|ecar_can|socket_can|imu_corrector|gyro_bias|map_hash|rqt_gui' | grep -v grep || true
+
+log_info "Remaining ROS nodes:"
+ros2 node list 2>/dev/null || true

slave/cleanup_slave_autoware.sh

@@ -0,0 +1,117 @@
+#!/usr/bin/env bash
+# Clean leftover slave-board Autoware/control/vehicle processes.
+#
+# Usage:
+#   ./cleanup_slave_autoware.sh
+#
+# Notes:
+#   This script is intended for the slave Orin. It stops the slave launch,
+#   control containers, chassis/CAN bridge, MRM, diagnostics, and derived ROS
+#   processes. Do not run it on a board where you intentionally keep another
+#   Autoware stack running in the same ROS environment.
+
+set -euo pipefail
+
+GREEN='\033[0;32m'
+YELLOW='\033[1;33m'
+NC='\033[0m'
+
+log_info() { echo -e "${GREEN}[INFO]${NC} $*"; }
+log_warn() { echo -e "${YELLOW}[WARN]${NC} $*"; }
+
+script_dir="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+
+find_workspace_root() {
+  if [[ -n "${SLAVE_WORKSPACE_ROOT:-}" ]]; then
+    echo "${SLAVE_WORKSPACE_ROOT}"
+    return
+  fi
+
+  local candidates=(
+    "${PWD}"
+    "${script_dir}"
+  )
+  for candidate in "${candidates[@]}"; do
+    if [[ -f "${candidate}/install/setup.bash" ]]; then
+      (cd "${candidate}" && pwd)
+      return
+    fi
+  done
+
+  echo "${script_dir}"
+}
+
+WORKSPACE_ROOT="$(find_workspace_root)"
+
+PATTERNS=(
+  "run_slave_real_vehicle.sh"
+  "ros2 launch.*ecar_slave"
+  "ros2 launch.*farui_launch.*/launch/ecar_slave"
+  "launch_ros_[0-9]+"
+  "launch_ros"
+  "component_container"
+  "component_container_mt"
+  "robot_state_publisher"
+  "ecar_can"
+  "ecar_can_receiver"
+  "ecar_can_sender"
+  "socket_can"
+  "socket_can_receiver"
+  "socket_can_sender"
+  "vehicle_velocity_converter"
+  "mrm_emergency_stop_operator"
+  "mrm_handler"
+  "hazard_status_converter"
+  "diagnostic_graph_aggregator"
+  "autoware_trajectory_follower"
+  "autoware_lane_departure_checker"
+  "autoware_shift_decider"
+  "autoware_vehicle_cmd_gate"
+  "autoware_operation_mode_transition_manager"
+  "autoware_autonomous_emergency_braking"
+  "autoware_control_evaluator"
+  "glog_component"
+  "heartbeat_monitor.sh"
+)
+
+MATCH_RE='run_slave_real_vehicle|ecar_slave|component_container|robot_state_publisher|ecar_can|socket_can|vehicle_velocity_converter|mrm_|hazard_status|diagnostic_graph|trajectory_follower|lane_departure|shift_decider|vehicle_cmd_gate|operation_mode_transition|autonomous_emergency_braking|control_evaluator|glog_component|launch_ros|heartbeat_monitor'
+
+terminate_patterns() {
+  local signal="$1"
+  local pattern
+
+  for pattern in "${PATTERNS[@]}"; do
+    pkill "-${signal}" -f "${pattern}" 2>/dev/null || true
+  done
+}
+
+log_info "Workspace: ${WORKSPACE_ROOT}"
+
+if [[ -f "${WORKSPACE_ROOT}/install/setup.bash" ]]; then
+  cd "${WORKSPACE_ROOT}"
+  set +u
+  source install/setup.bash
+  set -u
+else
+  log_warn "Workspace setup file not found; continuing without sourcing: ${WORKSPACE_ROOT}/install/setup.bash"
+fi
+
+log_info "Sending TERM to leftover slave Autoware processes."
+terminate_patterns TERM
+sleep 3
+
+if pgrep -f "${MATCH_RE}" >/dev/null 2>&1; then
+  log_warn "Some slave processes are still alive; sending KILL."
+  terminate_patterns KILL
+  sleep 1
+fi
+
+log_info "Restarting ROS daemon to refresh node discovery."
+ros2 daemon stop >/dev/null 2>&1 || true
+ros2 daemon start >/dev/null 2>&1 || true
+
+log_info "Remaining matching processes:"
+ps -ef | grep -E "${MATCH_RE}" | grep -v grep || true
+
+log_info "Remaining ROS nodes:"
+ros2 node list 2>/dev/null || true

slave/debug/check_system_driver_pipeline.sh

@@ -0,0 +1,124 @@
+#!/usr/bin/env bash
+# 诊断从板 control、system 状态、ECAR CAN driver 和 socketcan 链路。
+
+set -euo pipefail
+
+SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+WORKSPACE_ROOT="$(cd "${SCRIPT_DIR}/.." && pwd)"
+ROS_SETUP_FILE="${SLAVE_ROS_SETUP_FILE:-/opt/ros/${ROS_DISTRO:-humble}/setup.bash}"
+
+info() { printf '\n\033[0;32m[INFO]\033[0m %s\n' "$*"; }
+warn() { printf '\n\033[1;33m[WARN]\033[0m %s\n' "$*"; }
+run() {
+  printf '\n$ %s\n' "$*"
+  "$@" || true
+}
+
+echo_once() {
+  local topic="$1"
+  local timeout_sec="${2:-2}"
+
+  printf '\n$ timeout %ss ros2 topic echo --once %s\n' "${timeout_sec}" "${topic}"
+  timeout "${timeout_sec}"s ros2 topic echo --once "${topic}" || true
+}
+
+set +u
+if [[ -f "${ROS_SETUP_FILE}" ]]; then
+  source "${ROS_SETUP_FILE}"
+fi
+if [[ -f "${WORKSPACE_ROOT}/install/setup.bash" ]]; then
+  source "${WORKSPACE_ROOT}/install/setup.bash"
+else
+  warn "Workspace setup not found: ${WORKSPACE_ROOT}/install/setup.bash"
+fi
+set -u
+
+info "环境变量"
+printf 'ROS_DOMAIN_ID=%s\n' "${ROS_DOMAIN_ID:-}"
+printf 'RMW_IMPLEMENTATION=%s\n' "${RMW_IMPLEMENTATION:-}"
+printf 'CYCLONEDDS_URI=%s\n' "${CYCLONEDDS_URI:-}"
+
+info "相关节点"
+run ros2 node list
+
+info "Control/system 服务"
+run ros2 service list
+run ros2 service type /control/control_mode_request
+run ros2 service type /system/operation_mode/change_autoware_control
+run ros2 service type /system/operation_mode/change_operation_mode
+
+info "Topic 端点检查"
+topics=(
+  /socket_can/from_can_bus
+  /socket_can/to_can_bus
+  /vehicle/status/control_mode
+  /vehicle/status/velocity_status
+  /vehicle/status/steering_status
+  /vehicle/status/gear_status
+  /vehicle/status/turn_indicators_status
+  /vehicle/status/hazard_lights_status
+  /vehicle/status/actuation_status
+  /vehicle/status/steering_wheel_status
+  /vehicle/status/door_status
+  /localization/slip_angle
+  /sensing/vehicle_velocity_converter/twist_with_covariance
+  /planning/scenario_planning/trajectory
+  /control/trajectory_follower/control_cmd
+  /control/command/control_cmd
+  /control/command/gear_cmd
+  /control/vehicle_cmd_gate/operation_mode
+  /system/operation_mode/state
+  /system/operation_mode/availability
+  /system/fail_safe/mrm_state
+  /system/emergency/hazard_status
+  /ecar_can_sender/command/adcu_drive_cmd
+  /ecar_can_sender/command/adcu_brake_cmd
+  /ecar_can_sender/command/adcu_steer_cmd
+)
+
+for topic in "${topics[@]}"; do
+  run ros2 topic info -v "${topic}"
+done
+
+info "单次消息采样"
+echo_once /vehicle/status/control_mode 2
+echo_once /vehicle/status/velocity_status 2
+echo_once /vehicle/status/steering_status 2
+echo_once /vehicle/status/gear_status 2
+echo_once /vehicle/status/actuation_status 2
+echo_once /vehicle/status/steering_wheel_status 2
+echo_once /localization/slip_angle 2
+echo_once /control/vehicle_cmd_gate/operation_mode 2
+echo_once /system/operation_mode/state 2
+echo_once /system/operation_mode/availability 2
+echo_once /system/fail_safe/mrm_state 2
+echo_once /system/emergency/hazard_status 2
+echo_once /control/command/control_cmd 2
+echo_once /ecar_can_sender/command/adcu_drive_cmd 2
+echo_once /socket_can/to_can_bus 2
+
+info "结果解读"
+cat <<'EOF'
+从板状态链路:
+  /socket_can/from_can_bus
+    -> /ecar_can_receiver
+    -> /vehicle/status/control_mode、/vehicle/status/velocity_status 等 Autoware 车辆状态
+    -> /autoware_operation_mode_transition_manager + /vehicle_cmd_gate
+    -> /system/operation_mode/state
+    -> /system/operation_mode/availability、/system/fail_safe/mrm_state
+    -> 主板 planning scenario_selector 和 velocity_smoother
+
+控制命令链路:
+  /planning/scenario_planning/trajectory
+    -> /control/trajectory_follower/control_cmd
+    -> /control/command/control_cmd
+    -> /ecar_can_sender/command/adcu_drive_cmd
+    -> /socket_can/to_can_bus
+
+如果 /vehicle/status/control_mode 或 /control/vehicle_cmd_gate/operation_mode 缺失，
+/system/operation_mode/state 就可能不发布。主板 planning 会卡在
+"Waiting for operation mode state"，最终不输出 trajectory。
+
+如果 /control/command/control_cmd 有消息，但 /ecar_can_sender/command/adcu_drive_cmd
+不变化，说明驱动命令输入或命令超时路径仍然有问题。
+EOF

slave/run_slave_real_vehicle.sh

@@ -0,0 +1,252 @@
+#!/usr/bin/env bash
+# Start the slave Orin real-vehicle stack.
+
+set -euo pipefail
+
+GREEN='\033[0;32m'
+YELLOW='\033[1;33m'
+RED='\033[0;31m'
+NC='\033[0m'
+
+log_info() { echo -e "${GREEN}[INFO]${NC} $*"; }
+log_warn() { echo -e "${YELLOW}[WARN]${NC} $*"; }
+log_error() { echo -e "${RED}[ERROR]${NC} $*" >&2; }
+
+validate_bool() {
+  local name="$1"
+  local value="$2"
+
+  case "${value}" in
+    true|false)
+      ;;
+    *)
+      log_error "${name} must be true or false, got: ${value}"
+      exit 1
+      ;;
+  esac
+}
+
+script_dir="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+
+find_workspace_root() {
+  if [[ -n "${SLAVE_WORKSPACE_ROOT:-}" ]]; then
+    echo "${SLAVE_WORKSPACE_ROOT}"
+    return
+  fi
+
+  local candidates=(
+    "${script_dir}"
+    "${PWD}"
+  )
+  for candidate in "${candidates[@]}"; do
+    if [[ -f "${candidate}/install/setup.bash" ]]; then
+      (cd "${candidate}" && pwd)
+      return
+    fi
+  done
+
+  echo "${PWD}"
+}
+
+find_vehicle_config_dir() {
+  local vehicle_model="$1"
+  local source_config="${WORKSPACE_ROOT}/src/Vehicle/${vehicle_model}_description/config/description"
+  local install_config="${WORKSPACE_ROOT}/install/${vehicle_model}_description/share/${vehicle_model}_description/config/description"
+
+  if [[ -d "${source_config}" ]]; then
+    echo "${source_config}"
+    return
+  fi
+
+  echo "${install_config}"
+}
+
+kill_process_group() {
+  local pid="$1"
+  local signal="${2:-TERM}"
+
+  if [[ -z "${pid}" ]]; then
+    return 0
+  fi
+
+  local pgid
+  pgid="$(ps -o pgid= -p "${pid}" 2>/dev/null | tr -d ' ')"
+  if [[ -z "${pgid}" ]]; then
+    # Long-running commands are started with setsid, so the original pid is
+    # also the process-group id even if the parent command exits first.
+    pgid="${pid}"
+  fi
+
+  kill "-${signal}" -- "-${pgid}" 2>/dev/null || true
+}
+
+WORKSPACE_ROOT="$(find_workspace_root)"
+DEFAULT_SLAVE_LAUNCH_FILE="${WORKSPACE_ROOT}/src/Launcher/farui_launch/launch/ecar_slave.launch.xml"
+DEFAULT_HEARTBEAT_SCRIPT="${WORKSPACE_ROOT}/env_config/dual_orin/scripts/heartbeat_monitor.sh"
+DEFAULT_VEHICLE_MODEL="ecar_vehicle"
+ROS_SETUP_FILE="${SLAVE_ROS_SETUP_FILE:-/opt/ros/${ROS_DISTRO:-humble}/setup.bash}"
+
+SLAVE_LAUNCH_FILE="${SLAVE_LAUNCH_FILE:-${DEFAULT_SLAVE_LAUNCH_FILE}}"
+HEARTBEAT_SCRIPT="${SLAVE_HEARTBEAT_SCRIPT:-${DEFAULT_HEARTBEAT_SCRIPT}}"
+VEHICLE_MODEL="${1:-${SLAVE_VEHICLE_MODEL:-${DEFAULT_VEHICLE_MODEL}}}"
+CONFIG_DIR="${SLAVE_CONFIG_DIR:-$(find_vehicle_config_dir "${VEHICLE_MODEL}")}"
+CAN_INTERFACE="${2:-${SLAVE_CAN_INTERFACE:-can1}}"
+CAN_BITRATE="${3:-${SLAVE_CAN_BITRATE:-500000}}"
+LAUNCH_HEARTBEAT="${SLAVE_LAUNCH_HEARTBEAT:-true}"
+HEARTBEAT_DELAY="${SLAVE_HEARTBEAT_DELAY:-5}"
+CONFIGURE_CAN="${SLAVE_CONFIGURE_CAN:-false}"
+LAUNCH_SYSTEM="${SLAVE_LAUNCH_SYSTEM:-true}"
+SYSTEM_RUN_MODE="${SLAVE_SYSTEM_RUN_MODE:-online}"
+LAUNCH_SYSTEM_MONITOR="${SLAVE_LAUNCH_SYSTEM_MONITOR:-false}"
+LAUNCH_DUMMY_DIAG_PUBLISHER="${SLAVE_LAUNCH_DUMMY_DIAG_PUBLISHER:-false}"
+
+validate_bool "SLAVE_LAUNCH_HEARTBEAT" "${LAUNCH_HEARTBEAT}"
+validate_bool "SLAVE_CONFIGURE_CAN" "${CONFIGURE_CAN}"
+validate_bool "SLAVE_LAUNCH_SYSTEM" "${LAUNCH_SYSTEM}"
+validate_bool "SLAVE_LAUNCH_SYSTEM_MONITOR" "${LAUNCH_SYSTEM_MONITOR}"
+validate_bool "SLAVE_LAUNCH_DUMMY_DIAG_PUBLISHER" "${LAUNCH_DUMMY_DIAG_PUBLISHER}"
+
+case "${SYSTEM_RUN_MODE}" in
+  online|logging_simulation|planning_simulation)
+    ;;
+  *)
+    log_error "SLAVE_SYSTEM_RUN_MODE must be online, logging_simulation, or planning_simulation; got: ${SYSTEM_RUN_MODE}"
+    exit 1
+    ;;
+esac
+
+if [[ ! -f "${WORKSPACE_ROOT}/install/setup.bash" ]]; then
+  log_error "Workspace setup file not found: ${WORKSPACE_ROOT}/install/setup.bash"
+  exit 1
+fi
+
+if [[ ! -f "${SLAVE_LAUNCH_FILE}" ]]; then
+  log_error "Slave launch file not found: ${SLAVE_LAUNCH_FILE}"
+  exit 1
+fi
+
+if [[ ! -f "${CONFIG_DIR}/sensors_calibration.yaml" || ! -f "${CONFIG_DIR}/sensor_kit_calibration.yaml" ]]; then
+  log_error "Vehicle config files not found in: ${CONFIG_DIR}"
+  log_error "Expected sensors_calibration.yaml and sensor_kit_calibration.yaml"
+  exit 1
+fi
+
+if [[ "${LAUNCH_HEARTBEAT}" == "true" && ! -f "${HEARTBEAT_SCRIPT}" ]]; then
+  log_error "Heartbeat script not found: ${HEARTBEAT_SCRIPT}"
+  exit 1
+fi
+
+log_info "Workspace: ${WORKSPACE_ROOT}"
+log_info "Slave launch: ${SLAVE_LAUNCH_FILE}"
+log_info "Vehicle model: ${VEHICLE_MODEL}"
+log_info "Vehicle config dir: ${CONFIG_DIR}"
+log_info "CAN: ${CAN_INTERFACE}, bitrate: ${CAN_BITRATE}"
+log_info "System: launch=${LAUNCH_SYSTEM}, run_mode=${SYSTEM_RUN_MODE}, monitor=${LAUNCH_SYSTEM_MONITOR}"
+
+cd "${WORKSPACE_ROOT}"
+set +u
+if [[ -f "${ROS_SETUP_FILE}" ]]; then
+  log_info "Sourcing ROS setup: ${ROS_SETUP_FILE}"
+  source "${ROS_SETUP_FILE}"
+else
+  log_warn "ROS setup file not found, continuing with current shell environment: ${ROS_SETUP_FILE}"
+fi
+log_info "Sourcing workspace setup: ${WORKSPACE_ROOT}/install/setup.bash"
+source "${WORKSPACE_ROOT}/install/setup.bash"
+set -u
+
+log_ros_pkg_prefix() {
+  local package="$1"
+  local prefix
+  if prefix="$(ros2 pkg prefix "${package}" 2>/dev/null)"; then
+    log_info "ROS package ${package}: ${prefix}"
+  else
+    log_warn "ROS package ${package} not found in the sourced environment."
+  fi
+}
+
+log_ros_pkg_prefix "farui_launch"
+log_ros_pkg_prefix "ecar_can_driver"
+log_ros_pkg_prefix "autoware_trajectory_follower_node"
+log_ros_pkg_prefix "system_diagnostic_monitor"
+log_ros_pkg_prefix "component_state_monitor"
+
+if [[ "${CONFIGURE_CAN}" == "true" ]]; then
+  log_info "Configuring CAN interface ${CAN_INTERFACE}."
+  sudo ip link set "${CAN_INTERFACE}" down 2>/dev/null || true
+  sudo ip link set "${CAN_INTERFACE}" up type can bitrate "${CAN_BITRATE}"
+else
+  log_warn "CAN auto-configuration skipped. Set SLAVE_CONFIGURE_CAN=true to run sudo ip link setup."
+fi
+
+SLAVE_STACK_PID=""
+HEARTBEAT_PID=""
+
+log_info "Launching slave Orin stack."
+setsid ros2 launch "${SLAVE_LAUNCH_FILE}" \
+  vehicle_model:="${VEHICLE_MODEL}" \
+  config_dir:="${CONFIG_DIR}" \
+  can_interface:="${CAN_INTERFACE}" \
+  can_bitrate:="${CAN_BITRATE}" \
+  launch_system:="${LAUNCH_SYSTEM}" \
+  system_run_mode:="${SYSTEM_RUN_MODE}" \
+  launch_system_monitor:="${LAUNCH_SYSTEM_MONITOR}" \
+  launch_dummy_diag_publisher:="${LAUNCH_DUMMY_DIAG_PUBLISHER}" &
+SLAVE_STACK_PID=$!
+
+if [[ "${LAUNCH_HEARTBEAT}" == "true" ]]; then
+  log_info "Heartbeat monitor will launch in ${HEARTBEAT_DELAY} seconds."
+  setsid bash -c '
+    set -e
+    delay="$1"
+    heartbeat_script="$2"
+    sleep "${delay}"
+    echo -e "\033[0;32m[INFO]\033[0m Launching heartbeat monitor."
+    exec "${heartbeat_script}"
+  ' bash "${HEARTBEAT_DELAY}" "${HEARTBEAT_SCRIPT}" &
+  HEARTBEAT_PID=$!
+else
+  log_info "Heartbeat monitor disabled by SLAVE_LAUNCH_HEARTBEAT=false."
+fi
+
+cleanup() {
+  trap - EXIT INT TERM
+
+  local pids=()
+  if [[ -n "${HEARTBEAT_PID}" ]]; then
+    pids+=("${HEARTBEAT_PID}")
+  fi
+  if [[ -n "${SLAVE_STACK_PID}" ]]; then
+    pids+=("${SLAVE_STACK_PID}")
+  fi
+
+  if [[ ${#pids[@]} -eq 0 ]]; then
+    return
+  fi
+
+  log_info "Shutting down launched processes."
+
+  for pid in "${pids[@]}"; do
+    kill_process_group "${pid}" TERM
+  done
+
+  sleep 2
+
+  for pid in "${pids[@]}"; do
+    kill_process_group "${pid}" KILL
+  done
+
+  wait "${pids[@]}" 2>/dev/null || true
+}
+
+trap cleanup EXIT INT TERM
+
+WAIT_PIDS=()
+if [[ -n "${SLAVE_STACK_PID}" ]]; then
+  WAIT_PIDS+=("${SLAVE_STACK_PID}")
+fi
+if [[ -n "${HEARTBEAT_PID}" ]]; then
+  WAIT_PIDS+=("${HEARTBEAT_PID}")
+fi
+
+wait "${WAIT_PIDS[@]}"

slave/src/Api/autoware_ad_api_specs/CMakeLists.txt

@@ -0,0 +1,7 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_ad_api_specs)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_package()

slave/src/Api/autoware_ad_api_specs/README.md

@@ -0,0 +1,3 @@
+# autoware_adapi_specs
+
+This package is a specification of Autoware AD API.

slave/src/Api/autoware_ad_api_specs/include/autoware_ad_api_specs/fail_safe.hpp

@@ -0,0 +1,36 @@
+// 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.
+
+#ifndef AUTOWARE_AD_API_SPECS__FAIL_SAFE_HPP_
+#define AUTOWARE_AD_API_SPECS__FAIL_SAFE_HPP_
+
+#include <rclcpp/qos.hpp>
+
+#include <autoware_adapi_v1_msgs/msg/mrm_state.hpp>
+
+namespace autoware_ad_api::fail_safe
+{
+
+struct MrmState
+{
+  using Message = autoware_adapi_v1_msgs::msg::MrmState;
+  static constexpr char name[] = "/api/fail_safe/mrm_state";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+};
+
+}  // namespace autoware_ad_api::fail_safe
+
+#endif  // AUTOWARE_AD_API_SPECS__FAIL_SAFE_HPP_

slave/src/Api/autoware_ad_api_specs/include/autoware_ad_api_specs/interface.hpp

@@ -0,0 +1,31 @@
+// 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.
+
+#ifndef AUTOWARE_AD_API_SPECS__INTERFACE_HPP_
+#define AUTOWARE_AD_API_SPECS__INTERFACE_HPP_
+
+#include <autoware_adapi_version_msgs/srv/interface_version.hpp>
+
+namespace autoware_ad_api::interface
+{
+
+struct Version
+{
+  using Service = autoware_adapi_version_msgs::srv::InterfaceVersion;
+  static constexpr char name[] = "/api/interface/version";
+};
+
+}  // namespace autoware_ad_api::interface
+
+#endif  // AUTOWARE_AD_API_SPECS__INTERFACE_HPP_

slave/src/Api/autoware_ad_api_specs/include/autoware_ad_api_specs/localization.hpp

@@ -0,0 +1,43 @@
+// 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.
+
+#ifndef AUTOWARE_AD_API_SPECS__LOCALIZATION_HPP_
+#define AUTOWARE_AD_API_SPECS__LOCALIZATION_HPP_
+
+#include <rclcpp/qos.hpp>
+
+#include <autoware_adapi_v1_msgs/msg/localization_initialization_state.hpp>
+#include <autoware_adapi_v1_msgs/srv/initialize_localization.hpp>
+
+namespace autoware_ad_api::localization
+{
+
+struct Initialize
+{
+  using Service = autoware_adapi_v1_msgs::srv::InitializeLocalization;
+  static constexpr char name[] = "/api/localization/initialize";
+};
+
+struct InitializationState
+{
+  using Message = autoware_adapi_v1_msgs::msg::LocalizationInitializationState;
+  static constexpr char name[] = "/api/localization/initialization_state";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+};
+
+}  // namespace autoware_ad_api::localization
+
+#endif  // AUTOWARE_AD_API_SPECS__LOCALIZATION_HPP_

slave/src/Api/autoware_ad_api_specs/include/autoware_ad_api_specs/motion.hpp

@@ -0,0 +1,43 @@
+// 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.
+
+#ifndef AUTOWARE_AD_API_SPECS__MOTION_HPP_
+#define AUTOWARE_AD_API_SPECS__MOTION_HPP_
+
+#include <rclcpp/qos.hpp>
+
+#include <autoware_adapi_v1_msgs/msg/motion_state.hpp>
+#include <autoware_adapi_v1_msgs/srv/accept_start.hpp>
+
+namespace autoware_ad_api::motion
+{
+
+struct AcceptStart
+{
+  using Service = autoware_adapi_v1_msgs::srv::AcceptStart;
+  static constexpr char name[] = "/api/motion/accept_start";
+};
+
+struct State
+{
+  using Message = autoware_adapi_v1_msgs::msg::MotionState;
+  static constexpr char name[] = "/api/motion/state";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+};
+
+}  // namespace autoware_ad_api::motion
+
+#endif  // AUTOWARE_AD_API_SPECS__MOTION_HPP_

slave/src/Api/autoware_ad_api_specs/include/autoware_ad_api_specs/operation_mode.hpp

@@ -0,0 +1,73 @@
+// 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.
+
+#ifndef AUTOWARE_AD_API_SPECS__OPERATION_MODE_HPP_
+#define AUTOWARE_AD_API_SPECS__OPERATION_MODE_HPP_
+
+#include <rclcpp/qos.hpp>
+
+#include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
+#include <autoware_adapi_v1_msgs/srv/change_operation_mode.hpp>
+
+namespace autoware_ad_api::operation_mode
+{
+
+struct ChangeToStop
+{
+  using Service = autoware_adapi_v1_msgs::srv::ChangeOperationMode;
+  static constexpr char name[] = "/api/operation_mode/change_to_stop";
+};
+
+struct ChangeToAutonomous
+{
+  using Service = autoware_adapi_v1_msgs::srv::ChangeOperationMode;
+  static constexpr char name[] = "/api/operation_mode/change_to_autonomous";
+};
+
+struct ChangeToLocal
+{
+  using Service = autoware_adapi_v1_msgs::srv::ChangeOperationMode;
+  static constexpr char name[] = "/api/operation_mode/change_to_local";
+};
+
+struct ChangeToRemote
+{
+  using Service = autoware_adapi_v1_msgs::srv::ChangeOperationMode;
+  static constexpr char name[] = "/api/operation_mode/change_to_remote";
+};
+
+struct EnableAutowareControl
+{
+  using Service = autoware_adapi_v1_msgs::srv::ChangeOperationMode;
+  static constexpr char name[] = "/api/operation_mode/enable_autoware_control";
+};
+
+struct DisableAutowareControl
+{
+  using Service = autoware_adapi_v1_msgs::srv::ChangeOperationMode;
+  static constexpr char name[] = "/api/operation_mode/disable_autoware_control";
+};
+
+struct OperationModeState
+{
+  using Message = autoware_adapi_v1_msgs::msg::OperationModeState;
+  static constexpr char name[] = "/api/operation_mode/state";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+};
+
+}  // namespace autoware_ad_api::operation_mode
+
+#endif  // AUTOWARE_AD_API_SPECS__OPERATION_MODE_HPP_

slave/src/Api/autoware_ad_api_specs/include/autoware_ad_api_specs/perception.hpp

@@ -0,0 +1,36 @@
+// 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.
+
+#ifndef AUTOWARE_AD_API_SPECS__PERCEPTION_HPP_
+#define AUTOWARE_AD_API_SPECS__PERCEPTION_HPP_
+
+#include <rclcpp/qos.hpp>
+
+#include <autoware_adapi_v1_msgs/msg/dynamic_object_array.hpp>
+
+namespace autoware_ad_api::perception
+{
+
+struct DynamicObjectArray
+{
+  using Message = autoware_adapi_v1_msgs::msg::DynamicObjectArray;
+  static constexpr char name[] = "/api/perception/objects";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
+};
+
+}  // namespace autoware_ad_api::perception
+
+#endif  // AUTOWARE_AD_API_SPECS__PERCEPTION_HPP_

slave/src/Api/autoware_ad_api_specs/include/autoware_ad_api_specs/planning.hpp

@@ -0,0 +1,46 @@
+// 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.
+
+#ifndef AUTOWARE_AD_API_SPECS__PLANNING_HPP_
+#define AUTOWARE_AD_API_SPECS__PLANNING_HPP_
+
+#include <rclcpp/qos.hpp>
+
+#include <autoware_adapi_v1_msgs/msg/steering_factor_array.hpp>
+#include <autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp>
+
+namespace autoware_ad_api::planning
+{
+
+struct VelocityFactors
+{
+  using Message = autoware_adapi_v1_msgs::msg::VelocityFactorArray;
+  static constexpr char name[] = "/api/planning/velocity_factors";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
+};
+
+struct SteeringFactors
+{
+  using Message = autoware_adapi_v1_msgs::msg::SteeringFactorArray;
+  static constexpr char name[] = "/api/planning/steering_factors";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
+};
+
+}  // namespace autoware_ad_api::planning
+
+#endif  // AUTOWARE_AD_API_SPECS__PLANNING_HPP_

slave/src/Api/autoware_ad_api_specs/include/autoware_ad_api_specs/routing.hpp

@@ -0,0 +1,79 @@
+// 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.
+
+#ifndef AUTOWARE_AD_API_SPECS__ROUTING_HPP_
+#define AUTOWARE_AD_API_SPECS__ROUTING_HPP_
+
+#include <rclcpp/qos.hpp>
+
+#include <autoware_adapi_v1_msgs/msg/route.hpp>
+#include <autoware_adapi_v1_msgs/msg/route_state.hpp>
+#include <autoware_adapi_v1_msgs/srv/clear_route.hpp>
+#include <autoware_adapi_v1_msgs/srv/set_route.hpp>
+#include <autoware_adapi_v1_msgs/srv/set_route_points.hpp>
+
+namespace autoware_ad_api::routing
+{
+
+struct SetRoutePoints
+{
+  using Service = autoware_adapi_v1_msgs::srv::SetRoutePoints;
+  static constexpr char name[] = "/api/routing/set_route_points";
+};
+
+struct SetRoute
+{
+  using Service = autoware_adapi_v1_msgs::srv::SetRoute;
+  static constexpr char name[] = "/api/routing/set_route";
+};
+
+struct ChangeRoutePoints
+{
+  using Service = autoware_adapi_v1_msgs::srv::SetRoutePoints;
+  static constexpr char name[] = "/api/routing/change_route_points";
+};
+
+struct ChangeRoute
+{
+  using Service = autoware_adapi_v1_msgs::srv::SetRoute;
+  static constexpr char name[] = "/api/routing/change_route";
+};
+
+struct ClearRoute
+{
+  using Service = autoware_adapi_v1_msgs::srv::ClearRoute;
+  static constexpr char name[] = "/api/routing/clear_route";
+};
+
+struct RouteState
+{
+  using Message = autoware_adapi_v1_msgs::msg::RouteState;
+  static constexpr char name[] = "/api/routing/state";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+};
+
+struct Route
+{
+  using Message = autoware_adapi_v1_msgs::msg::Route;
+  static constexpr char name[] = "/api/routing/route";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+};
+
+}  // namespace autoware_ad_api::routing
+
+#endif  // AUTOWARE_AD_API_SPECS__ROUTING_HPP_

slave/src/Api/autoware_ad_api_specs/include/autoware_ad_api_specs/system.hpp

@@ -0,0 +1,36 @@
+// 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.
+
+#ifndef AUTOWARE_AD_API_SPECS__SYSTEM_HPP_
+#define AUTOWARE_AD_API_SPECS__SYSTEM_HPP_
+
+#include <rclcpp/qos.hpp>
+
+#include <autoware_adapi_v1_msgs/msg/heartbeat.hpp>
+
+namespace autoware_ad_api::system
+{
+
+struct Heartbeat
+{
+  using Message = autoware_adapi_v1_msgs::msg::Heartbeat;
+  static constexpr char name[] = "/api/system/heartbeat";
+  static constexpr size_t depth = 10;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
+};
+
+}  // namespace autoware_ad_api::system
+
+#endif  // AUTOWARE_AD_API_SPECS__SYSTEM_HPP_

slave/src/Api/autoware_ad_api_specs/include/autoware_ad_api_specs/vehicle.hpp

@@ -0,0 +1,77 @@
+// Copyright 2023 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.
+
+#ifndef AUTOWARE_AD_API_SPECS__VEHICLE_HPP_
+#define AUTOWARE_AD_API_SPECS__VEHICLE_HPP_
+
+#include <rclcpp/qos.hpp>
+
+#include <autoware_adapi_v1_msgs/msg/door_status_array.hpp>
+#include <autoware_adapi_v1_msgs/msg/vehicle_kinematics.hpp>
+#include <autoware_adapi_v1_msgs/msg/vehicle_status.hpp>
+#include <autoware_adapi_v1_msgs/srv/get_door_layout.hpp>
+#include <autoware_adapi_v1_msgs/srv/get_vehicle_dimensions.hpp>
+#include <autoware_adapi_v1_msgs/srv/set_door_command.hpp>
+
+namespace autoware_ad_api::vehicle
+{
+
+struct VehicleKinematics
+{
+  using Message = autoware_adapi_v1_msgs::msg::VehicleKinematics;
+  static constexpr char name[] = "/api/vehicle/kinematics";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
+};
+
+struct VehicleStatus
+{
+  using Message = autoware_adapi_v1_msgs::msg::VehicleStatus;
+  static constexpr char name[] = "/api/vehicle/status";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
+};
+
+struct Dimensions
+{
+  using Service = autoware_adapi_v1_msgs::srv::GetVehicleDimensions;
+  static constexpr char name[] = "/api/vehicle/dimensions";
+};
+
+struct DoorCommand
+{
+  using Service = autoware_adapi_v1_msgs::srv::SetDoorCommand;
+  static constexpr char name[] = "/api/vehicle/doors/command";
+};
+
+struct DoorLayout
+{
+  using Service = autoware_adapi_v1_msgs::srv::GetDoorLayout;
+  static constexpr char name[] = "/api/vehicle/doors/layout";
+};
+
+struct DoorStatus
+{
+  using Message = autoware_adapi_v1_msgs::msg::DoorStatusArray;
+  static constexpr char name[] = "/api/vehicle/doors/status";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+};
+
+}  // namespace autoware_ad_api::vehicle
+
+#endif  // AUTOWARE_AD_API_SPECS__VEHICLE_HPP_

slave/src/Api/autoware_ad_api_specs/package.xml

@@ -0,0 +1,20 @@
+<?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_ad_api_specs</name>
+  <version>0.0.0</version>
+  <description>The autoware_ad_api_specs package</description>
+  <maintainer email="isamu.takagi@tier4.jp">Takagi, Isamu</maintainer>
+  <maintainer email="ryohsuke.mitsudome@tier4.jp">Ryohsuke Mitsudome</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

slave/src/Api/default_ad_api_helpers/ad_api_adaptors/CMakeLists.txt

@@ -0,0 +1,24 @@
+cmake_minimum_required(VERSION 3.14)
+project(ad_api_adaptors)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/initial_pose_adaptor.cpp
+  src/routing_adaptor.cpp
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "ad_api_adaptors::InitialPoseAdaptor"
+  EXECUTABLE initial_pose_adaptor_node
+  EXECUTOR MultiThreadedExecutor
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "ad_api_adaptors::RoutingAdaptor"
+  EXECUTABLE routing_adaptor_node
+  EXECUTOR SingleThreadedExecutor
+)
+
+ament_auto_package(INSTALL_TO_SHARE config launch)

slave/src/Api/default_ad_api_helpers/ad_api_adaptors/README.md

@@ -0,0 +1,35 @@
+# ad_api_adaptors
+
+## initial_pose_adaptor
+
+This node makes it easy to use the localization AD API from RViz.
+When a initial pose topic is received, call the localization initialize API.
+This node depends on the map height fitter library.
+[See here for more details.](../../../map/autoware_map_height_fitter/README.md)
+
+| Interface    | Local Name  | Global Name                  | Description                               |
+| ------------ | ----------- | ---------------------------- | ----------------------------------------- |
+| Subscription | initialpose | /initialpose                 | The pose for localization initialization. |
+| Client       | -           | /api/localization/initialize | The localization initialize API.          |
+
+## routing_adaptor
+
+This node makes it easy to use the routing AD API from RViz.
+When a goal pose topic is received, reset the waypoints and call the API.
+When a waypoint pose topic is received, append it to the end of the waypoints to call the API.
+The clear API is called automatically before setting the route.
+
+| Interface    | Local Name         | Global Name                           | Description                                        |
+| ------------ | ------------------ | ------------------------------------- | -------------------------------------------------- |
+| Subscription | -                  | /api/routing/state                    | The state of the routing API.                      |
+| Subscription | ~/input/fixed_goal | /planning/mission_planning/goal       | The goal pose of route. Disable goal modification. |
+| Subscription | ~/input/rough_goal | /rviz/routing/rough_goal              | The goal pose of route. Enable goal modification.  |
+| Subscription | ~/input/reroute    | /rviz/routing/reroute                 | The goal pose of reroute.                          |
+| Subscription | ~/input/waypoint   | /planning/mission_planning/checkpoint | The waypoint pose of route.                        |
+| Client       | -                  | /api/routing/clear_route              | The route clear API.                               |
+| Client       | -                  | /api/routing/set_route_points         | The route points set API.                          |
+| Client       | -                  | /api/routing/change_route_points      | The route points change API.                       |
+
+## parameters
+
+{{ json_to_markdown("/system/default_ad_api_helpers/ad_api_adaptors/schema/ad_api_adaptors.schema.json") }}

slave/src/Api/default_ad_api_helpers/ad_api_adaptors/config/initial_pose.param.yaml

@@ -0,0 +1,13 @@
+/**:
+  ros__parameters:
+
+    # from initialpose (Rviz's 2DPoseEstimate)
+    initial_pose_particle_covariance:
+      [
+        4.0, 0.0, 0.0,  0.0,  0.0,  0.0,
+        0.0, 4.0, 0.0,  0.0,  0.0,  0.0,
+        0.0, 0.0, 0.01, 0.0,  0.0,  0.0,
+        0.0, 0.0, 0.0,  0.01, 0.0,  0.0,
+        0.0, 0.0, 0.0,  0.0,  0.01, 0.0,
+        0.0, 0.0, 0.0,  0.0,  0.0,  1.0,
+      ]

slave/src/Api/default_ad_api_helpers/ad_api_adaptors/launch/rviz_adaptors.launch.xml

@@ -0,0 +1,22 @@
+<launch>
+  <arg name="rviz_initial_pose_auto_fix_target" default="pointcloud_map"/>
+
+  <group>
+    <push-ros-namespace namespace="default_ad_api/helpers"/>
+    <node pkg="ad_api_adaptors" exec="initial_pose_adaptor_node">
+      <param from="$(find-pkg-share ad_api_adaptors)/config/initial_pose.param.yaml"/>
+      <param name="map_height_fitter.map_loader_name" value="/map/pointcloud_map_loader"/>
+      <param name="map_height_fitter.target" value="$(var rviz_initial_pose_auto_fix_target)"/>
+      <remap from="~/initialpose" to="/initialpose"/>
+      <remap from="~/pointcloud_map" to="/map/pointcloud_map"/>
+      <remap from="~/partial_map_load" to="/map/get_partial_pointcloud_map"/>
+      <remap from="~/vector_map" to="/map/vector_map"/>
+    </node>
+    <node pkg="ad_api_adaptors" exec="routing_adaptor_node">
+      <remap from="~/input/fixed_goal" to="/planning/mission_planning/goal"/>
+      <remap from="~/input/rough_goal" to="/rviz/routing/rough_goal"/>
+      <remap from="~/input/reroute" to="/rviz/routing/reroute"/>
+      <remap from="~/input/waypoint" to="/planning/mission_planning/checkpoint"/>
+    </node>
+  </group>
+</launch>

slave/src/Api/default_ad_api_helpers/ad_api_adaptors/package.xml

@@ -0,0 +1,28 @@
+<?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>ad_api_adaptors</name>
+  <version>0.1.0</version>
+  <description>The ad_api_adaptors package</description>
+  <maintainer email="isamu.takagi@tier4.jp">Takagi, Isamu</maintainer>
+  <maintainer email="ryohsuke.mitsudome@tier4.jp">Ryohsuke Mitsudome</maintainer>
+  <maintainer email="yukihiro.saito@tier4.jp">Yukihiro Saito</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>autoware_ad_api_specs</depend>
+  <depend>autoware_adapi_v1_msgs</depend>
+  <depend>autoware_map_height_fitter</depend>
+  <depend>component_interface_utils</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

slave/src/Api/default_ad_api_helpers/ad_api_adaptors/schema/ad_api_adaptors.schema.json

@@ -0,0 +1,37 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "ad_api_adaptors parameter",
+  "type": "object",
+  "definitions": {
+    "ad_api_adaptors": {
+      "type": "object",
+      "properties": {
+        "initial_pose_particle_covariance": {
+          "type": "array",
+          "description": "initial_pose_particle_covariance",
+          "default": [
+            4.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0,
+            0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0,
+            0.0, 1.0
+          ]
+        }
+      },
+      "required": ["initial_pose_particle_covariance"],
+      "additionalProperties": false
+    }
+  },
+  "properties": {
+    "/**": {
+      "type": "object",
+      "properties": {
+        "ros__parameters": {
+          "$ref": "#/definitions/initial_pose_particle_covariance"
+        }
+      },
+      "required": ["ros__parameters"],
+      "additionalProperties": false
+    }
+  },
+  "required": ["/**"],
+  "additionalProperties": false
+}

slave/src/Api/default_ad_api_helpers/ad_api_adaptors/src/initial_pose_adaptor.cpp

@@ -0,0 +1,66 @@
+// 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.
+
+#include "initial_pose_adaptor.hpp"
+
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace ad_api_adaptors
+{
+template <class ServiceT>
+using Future = typename rclcpp::Client<ServiceT>::SharedFuture;
+
+std::array<double, 36> get_covariance_parameter(rclcpp::Node * node, const std::string & name)
+{
+  const auto vector = node->declare_parameter<std::vector<double>>(name);
+  if (vector.size() != 36) {
+    throw std::invalid_argument("The covariance parameter size is not 36.");
+  }
+  std::array<double, 36> array;
+  std::copy_n(vector.begin(), array.size(), array.begin());
+  return array;
+}
+
+InitialPoseAdaptor::InitialPoseAdaptor(const rclcpp::NodeOptions & options)
+: Node("initial_pose_adaptor", options), fitter_(this)
+{
+  rviz_particle_covariance_ = get_covariance_parameter(this, "initial_pose_particle_covariance");
+  sub_initial_pose_ = create_subscription<PoseWithCovarianceStamped>(
+    "~/initialpose", rclcpp::QoS(1),
+    std::bind(&InitialPoseAdaptor::on_initial_pose, this, std::placeholders::_1));
+
+  const auto adaptor = component_interface_utils::NodeAdaptor(this);
+  adaptor.init_cli(cli_initialize_);
+}
+
+void InitialPoseAdaptor::on_initial_pose(const PoseWithCovarianceStamped::ConstSharedPtr msg)
+{
+  PoseWithCovarianceStamped pose = *msg;
+  const auto fitted = fitter_.fit(pose.pose.pose.position, pose.header.frame_id);
+  if (fitted) {
+    pose.pose.pose.position = fitted.value();
+  }
+  pose.pose.covariance = rviz_particle_covariance_;
+
+  const auto req = std::make_shared<Initialize::Service::Request>();
+  req->pose.push_back(pose);
+  cli_initialize_->async_send_request(req);
+}
+
+}  // namespace ad_api_adaptors
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(ad_api_adaptors::InitialPoseAdaptor)

slave/src/Api/default_ad_api_helpers/ad_api_adaptors/src/initial_pose_adaptor.hpp

@@ -0,0 +1,46 @@
+// 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.
+
+#ifndef INITIAL_POSE_ADAPTOR_HPP_
+#define INITIAL_POSE_ADAPTOR_HPP_
+
+#include <autoware/map_height_fitter/map_height_fitter.hpp>
+#include <autoware_ad_api_specs/localization.hpp>
+#include <component_interface_utils/rclcpp.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+
+namespace ad_api_adaptors
+{
+
+class InitialPoseAdaptor : public rclcpp::Node
+{
+public:
+  explicit InitialPoseAdaptor(const rclcpp::NodeOptions & options);
+
+private:
+  using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped;
+  using Initialize = autoware_ad_api::localization::Initialize;
+  rclcpp::Subscription<PoseWithCovarianceStamped>::SharedPtr sub_initial_pose_;
+  component_interface_utils::Client<Initialize>::SharedPtr cli_initialize_;
+  std::array<double, 36> rviz_particle_covariance_;
+  autoware::map_height_fitter::MapHeightFitter fitter_;
+
+  void on_initial_pose(const PoseWithCovarianceStamped::ConstSharedPtr msg);
+};
+
+}  // namespace ad_api_adaptors
+
+#endif  // INITIAL_POSE_ADAPTOR_HPP_

slave/src/Api/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.cpp

@@ -0,0 +1,115 @@
+// 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.
+
+#include "routing_adaptor.hpp"
+
+#include <memory>
+
+namespace ad_api_adaptors
+{
+
+RoutingAdaptor::RoutingAdaptor(const rclcpp::NodeOptions & options)
+: Node("routing_adaptor", options)
+{
+  using std::placeholders::_1;
+
+  sub_fixed_goal_ = create_subscription<PoseStamped>(
+    "~/input/fixed_goal", 3, std::bind(&RoutingAdaptor::on_fixed_goal, this, _1));
+  sub_rough_goal_ = create_subscription<PoseStamped>(
+    "~/input/rough_goal", 3, std::bind(&RoutingAdaptor::on_rough_goal, this, _1));
+  sub_reroute_ = create_subscription<PoseStamped>(
+    "~/input/reroute", 3, std::bind(&RoutingAdaptor::on_reroute, this, _1));
+  sub_waypoint_ = create_subscription<PoseStamped>(
+    "~/input/waypoint", 10, std::bind(&RoutingAdaptor::on_waypoint, this, _1));
+
+  const auto adaptor = component_interface_utils::NodeAdaptor(this);
+  adaptor.init_cli(cli_reroute_);
+  adaptor.init_cli(cli_route_);
+  adaptor.init_cli(cli_clear_);
+  adaptor.init_sub(
+    sub_state_, [this](const RouteState::Message::ConstSharedPtr msg) { state_ = msg->state; });
+
+  const auto rate = rclcpp::Rate(5.0);
+  timer_ = rclcpp::create_timer(
+    this, get_clock(), rate.period(), std::bind(&RoutingAdaptor::on_timer, this));
+
+  state_ = RouteState::Message::UNKNOWN;
+  route_ = std::make_shared<SetRoutePoints::Service::Request>();
+}
+
+void RoutingAdaptor::on_timer()
+{
+  // Wait a moment to combine consecutive goals and checkpoints into a single request.
+  // This value is rate dependent and set the wait time for merging.
+  constexpr int delay_count = 3;  // 0.4 seconds (rate * (value - 1))
+  if (0 < request_timing_control_ && request_timing_control_ < delay_count) {
+    ++request_timing_control_;
+  }
+  if (request_timing_control_ != delay_count) {
+    return;
+  }
+
+  if (!calling_service_) {
+    if (state_ != RouteState::Message::UNSET) {
+      const auto request = std::make_shared<ClearRoute::Service::Request>();
+      calling_service_ = true;
+      cli_clear_->async_send_request(request, [this](auto) { calling_service_ = false; });
+    } else {
+      request_timing_control_ = 0;
+      calling_service_ = true;
+      cli_route_->async_send_request(route_, [this](auto) { calling_service_ = false; });
+    }
+  }
+}
+
+void RoutingAdaptor::on_fixed_goal(const PoseStamped::ConstSharedPtr pose)
+{
+  request_timing_control_ = 1;
+  route_->header = pose->header;
+  route_->goal = pose->pose;
+  route_->waypoints.clear();
+  route_->option.allow_goal_modification = false;
+}
+
+void RoutingAdaptor::on_rough_goal(const PoseStamped::ConstSharedPtr pose)
+{
+  request_timing_control_ = 1;
+  route_->header = pose->header;
+  route_->goal = pose->pose;
+  route_->waypoints.clear();
+  route_->option.allow_goal_modification = true;
+}
+
+void RoutingAdaptor::on_waypoint(const PoseStamped::ConstSharedPtr pose)
+{
+  if (route_->header.frame_id != pose->header.frame_id) {
+    RCLCPP_ERROR_STREAM(get_logger(), "The waypoint frame does not match the goal.");
+    return;
+  }
+  request_timing_control_ = 1;
+  route_->waypoints.push_back(pose->pose);
+}
+
+void RoutingAdaptor::on_reroute(const PoseStamped::ConstSharedPtr pose)
+{
+  const auto route = std::make_shared<SetRoutePoints::Service::Request>();
+  route->header = pose->header;
+  route->goal = pose->pose;
+  cli_reroute_->async_send_request(route);
+}
+
+}  // namespace ad_api_adaptors
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(ad_api_adaptors::RoutingAdaptor)

slave/src/Api/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.hpp

@@ -0,0 +1,64 @@
+// 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.
+
+#ifndef ROUTING_ADAPTOR_HPP_
+#define ROUTING_ADAPTOR_HPP_
+
+#include <autoware_ad_api_specs/routing.hpp>
+#include <component_interface_utils/rclcpp.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <geometry_msgs/msg/pose_stamped.hpp>
+
+#include <string>
+
+namespace ad_api_adaptors
+{
+
+class RoutingAdaptor : public rclcpp::Node
+{
+public:
+  explicit RoutingAdaptor(const rclcpp::NodeOptions & options);
+
+private:
+  using PoseStamped = geometry_msgs::msg::PoseStamped;
+  using SetRoutePoints = autoware_ad_api::routing::SetRoutePoints;
+  using ChangeRoutePoints = autoware_ad_api::routing::ChangeRoutePoints;
+  using ClearRoute = autoware_ad_api::routing::ClearRoute;
+  using RouteState = autoware_ad_api::routing::RouteState;
+  component_interface_utils::Client<ChangeRoutePoints>::SharedPtr cli_reroute_;
+  component_interface_utils::Client<SetRoutePoints>::SharedPtr cli_route_;
+  component_interface_utils::Client<ClearRoute>::SharedPtr cli_clear_;
+  component_interface_utils::Subscription<RouteState>::SharedPtr sub_state_;
+  rclcpp::Subscription<PoseStamped>::SharedPtr sub_fixed_goal_;
+  rclcpp::Subscription<PoseStamped>::SharedPtr sub_rough_goal_;
+  rclcpp::Subscription<PoseStamped>::SharedPtr sub_waypoint_;
+  rclcpp::Subscription<PoseStamped>::SharedPtr sub_reroute_;
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  bool calling_service_ = false;
+  int request_timing_control_ = 0;
+  SetRoutePoints::Service::Request::SharedPtr route_;
+  RouteState::Message::_state_type state_;
+
+  void on_timer();
+  void on_fixed_goal(const PoseStamped::ConstSharedPtr pose);
+  void on_rough_goal(const PoseStamped::ConstSharedPtr pose);
+  void on_waypoint(const PoseStamped::ConstSharedPtr pose);
+  void on_reroute(const PoseStamped::ConstSharedPtr pose);
+};
+
+}  // namespace ad_api_adaptors
+
+#endif  // ROUTING_ADAPTOR_HPP_

slave/src/Api/tier4_ad_api_adaptor/.clang-format

@@ -0,0 +1,47 @@
+# Modified from https://github.com/ament/ament_lint/blob/master/ament_clang_format/ament_clang_format/configuration/.clang-format
+Language: Cpp
+BasedOnStyle: Google
+
+AccessModifierOffset: -2
+AlignAfterOpenBracket: AlwaysBreak
+AllowShortFunctionsOnASingleLine: InlineOnly
+BraceWrapping:
+  AfterClass: true
+  AfterFunction: true
+  AfterNamespace: true
+  AfterStruct: true
+BreakBeforeBraces: Custom
+ColumnLimit: 100
+ConstructorInitializerIndentWidth: 0
+ContinuationIndentWidth: 2
+DerivePointerAlignment: false
+PointerAlignment: Middle
+ReflowComments: true
+IncludeCategories:
+  # C++ system headers
+  - Regex: <[a-z_]*>
+    Priority: 6
+    CaseSensitive: true
+  # C system headers
+  - Regex: <.*\.h>
+    Priority: 5
+    CaseSensitive: true
+  # Boost headers
+  - Regex: boost/.*
+    Priority: 4
+    CaseSensitive: true
+  # Message headers
+  - Regex: .*_msgs/.*
+    Priority: 3
+    CaseSensitive: true
+  - Regex: .*_srvs/.*
+    Priority: 3
+    CaseSensitive: true
+  # Other Package headers
+  - Regex: <.*>
+    Priority: 2
+    CaseSensitive: true
+  # Local package headers
+  - Regex: '".*"'
+    Priority: 1
+    CaseSensitive: true

slave/src/Api/tier4_ad_api_adaptor/.github/PULL_REQUEST_TEMPLATE.md

@@ -0,0 +1,56 @@
+## PR Type
+
+<!-- Select one and remove others. If an appropriate one is not listed, please write by yourself. -->
+
+- New Feature
+- Improvement
+- Bug Fix
+
+## Related Links
+
+<!-- Please write related links to GitHub/Jira/Slack/etc. -->
+
+## Description
+
+<!-- Describe what this PR changes. -->
+
+## Review Procedure
+
+<!-- Explain how to review this PR. -->
+
+## Remarks
+
+<!-- Write remarks as you like if you need them. -->
+
+## Pre-Review Checklist for the PR Author
+
+**PR Author should check the checkboxes below when creating the PR.**
+
+- [ ] Code follows [coding guidelines][coding-guidelines]
+- [ ] Assign PR to reviewer
+
+## Checklist for the PR Reviewer
+
+**Reviewers should check the checkboxes below before approval.**
+
+- [ ] Commits are properly organized and messages are according to the guideline
+- [ ] Code follows [coding guidelines][coding-guidelines]
+- [ ] (Optional) Unit tests have been written for new behavior
+- [ ] PR title describes the changes
+
+## Post-Review Checklist for the PR Author
+
+**PR Author should check the checkboxes below before merging.**
+
+- [ ] All open points are addressed and tracked via issues or tickets
+- [ ] Write [release notes][release-notes]
+
+## CI Checks
+
+- **Build and test for PR / build-and-test-pr**: Required to pass before the merge.
+- **Build and test for PR / clang-tidy-pr**: NOT required to pass before the merge. It is up to the reviewer(s).
+- **Check spelling**: NOT required to pass before the merge. It is up to the reviewer(s). See [here][spell-check-dict] if you want to add some words to the spell check dictionary.
+
+[coding-guidelines]: https://tier4.atlassian.net/wiki/spaces/AIP/pages/1194394777/T4
+[release-notes]: https://tier4.atlassian.net/wiki/spaces/AIP/pages/563774416
+[spell-check-dict]: https://github.com/tier4/autoware-spell-check-dict#how-to-contribute

slave/src/Api/tier4_ad_api_adaptor/.github/dependabot.yaml

@@ -0,0 +1,10 @@
+version: 2
+updates:
+  - package-ecosystem: github-actions
+    directory: /
+    schedule:
+      interval: daily
+    open-pull-requests-limit: 1
+    labels:
+      - tag:bot
+      - type:github-actions

slave/src/Api/tier4_ad_api_adaptor/.github/sync-files.yaml

@@ -0,0 +1,20 @@
+- repository: autowarefoundation/autoware_common
+  files:
+    - source: .github/dependabot.yaml
+    - source: .github/workflows/build-and-test.yaml
+    - source: .github/workflows/build-and-test-differential.yaml
+    - source: .github/workflows/pre-commit.yaml
+    - source: .github/workflows/pre-commit-optional.yaml
+    - source: .github/workflows/semantic-pull-request.yaml
+    - source: .github/workflows/spell-check-differential.yaml
+    - source: .github/workflows/sync-files.yaml
+    - source: .clang-format
+    - source: .markdown-link-check.json
+    - source: .markdownlint.yaml
+    - source: .pre-commit-config.yaml
+    - source: .pre-commit-config-optional.yaml
+    - source: .prettierignore
+    - source: .prettierrc.yaml
+    - source: .yamllint.yaml
+    - source: CPPLINT.cfg
+    - source: setup.cfg

slave/src/Api/tier4_ad_api_adaptor/.github/workflows/build-and-test-differential.yaml

@@ -0,0 +1,91 @@
+name: build-and-test-differential
+
+on:
+  pull_request:
+
+jobs:
+  build-and-test-differential:
+    runs-on: ubuntu-latest
+    container: ${{ matrix.container }}
+    strategy:
+      fail-fast: false
+      matrix:
+        rosdistro:
+          - humble
+        include:
+          - rosdistro: humble
+            container: ros:humble
+            build-depends-repos: build_depends.repos
+    steps:
+      - name: Check out repository
+        uses: actions/checkout@v3
+        with:
+          fetch-depth: 0
+
+      - name: Remove exec_depend
+        uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1
+
+      - name: Get modified packages
+        id: get-modified-packages
+        uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1
+
+      - name: Build
+        if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }}
+        uses: autowarefoundation/autoware-github-actions/colcon-build@v1
+        with:
+          rosdistro: ${{ matrix.rosdistro }}
+          target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }}
+          build-depends-repos: ${{ matrix.build-depends-repos }}
+
+      - name: Test
+        id: test
+        if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }}
+        uses: autowarefoundation/autoware-github-actions/colcon-test@v1
+        with:
+          rosdistro: ${{ matrix.rosdistro }}
+          target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }}
+          build-depends-repos: ${{ matrix.build-depends-repos }}
+
+      - name: Upload coverage to CodeCov
+        if: ${{ steps.test.outputs.coverage-report-files != '' }}
+        uses: codecov/codecov-action@v3
+        with:
+          files: ${{ steps.test.outputs.coverage-report-files }}
+          fail_ci_if_error: false
+          verbose: true
+          flags: differential
+
+  clang-tidy-differential:
+    runs-on: ubuntu-latest
+    container: ros:humble
+    needs: build-and-test-differential
+    steps:
+      - name: Check out repository
+        uses: actions/checkout@v3
+        with:
+          fetch-depth: 0
+
+      - name: Remove exec_depend
+        uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1
+
+      - name: Get modified packages
+        id: get-modified-packages
+        uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1
+
+      - name: Get modified files
+        id: get-modified-files
+        uses: tj-actions/changed-files@v42
+        with:
+          files: |
+            **/*.cpp
+            **/*.hpp
+
+      - name: Run clang-tidy
+        if: ${{ steps.get-modified-files.outputs.all_changed_files != '' }}
+        uses: autowarefoundation/autoware-github-actions/clang-tidy@v1
+        with:
+          rosdistro: humble
+          target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }}
+          target-files: ${{ steps.get-modified-files.outputs.all_changed_files }}
+          clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/main/.clang-tidy
+          build-depends-repos: build_depends.repos

slave/src/Api/tier4_ad_api_adaptor/.github/workflows/build-and-test.yaml

@@ -0,0 +1,69 @@
+name: build-and-test
+
+on:
+  push:
+  schedule:
+    - cron: 0 0 * * *
+  workflow_dispatch:
+
+jobs:
+  build-and-test:
+    if: ${{ github.event_name != 'push' || github.ref_name == github.event.repository.default_branch }}
+    runs-on: ubuntu-latest
+    container: ${{ matrix.container }}
+    strategy:
+      fail-fast: false
+      matrix:
+        rosdistro:
+          - humble
+        include:
+          - rosdistro: humble
+            container: ros:humble
+            build-depends-repos: build_depends.repos
+    steps:
+      - name: Check out repository
+        uses: actions/checkout@v3
+
+      - name: Free disk space (Ubuntu)
+        uses: jlumbroso/free-disk-space@v1.3.1
+        with:
+          tool-cache: false
+          dotnet: false
+          swap-storage: false
+          large-packages: false
+
+      - name: Remove exec_depend
+        uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1
+
+      - name: Get self packages
+        id: get-self-packages
+        uses: autowarefoundation/autoware-github-actions/get-self-packages@v1
+
+      - name: Build
+        if: ${{ steps.get-self-packages.outputs.self-packages != '' }}
+        uses: autowarefoundation/autoware-github-actions/colcon-build@v1
+        with:
+          rosdistro: ${{ matrix.rosdistro }}
+          target-packages: ${{ steps.get-self-packages.outputs.self-packages }}
+          build-depends-repos: ${{ matrix.build-depends-repos }}
+
+      - name: Test
+        if: ${{ steps.get-self-packages.outputs.self-packages != '' }}
+        id: test
+        uses: autowarefoundation/autoware-github-actions/colcon-test@v1
+        with:
+          rosdistro: ${{ matrix.rosdistro }}
+          target-packages: ${{ steps.get-self-packages.outputs.self-packages }}
+          build-depends-repos: ${{ matrix.build-depends-repos }}
+
+      - name: Upload coverage to CodeCov
+        if: ${{ steps.test.outputs.coverage-report-files != '' }}
+        uses: codecov/codecov-action@v3
+        with:
+          files: ${{ steps.test.outputs.coverage-report-files }}
+          fail_ci_if_error: false
+          verbose: true
+          flags: total
+
+      - name: Show disk space after the tasks
+        run: df -h

slave/src/Api/tier4_ad_api_adaptor/.github/workflows/pre-commit-optional.yaml

@@ -0,0 +1,19 @@
+name: pre-commit-optional
+
+on:
+  pull_request:
+
+jobs:
+  pre-commit-optional:
+    runs-on: ubuntu-latest
+    steps:
+      - name: Check out repository
+        uses: actions/checkout@v4
+        with:
+          fetch-depth: 0
+
+      - name: Run pre-commit
+        uses: autowarefoundation/autoware-github-actions/pre-commit@v1
+        with:
+          pre-commit-config: .pre-commit-config-optional.yaml
+          base-branch: origin/${{ github.base_ref }}

slave/src/Api/tier4_ad_api_adaptor/.github/workflows/pre-commit.yaml

@@ -0,0 +1,27 @@
+name: pre-commit
+
+on:
+  pull_request:
+
+jobs:
+  pre-commit:
+    if: ${{ github.event.repository.private }} # Use pre-commit.ci for public repositories
+    runs-on: ubuntu-latest
+    steps:
+      - name: Generate token
+        id: generate-token
+        uses: tibdex/github-app-token@v2
+        with:
+          app_id: ${{ secrets.APP_ID }}
+          private_key: ${{ secrets.PRIVATE_KEY }}
+
+      - name: Check out repository
+        uses: actions/checkout@v4
+        with:
+          ref: ${{ github.event.pull_request.head.ref }}
+
+      - name: Run pre-commit
+        uses: autowarefoundation/autoware-github-actions/pre-commit@v1
+        with:
+          pre-commit-config: .pre-commit-config.yaml
+          token: ${{ steps.generate-token.outputs.token }}

slave/src/Api/tier4_ad_api_adaptor/.github/workflows/semantic-pull-request.yaml

@@ -0,0 +1,12 @@
+name: semantic-pull-request
+
+on:
+  pull_request_target:
+    types:
+      - opened
+      - edited
+      - synchronize
+
+jobs:
+  semantic-pull-request:
+    uses: autowarefoundation/autoware-github-actions/.github/workflows/semantic-pull-request.yaml@v1

slave/src/Api/tier4_ad_api_adaptor/.github/workflows/spell-check-differential.yaml

@@ -0,0 +1,16 @@
+name: spell-check-differential
+
+on:
+  pull_request:
+
+jobs:
+  spell-check-differential:
+    runs-on: ubuntu-latest
+    steps:
+      - name: Check out repository
+        uses: actions/checkout@v4
+
+      - name: Run spell-check
+        uses: autowarefoundation/autoware-github-actions/spell-check@v1
+        with:
+          cspell-json-url: https://raw.githubusercontent.com/tier4/autoware-spell-check-dict/main/.cspell.json

slave/src/Api/tier4_ad_api_adaptor/.github/workflows/sync-files.yaml

@@ -0,0 +1,33 @@
+name: sync-files
+
+on:
+  schedule:
+    - cron: 0 0 * * *
+  workflow_dispatch:
+
+jobs:
+  check-secret:
+    uses: autowarefoundation/autoware-github-actions/.github/workflows/check-secret.yaml@v1
+    secrets:
+      secret: ${{ secrets.APP_ID }}
+
+  sync-files:
+    needs: check-secret
+    if: ${{ needs.check-secret.outputs.set == 'true' }}
+    runs-on: ubuntu-latest
+    steps:
+      - name: Generate token
+        id: generate-token
+        uses: tibdex/github-app-token@v2
+        with:
+          app_id: ${{ secrets.APP_ID }}
+          private_key: ${{ secrets.PRIVATE_KEY }}
+
+      - name: Run sync-files
+        uses: autowarefoundation/autoware-github-actions/sync-files@v1
+        with:
+          token: ${{ steps.generate-token.outputs.token }}
+          pr-labels: |
+            tag:bot
+            tag:sync-files
+          auto-merge-method: squash

slave/src/Api/tier4_ad_api_adaptor/.github/workflows/sync-micro-bus.yaml

@@ -0,0 +1,30 @@
+name: sync-micro-bus
+
+on:
+  schedule:
+    - cron: 0 0 * * *
+  workflow_dispatch:
+
+jobs:
+  sync-micro-bus:
+    runs-on: ubuntu-latest
+    steps:
+      - name: Generate token
+        id: generate-token
+        uses: tibdex/github-app-token@v1
+        with:
+          app_id: ${{ secrets.APP_ID }}
+          private_key: ${{ secrets.PRIVATE_KEY }}
+
+      - name: Run sync-branches
+        uses: autowarefoundation/autoware-github-actions/sync-branches@v1
+        with:
+          token: ${{ steps.generate-token.outputs.token }}
+          base-branch: micro-bus
+          sync-pr-branch: sync-micro-bus
+          sync-target-repository: https://github.com/tier4/tier4_ad_api_adaptor
+          sync-target-branch: tier4/universe
+          pr-title: "chore: sync-micro-bus"
+          pr-labels: |
+            bot
+            sync-micro-bus

slave/src/Api/tier4_ad_api_adaptor/.markdown-link-check.json

@@ -0,0 +1,16 @@
+{
+  "aliveStatusCodes": [200, 206, 403],
+  "ignorePatterns": [
+    {
+      "pattern": "^http://localhost"
+    },
+    {
+      "pattern": "^http://127\\.0\\.0\\.1"
+    },
+    {
+      "pattern": "^https://github.com/.*/discussions/new"
+    }
+  ],
+  "retryOn429": true,
+  "retryCount": 10
+}

slave/src/Api/tier4_ad_api_adaptor/.markdownlint.yaml

@@ -0,0 +1,11 @@
+# See https://github.com/DavidAnson/markdownlint/blob/main/doc/Rules.md for all rules.
+default: true
+MD013: false
+MD024:
+  siblings_only: true
+MD029:
+  style: ordered
+MD033: false
+MD041: false
+MD046: false
+MD049: false

slave/src/Api/tier4_ad_api_adaptor/.pre-commit-config-optional.yaml

@@ -0,0 +1,6 @@
+repos:
+  - repo: https://github.com/tcort/markdown-link-check
+    rev: v3.11.2
+    hooks:
+      - id: markdown-link-check
+        args: [--quiet, --config=.markdown-link-check.json]

slave/src/Api/tier4_ad_api_adaptor/.pre-commit-config.yaml

@@ -0,0 +1,95 @@
+ci:
+  autofix_commit_msg: "style(pre-commit): autofix"
+
+repos:
+  - repo: https://github.com/pre-commit/pre-commit-hooks
+    rev: v4.4.0
+    hooks:
+      - id: check-json
+      - id: check-merge-conflict
+      - id: check-toml
+      - id: check-xml
+      - id: check-yaml
+        args: [--unsafe]
+      - id: detect-private-key
+      - id: end-of-file-fixer
+      - id: mixed-line-ending
+      - id: trailing-whitespace
+        args: [--markdown-linebreak-ext=md]
+
+  - repo: https://github.com/igorshubovych/markdownlint-cli
+    rev: v0.33.0
+    hooks:
+      - id: markdownlint
+        args: [-c, .markdownlint.yaml, --fix]
+
+  - repo: https://github.com/pre-commit/mirrors-prettier
+    rev: v3.0.0-alpha.6
+    hooks:
+      - id: prettier
+
+  - repo: https://github.com/adrienverge/yamllint
+    rev: v1.30.0
+    hooks:
+      - id: yamllint
+
+  - repo: https://github.com/tier4/pre-commit-hooks-ros
+    rev: v0.8.0
+    hooks:
+      - id: flake8-ros
+      - id: prettier-xacro
+      - id: prettier-launch-xml
+      - id: prettier-package-xml
+      - id: ros-include-guard
+      - id: sort-package-xml
+
+  - repo: https://github.com/shellcheck-py/shellcheck-py
+    rev: v0.9.0.2
+    hooks:
+      - id: shellcheck
+
+  - repo: https://github.com/scop/pre-commit-shfmt
+    rev: v3.6.0-2
+    hooks:
+      - id: shfmt
+        args: [-w, -s, -i=4]
+
+  - repo: https://github.com/pycqa/isort
+    rev: 5.12.0
+    hooks:
+      - id: isort
+
+  - repo: https://github.com/psf/black
+    rev: 23.3.0
+    hooks:
+      - id: black
+        args: [--line-length=100]
+
+  - repo: https://github.com/pre-commit/mirrors-clang-format
+    rev: v16.0.0
+    hooks:
+      - id: clang-format
+        types_or: [c++, c, cuda]
+
+  - repo: https://github.com/cpplint/cpplint
+    rev: 1.6.1
+    hooks:
+      - id: cpplint
+        args: [--quiet]
+        exclude: .cu
+
+  - repo: https://github.com/python-jsonschema/check-jsonschema
+    rev: 0.23.2
+    hooks:
+      - id: check-metaschema
+        files: ^.+/schema/.*schema\.json$
+
+  - repo: local
+    hooks:
+      - id: prettier-svg
+        name: prettier svg
+        description: Apply Prettier with plugin-xml to svg.
+        entry: prettier --write --list-different --ignore-unknown --print-width 200 --xml-self-closing-space false --xml-whitespace-sensitivity ignore
+        language: node
+        files: .svg$
+        additional_dependencies: [prettier@2.7.1, "@prettier/plugin-xml@2.2.0"]

slave/src/Api/tier4_ad_api_adaptor/.prettierignore

@@ -0,0 +1,2 @@
+*.param.yaml
+*.rviz

slave/src/Api/tier4_ad_api_adaptor/.prettierrc.yaml

@@ -0,0 +1,20 @@
+printWidth: 100
+tabWidth: 2
+overrides:
+  - files: package.xml
+    options:
+      printWidth: 1000
+      xmlSelfClosingSpace: false
+      xmlWhitespaceSensitivity: ignore
+
+  - files: "*.launch.xml"
+    options:
+      printWidth: 200
+      xmlSelfClosingSpace: false
+      xmlWhitespaceSensitivity: ignore
+
+  - files: "*.xacro"
+    options:
+      printWidth: 200
+      xmlSelfClosingSpace: false
+      xmlWhitespaceSensitivity: ignore

slave/src/Api/tier4_ad_api_adaptor/.yamllint.yaml

@@ -0,0 +1,22 @@
+extends: default
+
+ignore: |
+  *.param.yaml
+
+rules:
+  braces:
+    level: error
+    max-spaces-inside: 1 # To format with Prettier
+  comments:
+    level: error
+    min-spaces-from-content: 1 # To be compatible with C++ and Python
+  document-start:
+    level: error
+    present: false # Don't need document start markers
+  line-length: disable # Delegate to Prettier
+  truthy:
+    level: error
+    check-keys: false # To allow 'on' of GitHub Actions
+  quoted-strings:
+    level: error
+    required: only-when-needed # To keep consistent style

slave/src/Api/tier4_ad_api_adaptor/CPPLINT.cfg

@@ -0,0 +1,14 @@
+# Modified from https://github.com/ament/ament_lint/blob/ebd524bb9973d5ec1dc48a670ce54f958a5a0243/ament_cpplint/ament_cpplint/main.py#L64-L120
+set noparent
+linelength=100
+includeorder=standardcfirst
+filter=-build/c++11               # we do allow C++11
+filter=-build/namespaces_literals # we allow using namespace for literals
+filter=-runtime/references        # we consider passing non-const references to be ok
+filter=-whitespace/braces         # we wrap open curly braces for namespaces, classes and functions
+filter=-whitespace/indent         # we don't indent keywords like public, protected and private with one space
+filter=-whitespace/parens         # we allow closing parenthesis to be on the next line
+filter=-whitespace/semicolon      # we allow the developer to decide about whitespace after a semicolon
+filter=-build/header_guard        # we automatically fix the names of header guards using pre-commit
+filter=-build/include_order       # we use the custom include order
+filter=-build/include_subdir      # we allow the style of "foo.hpp"

slave/src/Api/tier4_ad_api_adaptor/README.md

@@ -0,0 +1 @@
+# Autoware High Level API

slave/src/Api/tier4_ad_api_adaptor/build_depends.repos

@@ -0,0 +1,25 @@
+repositories:
+  core/common:
+    type: git
+    url: https://github.com/autowarefoundation/autoware_common.git
+    version: main
+  msgs/autoware_msgs:
+    type: git
+    url: https://github.com/autowarefoundation/autoware_msgs.git
+    version: main
+  msgs/autoware_adapi_msgs:
+    type: git
+    url: https://github.com/autowarefoundation/autoware_adapi_msgs.git
+    version: main
+  msg/autoware_internal_msgs:
+    type: git
+    url: https://github.com/autowarefoundation/autoware_internal_msgs.git
+    version: main
+  universe/tier4_autoware_msgs:
+    type: git
+    url: https://github.com/tier4/tier4_autoware_msgs.git
+    version: tier4/universe
+  universe/universe:
+    type: git
+    url: https://github.com/autowarefoundation/autoware.universe
+    version: main

slave/src/Api/tier4_ad_api_adaptor/setup.cfg

@@ -0,0 +1,15 @@
+[flake8]
+# Modified from https://github.com/ament/ament_lint/blob/ebd524bb9973d5ec1dc48a670ce54f958a5a0243/ament_flake8/ament_flake8/configuration/ament_flake8.ini
+extend-ignore = B902,C816,D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404,I202,CNL100,E203,E501,Q000
+import-order-style = pep8
+max-line-length = 100
+show-source = true
+statistics = true
+
+[isort]
+profile=black
+line_length=100
+force_sort_within_sections=true
+force_single_line=true
+reverse_relative=true
+known_third_party=launch

slave/src/Api/tier4_api_utils/CMakeLists.txt

@@ -0,0 +1,13 @@
+cmake_minimum_required(VERSION 3.14)
+project(tier4_api_utils)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+if(BUILD_TESTING)
+  include_directories(include)
+  ament_add_ros_isolated_gtest(${PROJECT_NAME}_test test/test.cpp)
+  ament_target_dependencies(${PROJECT_NAME}_test rclcpp tier4_external_api_msgs)
+endif()
+
+ament_auto_package()

slave/src/Api/tier4_api_utils/README.md

@@ -0,0 +1,4 @@
+# tier4_api_utils
+
+This is an old implementation of a class that logs when calling a service.
+Please use [component_interface_utils](../component_interface_utils/README.md) instead.

slave/src/Api/tier4_api_utils/include/tier4_api_utils/rclcpp/client.hpp

@@ -0,0 +1,70 @@
+// Copyright 2021 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.
+
+#ifndef TIER4_API_UTILS__RCLCPP__CLIENT_HPP_
+#define TIER4_API_UTILS__RCLCPP__CLIENT_HPP_
+
+#include "rclcpp/client.hpp"
+#include "tier4_api_utils/types/response.hpp"
+
+#include <chrono>
+#include <utility>
+
+namespace tier4_api_utils
+{
+template <typename ServiceT>
+class Client
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(Client)
+
+  using ResponseStatus = tier4_external_api_msgs::msg::ResponseStatus;
+  using AutowareServiceResult = std::pair<ResponseStatus, typename ServiceT::Response::SharedPtr>;
+
+  Client(typename rclcpp::Client<ServiceT>::SharedPtr client, const rclcpp::Logger & logger)
+  : client_(client), logger_(logger)
+  {
+  }
+
+  AutowareServiceResult call(
+    const typename ServiceT::Request::SharedPtr & request,
+    const std::chrono::nanoseconds & timeout = std::chrono::seconds(2))
+  {
+    RCLCPP_DEBUG(logger_, "client request");
+
+    if (!client_->service_is_ready()) {
+      RCLCPP_DEBUG(logger_, "client available");
+      return {response_error("Internal service is not available."), nullptr};
+    }
+
+    auto future = client_->async_send_request(request);
+    if (future.wait_for(timeout) != std::future_status::ready) {
+      RCLCPP_DEBUG(logger_, "client timeout");
+      return {response_error("Internal service has timed out."), nullptr};
+    }
+
+    RCLCPP_DEBUG(logger_, "client response");
+    return {response_success(), future.get()};
+  }
+
+private:
+  RCLCPP_DISABLE_COPY(Client)
+
+  typename rclcpp::Client<ServiceT>::SharedPtr client_;
+  rclcpp::Logger logger_;
+};
+
+}  // namespace tier4_api_utils
+
+#endif  // TIER4_API_UTILS__RCLCPP__CLIENT_HPP_

slave/src/Api/tier4_api_utils/include/tier4_api_utils/rclcpp/proxy.hpp

@@ -0,0 +1,63 @@
+// Copyright 2021 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.
+
+#ifndef TIER4_API_UTILS__RCLCPP__PROXY_HPP_
+#define TIER4_API_UTILS__RCLCPP__PROXY_HPP_
+
+#include "rclcpp/rclcpp.hpp"
+#include "tier4_api_utils/rclcpp/client.hpp"
+#include "tier4_api_utils/rclcpp/service.hpp"
+
+#include <string>
+#include <utility>
+
+namespace tier4_api_utils
+{
+template <class NodeT>
+class ServiceProxyNodeInterface
+{
+public:
+  // Use a raw pointer because shared_from_this cannot be used in constructor.
+  explicit ServiceProxyNodeInterface(NodeT * node) { node_ = node; }
+
+  template <typename ServiceT, typename CallbackT>
+  typename Service<ServiceT>::SharedPtr create_service(
+    const std::string & service_name, CallbackT && callback,
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default,
+    rclcpp::CallbackGroup::SharedPtr group = nullptr)
+  {
+    auto wrapped_callback = Service<ServiceT>::template wrap<CallbackT>(
+      std::forward<CallbackT>(callback), node_->get_logger());
+    return Service<ServiceT>::make_shared(node_->template create_service<ServiceT>(
+      service_name, std::move(wrapped_callback), qos_profile, group));
+  }
+
+  template <typename ServiceT>
+  typename Client<ServiceT>::SharedPtr create_client(
+    const std::string & service_name,
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default,
+    rclcpp::CallbackGroup::SharedPtr group = nullptr)
+  {
+    return Client<ServiceT>::make_shared(
+      node_->template create_client<ServiceT>(service_name, qos_profile, group),
+      node_->get_logger());
+  }
+
+private:
+  NodeT * node_;
+};
+
+}  // namespace tier4_api_utils
+
+#endif  // TIER4_API_UTILS__RCLCPP__PROXY_HPP_

slave/src/Api/tier4_api_utils/include/tier4_api_utils/rclcpp/service.hpp

@@ -0,0 +1,51 @@
+// Copyright 2021 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.
+
+#ifndef TIER4_API_UTILS__RCLCPP__SERVICE_HPP_
+#define TIER4_API_UTILS__RCLCPP__SERVICE_HPP_
+
+#include "rclcpp/service.hpp"
+
+namespace tier4_api_utils
+{
+template <typename ServiceT>
+class Service
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(Service)
+
+  explicit Service(typename rclcpp::Service<ServiceT>::SharedPtr service) : service_(service) {}
+
+  template <typename CallbackT>
+  static auto wrap(CallbackT && callback, const rclcpp::Logger & logger)
+  {
+    auto wrapped_callback = [logger, callback](
+                              typename ServiceT::Request::SharedPtr request,
+                              typename ServiceT::Response::SharedPtr response) {
+      RCLCPP_INFO(logger, "service request");
+      callback(request, response);
+      RCLCPP_INFO(logger, "service response");
+    };
+    return wrapped_callback;
+  }
+
+private:
+  RCLCPP_DISABLE_COPY(Service)
+
+  typename rclcpp::Service<ServiceT>::SharedPtr service_;
+};
+
+}  // namespace tier4_api_utils
+
+#endif  // TIER4_API_UTILS__RCLCPP__SERVICE_HPP_

slave/src/Api/tier4_api_utils/include/tier4_api_utils/tier4_api_utils.hpp

@@ -0,0 +1,21 @@
+// Copyright 2021 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.
+
+#ifndef TIER4_API_UTILS__TIER4_API_UTILS_HPP_
+#define TIER4_API_UTILS__TIER4_API_UTILS_HPP_
+
+#include "tier4_api_utils/rclcpp/proxy.hpp"
+#include "tier4_api_utils/types/response.hpp"
+
+#endif  // TIER4_API_UTILS__TIER4_API_UTILS_HPP_

slave/src/Api/tier4_api_utils/include/tier4_api_utils/types/response.hpp

@@ -0,0 +1,78 @@
+// Copyright 2021 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.
+
+#ifndef TIER4_API_UTILS__TYPES__RESPONSE_HPP_
+#define TIER4_API_UTILS__TYPES__RESPONSE_HPP_
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "tier4_external_api_msgs/msg/response_status.hpp"
+
+#include <string>
+
+namespace tier4_api_utils
+{
+using ResponseStatus = tier4_external_api_msgs::msg::ResponseStatus;
+
+inline bool is_success(const tier4_external_api_msgs::msg::ResponseStatus & status)
+{
+  return status.code == tier4_external_api_msgs::msg::ResponseStatus::SUCCESS;
+}
+
+inline bool is_ignored(const tier4_external_api_msgs::msg::ResponseStatus & status)
+{
+  return status.code == tier4_external_api_msgs::msg::ResponseStatus::IGNORED;
+}
+
+inline bool is_warn(const tier4_external_api_msgs::msg::ResponseStatus & status)
+{
+  return status.code == tier4_external_api_msgs::msg::ResponseStatus::WARN;
+}
+
+inline bool is_error(const tier4_external_api_msgs::msg::ResponseStatus & status)
+{
+  return status.code == tier4_external_api_msgs::msg::ResponseStatus::ERROR;
+}
+
+inline ResponseStatus response_success(const std::string & message = "")
+{
+  return tier4_external_api_msgs::build<tier4_external_api_msgs::msg::ResponseStatus>()
+    .code(tier4_external_api_msgs::msg::ResponseStatus::SUCCESS)
+    .message(message);
+}
+
+inline ResponseStatus response_ignored(const std::string & message = "")
+{
+  return tier4_external_api_msgs::build<tier4_external_api_msgs::msg::ResponseStatus>()
+    .code(tier4_external_api_msgs::msg::ResponseStatus::IGNORED)
+    .message(message);
+}
+
+inline ResponseStatus response_warn(const std::string & message = "")
+{
+  return tier4_external_api_msgs::build<tier4_external_api_msgs::msg::ResponseStatus>()
+    .code(tier4_external_api_msgs::msg::ResponseStatus::WARN)
+    .message(message);
+}
+
+inline ResponseStatus response_error(const std::string & message = "")
+{
+  return tier4_external_api_msgs::build<tier4_external_api_msgs::msg::ResponseStatus>()
+    .code(tier4_external_api_msgs::msg::ResponseStatus::ERROR)
+    .message(message);
+}
+
+}  // namespace tier4_api_utils
+
+#endif  // TIER4_API_UTILS__TYPES__RESPONSE_HPP_

slave/src/Api/tier4_api_utils/package.xml

@@ -0,0 +1,27 @@
+<?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>tier4_api_utils</name>
+  <version>0.0.0</version>
+  <description>The tier4_api_utils package</description>
+  <maintainer email="isamu.takagi@tier4.jp">Takagi, Isamu</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>tier4_external_api_msgs</depend>
+
+  <test_depend>ament_cmake_ros</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+  <test_depend>rclcpp</test_depend>
+  <test_depend>tier4_external_api_msgs</test_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

slave/src/Api/tier4_api_utils/test/test.cpp

@@ -0,0 +1,32 @@
+// Copyright 2021 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.
+
+#include "gtest/gtest.h"
+#include "rclcpp/rclcpp.hpp"
+#include "tier4_api_utils/tier4_api_utils.hpp"
+
+TEST(tier4_api_utils, instantiate)
+{
+  rclcpp::Node node("tier4_api_utils_test");
+  tier4_api_utils::ServiceProxyNodeInterface proxy(&node);
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  rclcpp::init(argc, argv);
+  bool result = RUN_ALL_TESTS();
+  rclcpp::shutdown();
+  return result;
+}

slave/src/Control/autoware_autonomous_emergency_braking/CMakeLists.txt

@@ -0,0 +1,44 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_autonomous_emergency_braking)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+find_package(PCL REQUIRED)
+
+include_directories(
+  include
+  SYSTEM
+  ${PCL_INCLUDE_DIRS}
+)
+
+ament_auto_add_library(autoware_autonomous_emergency_braking_helpers SHARED
+  include/autoware/autonomous_emergency_braking/utils.hpp
+  src/utils.cpp
+)
+
+set(AEB_NODE ${PROJECT_NAME}_node)
+ament_auto_add_library(${AEB_NODE} SHARED
+  include/autoware/autonomous_emergency_braking/node.hpp
+  src/node.cpp
+)
+
+target_link_libraries(${AEB_NODE} autoware_autonomous_emergency_braking_helpers)
+rclcpp_components_register_node(${AEB_NODE}
+  PLUGIN "autoware::motion::control::autonomous_emergency_braking::AEB"
+  EXECUTABLE ${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  ament_add_ros_isolated_gtest(test_aeb
+  test/test.cpp)
+
+target_link_libraries(test_aeb ${AEB_NODE})
+
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  launch
+  config
+)

slave/src/Control/autoware_autonomous_emergency_braking/README.md

@@ -0,0 +1,240 @@
+# Autonomous Emergency Braking (AEB)
+
+## Purpose / Role
+
+`autonomous_emergency_braking` is a module that prevents collisions with obstacles on the predicted path created by a control module or sensor values estimated from the control module.
+
+### Assumptions
+
+This module has following assumptions.
+
+- The predicted path of the ego vehicle can be made from either the path created from sensors or the path created from a control module, or both.
+
+- The current speed and angular velocity can be obtained from the sensors of the ego vehicle, and it uses points as obstacles.
+
+- The AEBs target obstacles are 2D points that can be obtained from the input point cloud or by obtaining the intersection points between the predicted ego footprint path and a predicted object's shape.
+
+### IMU path generation: steering angle vs IMU's angular velocity
+
+Currently, the IMU-based path is generated using the angular velocity obtained by the IMU itself. It has been suggested that the steering angle could be used instead onf the angular velocity.
+
+The pros and cons of both approaches are:
+
+IMU angular velocity:
+
+- (+) Usually, it has high accuracy
+- (-) Vehicle vibration might introduce noise.
+
+Steering angle:
+
+- (+) Not so noisy
+- (-) May have a steering offset or a wrong gear ratio, and the steering angle of Autoware and the real steering may not be the same.
+
+For the moment, there are no plans to implement the steering angle on the path creation process of the AEB module.
+
+## Inner-workings / Algorithms
+
+AEB has the following steps before it outputs the emergency stop signal.
+
+1. Activate AEB if necessary.
+
+2. Generate a predicted path of the ego vehicle.
+
+3. Get target obstacles from the input point cloud and/or predicted object data.
+
+4. Estimate the closest obstacle speed.
+
+5. Collision check with target obstacles.
+
+6. Send emergency stop signals to `/diagnostics`.
+
+We give more details of each section below.
+
+### 1. Activate AEB if necessary
+
+We do not activate AEB module if it satisfies the following conditions.
+
+- Ego vehicle is not in autonomous driving state
+
+- When the ego vehicle is not moving (Current Velocity is below a 0.1 m/s threshold)
+
+### 2. Generate a predicted path of the ego vehicle
+
+AEB generates a predicted footprint path based on current velocity and current angular velocity obtained from attached sensors. Note that if `use_imu_path` is `false`, it skips this step. This predicted path is generated as:
+
+$$
+x_{k+1} = x_k + v cos(\theta_k) dt \\
+y_{k+1} = y_k + v sin(\theta_k) dt \\
+\theta_{k+1} = \theta_k + \omega dt
+$$
+
+where $v$ and $\omega$ are current longitudinal velocity and angular velocity respectively. $dt$ is time interval that users can define in advance.
+
+On the other hand, if `use_predicted_trajectory` is set to true, the AEB module will use the predicted path from the MPC as a base to generate a footprint path. Both the IMU footprint path and the MPC footprint path can be used at the same time.
+
+### 3. Get target obstacles
+
+After generating the ego footprint path(s), the target obstacles are identified. There are two methods to find target obstacles: using the input point cloud, or using the predicted object information coming from perception modules.
+
+#### Pointcloud obstacle filtering
+
+The AEB module can filter the input pointcloud to find target obstacles with which the ego vehicle might collide. This method can be enable if the `use_pointcloud_data` parameter is set to true. The pointcloud obstacle filtering has three major steps, which are rough filtering, noise filtering with clustering and rigorous filtering.
+
+##### Rough filtering
+
+In rough filtering step, we select target obstacle with simple filter. Create a search area up to a certain distance (default is half of the ego vehicle width plus the `path_footprint_extra_margin` parameter) away from the predicted path of the ego vehicle and ignore the point cloud that are not within it. The rough filtering step is illustrated below.
+
+![rough_filtering](./image/obstacle_filtering_1.drawio.svg)
+
+##### Noise filtering with clustering and convex hulls
+
+To prevent the AEB from considering noisy points, euclidean clustering is performed on the filtered point cloud. The points in the point cloud that are not close enough to other points to form a cluster are discarded. Furthermore, each point in a cluster is compared against the `cluster_minimum_height` parameter, if no point inside a cluster has a height/z value greater than `cluster_minimum_height`, the whole cluster of points is discarded. The parameters `cluster_tolerance`, `minimum_cluster_size` and `maximum_cluster_size` can be used to tune the clustering and the size of objects to be ignored, for more information about the clustering method used by the AEB module, please check the official documentation on euclidean clustering of the PCL library: <https://pcl.readthedocs.io/projects/tutorials/en/master/cluster_extraction.html>.
+
+Furthermore, a 2D convex hull is created around each detected cluster, the vertices of each hull represent the most extreme/outside points of the cluster. These vertices are then checked in the next step.
+
+##### Rigorous filtering
+
+After Noise filtering, the module performs a geometric collision check to determine whether the filtered obstacles/hull vertices actually have possibility to collide with the ego vehicle. In this check, the ego vehicle is represented as a rectangle, and the point cloud obstacles are represented as points. Only the vertices with a possibility of collision are kept.
+
+![rigorous_filtering](./image/obstacle_filtering_2.drawio.svg)
+
+#### Using predicted objects to get target obstacles
+
+If the `use_predicted_object_data` parameter is set to true, the AEB can use predicted object data coming from the perception modules, to get target obstacle points. This is done by obtaining the 2D intersection points between the ego's predicted footprint path and each of the predicted objects enveloping polygon or bounding box.
+
+![predicted_object_and_path_intersection](./image/using-predicted-objects.drawio.svg)
+
+### Finding the closest target obstacle
+
+Once all target obstacles have been identified, the AEB module chooses the point that is closest to the ego vehicle as the candidate for collision checking. Only the closest point is considered because RSS distance is used to judge if a collision will happen or not, and if the closest vertex to the ego is deemed to be safe from collision, the rest of the target obstacles will also be safe.
+
+![closest_object](./image/closest-point.drawio.svg)
+
+### 4. Obstacle velocity estimation
+
+To begin calculating the target point's velocity, the point must enter the speed calculation area,
+which is defined by the `speed_calculation_expansion_margin` parameter.
+Depending on the operational environment,
+this margin can reduce unnecessary autonomous emergency braking
+caused by velocity miscalculations during the initial calculation steps.
+
+![speed_calculation_expansion](./image/speed_calculation_expansion.drawio.svg)
+
+Once the position of the closest obstacle/point is determined, the AEB modules uses the history of previously detected objects to estimate the closest object relative speed using the following equations:
+
+$$
+d_{t} = t_{1} - t_{0}
+$$
+
+$$
+d_{x} = norm(o_{x} - prev_{x})
+$$
+
+$$
+v_{norm} = d_{x} / d_{t}
+$$
+
+Where $t_{1}$ and $t_{0}$ are the timestamps of the point clouds used to detect the current closest object and the closest object of the previous point cloud frame, and $o_{x}$ and $prev_{x}$ are the positions of those objects, respectively.
+
+![relative_speed](./image/object_relative_speed.drawio.svg)
+
+Note that, when the closest obstacle/point comes from using predicted object data, $v_{norm}$ is calculated by directly computing the norm of the predicted object's velocity in the x and y axes.
+
+The velocity vector is then compared against the ego's predicted path to get the longitudinal velocity $v_{obj}$:
+
+$$
+v_{obj} = v_{norm} * Cos(yaw_{diff}) + v_{ego}
+$$
+
+where $yaw_{diff}$ is the difference in yaw between the ego path and the displacement vector $$v_{pos} = o_{pos} - prev_{pos} $$ and $v_{ego}$ is the ego's current speed, which accounts for the movement of points caused by the ego moving and not the object. All these equations are performed disregarding the z axis (in 2D).
+
+Note that, the object velocity is calculated against the ego's current movement direction. If the object moves in the opposite direction to the ego's movement, the object velocity will be negative, which will reduce the rss distance on the next step.
+
+The resulting estimated object speed is added to a queue of speeds with timestamps. The AEB then checks for expiration of past speed estimations and eliminates expired speed measurements from the queue, the object expiration is determined by checking if the time elapsed since the speed was first added to the queue is larger than the parameter `previous_obstacle_keep_time`. Finally, the median speed of the queue is calculated. The median speed will be used to calculate the RSS distance used for collision checking.
+
+### 5. Collision check with target obstacles using RSS distance
+
+In the fourth step, it checks the collision with the closest obstacle point using RSS distance. RSS distance is formulated as:
+
+$$
+d = v_{ego}*t_{response} + v_{ego}^2/(2*a_{min}) -(sign(v_{obj})) * v_{obj}^2/(2*a_{obj_{min}}) + offset
+$$
+
+where $v_{ego}$ and $v_{obj}$ is current ego and obstacle velocity, $a_{min}$ and $a_{obj_{min}}$ is ego and object minimum acceleration (maximum deceleration), $t_{response}$ is response time of the ego vehicle to start deceleration. Therefore the distance from the ego vehicle to the obstacle is smaller than this RSS distance $d$, the ego vehicle send emergency stop signals. This is illustrated in the following picture.
+
+![rss_check](./image/rss_check.drawio.svg)
+
+### 6. Send emergency stop signals to `/diagnostics`
+
+If AEB detects collision with point cloud obstacles in the previous step, it sends emergency signal to `/diagnostics` in this step. Note that in order to enable emergency stop, it has to send ERROR level emergency. Moreover, AEB user should modify the setting file to keep the emergency level, otherwise Autoware does not hold the emergency state.
+
+## Use cases
+
+### Front vehicle suddenly brakes
+
+The AEB can activate when a vehicle in front suddenly brakes, and a collision is detected by the AEB module. Provided the distance between the ego vehicle and the front vehicle is large enough and the ego’s emergency acceleration value is high enough, it is possible to avoid or soften collisions with vehicles in front that suddenly brake. NOTE: the acceleration used by the AEB to calculate rss_distance is NOT necessarily the acceleration used by the ego while doing an emergency brake. The acceleration used by the real vehicle can be tuned by changing the [mrm_emergency stop jerk and acceleration values](https://github.com/tier4/autoware_launch/blob/d1b2688f2788acab95bb9995d72efd7182e9006a/autoware_launch/config/system/mrm_emergency_stop_operator/mrm_emergency_stop_operator.param.yaml#L4).
+
+![front vehicle collision prevention](./image/front_vehicle_collision.drawio.svg)
+
+### Stop for objects that appear suddenly
+
+When an object appears suddenly, the AEB can act as a fail-safe to stop the ego vehicle when other modules fail to detect the object on time. If sudden object cut ins are expected, it might be useful for the AEB module to detect collisions of objects BEFORE they enter the real ego vehicle path by increasing the `expand_width` parameter.
+
+![occluded object collision prevention](./image/occluded_space.drawio.svg)
+
+### Preventing Collisions with rear objects
+
+The AEB module can also prevent collisions when the ego vehicle is moving backwards.
+
+![backward driving](./image/backward-driving.drawio.svg)
+
+### Preventing collisions in case of wrong Odometry (IMU path only)
+
+When vehicle odometry information is faulty, it is possible that the MPC fails to predict a correct path for the ego vehicle. If the MPC predicted path is wrong, collision avoidance will not work as intended on the planning modules. However, the AEB’s IMU path does not depend on the MPC and could be able to predict a collision when the other modules cannot. As an example you can see a figure of a hypothetical case in which the MPC path is wrong and only the AEB’s IMU path detects a collision.
+
+![wrong mpc](./image/wrong-mpc.drawio.svg)
+
+## Parameters
+
+| Name                               | Unit   | Type   | Description                                                                                                                                                                                     | Default value |
+| :--------------------------------- | :----- | :----- | :---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
+| publish_debug_markers              | [-]    | bool   | flag to publish debug markers                                                                                                                                                                   | true          |
+| publish_debug_pointcloud           | [-]    | bool   | flag to publish the point cloud used for debugging                                                                                                                                              | false         |
+| use_predicted_trajectory           | [-]    | bool   | flag to use the predicted path from the control module                                                                                                                                          | true          |
+| use_imu_path                       | [-]    | bool   | flag to use the predicted path generated by sensor data                                                                                                                                         | true          |
+| use_object_velocity_calculation    | [-]    | bool   | flag to use the object velocity calculation. If set to false, object velocity is set to 0 [m/s]                                                                                                 | true          |
+| check_autoware_state               | [-]    | bool   | flag to enable or disable autoware state check. If set to false, the AEB module will run even when the ego vehicle is not in AUTONOMOUS state.                                                  | true          |
+| detection_range_min_height         | [m]    | double | minimum hight of detection range used for avoiding the ghost brake by false positive point clouds                                                                                               | 0.0           |
+| detection_range_max_height_margin  | [m]    | double | margin for maximum hight of detection range used for avoiding the ghost brake by false positive point clouds. `detection_range_max_height = vehicle_height + detection_range_max_height_margin` | 0.0           |
+| voxel_grid_x                       | [m]    | double | down sampling parameters of x-axis for voxel grid filter                                                                                                                                        | 0.05          |
+| voxel_grid_y                       | [m]    | double | down sampling parameters of y-axis for voxel grid filter                                                                                                                                        | 0.05          |
+| voxel_grid_z                       | [m]    | double | down sampling parameters of z-axis for voxel grid filter                                                                                                                                        | 100000.0      |
+| cluster tolerance                  | [m]    | double | maximum allowable distance between any two points to be considered part of the same cluster                                                                                                     | 0.15          |
+| cluster_minimum_height             | [m]    | double | at least one point in a cluster must be higher than this value for the cluster to be included in the set of possible collision targets                                                          | 0.1           |
+| minimum_cluster_size               | [-]    | int    | minimum required amount of points contained by a cluster for it to be considered as a possible target obstacle                                                                                  | 10            |
+| maximum_cluster_size               | [-]    | int    | maximum amount of points contained by a cluster for it to be considered as a possible target obstacle                                                                                           | 10000         |
+| min_generated_imu_path_length      | [m]    | double | minimum distance for a predicted path generated by sensors                                                                                                                                      | 0.5           |
+| max_generated_imu_path_length      | [m]    | double | maximum distance for a predicted path generated by sensors                                                                                                                                      | 10.0          |
+| expand_width                       | [m]    | double | expansion width of the ego vehicle for the collision check                                                                                                                                      | 0.1           |
+| longitudinal_offset                | [m]    | double | longitudinal offset distance for collision check                                                                                                                                                | 2.0           |
+| t_response                         | [s]    | double | response time for the ego to detect the front vehicle starting deceleration                                                                                                                     | 1.0           |
+| a_ego_min                          | [m/ss] | double | maximum deceleration value of the ego vehicle                                                                                                                                                   | -3.0          |
+| a_obj_min                          | [m/ss] | double | maximum deceleration value of objects                                                                                                                                                           | -3.0          |
+| imu_prediction_time_horizon        | [s]    | double | time horizon of the predicted path generated by sensors                                                                                                                                         | 1.5           |
+| imu_prediction_time_interval       | [s]    | double | time interval of the predicted path generated by sensors                                                                                                                                        | 0.1           |
+| mpc_prediction_time_horizon        | [s]    | double | time horizon of the predicted path generated by mpc                                                                                                                                             | 1.5           |
+| mpc_prediction_time_interval       | [s]    | double | time interval of the predicted path generated by mpc                                                                                                                                            | 0.1           |
+| aeb_hz                             | [-]    | double | frequency at which AEB operates per second                                                                                                                                                      | 10            |
+| speed_calculation_expansion_margin | [m]    | double | expansion width of the ego vehicle for the beginning speed calculation                                                                                                                          | 0.1           |
+
+## Limitations
+
+- The distance required to stop after collision detection depends on the ego vehicle's speed and deceleration performance. To avoid collisions, it's necessary to increase the detection distance and set a higher deceleration rate. However, this creates a trade-off as it may also increase the number of unnecessary activations. Therefore, it's essential to consider what role this module should play and adjust the parameters accordingly.
+
+- AEB might not be able to react with obstacles that are close to the ground. It depends on the performance of the pre-processing methods applied to the point cloud.
+
+- Longitudinal acceleration information obtained from sensors is not used due to the high amount of noise.
+
+- The accuracy of the predicted path created from sensor data depends on the accuracy of sensors attached to the ego vehicle.
+
+![aeb_range](./image/range.drawio.svg)

slave/src/Control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml

@@ -0,0 +1,46 @@
+/**:
+  ros__parameters:
+    # Ego path calculation
+    use_predicted_trajectory: true
+    use_imu_path: true
+    use_pointcloud_data: true
+    use_predicted_object_data: false
+    use_object_velocity_calculation: true
+    check_autoware_state: true
+    min_generated_imu_path_length: 0.5
+    max_generated_imu_path_length: 10.0
+    imu_prediction_time_horizon: 1.5
+    imu_prediction_time_interval: 0.1
+    mpc_prediction_time_horizon: 4.5
+    mpc_prediction_time_interval: 0.1
+
+    # Debug
+    publish_debug_pointcloud: false
+    publish_debug_markers: true
+
+    # Point cloud partitioning
+    detection_range_min_height: 0.0
+    detection_range_max_height_margin: 0.0
+    voxel_grid_x: 0.05
+    voxel_grid_y: 0.05
+    voxel_grid_z: 100000.0
+
+    # Point cloud cropping
+    expand_width: 0.1
+    path_footprint_extra_margin: 4.0
+    speed_calculation_expansion_margin: 0.5
+
+    # Point cloud clustering
+    cluster_tolerance: 0.15 #[m]
+    cluster_minimum_height: 0.1
+    minimum_cluster_size: 10
+    maximum_cluster_size: 10000
+
+    # RSS distance collision check
+    longitudinal_offset: 2.0
+    t_response: 1.0
+    a_ego_min: -3.0
+    a_obj_min: -1.0
+    collision_keeping_sec: 2.0
+    previous_obstacle_keep_time: 1.0
+    aeb_hz: 10.0

slave/src/Control/autoware_autonomous_emergency_braking/image/range.drawio.svg

@@ -0,0 +1,54 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!-- Do not edit this file with editors other than diagrams.net -->
+<!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
+<svg
+  xmlns="http://www.w3.org/2000/svg"
+  xmlns:xlink="http://www.w3.org/1999/xlink"
+  version="1.1"
+  width="463px"
+  height="116px"
+  viewBox="-0.5 -0.5 463 116"
+  content="&lt;mxfile host=&quot;app.diagrams.net&quot; modified=&quot;2023-03-24T08:29:01.963Z&quot; agent=&quot;5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/111.0.0.0 Safari/537.36&quot; version=&quot;20.7.3&quot; etag=&quot;Gm04SZSu1ST8FtVu6jWx&quot; type=&quot;google&quot;&gt;&lt;diagram id=&quot;DgYDBBnKMwtj5Ut0tRwE&quot; name=&quot;range&quot;&gt;1Vhdb5swFP01eewEtknKY5s22x6mdaumaX1zwAFrBjPjNGl//WwwHwbSZFHImkQK5ti+mHMO15dM4DzZfhQ4i7/wkLAJcMLtBN5NAHARROqgkZcSuXZACUSChmZQAzzSV2JAx6BrGpLcGig5Z5JmNhjwNCWBtDAsBN/Yw1ac2VfNcER6wGOAWR/9SUMZm7sAswb/RGgUV1d2p37Zk+BqsLmTPMYh37QgeD+Bc8G5LFvJdk6YJq/ipZy32NFbL0yQVB4y4eE7+kyWv35s0sXyW77yVk+UXRkxnjFbmxs2i5UvFQOCr9OQ6CDOBN5uYirJY4YD3btRmisslglTZ65qrihjc864KOZCp/go3FyGCEm2O9fv1qwoOxGeECle1BAzwffKGcZIcGp43TSyuNBgcUuSCsPGCVEduSFLNQxf/8Cdu587koY32oTqLOUpsbmyic2l4L9rj6GaMxL2DLqXsRYl3gAjFSYIw5I+2+GHaDJXeOBUXbgW5NqxBEFOh+icr0VAzKS2MTtx0L5AEouIyF6gQrT6ro/XcXaAjoyphEP2+x/nWZmFVnSrpR3zgXBnNm9DTwQakB+N9UD4l0qk+86IdNF+JtV2kummVNSRV67j3WZEULUCItr4QwPuz96Ka7MHv5XN83IIHCmzg4HMDgb4n43G//W42+Ki+JzIvNOOeWGfPeCdc1s8Zxo4KZNut8LwekyetcCoPH95TM7eG5Mj17knJQ90yPvfDzSAB9jw2EIXKiTEeVx0u3V3i1t0p78XUA+DrnAIffDsIMeWxBB1Ao1cEoMDCpCW5AHDeU6DQj0sZB/eaYa3pW8KDssz4ALMgFBHQf/Yt6NuIO+8b0eVqS0rTJnUSZAXC208Mf2z5lXHVVkk3qgBrp9tC8mqftWK9PHrUvklYCTXi0jVj4ypbgucKl2BkxSyACfl+nepIbxk+iB5IRFWO5pZjLq5cj1l6J5dVUKWthNtu5m01U7sBsKMRqm2s3JWUUXr9E4DzG5MR0LDkO3agm2/n2B/qB1qDAH8gf3BP83rijpt/qAqHdX8zQfv/wI=&lt;/diagram&gt;&lt;/mxfile&gt;"
+  style="background-color: rgb(255, 255, 255);"
+>
+  <defs/>
+  <g>
+    <rect x="17" y="73" width="130" height="30" fill="#000000" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <path d="M 2 113 L 402 113" fill="none" stroke="rgb(0, 0, 0)" stroke-width="4" stroke-miterlimit="10" pointer-events="stroke"/>
+    <ellipse cx="112" cy="93" rx="20" ry="20" fill="#000000" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <ellipse cx="52" cy="93" rx="20" ry="20" fill="#000000" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <path d="M 17 103 L 47 33 L 107 33 L 137 103 Z" fill="#000000" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/>
+    <rect x="82" y="43" width="25" height="30" fill="#ffffff" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <ellipse cx="52" cy="93" rx="15" ry="15" fill="#ffffff" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <ellipse cx="112" cy="93" rx="15" ry="15" fill="#ffffff" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="47" y="43" width="25" height="30" fill="#ffffff" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <path d="M 147 57.5 L 402 58" fill="none" stroke="#4d4d4d" stroke-width="3" stroke-miterlimit="10" stroke-dasharray="9 9" pointer-events="stroke"/>
+    <path d="M 362 94.76 L 362 71.24" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/>
+    <path d="M 362 100.76 L 358 92.76 L 362 94.76 L 366 92.76 Z" fill="#000000" stroke="#000000" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 362 65.24 L 366 73.24 L 362 71.24 L 358 73.24 Z" fill="#000000" stroke="#000000" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <rect x="172" y="3" width="290" height="40" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 288px; height: 1px; padding-top: 23px; margin-left: 173px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
+                <font style="font-size: 19px;">Obstacles in this range might not be able to react</font>
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="317" y="27" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">Obstacles in this range might not be able to rea...</text>
+      </switch>
+    </g>
+  </g>
+  <switch>
+    <g requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"/>
+    <a transform="translate(0,-5)" xlink:href="https://www.diagrams.net/doc/faq/svg-export-text-problems" target="_blank">
+      <text text-anchor="middle" font-size="10px" x="50%" y="100%">Text is not SVG - cannot display</text>
+    </a>
+  </switch>
+</svg>

slave/src/Control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp

@@ -0,0 +1,586 @@
+// Copyright 2023 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.
+
+#ifndef AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_
+#define AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_
+
+#include "autoware/universe_utils/system/time_keeper.hpp"
+
+#include <autoware/motion_utils/trajectory/trajectory.hpp>
+#include <autoware/universe_utils/geometry/geometry.hpp>
+#include <autoware/universe_utils/ros/polling_subscriber.hpp>
+#include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
+#include <diagnostic_updater/diagnostic_updater.hpp>
+#include <pcl_ros/transforms.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_perception_msgs/msg/predicted_objects.hpp>
+#include <autoware_planning_msgs/msg/trajectory.hpp>
+#include <autoware_system_msgs/msg/autoware_state.hpp>
+#include <autoware_vehicle_msgs/msg/velocity_report.hpp>
+#include <geometry_msgs/msg/vector3.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <tier4_debug_msgs/msg/float32_stamped.hpp>
+#include <visualization_msgs/msg/marker.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+#include <boost/optional.hpp>
+
+#include <pcl/common/transforms.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/surface/convex_hull.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <deque>
+#include <limits>
+#include <memory>
+#include <optional>
+#include <string>
+#include <tuple>
+#include <utility>
+#include <vector>
+namespace autoware::motion::control::autonomous_emergency_braking
+{
+
+using autoware_planning_msgs::msg::Trajectory;
+using autoware_system_msgs::msg::AutowareState;
+using autoware_vehicle_msgs::msg::VelocityReport;
+using nav_msgs::msg::Odometry;
+using sensor_msgs::msg::Imu;
+using sensor_msgs::msg::PointCloud2;
+using PointCloud = pcl::PointCloud<pcl::PointXYZ>;
+using autoware::universe_utils::Polygon2d;
+using autoware::universe_utils::Polygon3d;
+using autoware::vehicle_info_utils::VehicleInfo;
+using diagnostic_updater::DiagnosticStatusWrapper;
+using diagnostic_updater::Updater;
+using visualization_msgs::msg::Marker;
+using visualization_msgs::msg::MarkerArray;
+using Path = std::vector<geometry_msgs::msg::Pose>;
+using Vector3 = geometry_msgs::msg::Vector3;
+using autoware_perception_msgs::msg::PredictedObject;
+using autoware_perception_msgs::msg::PredictedObjects;
+using colorTuple = std::tuple<double, double, double, double>;
+
+/**
+ * @brief Struct to store object data
+ */
+struct ObjectData
+{
+  rclcpp::Time stamp;
+  geometry_msgs::msg::Point position;
+  double velocity{0.0};
+  double rss{0.0};
+  double distance_to_object{0.0};
+  bool is_target{true};
+};
+
+/**
+ * @brief Class to manage collision data
+ */
+class CollisionDataKeeper
+{
+public:
+  /**
+   * @brief Constructor for CollisionDataKeeper
+   * @param clock Shared pointer to the clock
+   */
+  explicit CollisionDataKeeper(rclcpp::Clock::SharedPtr clock) { clock_ = clock; }
+
+  /**
+   * @brief Set timeout values for collision and obstacle data
+   * @param collision_keep_time Time to keep collision data
+   * @param previous_obstacle_keep_time Time to keep previous obstacle data
+   */
+  void setTimeout(const double collision_keep_time, const double previous_obstacle_keep_time)
+  {
+    collision_keep_time_ = collision_keep_time;
+    previous_obstacle_keep_time_ = previous_obstacle_keep_time;
+  }
+
+  /**
+   * @brief Get timeout values for collision and obstacle data
+   * @return Pair of collision and obstacle data timeout values
+   */
+  std::pair<double, double> getTimeout()
+  {
+    return {collision_keep_time_, previous_obstacle_keep_time_};
+  }
+
+  /**
+   * @brief Check if object data has expired
+   * @param data Optional reference to the object data
+   * @param timeout Timeout value to check against
+   * @return True if object data has expired, false otherwise
+   */
+  bool checkObjectDataExpired(std::optional<ObjectData> & data, const double timeout)
+  {
+    if (!data.has_value()) return true;
+    const auto now = clock_->now();
+    const auto & prev_obj = data.value();
+    const auto & data_time_stamp = prev_obj.stamp;
+    if ((now - data_time_stamp).nanoseconds() * 1e-9 > timeout) {
+      data = std::nullopt;
+      return true;
+    }
+    return false;
+  }
+
+  /**
+   * @brief Check if collision data has expired
+   * @return True if collision data has expired, false otherwise
+   */
+  bool checkCollisionExpired()
+  {
+    return this->checkObjectDataExpired(closest_object_, collision_keep_time_);
+  }
+
+  /**
+   * @brief Check if previous object data has expired
+   * @return True if previous object data has expired, false otherwise
+   */
+  bool checkPreviousObjectDataExpired()
+  {
+    return this->checkObjectDataExpired(prev_closest_object_, previous_obstacle_keep_time_);
+  }
+
+  /**
+   * @brief Get the closest object data
+   * @return Object data of the closest object
+   */
+  [[nodiscard]] std::optional<ObjectData> get() const { return closest_object_; }
+
+  /**
+   * @brief Get the previous closest object data
+   * @return Object data of the previous closest object
+   */
+  [[nodiscard]] ObjectData getPreviousObjectData() const
+  {
+    return (prev_closest_object_.has_value()) ? prev_closest_object_.value() : ObjectData();
+  }
+
+  /**
+   * @brief Set collision data
+   * @param data Object data to set
+   */
+  void setCollisionData(const ObjectData & data)
+  {
+    closest_object_ = std::make_optional<ObjectData>(data);
+  }
+
+  /**
+   * @brief Set previous object data
+   * @param data Object data to set
+   */
+  void setPreviousObjectData(const ObjectData & data)
+  {
+    prev_closest_object_ = std::make_optional<ObjectData>(data);
+  }
+
+  /**
+   * @brief Reset the obstacle velocity history
+   */
+  void resetVelocityHistory() { obstacle_velocity_history_.clear(); }
+
+  /**
+   * @brief Update the velocity history with current object velocity
+   * @param current_object_velocity Current object velocity
+   * @param current_object_velocity_time_stamp Timestamp of the current object velocity
+   */
+  void updateVelocityHistory(
+    const double current_object_velocity, const rclcpp::Time & current_object_velocity_time_stamp)
+  {
+    // remove old msg from deque
+    const auto now = clock_->now();
+    obstacle_velocity_history_.erase(
+      std::remove_if(
+        obstacle_velocity_history_.begin(), obstacle_velocity_history_.end(),
+        [&](const auto & velocity_time_pair) {
+          const auto & vel_time = velocity_time_pair.second;
+          return ((now - vel_time).nanoseconds() * 1e-9 > previous_obstacle_keep_time_);
+        }),
+      obstacle_velocity_history_.end());
+    obstacle_velocity_history_.emplace_back(
+      current_object_velocity, current_object_velocity_time_stamp);
+  }
+
+  /**
+   * @brief Get the median obstacle velocity from history
+   * @return Optional median obstacle velocity
+   */
+  [[nodiscard]] std::optional<double> getMedianObstacleVelocity() const
+  {
+    if (obstacle_velocity_history_.empty()) return std::nullopt;
+    std::vector<double> raw_velocities;
+    raw_velocities.reserve(obstacle_velocity_history_.size());
+    for (const auto & vel_time_pair : obstacle_velocity_history_) {
+      raw_velocities.emplace_back(vel_time_pair.first);
+    }
+
+    const size_t med1 = (raw_velocities.size() % 2 == 0) ? (raw_velocities.size()) / 2 - 1
+                                                         : (raw_velocities.size()) / 2;
+    const size_t med2 = (raw_velocities.size()) / 2;
+    std::nth_element(raw_velocities.begin(), raw_velocities.begin() + med1, raw_velocities.end());
+    const double vel1 = raw_velocities.at(med1);
+    std::nth_element(raw_velocities.begin(), raw_velocities.begin() + med2, raw_velocities.end());
+    const double vel2 = raw_velocities.at(med2);
+    return (vel1 + vel2) / 2.0;
+  }
+
+  /**
+   * @brief Calculate object speed from velocity history
+   * @param closest_object Closest object data
+   * @param path Ego vehicle path
+   * @param current_ego_speed Current ego vehicle speed
+   * @return Optional calculated object speed
+   */
+  std::optional<double> calcObjectSpeedFromHistory(
+    const ObjectData & closest_object, const Path & path, const double current_ego_speed)
+  {
+    // in case the object comes from predicted objects info, we reuse the speed.
+    if (std::abs(closest_object.velocity) > std::numeric_limits<double>::epsilon()) {
+      this->setPreviousObjectData(closest_object);
+      this->updateVelocityHistory(closest_object.velocity, closest_object.stamp);
+      return this->getMedianObstacleVelocity();
+    }
+
+    if (this->checkPreviousObjectDataExpired()) {
+      this->setPreviousObjectData(closest_object);
+      this->resetVelocityHistory();
+      return std::nullopt;
+    }
+
+    const auto estimated_velocity_opt = std::invoke([&]() -> std::optional<double> {
+      const auto & prev_object = this->getPreviousObjectData();
+      const double p_dt =
+        (closest_object.stamp.nanoseconds() - prev_object.stamp.nanoseconds()) * 1e-9;
+      if (p_dt < std::numeric_limits<double>::epsilon()) return std::nullopt;
+      const auto & nearest_collision_point = closest_object.position;
+      const auto & prev_collision_point = prev_object.position;
+
+      const double p_dx = nearest_collision_point.x - prev_collision_point.x;
+      const double p_dy = nearest_collision_point.y - prev_collision_point.y;
+      const double p_dist = std::hypot(p_dx, p_dy);
+      const double p_yaw = std::atan2(p_dy, p_dx);
+      const double p_vel = p_dist / p_dt;
+
+      const auto nearest_idx =
+        autoware::motion_utils::findNearestIndex(path, nearest_collision_point);
+      const auto & nearest_path_pose = path.at(nearest_idx);
+      // When the ego moves backwards, the direction of movement axis is reversed
+      const auto & traj_yaw = (current_ego_speed > 0.0)
+                                ? tf2::getYaw(nearest_path_pose.orientation)
+                                : tf2::getYaw(nearest_path_pose.orientation) + M_PI;
+      const auto estimated_velocity =
+        p_vel * std::cos(p_yaw - traj_yaw) + std::abs(current_ego_speed);
+
+      // Current RSS distance calculation does not account for negative velocities
+      return estimated_velocity;
+    });
+
+    if (!estimated_velocity_opt.has_value()) {
+      return std::nullopt;
+    }
+
+    const auto & estimated_velocity = estimated_velocity_opt.value();
+    this->setPreviousObjectData(closest_object);
+    this->updateVelocityHistory(estimated_velocity, closest_object.stamp);
+    return this->getMedianObstacleVelocity();
+  }
+
+private:
+  std::optional<ObjectData> prev_closest_object_{std::nullopt};
+  std::optional<ObjectData> closest_object_{std::nullopt};
+  double collision_keep_time_{0.0};
+  double previous_obstacle_keep_time_{0.0};
+
+  std::deque<std::pair<double, rclcpp::Time>> obstacle_velocity_history_;
+  rclcpp::Clock::SharedPtr clock_;
+};
+
+/**
+ * @brief Autonomous Emergency Braking (AEB) node
+ */
+class AEB : public rclcpp::Node
+{
+public:
+  /**
+   * @brief Constructor for AEB
+   * @param node_options Options for the node
+   */
+  explicit AEB(const rclcpp::NodeOptions & node_options);
+
+  // subscriber
+  autoware::universe_utils::InterProcessPollingSubscriber<PointCloud2> sub_point_cloud_{
+    this, "~/input/pointcloud", autoware::universe_utils::SingleDepthSensorQoS()};
+  autoware::universe_utils::InterProcessPollingSubscriber<VelocityReport> sub_velocity_{
+    this, "~/input/velocity"};
+  autoware::universe_utils::InterProcessPollingSubscriber<Imu> sub_imu_{this, "~/input/imu"};
+  autoware::universe_utils::InterProcessPollingSubscriber<Trajectory> sub_predicted_traj_{
+    this, "~/input/predicted_trajectory"};
+  autoware::universe_utils::InterProcessPollingSubscriber<PredictedObjects> predicted_objects_sub_{
+    this, "~/input/objects"};
+  autoware::universe_utils::InterProcessPollingSubscriber<AutowareState> sub_autoware_state_{
+    this, "/autoware/state"};
+  // publisher
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_obstacle_pointcloud_;
+  rclcpp::Publisher<MarkerArray>::SharedPtr debug_marker_publisher_;
+  rclcpp::Publisher<MarkerArray>::SharedPtr virtual_wall_publisher_;
+  rclcpp::Publisher<autoware::universe_utils::ProcessingTimeDetail>::SharedPtr
+    debug_processing_time_detail_pub_;
+  rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr debug_rss_distance_publisher_;
+  // timer
+  rclcpp::TimerBase::SharedPtr timer_;
+  mutable std::shared_ptr<autoware::universe_utils::TimeKeeper> time_keeper_{nullptr};
+
+  // callback
+  /**
+   * @brief Callback for point cloud messages
+   * @param input_msg Shared pointer to the point cloud message
+   */
+  void onPointCloud(const PointCloud2::ConstSharedPtr input_msg);
+
+  /**
+   * @brief Callback for IMU messages
+   * @param input_msg Shared pointer to the IMU message
+   */
+  void onImu(const Imu::ConstSharedPtr input_msg);
+
+  /**
+   * @brief Timer callback function
+   */
+  void onTimer();
+
+  /**
+   * @brief Callback for parameter updates
+   * @param parameters Vector of updated parameters
+   * @return Set parameters result
+   */
+  rcl_interfaces::msg::SetParametersResult onParameter(
+    const std::vector<rclcpp::Parameter> & parameters);
+
+  /**
+   * @brief Fetch the latest data from subscribers
+   * @return True if data fetch was successful, false otherwise
+   */
+  bool fetchLatestData();
+
+  /**
+   * @brief Diagnostic check for collisions
+   * @param stat Diagnostic status wrapper
+   */
+  void onCheckCollision(DiagnosticStatusWrapper & stat);
+
+  /**
+   * @brief Check for collisions
+   * @param debug_markers Marker array for debugging
+   * @return True if a collision is detected, false otherwise
+   */
+  bool checkCollision(MarkerArray & debug_markers);
+
+  /**
+   * @brief Check if there is a collision with the closest object
+   * @param current_v Current velocity of the ego vehicle
+   * @param closest_object Data of the closest object
+   * @return True if a collision is detected, false otherwise
+   */
+  bool hasCollision(const double current_v, const ObjectData & closest_object);
+
+  /**
+   * @brief Generate the ego vehicle path
+   * @param curr_v Current velocity of the ego vehicle
+   * @param curr_w Current angular velocity of the ego vehicle
+   * @return Generated ego path
+   */
+  Path generateEgoPath(const double curr_v, const double curr_w);
+
+  /**
+   * @brief Generate the ego vehicle path from the predicted trajectory
+   * @param predicted_traj Predicted trajectory of the ego vehicle
+   * @return Optional generated ego path
+   */
+  std::optional<Path> generateEgoPath(const Trajectory & predicted_traj);
+
+  /**
+   * @brief Generate the footprint of the path with extra width margin
+   * @param path Ego vehicle path
+   * @param extra_width_margin Extra width margin for the footprint
+   * @param polygons vector to be filled with the polygons
+   * @return Vector of polygons representing the path footprint
+   */
+  void generatePathFootprint(
+    const Path & path, const double extra_width_margin, std::vector<Polygon2d> & polygons);
+
+  /**
+   * @brief Generate the footprint of the path with extra width margin
+   * @param path Ego vehicle path
+   * @param extra_width_margin Extra width margin for the footprint
+   * @return Vector of polygons representing the path footprint
+   */
+  std::vector<Polygon2d> generatePathFootprint(const Path & path, const double extra_width_margin);
+
+  /**
+   * @brief Create object data using point cloud clusters
+   * @param ego_path Ego vehicle path
+   * @param ego_polys Polygons representing the ego vehicle footprint
+   * @param speed_calc_ego_polys Polygons representing the expanded ego vehicle footprint for speed
+   * calculation area
+   * @param stamp Timestamp of the data
+   * @param objects Vector to store the created object data
+   * @param obstacle_points_ptr Pointer to the point cloud of obstacles
+   */
+  void getClosestObjectsOnPath(
+    const Path & ego_path, const rclcpp::Time & stamp,
+    const PointCloud::Ptr points_belonging_to_cluster_hulls, std::vector<ObjectData> & objects);
+
+  /**
+   * @brief Create object data using point cloud clusters
+   * @param obstacle_points_ptr Pointer to the point cloud of obstacles
+   * @param points_belonging_to_cluster_hulls output: pointer to the point cloud of points belonging
+   * to cluster hulls
+   */
+  void getPointsBelongingToClusterHulls(
+    const PointCloud::Ptr obstacle_points_ptr,
+    const PointCloud::Ptr points_belonging_to_cluster_hulls, MarkerArray & debug_markers);
+
+  /**
+   * @brief Create object data using predicted objects
+   * @param ego_path Ego vehicle path
+   * @param ego_polys Polygons representing the ego vehicle footprint
+   * @param objects Vector to store the created object data
+   */
+  void createObjectDataUsingPredictedObjects(
+    const Path & ego_path, const std::vector<Polygon2d> & ego_polys,
+    std::vector<ObjectData> & objects);
+
+  /**
+   * @brief Crop the point cloud with the ego vehicle footprint path
+   * @param ego_polys Polygons representing the ego vehicle footprint
+   * @param filtered_objects Pointer to the filtered point cloud of obstacles
+   */
+  void cropPointCloudWithEgoFootprintPath(
+    const std::vector<Polygon2d> & ego_polys, pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_objects);
+
+  /**
+   * @brief Add a marker for debugging
+   * @param current_time Current time
+   * @param path Ego vehicle path
+   * @param polygons Polygons representing the ego vehicle footprint
+   * @param objects Vector of object data
+   * @param closest_object Optional data of the closest object
+   * @param debug_colors Tuple of RGBA colors
+   * @param ns Namespace for the marker
+   * @param debug_markers Marker array for debugging
+   */
+  void addMarker(
+    const rclcpp::Time & current_time, const Path & path, const std::vector<Polygon2d> & polygons,
+    const std::vector<ObjectData> & objects, const std::optional<ObjectData> & closest_object,
+    const colorTuple & debug_colors, const std::string & ns, MarkerArray & debug_markers);
+
+  /**
+   * @brief Add a marker of convex hulls for debugging
+   * @param current_time Current time
+   * @param hulls vector of polygons of the convex hulls
+   * @param debug_colors Tuple of RGBA colors
+   * @param ns Namespace for the marker
+   * @param debug_markers Marker array for debugging
+   */
+  void addClusterHullMarkers(
+    const rclcpp::Time & current_time, const std::vector<Polygon3d> & hulls,
+    const colorTuple & debug_colors, const std::string & ns, MarkerArray & debug_markers);
+
+  /**
+   * @brief Add a collision marker for debugging
+   * @param data Data of the collision object
+   * @param debug_markers Marker array for debugging
+   */
+  void addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers);
+
+  /**
+   * @brief Add an info marker stop wall in front of the ego vehicle
+   * @param markers Data of the closest object
+   */
+  void addVirtualStopWallMarker(MarkerArray & markers);
+
+  /**
+   * @brief Calculate object speed from history
+   * @param closest_object Data of the closest object
+   * @param path Ego vehicle path
+   * @param current_ego_speed Current speed of the ego vehicle
+   * @return Optional calculated object speed
+   */
+  std::optional<double> calcObjectSpeedFromHistory(
+    const ObjectData & closest_object, const Path & path, const double current_ego_speed);
+
+  // Member variables
+  PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_{nullptr};
+  VelocityReport::ConstSharedPtr current_velocity_ptr_{nullptr};
+  Vector3::SharedPtr angular_velocity_ptr_{nullptr};
+  Trajectory::ConstSharedPtr predicted_traj_ptr_{nullptr};
+  PredictedObjects::ConstSharedPtr predicted_objects_ptr_{nullptr};
+  AutowareState::ConstSharedPtr autoware_state_{nullptr};
+
+  tf2_ros::Buffer tf_buffer_{get_clock()};
+  tf2_ros::TransformListener tf_listener_{tf_buffer_};
+
+  // vehicle info
+  VehicleInfo vehicle_info_;
+
+  // diag
+  Updater updater_{this};
+
+  // Member variables
+  bool publish_debug_pointcloud_;
+  bool publish_debug_markers_;
+  bool publish_debug_time_;
+  bool use_predicted_trajectory_;
+  bool use_imu_path_;
+  bool use_pointcloud_data_;
+  bool use_predicted_object_data_;
+  bool use_object_velocity_calculation_;
+  bool check_autoware_state_;
+  double path_footprint_extra_margin_;
+  double speed_calculation_expansion_margin_;
+  double detection_range_min_height_;
+  double detection_range_max_height_margin_;
+  double voxel_grid_x_;
+  double voxel_grid_y_;
+  double voxel_grid_z_;
+  double min_generated_imu_path_length_;
+  double max_generated_imu_path_length_;
+  double expand_width_;
+  double longitudinal_offset_;
+  double t_response_;
+  double a_ego_min_;
+  double a_obj_min_;
+  double cluster_tolerance_;
+  double cluster_minimum_height_;
+  int minimum_cluster_size_;
+  int maximum_cluster_size_;
+  double imu_prediction_time_horizon_;
+  double imu_prediction_time_interval_;
+  double mpc_prediction_time_horizon_;
+  double mpc_prediction_time_interval_;
+  CollisionDataKeeper collision_data_keeper_;
+  // Parameter callback
+  OnSetParametersCallbackHandle::SharedPtr set_param_res_;
+};
+}  // namespace autoware::motion::control::autonomous_emergency_braking
+
+#endif  // AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_

slave/src/Control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp

@@ -0,0 +1,119 @@
+// Copyright 2024 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.
+
+#ifndef AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__UTILS_HPP_
+#define AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__UTILS_HPP_
+
+#include <autoware/universe_utils/geometry/boost_polygon_utils.hpp>
+
+#include <autoware_perception_msgs/msg/predicted_objects.hpp>
+#include <geometry_msgs/msg/point.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+
+#include <boost/geometry/algorithms/correct.hpp>
+
+#include <tf2/utils.h>
+
+#include <string>
+#include <vector>
+
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_eigen/tf2_eigen.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_eigen/tf2_eigen.hpp>
+
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <visualization_msgs/msg/marker.hpp>
+
+#endif
+
+namespace autoware::motion::control::autonomous_emergency_braking::utils
+{
+using autoware::universe_utils::Polygon2d;
+using autoware::universe_utils::Polygon3d;
+using autoware_perception_msgs::msg::PredictedObject;
+using autoware_perception_msgs::msg::PredictedObjects;
+using geometry_msgs::msg::Point;
+using geometry_msgs::msg::Pose;
+using geometry_msgs::msg::TransformStamped;
+
+/**
+ * @brief Apply a transform to a predicted object
+ * @param input the predicted object
+ * @param transform_stamped the tf2 transform
+ */
+PredictedObject transformObjectFrame(
+  const PredictedObject & input, const geometry_msgs::msg::TransformStamped & transform_stamped);
+
+/**
+ * @brief Get the predicted objects polygon as a geometry polygon
+ * @param current_pose the predicted object's pose
+ * @param obj_shape the object's shape
+ */
+Polygon2d convertPolygonObjectToGeometryPolygon(
+  const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape);
+
+/**
+ * @brief Get the predicted objects cylindrical shape as a geometry polygon
+ * @param current_pose the predicted object's pose
+ * @param obj_shape the object's shape
+ */
+Polygon2d convertCylindricalObjectToGeometryPolygon(
+  const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape);
+
+/**
+ * @brief Get the predicted objects bounding box shape as a geometry polygon
+ * @param current_pose the predicted object's pose
+ * @param obj_shape the object's shape
+ */
+Polygon2d convertBoundingBoxObjectToGeometryPolygon(
+  const Pose & current_pose, const double & base_to_front, const double & base_to_rear,
+  const double & base_to_width);
+
+/**
+ * @brief Get the predicted object's shape as a geometry polygon
+ * @param obj the object
+ */
+Polygon2d convertObjToPolygon(const PredictedObject & obj);
+
+/**
+ * @brief Get the transform from source to target frame
+ * @param target_frame target frame
+ * @param source_frame source frame
+ * @param tf_buffer buffer of tf transforms
+ * @param logger node logger
+ */
+std::optional<geometry_msgs::msg::TransformStamped> getTransform(
+  const std::string & target_frame, const std::string & source_frame,
+  const tf2_ros::Buffer & tf_buffer, const rclcpp::Logger & logger);
+
+/**
+ * @brief Get the predicted object's shape as a geometry polygon
+ * @param polygons vector of Polygon2d
+ * @param polygon_marker marker to be filled with polygon points
+ */
+void fillMarkerFromPolygon(
+  const std::vector<Polygon2d> & polygons, visualization_msgs::msg::Marker & polygon_marker);
+
+/**
+ * @brief Get the predicted object's shape as a geometry polygon
+ * @param polygons vector of Polygon3d
+ * @param polygon_marker marker to be filled with polygon points
+ */
+void fillMarkerFromPolygon(
+  const std::vector<Polygon3d> & polygons, visualization_msgs::msg::Marker & polygon_marker);
+}  // namespace autoware::motion::control::autonomous_emergency_braking::utils
+
+#endif  // AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__UTILS_HPP_

slave/src/Control/autoware_autonomous_emergency_braking/launch/autoware_autonomous_emergency_braking.launch.xml

@@ -0,0 +1,20 @@
+<launch>
+  <arg name="param_path" default="$(find-pkg-share autoware_autonomous_emergency_braking)/config/autonomous_emergency_braking.param.yaml"/>
+  <arg name="input_pointcloud" default="/perception/obstacle_segmentation/pointcloud"/>
+  <arg name="input_velocity" default="/vehicle/status/velocity_status"/>
+  <arg name="input_imu" default="/sensing/imu/hipnuc/imu_data"/>
+  <arg name="input_predicted_trajectory" default="/control/trajectory_follower/lateral/predicted_trajectory"/>
+  <arg name="input_objects" default="/perception/object_recognition/objects"/>
+
+  <node pkg="autoware_autonomous_emergency_braking" exec="autoware_autonomous_emergency_braking" name="autonomous_emergency_braking" output="screen">
+    <!-- load config files -->
+    <param from="$(var param_path)"/>
+    <!-- remap topic name -->
+    <remap from="~/input/pointcloud" to="$(var input_pointcloud)"/>
+    <remap from="~/input/velocity" to="$(var input_velocity)"/>
+    <remap from="~/input/imu" to="$(var input_imu)"/>
+    <remap from="~/input/odometry" to="$(var input_odometry)"/>
+    <remap from="~/input/predicted_trajectory" to="$(var input_predicted_trajectory)"/>
+    <remap from="~/input/objects" to="$(var input_objects)"/>
+  </node>
+</launch>

slave/src/Control/autoware_autonomous_emergency_braking/package.xml

@@ -0,0 +1,50 @@
+<?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_autonomous_emergency_braking</name>
+  <version>0.1.0</version>
+  <description>Autonomous Emergency Braking package as a ROS 2 node</description>
+  <maintainer email="takamasa.horibe@tier4.jp">Takamasa Horibe</maintainer>
+  <maintainer email="tomoya.kimura@tier4.jp">Tomoya Kimura</maintainer>
+  <maintainer email="mamoru.sobue@tier4.jp">Mamoru Sobue</maintainer>
+  <maintainer email="daniel.sanchez@tier4.jp">Daniel Sanchez</maintainer>
+  <maintainer email="kyoichi.sugahara@tier4.jp">Kyoichi Sugahara</maintainer>
+
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_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_test_utils</test_depend>
+
+  <depend>autoware_control_msgs</depend>
+  <depend>autoware_motion_utils</depend>
+  <depend>autoware_planning_msgs</depend>
+  <depend>autoware_pointcloud_preprocessor</depend>
+  <depend>autoware_system_msgs</depend>
+  <depend>autoware_test_utils</depend>
+  <depend>autoware_universe_utils</depend>
+  <depend>autoware_vehicle_info_utils</depend>
+  <depend>autoware_vehicle_msgs</depend>
+  <depend>diagnostic_updater</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>pcl_conversions</depend>
+  <depend>pcl_ros</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_eigen</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
+  <depend>tier4_debug_msgs</depend>
+  <depend>visualization_msgs</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

slave/src/Control/autoware_autonomous_emergency_braking/src/utils.cpp

@@ -0,0 +1,206 @@
+// Copyright 2024 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.
+
+#include <autoware/autonomous_emergency_braking/utils.hpp>
+#include <autoware/universe_utils/geometry/geometry.hpp>
+
+#include <optional>
+
+namespace autoware::motion::control::autonomous_emergency_braking::utils
+{
+using autoware::universe_utils::Polygon2d;
+using autoware_perception_msgs::msg::PredictedObject;
+using autoware_perception_msgs::msg::PredictedObjects;
+using geometry_msgs::msg::Point;
+using geometry_msgs::msg::Pose;
+using geometry_msgs::msg::TransformStamped;
+using geometry_msgs::msg::Vector3;
+
+PredictedObject transformObjectFrame(
+  const PredictedObject & input, const geometry_msgs::msg::TransformStamped & transform_stamped)
+{
+  PredictedObject output = input;
+  const auto & linear_twist = input.kinematics.initial_twist_with_covariance.twist.linear;
+  const auto & angular_twist = input.kinematics.initial_twist_with_covariance.twist.angular;
+  const auto & pose = input.kinematics.initial_pose_with_covariance.pose;
+
+  geometry_msgs::msg::Pose t_pose;
+  Vector3 t_linear_twist;
+  Vector3 t_angular_twist;
+
+  tf2::doTransform(pose, t_pose, transform_stamped);
+  tf2::doTransform(linear_twist, t_linear_twist, transform_stamped);
+  tf2::doTransform(angular_twist, t_angular_twist, transform_stamped);
+
+  output.kinematics.initial_pose_with_covariance.pose = t_pose;
+  output.kinematics.initial_twist_with_covariance.twist.linear = t_linear_twist;
+  output.kinematics.initial_twist_with_covariance.twist.angular = t_angular_twist;
+  return output;
+}
+
+Polygon2d convertPolygonObjectToGeometryPolygon(
+  const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape)
+{
+  if (obj_shape.footprint.points.empty()) {
+    return {};
+  }
+  Polygon2d object_polygon;
+  tf2::Transform tf_map2obj;
+  fromMsg(current_pose, tf_map2obj);
+  const auto obj_points = obj_shape.footprint.points;
+  object_polygon.outer().reserve(obj_points.size() + 1);
+  for (const auto & obj_point : obj_points) {
+    tf2::Vector3 obj(obj_point.x, obj_point.y, obj_point.z);
+    tf2::Vector3 tf_obj = tf_map2obj * obj;
+    object_polygon.outer().emplace_back(tf_obj.x(), tf_obj.y());
+  }
+  object_polygon.outer().push_back(object_polygon.outer().front());
+  boost::geometry::correct(object_polygon);
+
+  return object_polygon;
+}
+
+Polygon2d convertCylindricalObjectToGeometryPolygon(
+  const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape)
+{
+  Polygon2d object_polygon;
+
+  const double obj_x = current_pose.position.x;
+  const double obj_y = current_pose.position.y;
+
+  constexpr int n = 20;
+  const double r = obj_shape.dimensions.x / 2;
+  object_polygon.outer().reserve(n + 1);
+  for (int i = 0; i < n; ++i) {
+    object_polygon.outer().emplace_back(
+      obj_x + r * std::cos(2.0 * M_PI / n * i), obj_y + r * std::sin(2.0 * M_PI / n * i));
+  }
+
+  object_polygon.outer().push_back(object_polygon.outer().front());
+  boost::geometry::correct(object_polygon);
+
+  return object_polygon;
+}
+
+Polygon2d convertBoundingBoxObjectToGeometryPolygon(
+  const Pose & current_pose, const double & base_to_front, const double & base_to_rear,
+  const double & base_to_width)
+{
+  const auto mapped_point = [](const double & length_scalar, const double & width_scalar) {
+    tf2::Vector3 map;
+    map.setX(length_scalar);
+    map.setY(width_scalar);
+    map.setZ(0.0);
+    map.setW(1.0);
+    return map;
+  };
+
+  // set vertices at map coordinate
+  const tf2::Vector3 p1_map = std::invoke(mapped_point, base_to_front, -base_to_width);
+  const tf2::Vector3 p2_map = std::invoke(mapped_point, base_to_front, base_to_width);
+  const tf2::Vector3 p3_map = std::invoke(mapped_point, -base_to_rear, base_to_width);
+  const tf2::Vector3 p4_map = std::invoke(mapped_point, -base_to_rear, -base_to_width);
+
+  // transform vertices from map coordinate to object coordinate
+  tf2::Transform tf_map2obj;
+  tf2::fromMsg(current_pose, tf_map2obj);
+  const tf2::Vector3 p1_obj = tf_map2obj * p1_map;
+  const tf2::Vector3 p2_obj = tf_map2obj * p2_map;
+  const tf2::Vector3 p3_obj = tf_map2obj * p3_map;
+  const tf2::Vector3 p4_obj = tf_map2obj * p4_map;
+
+  Polygon2d object_polygon;
+  object_polygon.outer().reserve(5);
+  object_polygon.outer().emplace_back(p1_obj.x(), p1_obj.y());
+  object_polygon.outer().emplace_back(p2_obj.x(), p2_obj.y());
+  object_polygon.outer().emplace_back(p3_obj.x(), p3_obj.y());
+  object_polygon.outer().emplace_back(p4_obj.x(), p4_obj.y());
+
+  object_polygon.outer().push_back(object_polygon.outer().front());
+  boost::geometry::correct(object_polygon);
+
+  return object_polygon;
+}
+
+Polygon2d convertObjToPolygon(const PredictedObject & obj)
+{
+  Polygon2d object_polygon{};
+  if (obj.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) {
+    object_polygon = utils::convertCylindricalObjectToGeometryPolygon(
+      obj.kinematics.initial_pose_with_covariance.pose, obj.shape);
+  } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) {
+    const double & length_m = obj.shape.dimensions.x / 2;
+    const double & width_m = obj.shape.dimensions.y / 2;
+    object_polygon = utils::convertBoundingBoxObjectToGeometryPolygon(
+      obj.kinematics.initial_pose_with_covariance.pose, length_m, length_m, width_m);
+  } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) {
+    object_polygon = utils::convertPolygonObjectToGeometryPolygon(
+      obj.kinematics.initial_pose_with_covariance.pose, obj.shape);
+  } else {
+    throw std::runtime_error("Unsupported shape type");
+  }
+  return object_polygon;
+}
+
+std::optional<geometry_msgs::msg::TransformStamped> getTransform(
+  const std::string & target_frame, const std::string & source_frame,
+  const tf2_ros::Buffer & tf_buffer, const rclcpp::Logger & logger)
+{
+  geometry_msgs::msg::TransformStamped tf_current_pose;
+  try {
+    tf_current_pose = tf_buffer.lookupTransform(
+      target_frame, source_frame, rclcpp::Time(0), rclcpp::Duration::from_seconds(1.0));
+  } catch (tf2::TransformException & ex) {
+    RCLCPP_ERROR_STREAM(
+      logger, "[AEB] Failed to look up transform from " + source_frame + " to " + target_frame);
+    return std::nullopt;
+  }
+  return std::make_optional(tf_current_pose);
+}
+
+void fillMarkerFromPolygon(
+  const std::vector<Polygon2d> & polygons, visualization_msgs::msg::Marker & polygon_marker)
+{
+  for (const auto & poly : polygons) {
+    for (size_t dp_idx = 0; dp_idx < poly.outer().size(); ++dp_idx) {
+      const auto & boost_cp = poly.outer().at(dp_idx);
+      const auto & boost_np = poly.outer().at((dp_idx + 1) % poly.outer().size());
+      const auto curr_point =
+        autoware::universe_utils::createPoint(boost_cp.x(), boost_cp.y(), 0.0);
+      const auto next_point =
+        autoware::universe_utils::createPoint(boost_np.x(), boost_np.y(), 0.0);
+      polygon_marker.points.push_back(curr_point);
+      polygon_marker.points.push_back(next_point);
+    }
+  }
+}
+
+void fillMarkerFromPolygon(
+  const std::vector<Polygon3d> & polygons, visualization_msgs::msg::Marker & polygon_marker)
+{
+  for (const auto & poly : polygons) {
+    for (size_t dp_idx = 0; dp_idx < poly.outer().size(); ++dp_idx) {
+      const auto & boost_cp = poly.outer().at(dp_idx);
+      const auto & boost_np = poly.outer().at((dp_idx + 1) % poly.outer().size());
+      const auto curr_point =
+        autoware::universe_utils::createPoint(boost_cp.x(), boost_cp.y(), boost_cp.z());
+      const auto next_point =
+        autoware::universe_utils::createPoint(boost_np.x(), boost_np.y(), boost_np.z());
+      polygon_marker.points.push_back(curr_point);
+      polygon_marker.points.push_back(next_point);
+    }
+  }
+}
+
+}  // namespace autoware::motion::control::autonomous_emergency_braking::utils

slave/src/Control/autoware_autonomous_emergency_braking/test/test.cpp

@@ -0,0 +1,351 @@
+// Copyright 2024 TIER IV
+//
+// 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 "test.hpp"
+
+#include "autoware/autonomous_emergency_braking/node.hpp"
+#include "autoware/universe_utils/geometry/geometry.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/time.hpp>
+
+#include <autoware_perception_msgs/msg/detail/shape__struct.hpp>
+#include <geometry_msgs/msg/detail/pose__struct.hpp>
+
+#include <gtest/gtest.h>
+#include <pcl/memory.h>
+#include <tf2/LinearMath/Transform.h>
+
+#include <memory>
+#include <thread>
+
+namespace autoware::motion::control::autonomous_emergency_braking::test
+{
+using autoware::universe_utils::Polygon2d;
+using autoware_perception_msgs::msg::PredictedObject;
+using autoware_perception_msgs::msg::PredictedObjects;
+using geometry_msgs::msg::Point;
+using geometry_msgs::msg::Pose;
+using geometry_msgs::msg::TransformStamped;
+using geometry_msgs::msg::Vector3;
+using std_msgs::msg::Header;
+
+Header get_header(const char * const frame_id, rclcpp::Time t)
+{
+  std_msgs::msg::Header header;
+  header.stamp = t;
+  header.frame_id = frame_id;
+  return header;
+};
+
+Imu make_imu_message(
+  const Header & header, const double ax, const double ay, const double yaw,
+  const double angular_velocity_z)
+{
+  Imu imu_msg;
+  imu_msg.header = header;
+  imu_msg.orientation = autoware::universe_utils::createQuaternionFromYaw(yaw);
+  imu_msg.angular_velocity.z = angular_velocity_z;
+  imu_msg.linear_acceleration.x = ax;
+  imu_msg.linear_acceleration.y = ay;
+  return imu_msg;
+};
+
+VelocityReport make_velocity_report_msg(
+  const Header & header, const double lat_velocity, const double long_velocity,
+  const double heading_rate)
+{
+  VelocityReport velocity_msg;
+  velocity_msg.header = header;
+  velocity_msg.lateral_velocity = lat_velocity;
+  velocity_msg.longitudinal_velocity = long_velocity;
+  velocity_msg.heading_rate = heading_rate;
+  return velocity_msg;
+}
+
+std::shared_ptr<AEB> generateNode()
+{
+  auto node_options = rclcpp::NodeOptions{};
+
+  const auto aeb_dir =
+    ament_index_cpp::get_package_share_directory("autoware_autonomous_emergency_braking");
+  const auto vehicle_info_util_dir =
+    ament_index_cpp::get_package_share_directory("autoware_vehicle_info_utils");
+
+  node_options.arguments(
+    {"--ros-args", "--params-file", aeb_dir + "/config/autonomous_emergency_braking.param.yaml",
+     "--ros-args", "--params-file", vehicle_info_util_dir + "/config/vehicle_info.param.yaml"});
+  return std::make_shared<AEB>(node_options);
+};
+
+std::shared_ptr<PubSubNode> generatePubSubNode()
+{
+  auto node_options = rclcpp::NodeOptions{};
+  node_options.arguments({"--ros-args"});
+  return std::make_shared<PubSubNode>(node_options);
+};
+
+PubSubNode::PubSubNode(const rclcpp::NodeOptions & node_options)
+: Node("test_aeb_pubsub", node_options)
+{
+  rclcpp::QoS qos{1};
+  qos.transient_local();
+
+  pub_imu_ = create_publisher<Imu>("~/input/imu", qos);
+  pub_point_cloud_ = create_publisher<PointCloud2>("~/input/pointcloud", qos);
+  pub_velocity_ = create_publisher<VelocityReport>("~/input/velocity", qos);
+  pub_predicted_traj_ = create_publisher<Trajectory>("~/input/predicted_trajectory", qos);
+  pub_predicted_objects_ = create_publisher<PredictedObjects>("~/input/objects", qos);
+  pub_autoware_state_ = create_publisher<AutowareState>("autoware/state", qos);
+}
+
+TEST_F(TestAEB, checkCollision)
+{
+  constexpr double longitudinal_velocity = 3.0;
+  ObjectData object_collision;
+  object_collision.distance_to_object = 0.5;
+  object_collision.velocity = 0.1;
+  object_collision.position.x = 1.0;
+  object_collision.position.y = 1.0;
+  ASSERT_TRUE(aeb_node_->hasCollision(longitudinal_velocity, object_collision));
+
+  ObjectData object_no_collision;
+  object_no_collision.distance_to_object = 10.0;
+  object_no_collision.velocity = 0.1;
+  ASSERT_FALSE(aeb_node_->hasCollision(longitudinal_velocity, object_no_collision));
+}
+
+TEST_F(TestAEB, checkImuPathGeneration)
+{
+  constexpr double longitudinal_velocity = 3.0;
+  constexpr double yaw_rate = 0.05;
+  const auto imu_path = aeb_node_->generateEgoPath(longitudinal_velocity, yaw_rate);
+  ASSERT_FALSE(imu_path.empty());
+
+  const double dt = aeb_node_->imu_prediction_time_interval_;
+  const double horizon = aeb_node_->imu_prediction_time_horizon_;
+  ASSERT_TRUE(imu_path.size() >= static_cast<size_t>(horizon / dt));
+
+  const auto footprint = aeb_node_->generatePathFootprint(imu_path, 0.0);
+  ASSERT_FALSE(footprint.empty());
+  ASSERT_TRUE(footprint.size() == imu_path.size() - 1);
+
+  const auto stamp = rclcpp::Time();
+  pcl::PointCloud<pcl::PointXYZ>::Ptr obstacle_points_ptr =
+    pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+  {
+    const double x_start{0.0};
+    const double y_start{0.0};
+
+    for (size_t i = 0; i < 15; ++i) {
+      pcl::PointXYZ p1(
+        x_start + static_cast<double>(i / 100.0), y_start - static_cast<double>(i / 100.0), 0.5);
+      pcl::PointXYZ p2(
+        x_start + static_cast<double>((i + 10) / 100.0), y_start - static_cast<double>(i / 100.0),
+        0.5);
+      obstacle_points_ptr->push_back(p1);
+      obstacle_points_ptr->push_back(p2);
+    }
+  }
+  PointCloud::Ptr points_belonging_to_cluster_hulls = pcl::make_shared<PointCloud>();
+  MarkerArray debug_markers;
+  aeb_node_->getPointsBelongingToClusterHulls(
+    obstacle_points_ptr, points_belonging_to_cluster_hulls, debug_markers);
+  std::vector<ObjectData> objects;
+  aeb_node_->getClosestObjectsOnPath(imu_path, stamp, points_belonging_to_cluster_hulls, objects);
+  ASSERT_FALSE(objects.empty());
+}
+
+TEST_F(TestAEB, checkIncompleteImuPathGeneration)
+{
+  const double dt = aeb_node_->imu_prediction_time_interval_;
+  const double horizon = aeb_node_->imu_prediction_time_horizon_;
+  const double min_generated_path_length = aeb_node_->min_generated_imu_path_length_;
+  const double slow_velocity = min_generated_path_length / (2.0 * horizon);
+  constexpr double yaw_rate = 0.05;
+  const auto imu_path = aeb_node_->generateEgoPath(slow_velocity, yaw_rate);
+
+  ASSERT_FALSE(imu_path.empty());
+  ASSERT_TRUE(imu_path.size() >= static_cast<size_t>(horizon / dt));
+  ASSERT_TRUE(autoware::motion_utils::calcArcLength(imu_path) >= min_generated_path_length);
+
+  const auto footprint = aeb_node_->generatePathFootprint(imu_path, 0.0);
+  ASSERT_FALSE(footprint.empty());
+  ASSERT_TRUE(footprint.size() == imu_path.size() - 1);
+}
+
+TEST_F(TestAEB, checkImuPathGenerationIsCut)
+{
+  const double dt = aeb_node_->imu_prediction_time_interval_;
+  const double horizon = aeb_node_->imu_prediction_time_horizon_;
+  const double max_generated_path_length = aeb_node_->max_generated_imu_path_length_;
+  const double fast_velocity = 2.0 * max_generated_path_length / (horizon);
+  constexpr double yaw_rate = 0.05;
+  const auto imu_path = aeb_node_->generateEgoPath(fast_velocity, yaw_rate);
+
+  ASSERT_FALSE(imu_path.empty());
+  constexpr double epsilon{1e-3};
+  ASSERT_TRUE(
+    autoware::motion_utils::calcArcLength(imu_path) <=
+    max_generated_path_length + dt * fast_velocity + epsilon);
+
+  const auto footprint = aeb_node_->generatePathFootprint(imu_path, 0.0);
+  ASSERT_FALSE(footprint.empty());
+  ASSERT_TRUE(footprint.size() == imu_path.size() - 1);
+}
+
+TEST_F(TestAEB, checkEmptyPathAtZeroSpeed)
+{
+  const double velocity = 0.0;
+  constexpr double yaw_rate = 0.0;
+  const auto imu_path = aeb_node_->generateEgoPath(velocity, yaw_rate);
+  ASSERT_EQ(imu_path.size(), 1);
+}
+
+TEST_F(TestAEB, checkParamUpdate)
+{
+  std::vector<rclcpp::Parameter> parameters{rclcpp::Parameter("param")};
+  const auto result = aeb_node_->onParameter(parameters);
+  ASSERT_TRUE(result.successful);
+}
+
+TEST_F(TestAEB, checkEmptyFetchData)
+{
+  ASSERT_FALSE(aeb_node_->fetchLatestData());
+}
+
+TEST_F(TestAEB, checkConvertObjectToPolygon)
+{
+  using autoware_perception_msgs::msg::Shape;
+  PredictedObject obj_cylinder;
+  obj_cylinder.shape.type = Shape::CYLINDER;
+  obj_cylinder.shape.dimensions.x = 1.0;
+  Pose obj_cylinder_pose;
+  obj_cylinder_pose.position.x = 1.0;
+  obj_cylinder_pose.position.y = 1.0;
+  obj_cylinder.kinematics.initial_pose_with_covariance.pose = obj_cylinder_pose;
+  const auto cylinder_polygon = utils::convertObjToPolygon(obj_cylinder);
+  ASSERT_FALSE(cylinder_polygon.outer().empty());
+
+  PredictedObject obj_box;
+  obj_box.shape.type = Shape::BOUNDING_BOX;
+  obj_box.shape.dimensions.x = 1.0;
+  obj_box.shape.dimensions.y = 2.0;
+  Pose obj_box_pose;
+  obj_box_pose.position.x = 1.0;
+  obj_box_pose.position.y = 1.0;
+  obj_box.kinematics.initial_pose_with_covariance.pose = obj_box_pose;
+  const auto box_polygon = utils::convertObjToPolygon(obj_box);
+  ASSERT_FALSE(box_polygon.outer().empty());
+
+  geometry_msgs::msg::TransformStamped tf_stamped;
+  geometry_msgs::msg::Transform transform;
+
+  constexpr double yaw{0.0};
+  transform.rotation = autoware::universe_utils::createQuaternionFromYaw(yaw);
+  geometry_msgs::msg::Vector3 translation;
+  translation.x = 1.0;
+  translation.y = 0.0;
+  translation.z = 0.0;
+  transform.translation = translation;
+  tf_stamped.set__transform(transform);
+  const auto t_obj_box = utils::transformObjectFrame(obj_box, tf_stamped);
+  const auto t_pose = t_obj_box.kinematics.initial_pose_with_covariance.pose;
+  Pose expected_pose;
+  expected_pose.position.x = obj_box_pose.position.x + translation.x;
+  expected_pose.position.y = obj_box_pose.position.y + translation.y;
+  expected_pose.position.z = obj_box_pose.position.z + translation.z;
+
+  ASSERT_DOUBLE_EQ(expected_pose.position.x, t_pose.position.x);
+  ASSERT_DOUBLE_EQ(expected_pose.position.y, t_pose.position.y);
+  ASSERT_DOUBLE_EQ(expected_pose.position.z, t_pose.position.z);
+}
+
+TEST_F(TestAEB, CollisionDataKeeper)
+{
+  using namespace std::literals::chrono_literals;
+  constexpr double collision_keeping_sec{1.0}, previous_obstacle_keep_time{1.0};
+  CollisionDataKeeper collision_data_keeper_(aeb_node_->get_clock());
+  collision_data_keeper_.setTimeout(collision_keeping_sec, previous_obstacle_keep_time);
+  ASSERT_TRUE(collision_data_keeper_.checkCollisionExpired());
+  ASSERT_TRUE(collision_data_keeper_.checkPreviousObjectDataExpired());
+
+  ObjectData obj;
+  obj.stamp = aeb_node_->now();
+  obj.velocity = 0.0;
+  obj.position.x = 0.0;
+  rclcpp::sleep_for(100ms);
+
+  ObjectData obj2;
+  obj2.stamp = aeb_node_->now();
+  obj2.velocity = 0.0;
+  obj2.position.x = 0.1;
+  rclcpp::sleep_for(100ms);
+
+  constexpr double ego_longitudinal_velocity = 3.0;
+  constexpr double yaw_rate = 0.0;
+  const auto imu_path = aeb_node_->generateEgoPath(ego_longitudinal_velocity, yaw_rate);
+
+  const auto speed_null =
+    collision_data_keeper_.calcObjectSpeedFromHistory(obj, imu_path, ego_longitudinal_velocity);
+  ASSERT_FALSE(speed_null.has_value());
+
+  const auto median_velocity =
+    collision_data_keeper_.calcObjectSpeedFromHistory(obj2, imu_path, ego_longitudinal_velocity);
+  ASSERT_TRUE(median_velocity.has_value());
+
+  // object speed is 1.0 m/s greater than ego's = 0.1 [m] / 0.1 [s] + longitudinal_velocity
+  ASSERT_TRUE(std::abs(median_velocity.value() - 4.0) < 1e-2);
+  rclcpp::sleep_for(1100ms);
+  ASSERT_TRUE(collision_data_keeper_.checkCollisionExpired());
+}
+
+TEST_F(TestAEB, TestCropPointCloud)
+{
+  constexpr double longitudinal_velocity = 3.0;
+  constexpr double yaw_rate = 0.05;
+  const auto imu_path = aeb_node_->generateEgoPath(longitudinal_velocity, yaw_rate);
+  ASSERT_FALSE(imu_path.empty());
+
+  constexpr size_t n_points{15};
+  // Create n_points inside the path and 1 point outside.
+  pcl::PointCloud<pcl::PointXYZ>::Ptr obstacle_points_ptr =
+    pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+  {
+    constexpr double x_start{0.0};
+    constexpr double y_start{0.0};
+
+    for (size_t i = 0; i < n_points; ++i) {
+      const double offset_1 = static_cast<double>(i / 100.0);
+      const double offset_2 = static_cast<double>((i + 10) / 100.0);
+      pcl::PointXYZ p1(x_start + offset_1, y_start - offset_1, 0.5);
+      pcl::PointXYZ p2(x_start + offset_2, y_start - offset_1, 0.5);
+      obstacle_points_ptr->push_back(p1);
+      obstacle_points_ptr->push_back(p2);
+    }
+    pcl::PointXYZ p_out(x_start + 100.0, y_start + 100, 0.5);
+    obstacle_points_ptr->push_back(p_out);
+  }
+  aeb_node_->obstacle_ros_pointcloud_ptr_ = std::make_shared<PointCloud2>();
+  pcl::toROSMsg(*obstacle_points_ptr, *aeb_node_->obstacle_ros_pointcloud_ptr_);
+  const auto footprint = aeb_node_->generatePathFootprint(imu_path, 0.0);
+
+  pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_objects =
+    pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+  aeb_node_->cropPointCloudWithEgoFootprintPath(footprint, filtered_objects);
+  // Check if the point outside the path was excluded
+  ASSERT_TRUE(filtered_objects->points.size() == 2 * n_points);
+}
+
+}  // namespace autoware::motion::control::autonomous_emergency_braking::test

slave/src/Control/autoware_autonomous_emergency_braking/test/test.hpp

@@ -0,0 +1,116 @@
+// Copyright 2024 TIER IV
+//
+// 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.
+
+#ifndef TEST_HPP_
+#define TEST_HPP_
+
+#include "ament_index_cpp/get_package_share_directory.hpp"
+#include "autoware_test_utils/autoware_test_utils.hpp"
+#include "autoware_test_utils/mock_data_parser.hpp"
+#include "gtest/gtest.h"
+
+#include <autoware/autonomous_emergency_braking/node.hpp>
+#include <autoware/autonomous_emergency_braking/utils.hpp>
+#include <rclcpp/clock.hpp>
+#include <rclcpp/logging.hpp>
+#include <rclcpp/time.hpp>
+
+#include <memory>
+#include <string>
+#include <vector>
+namespace autoware::motion::control::autonomous_emergency_braking::test
+{
+using autoware_planning_msgs::msg::Trajectory;
+using autoware_system_msgs::msg::AutowareState;
+using autoware_vehicle_msgs::msg::VelocityReport;
+using nav_msgs::msg::Odometry;
+using sensor_msgs::msg::Imu;
+using sensor_msgs::msg::PointCloud2;
+using PointCloud = pcl::PointCloud<pcl::PointXYZ>;
+using autoware::universe_utils::Polygon2d;
+using autoware::vehicle_info_utils::VehicleInfo;
+using diagnostic_updater::DiagnosticStatusWrapper;
+using diagnostic_updater::Updater;
+using visualization_msgs::msg::Marker;
+using visualization_msgs::msg::MarkerArray;
+using Path = std::vector<geometry_msgs::msg::Pose>;
+using Vector3 = geometry_msgs::msg::Vector3;
+using autoware_perception_msgs::msg::PredictedObject;
+using autoware_perception_msgs::msg::PredictedObjects;
+using std_msgs::msg::Header;
+
+std::shared_ptr<AEB> generateNode();
+Header get_header(const char * const frame_id, rclcpp::Time t);
+Imu make_imu_message(
+  const Header & header, const double ax, const double ay, const double yaw,
+  const double angular_velocity_z);
+VelocityReport make_velocity_report_msg(
+  const Header & header, const double lat_velocity, const double long_velocity,
+  const double heading_rate);
+class PubSubNode : public rclcpp::Node
+{
+public:
+  explicit PubSubNode(const rclcpp::NodeOptions & node_options);
+  // publisher
+  rclcpp::Publisher<Imu>::SharedPtr pub_imu_;
+  rclcpp::Publisher<PointCloud2>::SharedPtr pub_point_cloud_;
+  rclcpp::Publisher<VelocityReport>::SharedPtr pub_velocity_;
+  rclcpp::Publisher<Trajectory>::SharedPtr pub_predicted_traj_;
+  rclcpp::Publisher<PredictedObjects>::SharedPtr pub_predicted_objects_;
+  rclcpp::Publisher<AutowareState>::SharedPtr pub_autoware_state_;
+  // timer
+  // rclcpp::TimerBase::SharedPtr timer_;
+  void publishDefaultTopicsNoSpin()
+  {
+    const auto header = get_header("base_link", now());
+    const auto imu_msg = make_imu_message(header, 0.0, 0.0, 0.0, 0.05);
+    const auto velocity_msg = make_velocity_report_msg(header, 0.0, 3.0, 0.0);
+
+    pub_imu_->publish(imu_msg);
+    pub_velocity_->publish(velocity_msg);
+  };
+};
+
+std::shared_ptr<PubSubNode> generatePubSubNode();
+
+class TestAEB : public ::testing::Test
+{
+public:
+  TestAEB() {}
+  TestAEB(const TestAEB &) = delete;
+  TestAEB(TestAEB &&) = delete;
+  TestAEB & operator=(const TestAEB &) = delete;
+  TestAEB & operator=(TestAEB &&) = delete;
+  ~TestAEB() override = default;
+
+  void SetUp() override
+  {
+    rclcpp::init(0, nullptr);
+    pub_sub_node_ = generatePubSubNode();
+    aeb_node_ = generateNode();
+  }
+
+  void TearDown() override
+  {
+    aeb_node_.reset();
+    pub_sub_node_.reset();
+    rclcpp::shutdown();
+  }
+
+  std::shared_ptr<PubSubNode> pub_sub_node_;
+  std::shared_ptr<AEB> aeb_node_;
+};
+}  // namespace autoware::motion::control::autonomous_emergency_braking::test
+
+#endif  // TEST_HPP_

slave/src/Control/autoware_control_validator/CMakeLists.txt

@@ -0,0 +1,51 @@
+cmake_minimum_required(VERSION 3.22)
+project(autoware_control_validator)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_library(autoware_control_validator_helpers SHARED
+  src/utils.cpp
+  src/debug_marker.cpp
+)
+
+# control validator
+ament_auto_add_library(autoware_control_validator_component SHARED
+  include/autoware/control_validator/control_validator.hpp
+  src/control_validator.cpp
+)
+target_link_libraries(autoware_control_validator_component autoware_control_validator_helpers)
+rclcpp_components_register_node(autoware_control_validator_component
+  PLUGIN "autoware::control_validator::ControlValidator"
+  EXECUTABLE autoware_control_validator_node
+)
+
+rosidl_generate_interfaces(
+  ${PROJECT_NAME}
+  "msg/ControlValidatorStatus.msg"
+  DEPENDENCIES builtin_interfaces
+)
+
+# to use a message defined in the same package
+if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0)
+  rosidl_target_interfaces(autoware_control_validator_component
+    ${PROJECT_NAME} "rosidl_typesupport_cpp")
+else()
+  rosidl_get_typesupport_target(
+    cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp")
+  target_link_libraries(autoware_control_validator_component "${cpp_typesupport_target}")
+endif()
+
+if(BUILD_TESTING)
+  file(GLOB_RECURSE TEST_SOURCES test/*.cpp)
+  ament_add_gtest(test_autoware_control_validator
+    ${TEST_SOURCES}
+  )
+  target_link_libraries(test_autoware_control_validator autoware_control_validator_component)
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  config
+  launch
+)

slave/src/Control/autoware_control_validator/README.md

@@ -0,0 +1,65 @@
+# Control Validator
+
+The `control_validator` is a module that checks the validity of the output of the control component. The status of the validation can be viewed in the `/diagnostics` topic.
+
+![control_validator](./image/control_validator.drawio.svg)
+
+## Supported features
+
+The following features are supported for the validation and can have thresholds set by parameters.
+The listed features below does not always correspond to the latest implementation.
+
+| Description                                                                        | Arguments                                                                                       |                  Diagnostic equation                  |
+| ---------------------------------------------------------------------------------- | ----------------------------------------------------------------------------------------------- | :---------------------------------------------------: |
+| Inverse velocity: Measured velocity has a different sign from the target velocity. | measured velocity $v$, target velocity $\hat{v}$, and velocity parameter $c$                    |      $v \hat{v} < 0, \quad \lvert v \rvert > c$       |
+| Overspeed: Measured speed exceeds target speed significantly.                      | measured velocity $v$, target velocity $\hat{v}$, ratio parameter $r$, and offset parameter $c$ | $\lvert v \rvert > (1 + r) \lvert \hat{v} \rvert + c$ |
+
+- **Deviation check between reference trajectory and predicted trajectory** : invalid when the largest deviation between the predicted trajectory and reference trajectory is greater than the given threshold.
+
+![trajectory_deviation](./image/trajectory_deviation.drawio.svg)
+
+## Inputs/Outputs
+
+### Inputs
+
+The `control_validator` takes in the following inputs:
+
+| Name                           | Type                              | Description                                                                    |
+| ------------------------------ | --------------------------------- | ------------------------------------------------------------------------------ |
+| `~/input/kinematics`           | nav_msgs/Odometry                 | ego pose and twist                                                             |
+| `~/input/reference_trajectory` | autoware_planning_msgs/Trajectory | reference trajectory which is outputted from planning module to to be followed |
+| `~/input/predicted_trajectory` | autoware_planning_msgs/Trajectory | predicted trajectory which is outputted from control module                    |
+
+### Outputs
+
+It outputs the following:
+
+| Name                         | Type                                     | Description                                                               |
+| ---------------------------- | ---------------------------------------- | ------------------------------------------------------------------------- |
+| `~/output/validation_status` | control_validator/ControlValidatorStatus | validator status to inform the reason why the trajectory is valid/invalid |
+| `/diagnostics`               | diagnostic_msgs/DiagnosticStatus         | diagnostics to report errors                                              |
+
+## Parameters
+
+The following parameters can be set for the `control_validator`:
+
+### System parameters
+
+| Name                         | Type | Description                                                                                                                                                                                                                                | Default value |
+| :--------------------------- | :--- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
+| `publish_diag`               | bool | if true, diagnostics msg is published.                                                                                                                                                                                                     | true          |
+| `diag_error_count_threshold` | int  | the Diag will be set to ERROR when the number of consecutive invalid trajectory exceeds this threshold. (For example, threshold = 1 means, even if the trajectory is invalid, the Diag will not be ERROR if the next trajectory is valid.) | true          |
+| `display_on_terminal`        | bool | show error msg on terminal                                                                                                                                                                                                                 | true          |
+
+### Algorithm parameters
+
+#### Thresholds
+
+The input trajectory is detected as invalid if the index exceeds the following thresholds.
+
+| Name                                | Type   | Description                                                                                                 | Default value |
+| :---------------------------------- | :----- | :---------------------------------------------------------------------------------------------------------- | :------------ |
+| `thresholds.max_distance_deviation` | double | invalid threshold of the max distance deviation between the predicted path and the reference trajectory [m] | 1.0           |
+| `thresholds.rolling_back_velocity`  | double | threshold velocity to valid the vehicle velocity [m/s]                                                      | 0.5           |
+| `thresholds.over_velocity_offset`   | double | threshold velocity offset to valid the vehicle velocity [m/s]                                               | 2.0           |
+| `thresholds.over_velocity_ratio`    | double | threshold ratio to valid the vehicle velocity [*]                                                           | 0.2           |

slave/src/Control/autoware_control_validator/config/control_validator.param.yaml

@@ -0,0 +1,17 @@
+/**:
+  ros__parameters:
+    # If the number of consecutive invalid trajectory exceeds this threshold, the Diag will be set to ERROR.
+    # (For example, threshold = 1 means, even if the trajectory is invalid, Diag will not be ERROR if
+    #  the next trajectory is valid.)
+    diag_error_count_threshold: 0
+
+    display_on_terminal: false # show error msg on terminal
+
+    thresholds:
+      max_distance_deviation: 1.0
+      rolling_back_velocity: 0.5
+      over_velocity_offset: 2.0
+      over_velocity_ratio: 0.2
+
+    vel_lpf_gain: 0.9 # Time constant 0.33
+    hold_velocity_error_until_stop: true

slave/src/Control/autoware_control_validator/image/control_validator.drawio.svg

@@ -0,0 +1,155 @@
+<svg
+  xmlns="http://www.w3.org/2000/svg"
+  xmlns:xlink="http://www.w3.org/1999/xlink"
+  version="1.1"
+  width="496px"
+  height="165px"
+  viewBox="-0.5 -0.5 496 165"
+  content="&lt;mxfile host=&quot;0i6112g2df9b6ngu9buda4iblme4t77dtbb7m4umbjihr1cp3ej8&quot; modified=&quot;2023-08-12T13:28:30.582Z&quot; agent=&quot;Mozilla/5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) Code/1.81.1 Chrome/108.0.5359.215 Electron/22.3.18 Safari/537.36&quot; etag=&quot;H8KSlqQTqsTi61dqFnkA&quot; version=&quot;12.2.4&quot; pages=&quot;1&quot;&gt;&lt;diagram id=&quot;U03Aw2_6lbGUJ5V6ckVM&quot; name=&quot;ページ1&quot;&gt;7Vhdc9soFP01emxGH5YsPyZO2p3Z6TQT70f71CESlthg4QKK7f76XgRYQjhTN3XTdrzxTCwOcLm65xiOFCTz1fYNR+v6LSsxDeKw3AbJdRDHkzyD/wrYaSCexBqoOCk1FPXAgnzGBgwN2pISC2egZIxKsnbBgjUNLqSDIc7Zxh22ZNRddY0q7AGLAlEf/ZeUstZonoY9/gcmVW1XjkLTc4+Kh4qztjHrNazBumeFbBgzVNSoZJsBlNwEyZwzJvXVajvHVFXVVkzPe/1E7z5ljht5zATDyCOirblrqKbkjH4EiJRIMm4SlTtbl+7OsAoQBsnVpiYSL9aoUL0bEAJgtVxRaEVw6SdkcnzEXOLtADIJvsFshSXfwRDTm9iKGx2ltnibnpXIYvWQkcyAyCih2sfuSwIXpiqHK5R4FbrluCSFhAqAIDn6D7THTMxhmXBTXioVQqugSAhSuJWBgvDde1PFrvFBNS6iPLfA9XbYfb0btm4xJ3AzmBvwyULj0pG5X+ZBGdMDVbQYxxRJ8uj+OA5V1qxwywhksmcxDl0Ws3BEjmAtL7CZNZTsKNBYDhDoIpzO+j83rES8wtIL29G+L8JRSph6SsAV+/hAGgw/a1KIkykgTr8igN+O6/RUXI8DnY7dmcfuNUFVwwRQu5BItgf4hWovTLPb4112t0S+H1wPuYVmT61qfD+z30lZlo822elxlfYCTbLZxeDHOMvdsNCbJ0eJ4RkcWucwIHGuj7N/7Gl2hlRm8Q+h0gt7Qh59WxLEGYUyXi1Zl2FPXfapZbbjlehM5CUMiML1tu/sXCJl3JkRxEkeqs8Qyir1/dQRDwY0ULetc7nndvw91tkqjalJuh9uU6erB3miA/8jXaEJ0OoDnptUjRCXhNIRBCtVjTpWQFvKAVwpN0XAul6ajhUpS7XMQXvmGrgndXq8Q4vD6ehITgPPoSUHzpvkBAYt8h3ay4rlb9HpZKmscijrgRQIa85PC9H4yJ68oBbSn6yFv9T+IMD4jUQAe7poqbQ7iFaJ2AmJV3rZd3/Cvd3c3b27OzvFTDzrkXiK2TvKkysm+yUUU8NTgpXFXiqok5EFkeiEpLcZBqj6XrGypRj8zOtWYC7OUDuzkSnJ/N1mmh3QzuQE2vGfCG8pahrSVMH/rwae/7iYTkbP8M99XIRAL/NqIMq/tot8055g54o1ao7egbppo0B3eImB9QIf9LGda10yStnGMa16WTeVs9lRxl42/YH+BZr9q14tu/5NenLzBQ==&lt;/diagram&gt;&lt;/mxfile&gt;"
+>
+  <defs/>
+  <g>
+    <rect x="157" y="0" width="120" height="160" fill="#ffffff" stroke="#000000" pointer-events="all"/>
+    <g transform="translate(172.5,73.5)">
+      <foreignObject style="overflow:visible;" pointer-events="all" width="88" height="12">
+        <div
+          xmlns="http://www.w3.org/1999/xhtml"
+          style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 89px; white-space: nowrap; overflow-wrap: normal; text-align: center;"
+        >
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;white-space:normal;">control_validator</div>
+        </div>
+      </foreignObject>
+    </g>
+    <path d="M 7 80 L 150.63 80.08" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 155.88 80.08 L 148.88 83.58 L 150.63 80.08 L 148.88 76.58 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="all"/>
+    <g transform="translate(33.5,74.5)">
+      <foreignObject style="overflow:visible;" pointer-events="all" width="95" height="11">
+        <div
+          xmlns="http://www.w3.org/1999/xhtml"
+          style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; white-space: nowrap; text-align: center;"
+        >
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;background-color:#ffffff;">Predicted trajectory</div>
+        </div>
+      </foreignObject>
+    </g>
+    <path d="M 7 130 L 150.63 130" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 155.88 130 L 148.88 133.5 L 150.63 130 L 148.88 126.5 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="all"/>
+    <g transform="translate(43.5,124.5)">
+      <foreignObject style="overflow:visible;" pointer-events="all" width="76" height="11">
+        <div
+          xmlns="http://www.w3.org/1999/xhtml"
+          style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; white-space: nowrap; text-align: center;"
+        >
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;background-color:#ffffff;">ego_kinematics</div>
+        </div>
+      </foreignObject>
+    </g>
+    <path d="M 277 49.83 L 480.63 49.99" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 485.88 50 L 478.88 53.49 L 480.63 49.99 L 478.88 46.49 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="all"/>
+    <g transform="translate(340.5,43.5)">
+      <foreignObject style="overflow:visible;" pointer-events="all" width="82" height="11">
+        <div
+          xmlns="http://www.w3.org/1999/xhtml"
+          style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; white-space: nowrap; text-align: center;"
+        >
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;background-color:#ffffff;">DiagnosticStatus</div>
+        </div>
+      </foreignObject>
+    </g>
+    <path d="M 277 100 L 480.63 100" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 485.88 100 L 478.88 103.5 L 480.63 100 L 478.88 96.5 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="all"/>
+    <g transform="translate(326.5,94.5)">
+      <foreignObject style="overflow:visible;" pointer-events="all" width="109" height="11">
+        <div
+          xmlns="http://www.w3.org/1999/xhtml"
+          style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; white-space: nowrap; text-align: center;"
+        >
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;background-color:#ffffff;">ControlValidatorStatus</div>
+        </div>
+      </foreignObject>
+    </g>
+    <rect x="14" y="85" width="130" height="30" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(30.5,86.5)">
+      <foreignObject style="overflow:visible;" pointer-events="all" width="97" height="27">
+        <div
+          xmlns="http://www.w3.org/1999/xhtml"
+          style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 98px; white-space: nowrap; overflow-wrap: normal; text-align: center;"
+        >
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;white-space:normal;">
+            <font style="font-size: 10px" color="#808080">
+              Predicted trajectory to
+              <br/>
+              be validated
+            </font>
+          </div>
+        </div>
+      </foreignObject>
+    </g>
+    <rect x="17" y="134" width="130" height="30" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(32.5,142.5)">
+      <foreignObject style="overflow:visible;" pointer-events="all" width="99" height="12">
+        <div
+          xmlns="http://www.w3.org/1999/xhtml"
+          style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 100px; white-space: nowrap; overflow-wrap: normal; text-align: center;"
+        >
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;white-space:normal;">
+            <font style="font-size: 10px" color="#808080">Used for the validation</font>
+          </div>
+        </div>
+      </foreignObject>
+    </g>
+    <rect x="287" y="53" width="200" height="30" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(287.5,54.5)">
+      <foreignObject style="overflow:visible;" pointer-events="all" width="198" height="27">
+        <div
+          xmlns="http://www.w3.org/1999/xhtml"
+          style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 198px; white-space: nowrap; overflow-wrap: normal; text-align: center;"
+        >
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;white-space:normal;">
+            <font style="font-size: 10px" color="#808080">To send validation result to the system: OK/ERROR</font>
+          </div>
+        </div>
+      </foreignObject>
+    </g>
+    <rect x="296" y="106" width="176" height="34" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(296.5,109.5)">
+      <foreignObject style="overflow:visible;" pointer-events="all" width="174" height="27">
+        <div
+          xmlns="http://www.w3.org/1999/xhtml"
+          style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 174px; white-space: nowrap; overflow-wrap: normal; text-align: center;"
+        >
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;white-space:normal;">
+            <font style="font-size: 10px" color="#808080">To show the result and the reason for other modules/users</font>
+          </div>
+        </div>
+      </foreignObject>
+    </g>
+    <path d="M 7 29 L 150.63 29.08" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 155.88 29.08 L 148.88 32.58 L 150.63 29.08 L 148.88 25.58 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="all"/>
+    <g transform="translate(35.5,23.5)">
+      <foreignObject style="overflow:visible;" pointer-events="all" width="91" height="11">
+        <div
+          xmlns="http://www.w3.org/1999/xhtml"
+          style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; white-space: nowrap; text-align: center;"
+        >
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;background-color:#ffffff;">Planning trajectory</div>
+        </div>
+      </foreignObject>
+    </g>
+    <rect x="14" y="34" width="130" height="30" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(14.5,35.5)">
+      <foreignObject style="overflow:visible;" pointer-events="all" width="128" height="27">
+        <div
+          xmlns="http://www.w3.org/1999/xhtml"
+          style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 128px; white-space: nowrap; overflow-wrap: normal; text-align: center;"
+        >
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;white-space:normal;">
+            <font color="#808080">
+              <span style="font-size: 10px">Reference trajectory to be followed</span>
+            </font>
+          </div>
+        </div>
+      </foreignObject>
+    </g>
+  </g>
+</svg>

slave/src/Control/autoware_control_validator/image/trajectory_deviation.drawio.svg

@@ -0,0 +1,112 @@
+<svg
+  host="65bd71144e"
+  xmlns="http://www.w3.org/2000/svg"
+  xmlns:xlink="http://www.w3.org/1999/xlink"
+  version="1.1"
+  width="524px"
+  height="298px"
+  viewBox="-0.5 -0.5 524 298"
+  content="&lt;mxfile pages=&quot;1&quot;&gt;&lt;diagram id=&quot;U03Aw2_6lbGUJ5V6ckVM&quot; name=&quot;ページ1&quot;&gt;7Vpdk5sgFP01ee2I4NdjN920L53pdDvTZ6qs0hLJINlk++uLETRgsjXOmto2edhZj3DRcy5c7sUFXK737wXeFB95RtjC97L9Ar5b+H4ShupvDTw3APKjBsgFzRoIdMAD/Uk06Gl0SzNSWQ0l50zSjQ2mvCxJKi0MC8F3drNHzuxRNzgnPeAhxayPfqWZLBo0DrwO/0BoXpiRgafvfMPpj1zwbanHK3lJmjtrbMzoplWBM747guD9Ai4F57L5b71fElazahhr+q3O3G0fWZBSDuoQNz2eMNvq1yY5188mnw0Vh5chdR+wgHe7gkrysMFpfXentFdYIddM39YWiZBkf/axQPuyyn0IXxMpnlUT08Hwo10HGp/YdUK0bYojEUKNYa193pruSFD/aB5Oc2LsHnHSI4SU2dvaxdTVQV2LAJIpP9JtuZAFz3mJ2X2H3qVb8dTSWUnBf7QuBltkyRkXh+FgkiyXq1XLbT3ApcweMRecIM5ggjAs6ZNt/hSbeoRPnKqBW+F85AiHHEEqvhUp0b2OHdMxFMS2oVZtY0hikRPZM6RUwc9HzTZ1g+r8A7vjmAfufKWx2HlOy+kwZwIDnIkxtaaR388sXG2ahe6R7mv3eY2p5ieRTQCE/al2wmHAa8w0NHdyXHeGwfXICWZODgxCmxwT369BTjhzchDwnQiGrkdONHdyQmiR4ydXJKe/45kXOYGX2OSEV5xWydzJQXa0akPIFciBQ/aFf5Sc0PYcEMbXI8efOzmRHa0AuF4oh0MW5Jcyil5C4B1+veQBuLlFhquivVjMPnlwt6KRbWJo8uAjJ75EgW3oTPIwYn8Ph4STm7TQA28CR1wwUtzYjY/TiTskHN7EhUH8JnLU9UdOXQjtZdoPwFTqoiHx/KYuAk6aZzKZi6V1Si3thmoCaYdUXW7SotCetW0edqm0yPBrDJnsdwJph2w0b9IGADhRMh4pbWjPWpBMN2vhTdoh0qLYltYPRkqb2IZAiCaTdkil9yZtL4OPRm6SA+jMWjjZJhkNqVPfpA0iO7kFYOQOOQjtWAu8yWItHFJlf0naF845g1PKQ7haJclfoKZ7QO27UXHsOWc8LLpeesyJnPXAj173mBP1PeUzeSRKrZQo+IvA30kquS6bHfuPJHv50nqgneqRMuZAmNG8VJepcgmi8Lu6PkhTzN7qG2uaZexcubH7pMF7pW8WPIdidKJuf8ol3TRoTIUR9SuMnwTJaCrVK/4f/Puxk6eiEwXeyfi/rFKUMlxVNP1NHGx+vZXT7y+sWMi+6QO8osz+Lme+ayqKnZQlcQLb4H2tOYwwhrxwqghpXv1I9zXeKyCjiv5m8cvIE1W88PLfnXsocg90p1v71GX3fVwjWff5Ibz/BQ==&lt;/diagram&gt;&lt;/mxfile&gt;"
+>
+  <defs/>
+  <g>
+    <rect x="20" y="232" width="120" height="60" rx="9" ry="9" fill="rgb(255, 255, 255)" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 118px; height: 1px; padding-top: 262px; margin-left: 21px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
+                ego
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="80" y="266" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">ego</text>
+      </switch>
+    </g>
+    <path d="M 140 262 Q 480 262.03 480 42" fill="none" stroke="#99ccff" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/>
+    <ellipse cx="202" cy="260" rx="5" ry="5" fill="rgb(255, 255, 255)" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <ellipse cx="145" cy="262" rx="5" ry="5" fill="rgb(255, 255, 255)" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <ellipse cx="261" cy="254" rx="5" ry="5" fill="rgb(255, 255, 255)" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <ellipse cx="317" cy="241" rx="5" ry="5" fill="rgb(255, 255, 255)" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <ellipse cx="368" cy="221" rx="5" ry="5" fill="rgb(255, 255, 255)" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <ellipse cx="414" cy="194" rx="5" ry="5" fill="rgb(255, 255, 255)" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <ellipse cx="452" cy="151" rx="5" ry="5" fill="rgb(255, 255, 255)" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <ellipse cx="474" cy="95" rx="5" ry="5" fill="rgb(255, 255, 255)" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <ellipse cx="481" cy="42" rx="5" ry="5" fill="rgb(255, 255, 255)" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <path d="M 147 259 Q 147 259 143 197" fill="none" stroke="#000000" stroke-miterlimit="10" stroke-dasharray="3 3" pointer-events="stroke"/>
+    <path d="M 201.5 253 Q 201.5 253 189 187" fill="none" stroke="#000000" stroke-miterlimit="10" stroke-dasharray="3 3" pointer-events="stroke"/>
+    <path d="M 258.75 249 Q 258.75 249 236 173" fill="none" stroke="#000000" stroke-miterlimit="10" stroke-dasharray="3 3" pointer-events="stroke"/>
+    <path d="M 316 236 Q 316 236 280 152" fill="none" stroke="#000000" stroke-miterlimit="10" stroke-dasharray="3 3" pointer-events="stroke"/>
+    <path d="M 365 216 Q 365 216 322 134" fill="none" stroke="#000000" stroke-miterlimit="10" stroke-dasharray="3 3" pointer-events="stroke"/>
+    <path d="M 411 190 Q 411 190 360 112" fill="none" stroke="#000000" stroke-miterlimit="10" stroke-dasharray="3 3" pointer-events="stroke"/>
+    <path d="M 448 147 Q 448 147 398 86" fill="none" stroke="#000000" stroke-miterlimit="10" stroke-dasharray="3 3" pointer-events="stroke"/>
+    <path d="M 469 93 Q 469 93 430 57" fill="none" stroke="#000000" stroke-miterlimit="10" stroke-dasharray="3 3" pointer-events="stroke"/>
+    <path d="M 477 39 Q 477 39 462 24" fill="none" stroke="#000000" stroke-miterlimit="10" stroke-dasharray="3 3" pointer-events="stroke"/>
+    <path d="M 20 212 Q 330 192 480 2" fill="none" stroke="#33ff99" stroke-width="5" stroke-miterlimit="10" pointer-events="stroke"/>
+    <rect x="0" y="169" width="150" height="30" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 148px; height: 1px; padding-top: 184px; margin-left: 1px;">
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
+                Reference Trajectory
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="75" y="188" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">Reference Trajectory</text>
+      </switch>
+    </g>
+    <rect x="186" y="267" width="150" height="30" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 148px; height: 1px; padding-top: 282px; margin-left: 187px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
+                Predicted Trajectory
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="261" y="286" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">Predicted Trajectory</text>
+      </switch>
+    </g>
+    <path d="M 376.92 206.84 Q 376.92 206.84 336.08 135.16" fill="none" stroke="#333333" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 379.89 212.06 L 372.46 207.09 L 376.92 206.84 L 379.41 203.13 Z" fill="#333333" stroke="#333333" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 333.11 129.94 L 340.54 134.91 L 336.08 135.16 L 333.59 138.87 Z" fill="#333333" stroke="#333333" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/>
+    <rect x="373" y="219" width="150" height="30" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 148px; height: 1px; padding-top: 234px; margin-left: 374px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
+                max distance deviation
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="448" y="238" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">max distance deviation</text>
+      </switch>
+    </g>
+  </g>
+  <switch>
+    <g requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"/>
+    <a transform="translate(0,-5)" xlink:href="https://www.diagrams.net/doc/faq/svg-export-text-problems" target="_blank">
+      <text text-anchor="middle" font-size="10px" x="50%" y="100%">Text is not SVG - cannot display</text>
+    </a>
+  </switch>
+</svg>

slave/src/Control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp

@@ -0,0 +1,177 @@
+// Copyright 2023 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.
+
+#ifndef AUTOWARE__CONTROL_VALIDATOR__CONTROL_VALIDATOR_HPP_
+#define AUTOWARE__CONTROL_VALIDATOR__CONTROL_VALIDATOR_HPP_
+
+#include "autoware/control_validator/debug_marker.hpp"
+#include "autoware/universe_utils/ros/polling_subscriber.hpp"
+#include "autoware_vehicle_info_utils/vehicle_info.hpp"
+#include "diagnostic_updater/diagnostic_updater.hpp"
+
+#include <autoware/signal_processing/lowpass_filter_1d.hpp>
+#include <autoware/universe_utils/system/stop_watch.hpp>
+#include <autoware_control_validator/msg/control_validator_status.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_planning_msgs/msg/trajectory.hpp>
+#include <diagnostic_msgs/msg/diagnostic_array.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <tier4_debug_msgs/msg/float64_stamped.hpp>
+
+#include <cstdint>
+#include <memory>
+#include <string>
+#include <tuple>
+#include <utility>
+
+namespace autoware::control_validator
+{
+using autoware_control_validator::msg::ControlValidatorStatus;
+using autoware_planning_msgs::msg::Trajectory;
+using autoware_planning_msgs::msg::TrajectoryPoint;
+using diagnostic_updater::DiagnosticStatusWrapper;
+using diagnostic_updater::Updater;
+using nav_msgs::msg::Odometry;
+
+struct ValidationParams
+{
+  double max_distance_deviation_threshold;
+  double rolling_back_velocity;
+  double over_velocity_ratio;
+  double over_velocity_offset;
+};
+
+/**
+ * @class ControlValidator
+ * @brief Validates control commands by comparing predicted trajectories against reference
+ * trajectories.
+ */
+class ControlValidator : public rclcpp::Node
+{
+public:
+  /**
+   * @brief Constructor
+   * @param options Node options
+   */
+  explicit ControlValidator(const rclcpp::NodeOptions & options);
+
+  /**
+   * @brief Callback function for the predicted trajectory.
+   * @param msg Predicted trajectory message
+   */
+  void on_predicted_trajectory(const Trajectory::ConstSharedPtr msg);
+
+  /**
+   * @brief Calculate the maximum lateral distance between the reference trajectory and predicted
+   * trajectory.
+   * @param predicted_trajectory Predicted trajectory
+   * @param reference_trajectory Reference trajectory
+   * @return A pair consisting of the maximum lateral deviation and a boolean indicating validity
+   */
+  std::pair<double, bool> calc_lateral_deviation_status(
+    const Trajectory & predicted_trajectory, const Trajectory & reference_trajectory) const;
+
+  void calc_velocity_deviation_status(
+    const Trajectory & reference_trajectory, const Odometry & kinematics);
+
+private:
+  /**
+   * @brief Setup diagnostic updater
+   */
+  void setup_diag();
+
+  /**
+   * @brief Setup parameters from the parameter server
+   */
+  void setup_parameters();
+
+  /**
+   * @brief Check if all required data is ready for validation
+   * @return Boolean indicating readiness of data
+   */
+  bool is_data_ready();
+
+  /**
+   * @brief Validate the predicted trajectory against the reference trajectory and current
+   * kinematics
+   * @param predicted_trajectory Predicted trajectory
+   * @param reference_trajectory Reference trajectory
+   * @param kinematics Current vehicle kinematics
+   */
+  void validate(
+    const Trajectory & predicted_trajectory, const Trajectory & reference_trajectory,
+    const Odometry & kinematics);
+
+  /**
+   * @brief Publish debug information
+   */
+  void publish_debug_info();
+
+  /**
+   * @brief Display validation status on terminal
+   */
+  void display_status();
+
+  /**
+   * @brief Set the diagnostic status
+   * @param stat Diagnostic status wrapper
+   * @param is_ok Boolean indicating if the status is okay
+   * @param msg Status message
+   */
+  void set_status(
+    DiagnosticStatusWrapper & stat, const bool & is_ok, const std::string & msg) const;
+
+  // Subscribers and publishers
+  rclcpp::Subscription<Trajectory>::SharedPtr sub_predicted_traj_;
+  universe_utils::InterProcessPollingSubscriber<Odometry>::SharedPtr sub_kinematics_;
+  universe_utils::InterProcessPollingSubscriber<Trajectory>::SharedPtr sub_reference_traj_;
+  rclcpp::Publisher<ControlValidatorStatus>::SharedPtr pub_status_;
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_markers_;
+  rclcpp::Publisher<tier4_debug_msgs::msg::Float64Stamped>::SharedPtr pub_processing_time_;
+
+  // system parameters
+  int64_t diag_error_count_threshold_ = 0;
+  bool display_on_terminal_ = true;
+
+  Updater diag_updater_{this};
+
+  ControlValidatorStatus validation_status_;
+  ValidationParams validation_params_;  // for thresholds
+  bool is_velocity_valid_{true};
+  autoware::signal_processing::LowpassFilter1d vehicle_vel_{0.0};
+  autoware::signal_processing::LowpassFilter1d target_vel_{0.0};
+  bool hold_velocity_error_until_stop_{false};
+
+  vehicle_info_utils::VehicleInfo vehicle_info_;
+
+  /**
+   * @brief Check if all validation criteria are met
+   * @param status Validation status
+   * @return Boolean indicating if all criteria are met
+   */
+  static bool is_all_valid(const ControlValidatorStatus & status);
+
+  Trajectory::ConstSharedPtr current_reference_trajectory_;
+  Trajectory::ConstSharedPtr current_predicted_trajectory_;
+
+  Odometry::ConstSharedPtr current_kinematics_;
+
+  autoware::universe_utils::StopWatch<std::chrono::milliseconds> stop_watch;
+
+  std::shared_ptr<ControlValidatorDebugMarkerPublisher> debug_pose_publisher_;
+};
+}  // namespace autoware::control_validator
+
+#endif  // AUTOWARE__CONTROL_VALIDATOR__CONTROL_VALIDATOR_HPP_

slave/src/Control/autoware_control_validator/include/autoware/control_validator/debug_marker.hpp

@@ -0,0 +1,70 @@
+// 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.
+
+#ifndef AUTOWARE__CONTROL_VALIDATOR__DEBUG_MARKER_HPP_
+#define AUTOWARE__CONTROL_VALIDATOR__DEBUG_MARKER_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_planning_msgs/msg/trajectory.hpp>
+#include <visualization_msgs/msg/marker.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+#include <map>
+#include <string>
+
+/**
+ * @brief Class for publishing debug markers
+ */
+class ControlValidatorDebugMarkerPublisher
+{
+public:
+  /**
+   * @brief Constructor
+   */
+  explicit ControlValidatorDebugMarkerPublisher(rclcpp::Node * node);
+
+  /**
+   * @brief Push a virtual wall
+   * @param pose pose of the virtual wall
+   */
+  void push_virtual_wall(const geometry_msgs::msg::Pose & pose);
+
+  /**
+   * @brief Push a warning message
+   * @param pose pose of the warning message
+   * @param msg warning message
+   */
+  void push_warning_msg(const geometry_msgs::msg::Pose & pose, const std::string & msg);
+
+  /**
+   * @brief Publish markers
+   */
+  void publish();
+
+  /**
+   * @brief Clear markers
+   */
+  void clear_markers();
+
+private:
+  rclcpp::Node * node_;
+  visualization_msgs::msg::MarkerArray marker_array_;
+  visualization_msgs::msg::MarkerArray marker_array_virtual_wall_;
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_viz_pub_;
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr virtual_wall_pub_;
+  std::map<std::string, int> marker_id_;
+};
+
+#endif  // AUTOWARE__CONTROL_VALIDATOR__DEBUG_MARKER_HPP_

slave/src/Control/autoware_control_validator/include/autoware/control_validator/utils.hpp

@@ -0,0 +1,40 @@
+// Copyright 2023 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.
+
+#ifndef AUTOWARE__CONTROL_VALIDATOR__UTILS_HPP_
+#define AUTOWARE__CONTROL_VALIDATOR__UTILS_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_planning_msgs/msg/trajectory.hpp>
+
+namespace autoware::control_validator
+{
+/**
+ * @brief Shift pose along the yaw direction
+ */
+void shift_pose(geometry_msgs::msg::Pose & pose, double longitudinal);
+
+/**
+ * @brief Calculate the maximum lateral distance between the reference trajectory and the predicted
+ * trajectory
+ * @param reference_trajectory reference trajectory
+ * @param predicted_trajectory predicted trajectory
+ */
+double calc_max_lateral_distance(
+  const autoware_planning_msgs::msg::Trajectory & reference_trajectory,
+  const autoware_planning_msgs::msg::Trajectory & predicted_trajectory);
+}  // namespace autoware::control_validator
+
+#endif  // AUTOWARE__CONTROL_VALIDATOR__UTILS_HPP_

slave/src/Control/autoware_control_validator/launch/control_validator.launch.xml

@@ -0,0 +1,16 @@
+<launch>
+  <arg name="control_validator_param_path" default="$(find-pkg-share autoware_control_validator)/config/control_validator.param.yaml"/>
+  <arg name="input_reference_trajectory" default="/planning/scenario_planning/trajectory"/>
+  <arg name="input_predicted_trajectory" default="/control/trajectory_follower/lateral/predicted_trajectory"/>
+
+  <node name="control_validator" exec="autoware_control_validator_node" pkg="autoware_control_validator" output="screen">
+    <!-- load config a file -->
+    <param from="$(var control_validator_param_path)"/>
+
+    <!-- remap topic name -->
+    <remap from="~/input/reference_trajectory" to="$(var input_reference_trajectory)"/>
+    <remap from="~/input/predicted_trajectory" to="$(var input_predicted_trajectory)"/>
+    <remap from="~/input/kinematics" to="/localization/kinematic_state"/>
+    <remap from="~/output/validation_status" to="~/validation_status"/>
+  </node>
+</launch>

slave/src/Control/autoware_control_validator/msg/ControlValidatorStatus.msg

@@ -0,0 +1,13 @@
+builtin_interfaces/Time stamp
+
+# states
+bool is_valid_max_distance_deviation
+bool is_rolling_back
+bool is_over_velocity
+
+# values
+float64 max_distance_deviation
+float64 target_vel
+float64 vehicle_vel
+
+int64 invalid_count

slave/src/Control/autoware_control_validator/package.xml

@@ -0,0 +1,46 @@
+<?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_validator</name>
+  <version>0.1.0</version>
+  <description>ros node for autoware_control_validator</description>
+  <maintainer email="kyoichi.sugahara@tier4.jp">Kyoichi Sugahara</maintainer>
+  <maintainer email="takamasa.horibe@tier4.jp">Takamasa Horibe</maintainer>
+  <maintainer email="makoto.kurihara@tier4.jp">Makoto Kurihara</maintainer>
+  <maintainer email="mamoru.sobue@tier4.jp">Mamoru Sobue</maintainer>
+  <maintainer email="takayuki.murooka@tier4.jp">Takayuki Murooka</maintainer>
+
+  <license>Apache License 2.0</license>
+
+  <author email="kyoichi.sugahara@tier4.jp">Kyoichi Sugahara</author>
+  <author email="takamasa.horibe@tier4.jp">Takamasa Horibe</author>
+  <author email="makoto.kurihara@tier4.jp">Makoto Kurihara</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+  <build_depend>rosidl_default_generators</build_depend>
+
+  <depend>autoware_motion_utils</depend>
+  <depend>autoware_planning_msgs</depend>
+  <depend>autoware_signal_processing</depend>
+  <depend>autoware_universe_utils</depend>
+  <depend>autoware_vehicle_info_utils</depend>
+  <depend>diagnostic_updater</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>visualization_msgs</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_test_utils</test_depend>
+
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

slave/src/Control/autoware_control_validator/src/control_validator.cpp

@@ -0,0 +1,282 @@
+// Copyright 2023 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.
+
+#include "autoware/control_validator/control_validator.hpp"
+
+#include "autoware/control_validator/utils.hpp"
+#include "autoware/motion_utils/trajectory/interpolation.hpp"
+#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp"
+
+#include <nav_msgs/msg/odometry.hpp>
+
+#include <cstdint>
+#include <memory>
+#include <string>
+
+namespace autoware::control_validator
+{
+using diagnostic_msgs::msg::DiagnosticStatus;
+
+ControlValidator::ControlValidator(const rclcpp::NodeOptions & options)
+: Node("control_validator", options), validation_params_(), vehicle_info_()
+{
+  using std::placeholders::_1;
+
+  sub_predicted_traj_ = create_subscription<Trajectory>(
+    "~/input/predicted_trajectory", 1,
+    std::bind(&ControlValidator::on_predicted_trajectory, this, _1));
+  sub_kinematics_ =
+    universe_utils::InterProcessPollingSubscriber<nav_msgs::msg::Odometry>::create_subscription(
+      this, "~/input/kinematics", 1);
+  sub_reference_traj_ =
+    autoware::universe_utils::InterProcessPollingSubscriber<Trajectory>::create_subscription(
+      this, "~/input/reference_trajectory", 1);
+
+  pub_status_ = create_publisher<ControlValidatorStatus>("~/output/validation_status", 1);
+
+  pub_markers_ = create_publisher<visualization_msgs::msg::MarkerArray>("~/output/markers", 1);
+
+  pub_processing_time_ =
+    this->create_publisher<tier4_debug_msgs::msg::Float64Stamped>("~/debug/processing_time_ms", 1);
+
+  debug_pose_publisher_ = std::make_shared<ControlValidatorDebugMarkerPublisher>(this);
+
+  setup_parameters();
+
+  setup_diag();
+}
+
+void ControlValidator::setup_parameters()
+{
+  diag_error_count_threshold_ = declare_parameter<int64_t>("diag_error_count_threshold");
+  display_on_terminal_ = declare_parameter<bool>("display_on_terminal");
+
+  {
+    auto & p = validation_params_;
+    const std::string t = "thresholds.";
+    p.max_distance_deviation_threshold = declare_parameter<double>(t + "max_distance_deviation");
+    p.rolling_back_velocity = declare_parameter<double>(t + "rolling_back_velocity");
+    p.over_velocity_offset = declare_parameter<double>(t + "over_velocity_offset");
+    p.over_velocity_ratio = declare_parameter<double>(t + "over_velocity_ratio");
+  }
+  const auto lpf_gain = declare_parameter<double>("vel_lpf_gain");
+  vehicle_vel_.setGain(lpf_gain);
+  target_vel_.setGain(lpf_gain);
+
+  hold_velocity_error_until_stop_ = declare_parameter<bool>("hold_velocity_error_until_stop");
+
+  try {
+    vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo();
+  } catch (...) {
+    vehicle_info_.front_overhang_m = 0.5;
+    vehicle_info_.wheel_base_m = 4.0;
+    RCLCPP_ERROR(
+      get_logger(),
+      "failed to get vehicle info. use default value. vehicle_info_.front_overhang_m: %.2f, "
+      "vehicle_info_.wheel_base_m: %.2f",
+      vehicle_info_.front_overhang_m, vehicle_info_.wheel_base_m);
+  }
+}
+
+void ControlValidator::set_status(
+  DiagnosticStatusWrapper & stat, const bool & is_ok, const std::string & msg) const
+{
+  if (is_ok) {
+    stat.summary(DiagnosticStatus::OK, "validated.");
+  } else if (validation_status_.invalid_count < diag_error_count_threshold_) {
+    const auto warn_msg = msg + " (invalid count is less than error threshold: " +
+                          std::to_string(validation_status_.invalid_count) + " < " +
+                          std::to_string(diag_error_count_threshold_) + ")";
+    stat.summary(DiagnosticStatus::WARN, warn_msg);
+  } else {
+    stat.summary(DiagnosticStatus::ERROR, msg);
+  }
+}
+
+void ControlValidator::setup_diag()
+{
+  auto & d = diag_updater_;
+  d.setHardwareID("control_validator");
+
+  std::string ns = "control_validation_";
+  d.add(ns + "max_distance_deviation", [&](auto & stat) {
+    set_status(
+      stat, validation_status_.is_valid_max_distance_deviation,
+      "control output is deviated from trajectory");
+  });
+  d.add(ns + "rolling_back", [&](auto & stat) {
+    set_status(
+      stat, !validation_status_.is_rolling_back,
+      "The vehicle is rolling back. The velocity has the opposite sign to the target.");
+  });
+  d.add(ns + "over_velocity", [&](auto & stat) {
+    set_status(
+      stat, !validation_status_.is_over_velocity,
+      "The vehicle is over-speeding against the target.");
+  });
+}
+
+bool ControlValidator::is_data_ready()
+{
+  const auto waiting = [this](const auto topic_name) {
+    RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for %s", topic_name);
+    return false;
+  };
+
+  if (!current_kinematics_) {
+    return waiting(sub_kinematics_->subscriber()->get_topic_name());
+  }
+  if (!current_reference_trajectory_) {
+    return waiting(sub_reference_traj_->subscriber()->get_topic_name());
+  }
+  if (!current_predicted_trajectory_) {
+    return waiting(sub_predicted_traj_->get_topic_name());
+  }
+  return true;
+}
+
+void ControlValidator::on_predicted_trajectory(const Trajectory::ConstSharedPtr msg)
+{
+  stop_watch.tic();
+
+  current_predicted_trajectory_ = msg;
+  current_reference_trajectory_ = sub_reference_traj_->takeData();
+  current_kinematics_ = sub_kinematics_->takeData();
+
+  if (!is_data_ready()) return;
+
+  debug_pose_publisher_->clear_markers();
+
+  validate(*current_predicted_trajectory_, *current_reference_trajectory_, *current_kinematics_);
+
+  diag_updater_.force_update();
+
+  // for debug
+  publish_debug_info();
+  display_status();
+}
+
+void ControlValidator::publish_debug_info()
+{
+  pub_status_->publish(validation_status_);
+
+  if (!is_all_valid(validation_status_)) {
+    geometry_msgs::msg::Pose front_pose = current_kinematics_->pose.pose;
+    shift_pose(front_pose, vehicle_info_.front_overhang_m + vehicle_info_.wheel_base_m);
+    debug_pose_publisher_->push_virtual_wall(front_pose);
+    debug_pose_publisher_->push_warning_msg(front_pose, "INVALID CONTROL");
+  }
+  debug_pose_publisher_->publish();
+
+  // Publish ProcessingTime
+  tier4_debug_msgs::msg::Float64Stamped processing_time_msg;
+  processing_time_msg.stamp = get_clock()->now();
+  processing_time_msg.data = stop_watch.toc();
+  pub_processing_time_->publish(processing_time_msg);
+}
+
+void ControlValidator::validate(
+  const Trajectory & predicted_trajectory, const Trajectory & reference_trajectory,
+  const Odometry & kinematics)
+{
+  if (predicted_trajectory.points.size() < 2) {
+    RCLCPP_ERROR_THROTTLE(
+      get_logger(), *get_clock(), 1000,
+      "predicted_trajectory size is less than 2. Cannot validate.");
+    return;
+  }
+  if (reference_trajectory.points.size() < 2) {
+    RCLCPP_ERROR_THROTTLE(
+      get_logger(), *get_clock(), 1000,
+      "reference_trajectory size is less than 2. Cannot validate.");
+    return;
+  }
+
+  validation_status_.stamp = get_clock()->now();
+
+  std::tie(
+    validation_status_.max_distance_deviation, validation_status_.is_valid_max_distance_deviation) =
+    calc_lateral_deviation_status(predicted_trajectory, *current_reference_trajectory_);
+
+  calc_velocity_deviation_status(*current_reference_trajectory_, kinematics);
+
+  validation_status_.invalid_count =
+    is_all_valid(validation_status_) ? 0 : validation_status_.invalid_count + 1;
+}
+
+std::pair<double, bool> ControlValidator::calc_lateral_deviation_status(
+  const Trajectory & predicted_trajectory, const Trajectory & reference_trajectory) const
+{
+  auto max_distance_deviation =
+    calc_max_lateral_distance(reference_trajectory, predicted_trajectory);
+  return {
+    max_distance_deviation,
+    max_distance_deviation <= validation_params_.max_distance_deviation_threshold};
+}
+
+void ControlValidator::calc_velocity_deviation_status(
+  const Trajectory & reference_trajectory, const Odometry & kinematics)
+{
+  auto & status = validation_status_;
+  const auto & params = validation_params_;
+  status.vehicle_vel = vehicle_vel_.filter(kinematics.twist.twist.linear.x);
+  status.target_vel = target_vel_.filter(
+    autoware::motion_utils::calcInterpolatedPoint(reference_trajectory, kinematics.pose.pose)
+      .longitudinal_velocity_mps);
+
+  const bool is_rolling_back = std::signbit(status.vehicle_vel * status.target_vel) &&
+                               std::abs(status.vehicle_vel) > params.rolling_back_velocity;
+  if (
+    !hold_velocity_error_until_stop_ || !status.is_rolling_back ||
+    std::abs(status.vehicle_vel) < 0.05) {
+    status.is_rolling_back = is_rolling_back;
+  }
+
+  const bool is_over_velocity =
+    std::abs(status.vehicle_vel) >
+    std::abs(status.target_vel) * (1.0 + params.over_velocity_ratio) + params.over_velocity_offset;
+  if (
+    !hold_velocity_error_until_stop_ || !status.is_over_velocity ||
+    std::abs(status.vehicle_vel) < 0.05) {
+    status.is_over_velocity = is_over_velocity;
+  }
+}
+
+bool ControlValidator::is_all_valid(const ControlValidatorStatus & s)
+{
+  return s.is_valid_max_distance_deviation && !s.is_rolling_back && !s.is_over_velocity;
+}
+
+void ControlValidator::display_status()
+{
+  if (!display_on_terminal_) return;
+  rclcpp::Clock clock{RCL_ROS_TIME};
+
+  const auto warn = [this, &clock](const bool status, const std::string & msg) {
+    if (!status) {
+      RCLCPP_WARN_THROTTLE(get_logger(), clock, 1000, "%s", msg.c_str());
+    }
+  };
+
+  const auto & s = validation_status_;
+
+  warn(
+    s.is_valid_max_distance_deviation,
+    "predicted trajectory is too far from planning trajectory!!");
+}
+
+}  // namespace autoware::control_validator
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(autoware::control_validator::ControlValidator)

slave/src/Control/autoware_control_validator/src/debug_marker.cpp

@@ -0,0 +1,65 @@
+// 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.
+
+#include "autoware/control_validator/debug_marker.hpp"
+
+#include <autoware/motion_utils/marker/marker_helper.hpp>
+#include <autoware/universe_utils/ros/marker_helper.hpp>
+
+#include <string>
+
+using visualization_msgs::msg::Marker;
+
+ControlValidatorDebugMarkerPublisher::ControlValidatorDebugMarkerPublisher(rclcpp::Node * node)
+: node_(node)
+{
+  debug_viz_pub_ =
+    node_->create_publisher<visualization_msgs::msg::MarkerArray>("~/debug/marker", 1);
+
+  virtual_wall_pub_ =
+    node_->create_publisher<visualization_msgs::msg::MarkerArray>("~/virtual_wall", 1);
+}
+
+void ControlValidatorDebugMarkerPublisher::clear_markers()
+{
+  marker_array_.markers.clear();
+  marker_array_virtual_wall_.markers.clear();
+}
+
+void ControlValidatorDebugMarkerPublisher::push_warning_msg(
+  const geometry_msgs::msg::Pose & pose, const std::string & msg)
+{
+  visualization_msgs::msg::Marker marker = autoware::universe_utils::createDefaultMarker(
+    "map", node_->get_clock()->now(), "warning_msg", 0, Marker::TEXT_VIEW_FACING,
+    autoware::universe_utils::createMarkerScale(0.0, 0.0, 1.0),
+    autoware::universe_utils::createMarkerColor(1.0, 0.1, 0.1, 0.999));
+  marker.lifetime = rclcpp::Duration::from_seconds(0.2);
+  marker.pose = pose;
+  marker.text = msg;
+  marker_array_virtual_wall_.markers.push_back(marker);
+}
+
+void ControlValidatorDebugMarkerPublisher::push_virtual_wall(const geometry_msgs::msg::Pose & pose)
+{
+  const auto now = node_->get_clock()->now();
+  const auto stop_wall_marker =
+    autoware::motion_utils::createStopVirtualWallMarker(pose, "control_validator", now, 0);
+  autoware::universe_utils::appendMarkerArray(stop_wall_marker, &marker_array_virtual_wall_, now);
+}
+
+void ControlValidatorDebugMarkerPublisher::publish()
+{
+  debug_viz_pub_->publish(marker_array_);
+  virtual_wall_pub_->publish(marker_array_virtual_wall_);
+}

slave/src/Control/autoware_control_validator/src/utils.cpp

@@ -0,0 +1,177 @@
+// Copyright 2023 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.
+
+#include "autoware/control_validator/utils.hpp"
+
+#include "autoware/motion_utils/trajectory/conversion.hpp"
+#include "autoware/motion_utils/trajectory/interpolation.hpp"
+#include "autoware/motion_utils/trajectory/trajectory.hpp"
+#include "autoware/universe_utils/geometry/geometry.hpp"
+
+#include <vector>
+
+namespace autoware::control_validator
+{
+
+using autoware::motion_utils::convertToTrajectory;
+using autoware::motion_utils::convertToTrajectoryPointArray;
+using autoware_planning_msgs::msg::Trajectory;
+using autoware_planning_msgs::msg::TrajectoryPoint;
+using geometry_msgs::msg::Pose;
+using TrajectoryPoints = std::vector<TrajectoryPoint>;
+
+void shift_pose(Pose & pose, double longitudinal)
+{
+  const auto yaw = tf2::getYaw(pose.orientation);
+  pose.position.x += std::cos(yaw) * longitudinal;
+  pose.position.y += std::sin(yaw) * longitudinal;
+}
+
+/**
+ * @brief Insert interpolated point along the predicted_trajectory to the modified_trajectory
+ * @param[inout] modified_trajectory modified trajectory
+ * @param[in] reference_pose reference pose
+ * @param[in] predicted_trajectory predicted trajectory
+ */
+void insert_point_in_predicted_trajectory(
+  TrajectoryPoints & modified_trajectory, const Pose & reference_pose,
+  const TrajectoryPoints & predicted_trajectory)
+{
+  const auto point_to_interpolate = autoware::motion_utils::calcInterpolatedPoint(
+    convertToTrajectory(predicted_trajectory), reference_pose);
+  modified_trajectory.insert(modified_trajectory.begin(), point_to_interpolate);
+}
+
+TrajectoryPoints reverse_trajectory_points(const TrajectoryPoints & trajectory_points)
+{
+  TrajectoryPoints reversed_trajectory_points;
+  reversed_trajectory_points.reserve(trajectory_points.size());
+  std::reverse_copy(
+    trajectory_points.begin(), trajectory_points.end(),
+    std::back_inserter(reversed_trajectory_points));
+  return reversed_trajectory_points;
+}
+
+bool remove_front_trajectory_point(
+  const TrajectoryPoints & trajectory_points, TrajectoryPoints & modified_trajectory_points,
+  const TrajectoryPoints & predicted_trajectory_points)
+{
+  bool predicted_trajectory_point_removed = false;
+  for (const auto & point : predicted_trajectory_points) {
+    if (
+      autoware::motion_utils::calcLongitudinalOffsetToSegment(
+        trajectory_points, 0, point.pose.position) < 0.0) {
+      modified_trajectory_points.erase(modified_trajectory_points.begin());
+
+      predicted_trajectory_point_removed = true;
+    } else {
+      break;
+    }
+  }
+
+  return predicted_trajectory_point_removed;
+}
+
+Trajectory align_trajectory_with_reference_trajectory(
+  const Trajectory & trajectory, const Trajectory & predicted_trajectory)
+{
+  const auto last_seg_length = autoware::motion_utils::calcSignedArcLength(
+    trajectory.points, trajectory.points.size() - 2, trajectory.points.size() - 1);
+
+  // If no overlapping between trajectory and predicted_trajectory, return empty trajectory
+  // predicted_trajectory:   p1------------------pN
+  // trajectory:                                     t1------------------tN
+  //     OR
+  // predicted_trajectory:                           p1------------------pN
+  // trajectory:             t1------------------tN
+  const bool & is_p_n_before_t1 =
+    autoware::motion_utils::calcLongitudinalOffsetToSegment(
+      trajectory.points, 0, predicted_trajectory.points.back().pose.position) < 0.0;
+  const bool & is_p1_behind_t_n = autoware::motion_utils::calcLongitudinalOffsetToSegment(
+                                    trajectory.points, trajectory.points.size() - 2,
+                                    predicted_trajectory.points.front().pose.position) -
+                                    last_seg_length >
+                                  0.0;
+  const bool is_no_overlapping = (is_p_n_before_t1 || is_p1_behind_t_n);
+
+  if (is_no_overlapping) {
+    return Trajectory();
+  }
+
+  auto modified_trajectory_points = convertToTrajectoryPointArray(predicted_trajectory);
+  auto predicted_trajectory_points = convertToTrajectoryPointArray(predicted_trajectory);
+  auto trajectory_points = convertToTrajectoryPointArray(trajectory);
+
+  // If first point of predicted_trajectory is in front of start of trajectory, erase points which
+  // are in front of trajectory start point and insert pNew along the predicted_trajectory
+  // predicted_trajectory:           p1-----p2-----p3----//------pN
+  // trajectory:                               t1--------//------tN
+  // ↓
+  // predicted_trajectory:                   pNew--p3----//------pN
+  // trajectory:                               t1--------//------tN
+  auto predicted_trajectory_point_removed = remove_front_trajectory_point(
+    trajectory_points, modified_trajectory_points, predicted_trajectory_points);
+
+  if (predicted_trajectory_point_removed) {
+    insert_point_in_predicted_trajectory(
+      modified_trajectory_points, trajectory_points.front().pose, predicted_trajectory_points);
+  }
+
+  // If last point of predicted_trajectory is behind of end of trajectory, erase points which are
+  // behind trajectory last point and insert pNew along the predicted_trajectory
+  // predicted_trajectory:           p1-----//------pN-2-----pN-1-----pN
+  // trajectory:                     t1-----//-----tN-1--tN
+  // ↓
+  // predicted_trajectory:           p1-----//------pN-2-pNew
+  // trajectory:                     t1-----//-----tN-1--tN
+
+  auto reversed_predicted_trajectory_points =
+    reverse_trajectory_points(predicted_trajectory_points);
+  auto reversed_trajectory_points = reverse_trajectory_points(trajectory_points);
+  auto reversed_modified_trajectory_points = reverse_trajectory_points(modified_trajectory_points);
+
+  auto reversed_predicted_trajectory_point_removed = remove_front_trajectory_point(
+    reversed_trajectory_points, reversed_modified_trajectory_points,
+    reversed_predicted_trajectory_points);
+
+  if (reversed_predicted_trajectory_point_removed) {
+    insert_point_in_predicted_trajectory(
+      reversed_modified_trajectory_points, reversed_trajectory_points.front().pose,
+      reversed_predicted_trajectory_points);
+  }
+
+  return convertToTrajectory(reverse_trajectory_points(reversed_modified_trajectory_points));
+}
+
+double calc_max_lateral_distance(
+  const Trajectory & reference_trajectory, const Trajectory & predicted_trajectory)
+{
+  const auto alined_predicted_trajectory =
+    align_trajectory_with_reference_trajectory(reference_trajectory, predicted_trajectory);
+  double max_dist = 0;
+  for (const auto & point : alined_predicted_trajectory.points) {
+    const auto p0 = autoware::universe_utils::getPoint(point);
+    // find nearest segment
+    const size_t nearest_segment_idx =
+      autoware::motion_utils::findNearestSegmentIndex(reference_trajectory.points, p0);
+    const double temp_dist = std::abs(autoware::motion_utils::calcLateralOffset(
+      reference_trajectory.points, p0, nearest_segment_idx));
+    if (temp_dist > max_dist) {
+      max_dist = temp_dist;
+    }
+  }
+  return max_dist;
+}
+
+}  // namespace autoware::control_validator

slave/src/Control/autoware_control_validator/test/test_control_validator.cpp

@@ -0,0 +1,170 @@
+// Copyright 2024 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.
+
+#include "autoware/control_validator/control_validator.hpp"
+
+#include <ament_index_cpp/get_package_share_directory.hpp>
+#include <rclcpp/node_options.hpp>
+
+#include <autoware_planning_msgs/msg/trajectory.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+#include <gtest/gtest-param-test.h>
+#include <gtest/gtest.h>
+#include <tf2/LinearMath/Quaternion.h>
+
+#include <tuple>
+
+using autoware_planning_msgs::msg::Trajectory;
+using autoware_planning_msgs::msg::TrajectoryPoint;
+
+Trajectory make_linear_trajectory(
+  const TrajectoryPoint & start, const TrajectoryPoint & end, size_t num_points, double velocity)
+{
+  auto create_quaternion = [](double yaw) {
+    tf2::Quaternion q;
+    q.setRPY(0, 0, yaw);
+    return tf2::toMsg(q);
+  };
+
+  double yaw = std::atan2(
+    end.pose.position.y - start.pose.position.y, end.pose.position.x - start.pose.position.x);
+  yaw += (velocity < 0) ? M_PI : 0;
+
+  Trajectory trajectory;
+  trajectory.points.reserve(num_points);
+
+  for (size_t i = 0; i < num_points; ++i) {
+    double ratio = static_cast<double>(i) / static_cast<double>(num_points - 1);
+
+    TrajectoryPoint point;
+    point.pose.position.x =
+      start.pose.position.x + ratio * (end.pose.position.x - start.pose.position.x);
+    point.pose.position.y =
+      start.pose.position.y + ratio * (end.pose.position.y - start.pose.position.y);
+    point.pose.orientation = create_quaternion(yaw);
+    point.longitudinal_velocity_mps = static_cast<float>(velocity);
+    point.lateral_velocity_mps = 0.0;
+
+    trajectory.points.emplace_back(point);
+  }
+
+  return trajectory;
+}
+
+TrajectoryPoint make_trajectory_point(double x, double y)
+{
+  TrajectoryPoint point;
+  point.pose.position.x = x;
+  point.pose.position.y = y;
+  return point;
+}
+
+class ControlValidatorTest
+: public ::testing::TestWithParam<std::tuple<Trajectory, Trajectory, double, bool>>
+{
+protected:
+  void SetUp() override
+  {
+    rclcpp::init(0, nullptr);
+    rclcpp::NodeOptions options;
+    options.arguments(
+      {"--ros-args", "--params-file",
+       ament_index_cpp::get_package_share_directory("autoware_control_validator") +
+         "/config/control_validator.param.yaml",
+       "--params-file",
+       ament_index_cpp::get_package_share_directory("autoware_test_utils") +
+         "/config/test_vehicle_info.param.yaml"});
+
+    node = std::make_shared<autoware::control_validator::ControlValidator>(options);
+  }
+
+  void TearDown() override { rclcpp::shutdown(); }
+
+  std::shared_ptr<autoware::control_validator::ControlValidator> node;
+};
+
+TEST_P(ControlValidatorTest, test_calc_lateral_deviation_status)
+{
+  auto [reference_trajectory, predicted_trajectory, expected_deviation, expected_condition] =
+    GetParam();
+  auto [deviation, is_valid] =
+    node->calc_lateral_deviation_status(predicted_trajectory, reference_trajectory);
+
+  EXPECT_EQ(is_valid, expected_condition);
+  EXPECT_NEAR(deviation, expected_deviation, 1e-5);
+}
+
+INSTANTIATE_TEST_SUITE_P(
+  ControlValidatorTests, ControlValidatorTest,
+  ::testing::Values(
+
+    std::make_tuple(
+      make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 0), 11, 1.0),
+      make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 0.99), 11, 1.0),
+      0.99, true),
+
+    std::make_tuple(
+      make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 0), 11, 1.0),
+      make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 1.0), 11, 1.0),
+      1.0, true),
+
+    std::make_tuple(
+      make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 0), 11, 1.0),
+      make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 1.01), 11, 1.0),
+      1.01, false),
+
+    std::make_tuple(
+      make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 0), 11, -1.0),
+      make_linear_trajectory(
+        make_trajectory_point(0, 0), make_trajectory_point(10, 0.99), 11, -1.0),
+      0.99, true),
+
+    std::make_tuple(
+      make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 0), 11, -1.0),
+      make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 1.0), 11, -1.0),
+      1.0, true),
+
+    std::make_tuple(
+      make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 0), 11, -1.0),
+      make_linear_trajectory(
+        make_trajectory_point(0, 0), make_trajectory_point(10, 1.01), 11, -1.0),
+      1.01, false),
+
+    std::make_tuple(
+      make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 0), 11, 1.0),
+      make_linear_trajectory(make_trajectory_point(11, 0), make_trajectory_point(20, 0.0), 11, 1.0),
+      0.0, true),
+
+    std::make_tuple(
+      make_linear_trajectory(make_trajectory_point(11, 0), make_trajectory_point(20, 0.0), 11, 1.0),
+      make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 0), 11, 1.0),
+      0.0, true),
+
+    std::make_tuple(
+      make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 0), 11, 1.0),
+      make_linear_trajectory(make_trajectory_point(1, 0), make_trajectory_point(10, 1.0), 11, 1.0),
+      1.0, true),
+
+    std::make_tuple(
+      make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 0), 11, 1.0),
+      make_linear_trajectory(make_trajectory_point(-1, 0), make_trajectory_point(10, 1.0), 11, 1.0),
+      1.0, true),
+
+    std::make_tuple(
+      make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 0), 11, 1.0),
+      make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(20, 2.0), 21, 1.0),
+      1.0, true))
+
+);

slave/src/Control/autoware_lane_departure_checker/CMakeLists.txt

@@ -0,0 +1,36 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_lane_departure_checker)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+include_directories(
+  include
+  ${Boost_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIRS}
+)
+
+ament_auto_add_library(autoware_lane_departure_checker SHARED
+  src/lane_departure_checker_node/lane_departure_checker.cpp
+  src/lane_departure_checker_node/lane_departure_checker_node.cpp
+  src/lane_departure_checker_node/utils.cpp
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "autoware::lane_departure_checker::LaneDepartureCheckerNode"
+  EXECUTABLE lane_departure_checker_node
+)
+
+if(BUILD_TESTING)
+  file(GLOB_RECURSE TEST_SOURCES test/*.cpp)
+  ament_add_gtest(test_autoware_lane_departure_checker
+    ${TEST_SOURCES}
+  )
+  target_link_libraries(test_autoware_lane_departure_checker autoware_lane_departure_checker)
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+    launch
+    config
+)