diff --git a/.beta.repos b/.beta.repos index eeda984..3c6dc00 100644 --- a/.beta.repos +++ b/.beta.repos @@ -14,4 +14,4 @@ repositories: autoware/autoware_internal_msgs: type: git url: https://github.com/autowarefoundation/autoware_internal_msgs.git - version: 1.6.0 \ No newline at end of file + version: 1.6.0 diff --git a/.github/workflows/pre-commit.yml b/.github/workflows/pre-commit.yml index 22e56ca..f13144e 100644 --- a/.github/workflows/pre-commit.yml +++ b/.github/workflows/pre-commit.yml @@ -18,4 +18,4 @@ jobs: uses: actions/setup-python@v2 - name: Run pre-commit - uses: pre-commit/action@v2.0.3 + uses: pre-commit/action@v3.0.1 diff --git a/.github/workflows/sonar.yml b/.github/workflows/sonar.yml new file mode 100644 index 0000000..cee8da2 --- /dev/null +++ b/.github/workflows/sonar.yml @@ -0,0 +1,34 @@ +name: Test and SonarCloud + +on: + pull_request: + push: + branches: [develop] + workflow_dispatch: + +jobs: + test: + runs-on: ubuntu-latest + + steps: + - name: Checkout repository + uses: actions/checkout@v4 + with: + fetch-depth: 0 + + - name: Setup Python + uses: actions/setup-python@v5 + with: + python-version: "3.10" + + - name: Install test dependencies + run: pip install pytest pytest-cov python-dateutil + + - name: Run tests with coverage + run: pytest --cov=src/signage/src --cov=src/external_signage/src --cov-report=xml --cov-report=term -v + + - name: SonarCloud Scan + uses: SonarSource/sonarqube-scan-action@a31c9398be7ace6bbfaf30c0bd5d415f843d45e9 + env: + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} + SONAR_TOKEN: ${{ secrets.SONAR_TOKEN }} diff --git a/.github/workflows/spell_check_all.yml b/.github/workflows/spell_check_all.yml index d107304..a9c3f47 100644 --- a/.github/workflows/spell_check_all.yml +++ b/.github/workflows/spell_check_all.yml @@ -21,11 +21,8 @@ jobs: run: | npm install cspell - - name: Retrieve spell check dictionary - run: | - curl --silent --show-error \ - --output .github/workflows/.cspell.json \ - https://raw.githubusercontent.com/tier4/autoware-spell-check-dict/main/cspell/.cspell.json + - name: Install spell check dictionary + run: npm install --ignore-scripts https://github.com/tier4/autoware-spell-check-dict - name: Check spelling run: | @@ -34,6 +31,6 @@ jobs: # How to exclude specific files: # Ex. "**/!(*.osm|*.svg|CHANGELOG.rst)" ./node_modules/.bin/cspell \ - --config .github/workflows/.cspell.json \ - "**/!(CHANGELOG.rst|${{ github.event.inputs.ignore_patterns }})" \ + --config node_modules/@tier4/autoware-spell-check-dict/.cspell.json \ + "**/!(CHANGELOG.rst|package.xml|${{ github.event.inputs.ignore_patterns }})" \ 2> /dev/null diff --git a/.github/workflows/spell_check_pr.yml b/.github/workflows/spell_check_pr.yml index bffc9ff..8e1d4bc 100644 --- a/.github/workflows/spell_check_pr.yml +++ b/.github/workflows/spell_check_pr.yml @@ -10,12 +10,17 @@ jobs: - name: Checkout repository uses: actions/checkout@v2 - - name: Retrieve spell check dictionary + - name: Setup Node.js + uses: actions/setup-node@v2 + + - name: Install spell check dictionary + run: npm install --ignore-scripts https://github.com/tier4/autoware-spell-check-dict + + - name: Add project-specific cspell overrides run: | - curl --silent --show-error \ - --output .github/workflows/.cspell.json \ - https://raw.githubusercontent.com/tier4/autoware-spell-check-dict/main/cspell/.cspell.json + jq '.words += ["testpaths", "pythonpath", "addopts"] | .ignorePaths += ["**/package.xml"]' node_modules/@tier4/autoware-spell-check-dict/.cspell.json > /tmp/cspell.json \ + && mv /tmp/cspell.json node_modules/@tier4/autoware-spell-check-dict/.cspell.json - uses: streetsidesoftware/cspell-action@v1.3.2 with: - config: ".github/workflows/.cspell.json" + config: "node_modules/@tier4/autoware-spell-check-dict/.cspell.json" diff --git a/.gitignore b/.gitignore index b9f8c3b..754581b 100644 --- a/.gitignore +++ b/.gitignore @@ -132,3 +132,6 @@ dmypy.json # ros output install/ log/ + +# GraphRAG knowledge vault (managed in separate repository) +.graphrag/ diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 2d57d9b..9726f17 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -66,7 +66,7 @@ repos: entry: ros:galactic ament_uncrustify --reformat - repo: https://github.com/psf/black - rev: 22.3.0 + rev: 26.1.0 hooks: - id: black args: ["--line-length=100"] diff --git a/README.md b/README.md index 687d196..9dc7eba 100644 --- a/README.md +++ b/README.md @@ -22,10 +22,25 @@ source {AUTOWARE_PATH}/install/setup.bash bash setup.sh ``` -IF you wont to use other FMS Domain, change environment variable. +### Environment variables + +The following environment variables must be set before launching: + +- `FMS_URL`: FMS domain (e.g. set by your operations team) +- `AUTOWARE_IP`: IP address of the Autoware PC running the FMS gateway +- `AUTOWARE_PORT`: TCP port of the FMS gateway on the Autoware PC + +On production vehicles these are provisioned by the `x2_signage_env_setup` role +in [`autoware_ecu_system_setup`](https://github.com/tier4/autoware_ecu_system_setup), +which writes them into `autoware.env`. The signage service then sources +`/opt/autoware/services/set-autoware-env/setup.sh` before launch. + +For local development, export them in your shell before running `start.sh`: ```bash -export FMS_URL=fms.web.auto +export FMS_URL= +export AUTOWARE_IP= +export AUTOWARE_PORT= ``` ## start diff --git a/ignore_packages.txt b/ignore_packages.txt index 0541e09..0822b46 100644 --- a/ignore_packages.txt +++ b/ignore_packages.txt @@ -5,4 +5,4 @@ level4_mode_manager slip_detector slip_detector_msgs sudden_deceleration_monitor -vehicle_stuck_checker \ No newline at end of file +vehicle_stuck_checker diff --git a/pytest.ini b/pytest.ini new file mode 100644 index 0000000..50b774f --- /dev/null +++ b/pytest.ini @@ -0,0 +1,4 @@ +[pytest] +testpaths = tests +pythonpath = src/signage/src src/external_signage/src +addopts = -p no:launch_testing -p no:launch_ros -p no:launch_pytest diff --git a/sonar-project.properties b/sonar-project.properties new file mode 100644 index 0000000..a4eb789 --- /dev/null +++ b/sonar-project.properties @@ -0,0 +1,13 @@ +sonar.projectKey=tier4_AutowareSignage +sonar.organization=tier4 + +# Source code +sonar.sources=src +sonar.tests=tests +sonar.test.inclusions=tests/**/*.py + +# Exclude test code from duplication check +sonar.cpd.exclusions=tests/**/*.py + +# Python coverage report path (generated by pytest-cov) +sonar.python.coverage.reportPaths=coverage.xml diff --git a/src/external_signage/config/destination_mapping.yaml b/src/external_signage/config/destination_mapping.yaml new file mode 100644 index 0000000..0d4d577 --- /dev/null +++ b/src/external_signage/config/destination_mapping.yaml @@ -0,0 +1,35 @@ +# SYS-HMI-03 車外サイネージ行先表示: destination.point_id と td5ファイル名prefix の対応表 +# +# 【これはテンプレート】実行時は /opt/autoware/destination_mapping.yaml を読み込む。 +# 同ファイルが無ければ本テンプレートが自動コピーされる。運用時の紐付け編集は +# /opt/autoware/destination_mapping.yaml 側を直接編集する(リビルド不要で反映)。 +# +# active_schedule (FMS) の move タスクの destination.point_id (整数・マップ上のグローバル地点ID) を +# キーに、表示する td5 ファイルの prefix を引く。 +# ※ destination には point_id の他に stop_point_id(小さい連番・バス停順序の可能性) と +# name(例 "エフピコ　アリーナ　乗車口"・全角スペース含む) もあるが、キーは point_id を使う +# (stop_point_id は路線間で衝突しうるため不適)。 +# td5 ファイルは以下の命名規約で配置する: +# +# resource/td5_file/destination/{prefix}_{width}x{height}.td5 +# front / back = 128x16, side = 80x24 (両サイズを用意する) +# +# 例) point_id 2 (name "BusStop2") を prefix "busstop2" に対応させる場合、 +# destination/busstop2_128x16.td5 と destination/busstop2_80x24.td5 を配置し、 +# 下記に "2": "busstop2" を追加する。 +# ※キーは int でも str でも可 (ロード時に str へ正規化する)。可読性のため引用推奨。 +# +# 未登録の point_id、td5 未配置/ロード失敗の場合は空白 (null td5) を表示する。 +# 有効な行先が取れない状態 (スケジュール無し/未受信/未登録/doing・todo 全完了) は +# 回送中 (kaiso td5) を表示する。行先表示モードでは走行中の自動運行中表示は行わない。 +# +# 実 td5 ファイルはディスプレイベンダ提供ツールで事前作成する。 +# +# 【暫定 2026-07-27】行先td5のテスト用として miraikan/shinagawa/teleport (+回送 kaiso) を用意した。 +# ただし 128x16 (front/back) のみで 80x24 (side) 版は未用意のため、side は空白(null)表示となる +# (_display_state を「td5を持つディスプレイだけ表示」に改修済み。side用td5を配置すれば自動で出る)。 +# point_id は現状の仮値 (2567/2568/2569) にテスト用行先を割り当てている。 +destination_mapping: + "2567": "miraikan" # destination/miraikan_128x16.td5 + "2568": "shinagawa" # destination/shinagawa_128x16.td5 + "2569": "teleport" # destination/teleport_128x16.td5 diff --git a/src/external_signage/resource/td5_file/destination/README.md b/src/external_signage/resource/td5_file/destination/README.md new file mode 100644 index 0000000..05533f6 --- /dev/null +++ b/src/external_signage/resource/td5_file/destination/README.md @@ -0,0 +1,41 @@ +# 行先表示 td5 ファイル配置ディレクトリ (SYS-HMI-03) + +車外サイネージの行先表示に使う td5 ファイルをここに配置する。 + +## 命名規約 + +``` +{prefix}_{width}x{height}.td5 +``` + +- `prefix` … `config/destination_mapping.yaml` で `destination.point_id` に対応付けた prefix +- サイズ … front / back = `128x16`、side = `80x24` の両方を用意する + +例) prefix `busstop2` の場合: + +``` +busstop2_128x16.td5 +busstop2_80x24.td5 +``` + +## destination.point_id との紐付け + +active_schedule (FMS) の move タスク `destination.point_id` (整数) と prefix の対応は +外部ファイル **`/opt/autoware/destination_mapping.yaml`** に記載する +（`signage_settings.json` と同じ場所。リビルド不要で編集反映）。同ファイルが無い場合は +パッケージ同梱テンプレート `config/destination_mapping.yaml` が起動時に自動コピーされる。 + +## 補足 + +- td5 ファイルはディスプレイベンダ提供ツールで事前作成する（実行時に動的生成しない）。 +- 未登録 id・td5 未配置・ロード失敗時は空白（`null` td5）へ自動フォールバックする。 +- 有効な行先が取れない状態（スケジュール無し/未受信/未登録/全完了）は回送中 + `resource/td5_file/kaiso_{width}x{height}.td5` を表示する（未配置時は `null` へフォールバック）。 +- 行先表示モードでは走行中の自動運行中（`auto`）表示は行わない。 + +## 【暫定 2026-07-27】テスト用 td5（128x16 のみ） + +行先表示テスト用に `miraikan` / `shinagawa` / `teleport`（行先）と `kaiso`（回送, `td5_file/kaiso_128x16.td5`） +を用意している。ただし **128x16（front/back）のみで 80x24（side）版は未用意**。 +このため side は空白（`null`）表示となる（`_display_state` は「td5 を持つディスプレイだけ表示」する）。 +side 用の `*_80x24.td5` を配置すれば、コード変更なしで side にも自動表示される。 diff --git a/src/external_signage/resource/td5_file/destination/miraikan_128x16.td5 b/src/external_signage/resource/td5_file/destination/miraikan_128x16.td5 new file mode 100644 index 0000000..3cf3572 Binary files /dev/null and b/src/external_signage/resource/td5_file/destination/miraikan_128x16.td5 differ diff --git a/src/external_signage/resource/td5_file/destination/miraikan_80x24.td5 b/src/external_signage/resource/td5_file/destination/miraikan_80x24.td5 new file mode 100644 index 0000000..e931c5b Binary files /dev/null and b/src/external_signage/resource/td5_file/destination/miraikan_80x24.td5 differ diff --git a/src/external_signage/resource/td5_file/destination/shinagawa_128x16.td5 b/src/external_signage/resource/td5_file/destination/shinagawa_128x16.td5 new file mode 100644 index 0000000..730d789 Binary files /dev/null and b/src/external_signage/resource/td5_file/destination/shinagawa_128x16.td5 differ diff --git a/src/external_signage/resource/td5_file/destination/shinagawa_80x24.td5 b/src/external_signage/resource/td5_file/destination/shinagawa_80x24.td5 new file mode 100644 index 0000000..e97f020 Binary files /dev/null and b/src/external_signage/resource/td5_file/destination/shinagawa_80x24.td5 differ diff --git a/src/external_signage/resource/td5_file/destination/teleport_128x16.td5 b/src/external_signage/resource/td5_file/destination/teleport_128x16.td5 new file mode 100644 index 0000000..ef90692 Binary files /dev/null and b/src/external_signage/resource/td5_file/destination/teleport_128x16.td5 differ diff --git a/src/external_signage/resource/td5_file/destination/teleport_80x24.td5 b/src/external_signage/resource/td5_file/destination/teleport_80x24.td5 new file mode 100644 index 0000000..d8439c8 Binary files /dev/null and b/src/external_signage/resource/td5_file/destination/teleport_80x24.td5 differ diff --git a/src/external_signage/resource/td5_file/kaiso_128x16.td5 b/src/external_signage/resource/td5_file/kaiso_128x16.td5 new file mode 100644 index 0000000..271d3db Binary files /dev/null and b/src/external_signage/resource/td5_file/kaiso_128x16.td5 differ diff --git a/src/external_signage/resource/td5_file/kaiso_80x24.td5 b/src/external_signage/resource/td5_file/kaiso_80x24.td5 new file mode 100644 index 0000000..8188d7c Binary files /dev/null and b/src/external_signage/resource/td5_file/kaiso_80x24.td5 differ diff --git a/src/external_signage/scripts/destination_mode.sh b/src/external_signage/scripts/destination_mode.sh new file mode 100755 index 0000000..69cd114 --- /dev/null +++ b/src/external_signage/scripts/destination_mode.sh @@ -0,0 +1,42 @@ +#!/bin/bash +# 車外サイネージ 行先表示モード (SYS-HMI-03) の切替・確認ヘルパー。 +# +# 使い方: +# ./destination_mode.sh on # 行先表示モードON (active_schedule から行先td5を表示) +# ./destination_mode.sh off # 行先表示モードOFF (既存の experiment/auto/mrm/null 表示へ復帰) +# ./destination_mode.sh status # 現在の設定・モード状態を確認 +# +# 前提: ROS 2 環境と対象ワークスペースを source 済みで、external_signage ノードが +# 起動していること (signage.launch.xml を use_external_signage:=true で起動)。 +set -euo pipefail + +SERVICE="/signage/destination_mode" + +usage() { + echo "usage: $0 {on|off|status}" >&2 + exit 1 +} + +[ $# -eq 1 ] || usage + +case "$1" in + on|true) + echo "[destination_mode] ON にします..." + ros2 service call "$SERVICE" std_srvs/srv/SetBool "{data: true}" + ;; + off|false) + echo "[destination_mode] OFF にします..." + ros2 service call "$SERVICE" std_srvs/srv/SetBool "{data: false}" + ;; + status) + echo "[destination_mode] 現在の設定 (/signage/external/settings) を1件取得:" + timeout 5 ros2 topic echo --once /signage/external/settings || \ + echo " (settings を取得できませんでした。ノード起動を確認してください)" + echo "[destination_mode] mode_status (/signage/mode_status) を1件取得:" + timeout 5 ros2 topic echo --once /signage/mode_status || \ + echo " (mode_status を取得できませんでした)" + ;; + *) + usage + ;; +esac diff --git a/src/external_signage/scripts/publish_test_schedule.sh b/src/external_signage/scripts/publish_test_schedule.sh new file mode 100755 index 0000000..f9da89c --- /dev/null +++ b/src/external_signage/scripts/publish_test_schedule.sh @@ -0,0 +1,104 @@ +#!/bin/bash +# 車外サイネージ 行先表示 (SYS-HMI-03) テスト用ヘルパー。 +# active_schedule を各パターンで publish、または web.auto agent から実スケジュールを +# 取得して publish / 表示する。 +# +# 前提: +# - ROS 2 環境と対象ワークスペースを source 済み +# - external_signage ノードが起動済み (signage.launch.xml use_external_signage:=true) +# - 行先表示モードが ON (./destination_mode.sh on) ※OFF だと描画に反映されない +# +# 使い方: +# ./publish_test_schedule.sh doing [POINT_ID] # 走行中(doing): 登録済み point_id -> 行先td5 +# ./publish_test_schedule.sh todo [POINT_ID] # 次タスク(todo): 同上 +# ./publish_test_schedule.sh done [POINT_ID] # 全タスク完了 -> 回送中 (kaiso) +# ./publish_test_schedule.sh none # スケジュール空 -> 回送中 (kaiso) +# ./publish_test_schedule.sh unmapped [POINT_ID] # 未登録 point_id -> 空白 (null) +# ./publish_test_schedule.sh no-id # point_id 無し -> 空白 (null) +# ./publish_test_schedule.sh real # web.auto agent の実スケジュールを publish +# ./publish_test_schedule.sh show # 実スケジュールを整形表示 (publish しない) +# +# POINT_ID 省略時: doing/todo/done は 2567 (実FMS照合時の destination.point_id)、 +# unmapped は 999999 (未登録想定)。 +# +# 期待結果はノードログ "destination display -> ()" で確認する。 +# td5 が一部サイズのみの場合 (現状: 行先/回送 td5 は 128x16 のみ) は、そのサイズを持つ +# front/back に表示され、80x24 未用意の side は "td5 not available ... showing null" で空白表示。 +# 該当サイズの td5 が全く無い key は全ディスプレイ空白 (null) となる。 +set -euo pipefail + +TOPIC="/signage/active_schedule" +MOVE_POINT_ID=2567 +UNMAPPED_POINT_ID=999999 + +AGENT_URL="http://${AUTOWARE_IP:-localhost}:${AUTOWARE_PORT:-4711}/v1/services/order" +FMS_ACTIVE_SCHEDULE_URL="https://${FMS_URL:-fms.dev.web.auto}/v1/projects/{project_id}/environments/{environment_id}/vehicles/{vehicle_id}/active_schedule" + +usage() { + sed -n '2,29p' "$0" >&2 + exit 1 +} + +# YAML 単一引用符スカラー内に埋め込むため、JSON 中の ' を '' へエスケープして publish する。 +publish() { + local json="$1" + local esc="${json//\'/\'\'}" + echo "[publish] ${TOPIC} <- ${json}" + ros2 topic pub --times 3 --rate 2 "${TOPIC}" std_msgs/msg/String "{data: '${esc}'}" +} + +# move タスク1件のスケジュール JSON を組み立てる。 +# $1 = task status (doing/todo/done ...) +# $2 = point_id (空文字なら destination に point_id を入れない = no-id ケース) +move_schedule() { + local status="$1" pid="${2-}" + if [ -z "${pid}" ]; then + printf '{"tasks":[{"task_type":"move","status":"%s","destination":{"name":"TestStop"}}]}' "${status}" + else + printf '{"tasks":[{"task_type":"move","status":"%s","destination":{"point_id":%s,"stop_point_id":1,"name":"TestStop"}}]}' "${status}" "${pid}" + fi +} + +fetch_real() { + curl -s --max-time 12 -X POST "${AGENT_URL}" -H 'Content-Type: application/json' \ + -d "{\"method\":\"get\",\"url\":\"${FMS_ACTIVE_SCHEDULE_URL}\",\"body\":{}}" +} + +[ $# -ge 1 ] || usage +cmd="$1"; shift || true + +case "${cmd}" in + doing|todo|done) + publish "$(move_schedule "${cmd}" "${1:-${MOVE_POINT_ID}}")" + ;; + none) + # 空スケジュール -> _resolve_destination は "kaiso" (no schedule) + publish '{}' + ;; + unmapped) + publish "$(move_schedule doing "${1:-${UNMAPPED_POINT_ID}}")" + ;; + no-id) + # destination に point_id を入れない -> "null" (no destination point_id) + publish "$(move_schedule doing "")" + ;; + real) + echo "[real] web.auto agent (${AGENT_URL}) から実スケジュールを取得して publish します..." + raw="$(fetch_real)" + [ -n "${raw}" ] || { echo " 取得できませんでした (agent 未起動/認証/ID 未解決の可能性)" >&2; exit 1; } + publish "${raw}" + ;; + show) + echo "[show] web.auto agent (${AGENT_URL}) の実スケジュール:" + raw="$(fetch_real)" + [ -n "${raw}" ] || { echo " 取得できませんでした" >&2; exit 1; } + if command -v python3 >/dev/null 2>&1; then + echo "${raw}" | python3 -m json.tool --no-ensure-ascii 2>/dev/null || echo "${raw}" + else + echo "${raw}" + fi + ;; + *) + usage + ;; +esac diff --git a/src/external_signage/setup.py b/src/external_signage/setup.py index 8444c2b..de62013 100644 --- a/src/external_signage/setup.py +++ b/src/external_signage/setup.py @@ -6,11 +6,13 @@ def package_files(directory): - paths = [] - for (path, directories, filenames) in os.walk(directory): - for filename in filenames: - paths.append(os.path.join(path, filename)) - return paths + # 指定ディレクトリ直下のファイルのみを返す (サブディレクトリは別 data_files エントリで扱う)。 + # data_files はインストール先を1階層にまとめるため、サブディレクトリ構造は保持されない。 + return [ + os.path.join(directory, filename) + for filename in os.listdir(directory) + if os.path.isfile(os.path.join(directory, filename)) + ] package_name = "external_signage" @@ -22,9 +24,14 @@ def package_files(directory): data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), ("share/" + package_name + "/resource/td5_file", package_files("resource/td5_file")), + ( + "share/" + package_name + "/resource/td5_file/destination", + package_files("resource/td5_file/destination"), + ), + ("share/" + package_name + "/config", package_files("config")), ("share/" + package_name, ["package.xml"]), ("share/" + package_name + "/launch", ["launch/external_signage.launch.xml"]), - ], + ], install_requires=["setuptools"], zip_safe=True, author="Makoto Yabuta", diff --git a/src/external_signage/src/external_signage/external_signage.py b/src/external_signage/src/external_signage/external_signage.py index 3693ab0..b73a207 100644 --- a/src/external_signage/src/external_signage/external_signage.py +++ b/src/external_signage/src/external_signage/external_signage.py @@ -7,6 +7,7 @@ from external_signage.external_signage_core import ExternalSignage from ament_index_python.packages import get_package_share_directory + def main(args=None): package_path = get_package_share_directory("external_signage") diff --git a/src/external_signage/src/external_signage/external_signage_core.py b/src/external_signage/src/external_signage/external_signage_core.py index 59b515b..b9c15e5 100644 --- a/src/external_signage/src/external_signage/external_signage_core.py +++ b/src/external_signage/src/external_signage/external_signage_core.py @@ -5,6 +5,7 @@ import json import os import uuid +import yaml import rclpy from std_srvs.srv import SetBool from std_msgs.msg import Bool @@ -14,6 +15,12 @@ from std_msgs.msg import String from level4_mode_manager_msgs.msg import Level4DrivingStatus +# SYS-HMI-03: destination.point_id -> td5ファイル名prefix の紐付けを記載する外部ファイル。 +# signage_settings.json と同様に /opt/autoware 下へ置き、リビルド不要で運用時に編集できる。 +# 存在しなければパッケージ同梱テンプレート (config/destination_mapping.yaml) から生成する。 +DESTINATION_MAPPING_PATH = "/opt/autoware/destination_mapping.yaml" + + @dataclass class Display: address1: int @@ -27,9 +34,9 @@ class Display: class Protocol: SOT = 0xAA EOT = 0x55 - SEND_COLOR = "\x1B[34;1m" - RECV_COLOR = "\x1B[32;1m" - ERR_COLOR = "\x1B[31;1m" + SEND_COLOR = "\x1b[34;1m" + RECV_COLOR = "\x1b[32;1m" + ERR_COLOR = "\x1b[31;1m" def __init__(self): self.front = Display( @@ -106,11 +113,16 @@ class ExternalSignage: def __init__(self, node): self.node = node self.protocol = Protocol() + self.current_state = "" package_path = get_package_share_directory("external_signage") + "/resource/td5_file/" node.declare_parameter("serial_port", "/dev/ttyS0") self._serial_port = node.get_parameter("serial_port").get_parameter_value().string_value + # SYS-HMI-03: 行先表示用の destination.point_id -> td5ファイル名prefix 対応を起動時にロードする + self._destination_mapping = self._load_destination_mapping() + self._last_schedule_raw = "" + try: self.bus = serial.Serial( self._serial_port, @@ -146,10 +158,18 @@ def __init__(self, node): reliability=rclpy.qos.QoSReliabilityPolicy.RELIABLE, durability=rclpy.qos.QoSDurabilityPolicy.TRANSIENT_LOCAL, ) + airport_mode_qos = rclpy.qos.QoSProfile( + history=rclpy.qos.QoSHistoryPolicy.KEEP_LAST, + depth=10, + reliability=rclpy.qos.QoSReliabilityPolicy.RELIABLE, + durability=rclpy.qos.QoSDurabilityPolicy.VOLATILE, + ) node.create_service(SetBool, "/signage/trigger_external", self.trigger_external_signage) node.create_service(SetBool, "/signage/mode_change", self.change_mode) - node.create_service(SetBool, "/signage/airport_mode_change", self.change_airport_mode) + self._sub_airport_mode = node.create_subscription( + Bool, "/signage/airport_mode", self.change_airport_mode, airport_mode_qos + ) self.mode_status_pub_ = node.create_publisher(Bool, "/signage/mode_status", api_qos) self.setting_pub_ = node.create_publisher(String, "/signage/external/settings", api_qos) self._sub_mrm = node.create_subscription( @@ -166,6 +186,20 @@ def __init__(self, node): api_qos, ) + # SYS-HMI-03: 行先表示モードの切替サービスと、行先決定に用いる active_schedule 購読 + # active_schedule の publisher (signage_fms_client) は RELIABLE + VOLATILE (depth10) のため + # durability を VOLATILE で合わせる (api_qos の TRANSIENT_LOCAL だと不整合で受信できない)。 + schedule_qos = rclpy.qos.QoSProfile( + history=rclpy.qos.QoSHistoryPolicy.KEEP_LAST, + depth=10, + reliability=rclpy.qos.QoSReliabilityPolicy.RELIABLE, + durability=rclpy.qos.QoSDurabilityPolicy.VOLATILE, + ) + node.create_service(SetBool, "/signage/destination_mode", self.set_destination_mode) + self._sub_active_schedule = node.create_subscription( + String, "/signage/active_schedule", self.sub_active_schedule, schedule_qos + ) + # read settings.If not, creatte settings. self._settings_file = "/home/" + os.environ.get("USER") + "/settings.json" if os.path.exists(self._settings_file): @@ -173,11 +207,16 @@ def __init__(self, node): self._settings = json.load(f) else: self._settings = {"in_experiment": True, "airport": False} - with open(self._settings_file, "w") as f: - json.dump(self._settings, f, indent=4) + # SYS-HMI-03: 行先表示モード (in_experiment/airport と排他) の既定値を補完し永続化する + self._settings.setdefault("destination_mode", False) + self._save_settings() # initial display - if self._settings.get("in_experiment", True): + if self._settings.get("destination_mode"): + # 行先表示モード: active_schedule 受信までは null (空白) を表示 + self.pub_mode_status(False) + self._render_destination() + elif self._settings.get("in_experiment", True): self.pub_mode_status(True) self.display_signage("experiment") else: @@ -195,26 +234,82 @@ def pub_mode_status(self, status): msg.data = status self.mode_status_pub_.publish(msg) + def _load_destination_mapping(self): + # destination.point_id -> td5ファイル名prefix の紐付けを外部ファイル (/opt/autoware) から読む。 + # 無ければパッケージ同梱テンプレートから生成する。壊れている場合は空マッピングで継続する + # (フォールバックで null 表示)。運用時はこの外部ファイルを編集すればリビルド不要で反映される。 + # point_id は整数だが YAML キーの引用有無で int/str が揺れるため、キーを str に正規化して保持する。 + try: + if not os.path.isfile(DESTINATION_MAPPING_PATH): + self._seed_destination_mapping() + with open(DESTINATION_MAPPING_PATH, "r") as f: + data = yaml.safe_load(f) or {} + raw_mapping = data.get("destination_mapping", {}) or {} + mapping = {str(k): v for k, v in raw_mapping.items()} + self.node.get_logger().info( + "loaded destination_mapping from {}: {} entries".format( + DESTINATION_MAPPING_PATH, len(mapping) + ) + ) + return mapping + except Exception as e: + self.node.get_logger().warning( + "destination_mapping load failed ({}), use empty mapping: {}".format( + DESTINATION_MAPPING_PATH, str(e) + ) + ) + return {} + + def _seed_destination_mapping(self): + # 初回のみ: パッケージ同梱テンプレートを /opt/autoware へコピーして初期化する。 + template = ( + get_package_share_directory("external_signage") + "/config/destination_mapping.yaml" + ) + os.makedirs(os.path.dirname(DESTINATION_MAPPING_PATH), exist_ok=True) + with open(template, "r") as src, open(DESTINATION_MAPPING_PATH, "w") as dst: + dst.write(src.read()) + self.node.get_logger().info( + "seeded destination_mapping template to {}".format(DESTINATION_MAPPING_PATH) + ) + + def _load_td5(self, path, display): + return packet_tools.TD5Data( + path, display.address1, display.address2, display.height, display.width + ) + def _load_display_data(self, display, package_path): auto_path = package_path + f"automatic_{display.width}x{display.height}.td5" experiment_path = package_path + f"experiment_{display.width}x{display.height}.td5" mrm_path = package_path + f"mrm_{display.width}x{display.height}.td5" null_path = package_path + f"null_{display.width}x{display.height}.td5" - return { - "auto": packet_tools.TD5Data( - auto_path, display.address1, display.address2, display.height, display.width - ), - "experiment": packet_tools.TD5Data( - experiment_path, display.address1, display.address2, display.height, display.width - ), - "mrm": packet_tools.TD5Data( - mrm_path, display.address1, display.address2, display.height, display.width - ), - "null": packet_tools.TD5Data( - null_path, display.address1, display.address2, display.height, display.width - ), + data = { + "auto": self._load_td5(auto_path, display), + "experiment": self._load_td5(experiment_path, display), + "mrm": self._load_td5(mrm_path, display), + "null": self._load_td5(null_path, display), } + # SYS-HMI-03: 行先td5(mappingのprefix)と回送中(kaiso)td5を一括ロードしキャッシュする。 + # 未配置/ロード失敗のファイルはスキップし、表示時に null へフォールバックする。 + suffix = f"_{display.width}x{display.height}.td5" + for dest_id, prefix in self._destination_mapping.items(): + path = package_path + "destination/" + f"{prefix}{suffix}" + try: + data[prefix] = self._load_td5(path, display) + except Exception as e: + self.node.get_logger().warning( + "destination td5 load failed ({} -> {}, {}x{}): {}".format( + dest_id, prefix, display.width, display.height, str(e) + ) + ) + try: + data["kaiso"] = self._load_td5(package_path + f"kaiso{suffix}", display) + except Exception as e: + self.node.get_logger().warning( + "kaiso td5 load failed ({}x{}): {}".format(display.width, display.height, str(e)) + ) + return data + def send_data(self, display_key, data_key): display = self.displays[display_key] data = display[data_key] @@ -227,6 +322,10 @@ def send_data(self, display_key, data_key): def trigger_external_signage(self, request, response): try: self.autoware_status["driving"] = request.data + # SYS-HMI-03: 行先表示モード中は既存表示を出さない (状態のみ更新) + if self._settings.get("destination_mode"): + response.success = True + return response if self._settings["in_experiment"]: return response elif self._settings["airport"]: @@ -237,7 +336,7 @@ def trigger_external_signage(self, request, response): self.display_signage("auto") else: self.display_signage("null") - else: + else: if request.data: self.display_signage("auto") else: @@ -250,9 +349,12 @@ def trigger_external_signage(self, request, response): # MRMが発生していて空港モードの場合は「緊急停止中」表示にする def sub_mrm_callback(self, msg): try: + self.autoware_status["mrm"] = msg.state in [2, 3, 4] + # SYS-HMI-03: 行先表示モード中は既存表示(mrm含む)を出さない (状態のみ更新) + if self._settings.get("destination_mode"): + return if self._settings["in_experiment"]: - return - self.autoware_status["mrm"] = msg.state in [2,3,4] + return if self._settings["airport"] and self.autoware_status["mrm"]: self.display_signage("mrm") else: @@ -267,37 +369,42 @@ def sub_mrm_callback(self, msg): def sub_is_driving_level(self, msg): try: self.node.get_logger().info(str(msg.is_level4_driving)) - if msg.is_level4_driving: # True is L4, False is L2. + # SYS-HMI-03: 行先表示モード中は状態更新のみ行い既存表示は出さない + skip_display = self._settings.get("destination_mode") + if msg.is_level4_driving: # True is L4, False is L2. self.pub_mode_status(True) self._settings["in_experiment"] = False - if self.autoware_status["driving"]: - self.display_signage("auto") - else: - self.display_signage("null") + if not skip_display: + if self.autoware_status["driving"]: + self.display_signage("auto") + else: + self.display_signage("null") else: self.pub_mode_status(False) self._settings["in_experiment"] = True - self.display_signage("experiment") + if not skip_display: + self.display_signage("experiment") - with open(self._settings_file, "w") as f: - json.dump(self._settings, f, indent=4) + self._save_settings() except Exception as e: self.node.get_logger().error(str(e)) # l4かどうかのサービスを受け取り走行モードを変更する def change_mode(self, request, response): try: - if request.data: # True is L2, False is L4. + skip_display = self._settings.get("destination_mode") + if request.data: # True is L2, False is L4. self.pub_mode_status(True) self._settings["in_experiment"] = True - self.display_signage("experiment") + if not skip_display: + self.display_signage("experiment") else: self.pub_mode_status(False) self._settings["in_experiment"] = False - self.display_signage("null") + if not skip_display: + self.display_signage("null") - with open(self._settings_file, "w") as f: - json.dump(self._settings, f, indent=4) + self._save_settings() response.success = True except Exception as e: self.node.get_logger().error(str(e)) @@ -305,25 +412,127 @@ def change_mode(self, request, response): return response # 空港モードにするかどうかのトピックを受け取り空港モードを変更する - def change_airport_mode(self, request, response): + def change_airport_mode(self, msg): try: - if request.data: - self._settings["airport"] = True - else: - self._settings["airport"] = False + self._settings["airport"] = msg.data with open(self._settings_file, "w") as f: json.dump(self._settings, f, indent=4) + except Exception as e: + self.node.get_logger().error(str(e)) + + def _save_settings(self): + with open(self._settings_file, "w") as f: + json.dump(self._settings, f, indent=4) + + # SYS-HMI-03: 行先表示モードの切替 (in_experiment/airport と排他) + def set_destination_mode(self, request, response): + try: + self._settings["destination_mode"] = request.data + self._save_settings() + if request.data: + # 行先表示モードON: 最新スケジュールから行先td5を描画する + self._render_destination() + else: + # OFF: 現在の autoware_status / settings に従い既存表示へ復帰する + self._restore_existing_display() response.success = True except Exception as e: self.node.get_logger().error(str(e)) response.success = False return response + def sub_active_schedule(self, msg): + # active_schedule は常に保持し、行先表示モード時のみ描画に反映する + self._last_schedule_raw = msg.data + if not self._settings.get("destination_mode"): + return + self._render_destination() + + def _render_destination(self): + # 最新の active_schedule から表示すべき td5 状態を決定して描画する。 + try: + data = json.loads(self._last_schedule_raw) if self._last_schedule_raw else None + except Exception as e: + self.node.get_logger().warning("active_schedule JSON parse failed: " + str(e)) + self._display_state("null") + return + state_key, detail = self._resolve_destination(data) + self.node.get_logger().info("destination display -> {} ({})".format(state_key, detail)) + self._display_state(state_key) + + def _resolve_destination(self, data): + # 行先決定ロジック: doing[0] -> todo[0] -> 有効な行先なし。 + # 戻り値 (state_key, detail)。state_key は displays のデータキー (prefix / "kaiso" / "null")。 + # 有効な行先が取れない状態 (スケジュール無し/未登録/全完了) は回送中 (kaiso) とする (SYS-HMI-08)。 + # 行先表示モードでは走行中の自動運行中(auto)表示は行わない。 + if not data: + # スケジュール無し/未受信 -> 回送中 + return "kaiso", "no schedule" + tasks = data.get("tasks", []) + doing = [t for t in tasks if t.get("task_type") == "move" and t.get("status") == "doing"] + todo = [t for t in tasks if t.get("task_type") == "move" and t.get("status") == "todo"] + task = doing[0] if doing else (todo[0] if todo else None) + if task is None: + # doing/todo の move タスクなし = スケジュール完了/未登録 -> 回送中 + return "kaiso", "schedule complete" + dest = task.get("destination") or {} + point_id = dest.get("point_id") + dest_name = dest.get("name") + if point_id is None: + return "null", "no destination point_id" + prefix = self._destination_mapping.get(str(point_id)) + if not prefix: + self.node.get_logger().warning( + "destination point_id not in mapping: {} ({})".format(point_id, dest_name) + ) + return "null", "unmapped:{}".format(point_id) + return prefix, "point_id={} name={}".format(point_id, dest_name) + + def _display_state(self, state_key): + # 指定キーの td5 を持つディスプレイにはそれを、持たないディスプレイには null (空白) を表示する。 + # 例) 行先td5が 128x16 (front/back) のみで side (80x24) 未用意の場合、front/back は行先を、 + # side は空白を表示する。全サイズが揃えば自動で全ディスプレイに出る。 + keys = {} + for d in self.displays: + if state_key in self.displays.get(d, {}): + keys[d] = state_key + else: + keys[d] = "null" + if state_key != "null": + self.node.get_logger().warning( + "td5 not available for '{}' on display '{}', showing null".format( + state_key, d + ) + ) + self._display_signage_per_display(keys) + + def _restore_existing_display(self): + # 行先表示モードOFF時に、既存ロジック(実験/空港MRM/自動運転/停止)へ復帰する。 + if self._settings.get("in_experiment", True): + self.display_signage("experiment") + elif self._settings.get("airport") and self.autoware_status["mrm"]: + self.display_signage("mrm") + elif self.autoware_status["driving"]: + self.display_signage("auto") + else: + self.display_signage("null") def display_signage(self, display_file): + # 全ディスプレイに同一キーを表示する (既存呼び出し用の薄いラッパ)。 + self._display_signage_per_display({d: display_file for d in self.displays}) + + def _display_signage_per_display(self, keys): + # ディスプレイごとに表示する td5 キーを指定して送信する。 + # keys: {display_key: data_key} (data_key は各ディスプレイの displays[display_key] に存在すること) if not self._external_signage_available: return + # 前回の状態と同じの場合更新をスキップする + previous_state = self.current_state + self.current_state = keys + if previous_state == keys: + return + for display_key in self.displays: - self.send_data(display_key, display_file) + self.send_data(display_key, keys[display_key]) time.sleep(1) diff --git a/src/level4_mode_manager_msgs/CMakeLists.txt b/src/level4_mode_manager_msgs/CMakeLists.txt deleted file mode 100644 index cf2acdf..0000000 --- a/src/level4_mode_manager_msgs/CMakeLists.txt +++ /dev/null @@ -1,22 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(level4_mode_manager_msgs) - -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() - -rosidl_generate_interfaces(${PROJECT_NAME} - "msg/Level4Availability.msg" - "msg/Level4DrivingStatus.msg" - "srv/EnableLevel4Driving.srv" - DEPENDENCIES - builtin_interfaces - tier4_external_api_msgs - ADD_LINTER_TESTS -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - -ament_auto_package() diff --git a/src/level4_mode_manager_msgs/msg/Level4Availability.msg b/src/level4_mode_manager_msgs/msg/Level4Availability.msg deleted file mode 100644 index 2a3d968..0000000 --- a/src/level4_mode_manager_msgs/msg/Level4Availability.msg +++ /dev/null @@ -1,2 +0,0 @@ -builtin_interfaces/Time stamp -bool is_level4_available diff --git a/src/level4_mode_manager_msgs/msg/Level4DrivingStatus.msg b/src/level4_mode_manager_msgs/msg/Level4DrivingStatus.msg deleted file mode 100644 index 4bfd35c..0000000 --- a/src/level4_mode_manager_msgs/msg/Level4DrivingStatus.msg +++ /dev/null @@ -1,2 +0,0 @@ -builtin_interfaces/Time stamp -bool is_level4_driving diff --git a/src/level4_mode_manager_msgs/package.xml b/src/level4_mode_manager_msgs/package.xml deleted file mode 100644 index 4f09114..0000000 --- a/src/level4_mode_manager_msgs/package.xml +++ /dev/null @@ -1,25 +0,0 @@ - - - - level4_mode_manager_msgs - 0.1.0 - msg and srv definitions for level4_mode_manager - Taiki Yamada - Apache License 2.0 - - ament_cmake_auto - - rosidl_default_generators - - builtin_interfaces - tier4_external_api_msgs - - ament_lint_auto - ament_lint_common - - rosidl_interface_packages - - - ament_cmake - - diff --git a/src/level4_mode_manager_msgs/srv/EnableLevel4Driving.srv b/src/level4_mode_manager_msgs/srv/EnableLevel4Driving.srv deleted file mode 100644 index c1a4a52..0000000 --- a/src/level4_mode_manager_msgs/srv/EnableLevel4Driving.srv +++ /dev/null @@ -1,6 +0,0 @@ -bool enable_level4_driving ---- -uint32 ERROR_NOT_INITIALIZED = 5 -uint32 ERROR_INVALID_LEVEL = 6 -uint32 ERROR_CANNOT_START_AUTOWARE = 7 -tier4_external_api_msgs/ResponseStatus status diff --git a/src/signage/config/signage_param.yaml b/src/signage/config/signage_param.yaml index 6848dad..35220a0 100644 --- a/src/signage/config/signage_param.yaml +++ b/src/signage/config/signage_param.yaml @@ -11,8 +11,16 @@ signage: emergency_ignore_period: 5.0 # second emergency_repeat_period: 180.0 # second accept_start: 5.0 # second + arriving_distance: 10.0 # m (UC-05 到着接近の車内安全配慮しきい値 SYS-HMI-07) monitor_width: 1920 monitor_height: 540 + cvm_device_id: in_vehicle_signage + standing_mode_default: false # 立席運用モードの起動時デフォルト (ファイルが優先) + sudden_decel_threshold: 0.8 # m/s^2 (acceleration <= -本値 で急減速) + sudden_decel_jerk_threshold: 0.6 # m/s^3 (jerk <= -本値 で急減速。減速方向のみ判定=加速時は対象外) + sudden_lateral_accel_threshold: 0.8 # m/s^2 (lateral_acceleration_abs >= 本値 で急操舵) + sudden_lateral_jerk_threshold: 0.5 # m/s^3 (|横ジャーク| >= 本値 で急操舵。横ジャーク≒v^2*steering_rate/wheel_base) + wheel_base: 2.75 # m (横ジャーク計算用フォールバック。/api/vehicle/dimensions 取得成功で上書き) announce: emergency: true restart_engage: true @@ -22,4 +30,13 @@ signage: thank_you: true in_emergency: true going_to_depart: true - going_to_arrive: true + going_to_arrive: false # 到着予告(もうすぐ到着)はPhase1方針でデフォルト除外(次バス停案内・停車時安全配慮と重複するため) + arrive_caution: true # UC-05 接近10mの車内安全配慮アナウンス (SYS-HMI-07) + temporary_stop: true # UC-04 停止案内(障害物/横断歩道/一般 共通「停車します」) SYS-HMI-04/05/06 + standing_depart: true # UC-02 発車警告 SYS2-UC02-01 + standing_sudden: true # UC-03 急減速・急操舵警告 SYS2-UC03-06 + announce_interval: # 発話後にその期間だけ再発話を抑止する秒数 (表示時間も連動) + stop_reason: 20.0 # second (UC-04 停止案内 SYS-HMI-04/05/06) + arrived: 5.0 # second (UC-05 到着表示の表示秒数 SYS-HMI-07) + standing_depart: 5.0 # second (UC-02 発車警告 SYS2-UC02-01) + standing_sudden: 10.0 # second (UC-03 急減速・急操舵警告 SYS2-UC03-06) diff --git a/src/signage/launch/signage.launch.xml b/src/signage/launch/signage.launch.xml index fafc03f..c6abe20 100755 --- a/src/signage/launch/signage.launch.xml +++ b/src/signage/launch/signage.launch.xml @@ -3,12 +3,13 @@ + + - - + diff --git a/src/signage/package.xml b/src/signage/package.xml index 349e057..14b385c 100644 --- a/src/signage/package.xml +++ b/src/signage/package.xml @@ -10,6 +10,7 @@ ament_index_python signage_version + command_view_manager_msgs diagnostic_updater external_signage python-pulsectl-pip @@ -20,6 +21,7 @@ tier4_debug_msgs tier4_external_api_msgs tier4_hmi_msgs + tier4_metric_msgs ament_python diff --git a/src/signage/resource/page/ArrivedView.qml b/src/signage/resource/page/ArrivedView.qml new file mode 100644 index 0000000..7aadc45 --- /dev/null +++ b/src/signage/resource/page/ArrivedView.qml @@ -0,0 +1,35 @@ +import QtQuick 2.9 +import QtQuick.Controls 2.2 + +import "Common" + +// UC-05 (SYS-HMI-07): バス停到着時に「‹停留所名›に到着しました」を +// announce_interval.arrived 秒間 (既定 5s) 専用画面で表示する。 +// BusStopView のローテーション (10s 周期) では到着直後に表示が回ってこないため、 +// 到着表示は独立した view_mode として最優先で描画する。 +Rectangle { + id: arrivedView + width: viewController.monitor_width + height: viewController.monitor_height + color: "#ffffff" + + CurrentTime { + id: displayCurrentTime + } + + Text { + id: arrivedText + width: viewController.monitor_width - 200 + height: viewController.monitor_height + color: "#000000" + text: viewController.display_phrase + anchors.horizontalCenter: parent.horizontalCenter + anchors.verticalCenter: parent.verticalCenter + horizontalAlignment: Text.AlignHCenter + verticalAlignment: Text.AlignVCenter + font.pointSize: 80*viewController.size_ratio*0.7 + font.bold: true + elide: Text.ElideMiddle + wrapMode: Text.WordWrap + } +} diff --git a/src/signage/resource/page/CvmDisplayView/CvmDisplayView.qml b/src/signage/resource/page/CvmDisplayView/CvmDisplayView.qml new file mode 100644 index 0000000..bb1ae55 --- /dev/null +++ b/src/signage/resource/page/CvmDisplayView/CvmDisplayView.qml @@ -0,0 +1,12 @@ +import QtQuick 2.9 +import QtQuick.Controls 2.2 + +Rectangle { + id: cvmDisplayView + width: viewController.monitor_width + height: viewController.monitor_height + + CvmEmergencyStop { + visible: viewController.cvm_display_mode_id === "remote_emergency_display" + } +} diff --git a/src/signage/resource/page/CvmDisplayView/CvmEmergencyStop.qml b/src/signage/resource/page/CvmDisplayView/CvmEmergencyStop.qml new file mode 100644 index 0000000..aa283be --- /dev/null +++ b/src/signage/resource/page/CvmDisplayView/CvmEmergencyStop.qml @@ -0,0 +1,48 @@ +import QtQuick 2.9 +import QtQuick.Controls 2.2 + +Rectangle { + id: cvmEmergencyStop + width: viewController.monitor_width + height: viewController.monitor_height + color: "#ff8c00" + + Text { + id: cvmEmergencyStopIcon + color: "#ffffff" + text: "⚠" + anchors.horizontalCenter: parent.horizontalCenter + anchors.bottom: cvmEmergencyStopText.top + anchors.bottomMargin: 20*viewController.size_ratio + horizontalAlignment: Text.AlignHCenter + font.pointSize: 120*viewController.size_ratio + font.bold: true + } + + Text { + id: cvmEmergencyStopText + color: "#ffffff" + text: qsTr("オペレーター停止指示") + anchors.horizontalCenter: parent.horizontalCenter + anchors.bottom: parent.verticalCenter + horizontalAlignment: Text.AlignHCenter + verticalAlignment: Text.AlignVCenter + font.pointSize: 80*viewController.size_ratio + font.bold: true + elide: Text.ElideMiddle + } + + Text { + id: cvmEmergencyStopEnText + color: "#ffffff" + text: qsTr("Stopped by Operator") + anchors.top: cvmEmergencyStopText.bottom + anchors.topMargin: 20*viewController.size_ratio + anchors.horizontalCenter: parent.horizontalCenter + horizontalAlignment: Text.AlignHCenter + verticalAlignment: Text.AlignVCenter + font.pointSize: 60*viewController.size_ratio + font.bold: true + elide: Text.ElideMiddle + } +} diff --git a/src/signage/resource/page/EmergencyStopView/EmergencyStopView.qml b/src/signage/resource/page/EmergencyStopView/EmergencyStopView.qml index 23bd337..edb20da 100644 --- a/src/signage/resource/page/EmergencyStopView/EmergencyStopView.qml +++ b/src/signage/resource/page/EmergencyStopView/EmergencyStopView.qml @@ -6,35 +6,14 @@ Rectangle { width: viewController.monitor_width height: viewController.monitor_height - property int counter: 0 - - Timer { - interval: 10000 - running: true - repeat: true - onTriggered: { - emergencyStopView.counter = emergencyStopView.counter + 1 - } - } - + // comfortable stop (slowing/slow_stop) と揃えて、減速中と停止後で表示を固定する。 + // 減速中 (emergency_slowing) は EmergencyStop、停止後 (emergency_stopped) は + // EmergencyStopping を表示する (10 秒ごとの切り替えは廃止)。 EmergencyStop { - visible: emergencyStopView.counter % 2 === 0 + visible: viewController.view_mode === "emergency_slowing" } EmergencyStopping { - visible: emergencyStopView.counter % 2 === 1 + visible: viewController.view_mode === "emergency_stopped" } - - states: [ - State { - name: "init" - when: viewController.view_mode === "emergency_stopped" - StateChangeScript { - name: "init value" - script: { - emergencyStopView.counter = 0 - } - } - } - ] } diff --git a/src/signage/resource/page/StandingWarningView.qml b/src/signage/resource/page/StandingWarningView.qml new file mode 100644 index 0000000..9b922e7 --- /dev/null +++ b/src/signage/resource/page/StandingWarningView.qml @@ -0,0 +1,81 @@ +import QtQuick 2.9 +import QtQuick.Controls 2.2 + +import "Common" + +Rectangle { + id: standingWarningView + width: viewController.monitor_width + height: viewController.monitor_height + color: "#ffffff" + + // 警告種別ごとの表示文言 + // depart : 発車時 (UC-02) + // sudden_stop : 急減速 (UC-03) + // sudden_turn : 急操舵 (UC-03) + // sudden_stop_turn : 急減速かつ急操舵 (UC-03) + function mainText() { + switch (viewController.standing_warning_type) { + case "depart": + return qsTr("発車します。手すり・つり革にしっかりとおつかまりください") + case "sudden_stop": + return qsTr("急停車します！") + case "sudden_turn": + return qsTr("急旋回します！") + case "sudden_stop_turn": + return qsTr("急停車・急旋回します！") + default: + return "" + } + } + + function subText() { + switch (viewController.standing_warning_type) { + case "depart": + return qsTr("Bus moving. Please hold on") + default: + return qsTr("Sudden stops and turns.") + } + } + + // 発進メッセージは長文のため小さめにし、時刻表示との重なりを避ける + function mainFontSize() { + return viewController.standing_warning_type === "depart" ? 40 : 60 + } + + function subFontSize() { + return viewController.standing_warning_type === "depart" ? 30 : 50 + } + + CurrentTime { + id: displayCurrentTime + } + + Text { + id: standingWarningText + color: "#000000" + text: standingWarningView.mainText() + width: parent.width * 0.9 + anchors.horizontalCenter: parent.horizontalCenter + anchors.bottom: parent.verticalCenter + horizontalAlignment: Text.AlignHCenter + verticalAlignment: Text.AlignVCenter + font.pointSize: standingWarningView.mainFontSize()*viewController.size_ratio + font.bold: true + wrapMode: Text.WordWrap + } + + Text { + id: standingWarningSubText + color: "#000000" + text: standingWarningView.subText() + anchors.top: standingWarningText.bottom + anchors.topMargin: 20*viewController.size_ratio + anchors.horizontalCenter: parent.horizontalCenter + horizontalAlignment: Text.AlignHCenter + verticalAlignment: Text.AlignVCenter + font.pointSize: standingWarningView.subFontSize()*viewController.size_ratio + font.bold: true + elide: Text.ElideMiddle + } +} diff --git a/src/signage/resource/page/main.qml b/src/signage/resource/page/main.qml index e74fd31..5a5a68e 100644 --- a/src/signage/resource/page/main.qml +++ b/src/signage/resource/page/main.qml @@ -5,6 +5,7 @@ import QtQuick.Controls 2.2 import "BusStopView" import "BusMoveView" import "EmergencyStopView" +import "CvmDisplayView" Window { id: window @@ -16,7 +17,12 @@ Window { EmergencyStopView { id: emergencyStopView - visible: viewController.view_mode === "emergency_stopped" + visible: viewController.view_mode === "emergency_slowing" || viewController.view_mode === "emergency_stopped" + } + + CvmDisplayView { + id: cvmDisplayView + visible: viewController.view_mode === "cvm_display" } Disconnected { @@ -34,6 +40,12 @@ Window { visible: viewController.view_mode === "slowing" } + StandingWarningView { + id: standingWarningView + visible: viewController.view_mode === "standing_sudden_warning" + || viewController.view_mode === "standing_depart_warning" + } + ManualDriving { id: manualDriving visible: viewController.view_mode === "manual_driving" @@ -49,6 +61,11 @@ Window { visible: viewController.view_mode === "out_of_service" } + ArrivedView { + id: arrivedView + visible: viewController.view_mode === "arrived" + } + BusStopView { id: busStopView visible: viewController.view_mode === "stopping" diff --git a/src/signage/resource/sound/arrive_caution.wav b/src/signage/resource/sound/arrive_caution.wav new file mode 100644 index 0000000..a24bce8 Binary files /dev/null and b/src/signage/resource/sound/arrive_caution.wav differ diff --git a/src/signage/resource/sound/standing_depart.wav b/src/signage/resource/sound/standing_depart.wav new file mode 100644 index 0000000..0ef9768 Binary files /dev/null and b/src/signage/resource/sound/standing_depart.wav differ diff --git a/src/signage/resource/sound/standing_sudden_stop.wav b/src/signage/resource/sound/standing_sudden_stop.wav new file mode 100644 index 0000000..9c608e5 Binary files /dev/null and b/src/signage/resource/sound/standing_sudden_stop.wav differ diff --git a/src/signage/resource/sound/standing_sudden_stop_turn.wav b/src/signage/resource/sound/standing_sudden_stop_turn.wav new file mode 100644 index 0000000..d410d5c Binary files /dev/null and b/src/signage/resource/sound/standing_sudden_stop_turn.wav differ diff --git a/src/signage/resource/sound/standing_sudden_turn.wav b/src/signage/resource/sound/standing_sudden_turn.wav new file mode 100644 index 0000000..9d84de5 Binary files /dev/null and b/src/signage/resource/sound/standing_sudden_turn.wav differ diff --git a/src/signage/resource/sound/temporary_stop.wav b/src/signage/resource/sound/temporary_stop.wav new file mode 100644 index 0000000..086b99f Binary files /dev/null and b/src/signage/resource/sound/temporary_stop.wav differ diff --git a/src/signage/setup.py b/src/signage/setup.py index 084fcdb..bbb257a 100644 --- a/src/signage/setup.py +++ b/src/signage/setup.py @@ -7,7 +7,7 @@ def package_files(directory): paths = [] - for (path, directories, filenames) in os.walk(directory): + for path, directories, filenames in os.walk(directory): for filename in filenames: paths.append(os.path.join(path, filename)) return paths @@ -36,6 +36,10 @@ def package_files(directory): "share/" + package_name + "/resource/page/EmergencyStopView", package_files("resource/page/EmergencyStopView"), ), + ( + "share/" + package_name + "/resource/page/CvmDisplayView", + package_files("resource/page/CvmDisplayView"), + ), ( "share/" + package_name + "/resource/page/Common", package_files("resource/page/Common"), diff --git a/src/signage/src/signage/announce_controller.py b/src/signage/src/signage/announce_controller.py index 0d8851a..8078407 100644 --- a/src/signage/src/signage/announce_controller.py +++ b/src/signage/src/signage/announce_controller.py @@ -14,6 +14,8 @@ from tier4_hmi_msgs.srv import SetVolume from tier4_external_api_msgs.msg import ResponseStatus +from signage.settings_store import KEY_VOLUME + # The higher the value, the higher the priority PRIORITY_DICT = { "emergency": 3, @@ -24,20 +26,32 @@ "arrived": 2, "thank_you": 2, "in_emergency": 2, + # UC-04 停止案内 (SYS-HMI-04/05/06)。障害物/横断歩道/一般とも共通で「停車します」。 + "temporary_stop": 2, + # 立席運行 転倒防止アナウンス (MRM=3 の下に配置) + # 急減速・急操舵は priority 2。発車警告は engage(2) に続けて再生させるため + # priority 1 とし、engage 再生中はキューに積んで後続で再生する。 + "standing_sudden_stop": 2, + "standing_sudden_turn": 2, + "standing_sudden_stop_turn": 2, + "standing_depart": 1, "going_to_depart": 1, "going_to_arrive": 1, + "arrive_caution": 1, # UC-05 接近時(10m)の車内安全配慮アナウンス (SYS-HMI-07) } -CURRENT_VOLUME_PATH = "/opt/autoware/volume.txt" - - class AnnounceControllerProperty: - def __init__(self, node, autoware_interface, parameter_interface): + def __init__(self, node, autoware_interface, parameter_interface, settings_store): super(AnnounceControllerProperty, self).__init__() self._node = node self._parameter = parameter_interface.parameter + self._settings = settings_store self._announce_settings = parameter_interface.announce_settings + self._announce_interval = parameter_interface.announce_interval + # 発話カテゴリごとの前回発話時刻 (VVAS の TimeoutClass 相当)。 + # 未発話のカテゴリはキーを持たず、in_interval は False を返す。 + self._announce_timeout = {} self._current_announce = "" self._pending_announce_list = [] self._sound = QSound("") @@ -46,14 +60,12 @@ def __init__(self, node, autoware_interface, parameter_interface): self._check_playing_timer = self._node.create_timer(1, self.check_playing_callback) self._pulse = Pulse() - if os.path.isfile(CURRENT_VOLUME_PATH): - with open(CURRENT_VOLUME_PATH, "r") as f: - volume = f.readline() - if volume != "": - self._sink = self._pulse.get_sink_by_name( - self._pulse.server_info().default_sink_name - ) - self._pulse.volume_set_all_chans(self._sink, float(volume)) + volume = self._settings.get(KEY_VOLUME) + if volume is not None: + self._sink = self._pulse.get_sink_by_name( + self._pulse.server_info().default_sink_name + ) + self._pulse.volume_set_all_chans(self._sink, float(volume)) self._get_volume_pub = self._node.create_publisher(Float32, "~/get/volume", 1) self._node.create_timer(1.0, self.publish_volume_callback) @@ -85,36 +97,71 @@ def check_playing_callback(self): self._node.get_logger().error("not able to check the current playing: " + str(e)) def play_sound(self, message): - self._sound = QSound(self._package_path + message + ".wav") - self._sound.play() + # 音声ファイルが無い/再生に失敗した場合は False を返し、呼び出し側で + # 提供失敗として記録できるようにする (NFR-08/UC02-04/UC03-05) + sound_path = self._package_path + message + ".wav" + if not os.path.isfile(sound_path): + self._node.get_logger().error( + "announce sound file not found: {}".format(sound_path) + ) + return False + try: + self._sound = QSound(sound_path) + self._sound.play() + return True + except Exception as e: + self._node.get_logger().error( + "not able to play the announce '{}': {}".format(message, str(e)) + ) + return False # skip announce by setting def check_announce_or_not(self, message): try: + # 急減速・急操舵の各アナウンスは単一の standing_sudden フラグで制御する + if message.startswith("standing_sudden"): + return self._announce_settings.standing_sudden return getattr(self._announce_settings, message) except Exception as e: self._node.get_logger().error("check announce or not: " + str(e)) return False + def _log_announce_outcome(self, message, outcome): + # 全アナウンス共通の提供結果ログ (NFR-08/NFR-10)。 + # played / queued / skipped_by_setting / suppressed_by_priority / failed + # 提供失敗のみ error、それ以外は info で記録する。戻り値はそのまま返す + log = "announce '{}' provided -> {}".format(message, outcome) + if outcome == "failed": + self._node.get_logger().error(log) + else: + self._node.get_logger().info(log) + return outcome + def send_announce(self, message): + # 提供結果を返す: played / queued / skipped_by_setting / + # suppressed_by_priority / failed priority = PRIORITY_DICT.get(message, 0) previous_priority = PRIORITY_DICT.get(self._current_announce, 0) if not self.check_announce_or_not(message): - return + return self._log_announce_outcome(message, "skipped_by_setting") + + def _play(): + return "played" if self.play_sound(message) else "failed" + outcome = "suppressed_by_priority" if priority == 3: self._sound.stop() - self.play_sound(message) + outcome = _play() elif priority == 2: if priority > previous_priority: self._sound.stop() - self.play_sound(message) + outcome = _play() elif priority == previous_priority: - self.play_sound(message) + outcome = _play() elif priority == 1: if not previous_priority: - self.play_sound(message) + outcome = _play() elif previous_priority in [2, 1]: self._pending_announce_list.append( { @@ -122,12 +169,48 @@ def send_announce(self, message): "requested_time": self._node.get_clock().now(), } ) + outcome = "queued" self._current_announce = message + return self._log_announce_outcome(message, outcome) + + def stop_standing_announce(self): + # MRM最優先: 再生中の立席アナウンスを停止する (画面はMRMへ切替わるため音声も揃える) + if self._current_announce.startswith("standing_"): + self._sound.stop() + self._current_announce = "" + + def in_interval(self, category): + # VVAS の announce_interval と同方式: 前回発話から interval 秒未満なら True + # (= 再発話抑止中)。未発話のカテゴリは False (起動直後に警告表示が出ないように)。 + trigger_time = self._announce_timeout.get(category) + if trigger_time is None: + return False + duration = getattr(self._announce_interval, category) + return self._node.get_clock().now() - trigger_time < Duration(seconds=duration) + + def set_timeout(self, category): + # 発話トリガー時に呼び、当該カテゴリの前回発話時刻を「今」に更新する + self._announce_timeout[category] = self._node.get_clock().now() def announce_arrived(self): if self._parameter.signage_stand_alone: + # 終点・通常停留所とも音声は「ご乗車ありがとうございました」(thank_you) を流用する。 + # 到着停留所名は route_handler 側の乗客サイネージ表示で提示する (SYS-HMI-07)。 self.send_announce("thank_you") + def in_interval(self, category): + # VVAS の announce_interval と同方式: 前回発話から interval 秒未満なら True + # (= 再発話抑止中)。未発話のカテゴリは False (起動直後に表示が出ないように)。 + trigger_time = self._announce_timeout.get(category) + if trigger_time is None: + return False + duration = getattr(self._announce_interval, category) + return self._node.get_clock().now() - trigger_time < Duration(seconds=duration) + + def set_timeout(self, category): + # 発話トリガー時に呼び、当該カテゴリの前回発話時刻を「今」に更新する + self._announce_timeout[category] = self._node.get_clock().now() + def announce_emergency(self, message): if self._parameter.signage_stand_alone: self.send_announce(message) @@ -143,8 +226,7 @@ def set_volume(self, request, response): try: self._sink = self._pulse.get_sink_by_name(self._pulse.server_info().default_sink_name) self._pulse.volume_set_all_chans(self._sink, request.volume) - with open(CURRENT_VOLUME_PATH, "w") as f: - f.write(f"{self._sink.volume.value_flat}\n") + self._settings.set(KEY_VOLUME, self._sink.volume.value_flat) response.status.code = ResponseStatus.SUCCESS except Exception: response.status.code = ResponseStatus.ERROR @@ -157,4 +239,3 @@ def test_volume(self, request, response): except Exception: response.success = False return response - diff --git a/src/signage/src/signage/autoware_diagnostic.py b/src/signage/src/signage/autoware_diagnostic.py index ee8650a..2e370e5 100644 --- a/src/signage/src/signage/autoware_diagnostic.py +++ b/src/signage/src/signage/autoware_diagnostic.py @@ -20,7 +20,8 @@ import diagnostic_updater -class AutowareDiagnostic(): + +class AutowareDiagnostic: def init_updater(self, node, name, update_function, hardware_id): updater = diagnostic_updater.Updater(node, 1) updater.setHardwareID(hardware_id) diff --git a/src/signage/src/signage/autoware_interface.py b/src/signage/src/signage/autoware_interface.py index c39bcb8..81c7a47 100644 --- a/src/signage/src/signage/autoware_interface.py +++ b/src/signage/src/signage/autoware_interface.py @@ -1,24 +1,38 @@ # !/usr/bin/env python3 # -*- coding: utf-8 -*- +import math import rclpy -from dataclasses import dataclass +from dataclasses import dataclass, field from autoware_adapi_v1_msgs.msg import ( RouteState, + Route, MrmState, OperationModeState, MotionState, LocalizationInitializationState, - VelocityFactorArray, + VehicleKinematics, Heartbeat, ) +from autoware_adapi_v1_msgs.srv import GetVehicleDimensions from std_msgs.msg import String import signage.signage_utils as utils -from autoware_internal_debug_msgs.msg import Float64Stamped from tier4_external_api_msgs.msg import DoorStatus +from tier4_metric_msgs.msg import MetricArray DISCONNECT_THRESHOLD = 2 +# /control/control_evaluator/metrics の metric名 -> AutowareInformation の属性名 +# (UC-03: 急減速・急操舵判定に使用) +CONTROL_METRIC_ATTR_MAP = { + "acceleration": "longitudinal_acceleration", + "jerk": "longitudinal_jerk", + "lateral_acceleration_abs": "lateral_acceleration_abs", + "steering_angle_abs": "steering_angle_abs", + "steering_rate": "steering_rate", + "steering_acceleration": "steering_acceleration", +} + @dataclass class AutowareInformation: @@ -31,6 +45,16 @@ class AutowareInformation: motion_state: int = 0 localization_init_state: int = 0 active_schedule: str = "" + # UC-04: 停止種別判定に使用する velocity_factors (VelocityFactor のリスト) + velocity_factors: list = field(default_factory=list) + longitudinal_acceleration: float = 0.0 + longitudinal_jerk: float = 0.0 + lateral_acceleration_abs: float = 0.0 + steering_angle_abs: float = 0.0 + steering_rate: float = 0.0 + steering_acceleration: float = 0.0 + velocity: float = 0.0 # 縦速度 [m/s] (横ジャーク計算に使用) + wheel_base: float = 2.75 # ホイールベース [m] (起動時に dimensions サービスで上書き) class AutowareInterface: @@ -39,6 +63,8 @@ def __init__(self, node, parameter_interface): self.information = AutowareInformation() self._parameter = parameter_interface.parameter self.is_disconnected = False + # 現在のルートのゴール座標 (map系)。ルート未設定時は None + self._goal_position = None sub_qos = rclpy.qos.QoSProfile( history=rclpy.qos.QoSHistoryPolicy.KEEP_LAST, @@ -74,10 +100,21 @@ def __init__(self, node, parameter_interface): self._sub_vehicle_door = node.create_subscription( DoorStatus, "/api/external/get/door", self.sub_vehicle_door_callback, sub_qos ) - self._sub_path_distance = node.create_subscription( - Float64Stamped, - "/autoware_api/utils/path_distance_calculator/distance", - self.sub_path_distance_callback, + # /autoware_api/utils/path_distance_calculator/distance が廃止予定のため、 + # ゴール姿勢 (/api/routing/route) と自車位置 (/api/vehicle/kinematics) から + # ゴールまでの直線距離を算出して goal_distance を求める。 + # ゴール直前がカーブ / 経路がゴール近傍を通過する場合は実際の経路長と乖離するが、 + # ゴール近傍 (<100m) の到着判定用途としては許容範囲とする。 + self._sub_route = node.create_subscription( + Route, + "/api/routing/route", + self.sub_route_callback, + api_qos, + ) + self._sub_kinematics = node.create_subscription( + VehicleKinematics, + "/api/vehicle/kinematics", + self.sub_kinematics_callback, sub_qos, ) self._sub_motion_state = node.create_subscription( @@ -95,12 +132,28 @@ def __init__(self, node, parameter_interface): self.sub_active_schedule_callback, sub_qos, ) - self._sub_active_schedule = node.create_subscription( + self._sub_heartbeat = node.create_subscription( Heartbeat, "/api/system/heartbeat", self.sub_heartbeat_callback, sub_qos, - ) + ) + self._sub_control_metrics = node.create_subscription( + MetricArray, + "/control/control_evaluator/metrics", + self.sub_control_metrics_callback, + sub_qos, + ) + # wheelbase は横ジャーク計算 (v^2 * steering_rate / wheel_base) に使う静的値。 + # /api/vehicle/dimensions はトピックではなくサービスのため、起動時に1回だけ取得する。 + # signage 起動時にサービスがまだ立ち上がっていないことがあるので、利用可能になるまで + # タイマーでリトライし、取得できるまでは param のフォールバック値を使う。 + self.information.wheel_base = self._parameter.wheel_base + self._dimensions_client = node.create_client( + GetVehicleDimensions, "/api/vehicle/dimensions" + ) + self._dimensions_future = None + self._dimensions_timer = node.create_timer(2.0, self.fetch_vehicle_dimensions) if not self._parameter.debug_mode: self._autoware_connection_time = self._node.get_clock().now() self._node.create_timer(1, self.reset_timer) @@ -142,9 +195,31 @@ def sub_vehicle_door_callback(self, msg): except Exception as e: self._node.get_logger().error("Unable to get the vehicle door status, ERROR: " + str(e)) - def sub_path_distance_callback(self, msg): + def sub_route_callback(self, msg): + try: + if msg.data: + self._goal_position = msg.data[0].goal.position + else: + # ルートがクリアされた場合はゴール距離をデフォルトへ戻す + self._goal_position = None + self.information.goal_distance = 1000.0 + except Exception as e: + self._node.get_logger().error("Unable to get the route goal, ERROR: " + str(e)) + + def sub_kinematics_callback(self, msg): + # 縦速度 [m/s] を保持する (横ジャーク計算 v^2 * steering_rate / wheel_base に使用) + try: + self.information.velocity = msg.twist.twist.twist.linear.x + except Exception as e: + self._node.get_logger().error("Unable to get the vehicle velocity, ERROR: " + str(e)) + # ゴールまでの直線距離を算出する (UC-05 到着判定用) try: - self.information.goal_distance = msg.data + if self._goal_position is None: + return + ego = msg.pose.pose.pose.position + self.information.goal_distance = math.hypot( + self._goal_position.x - ego.x, self._goal_position.y - ego.y + ) except Exception as e: self._node.get_logger().error("Unable to get the goal distance, ERROR: " + str(e)) @@ -173,3 +248,58 @@ def sub_heartbeat_callback(self, msg): self._autoware_connection_time = self._node.get_clock().now() except Exception as e: self._node.get_logger().error("Unable to get the heartbeat, ERROR: " + str(e)) + + def sub_control_metrics_callback(self, msg): + # MetricArray の value は文字列のため float に変換して保持する + try: + for metric in msg.metric_array: + attr = CONTROL_METRIC_ATTR_MAP.get(metric.name) + if attr is None: + continue + try: + setattr(self.information, attr, float(metric.value)) + except ValueError: + continue + except Exception as e: + self._node.get_logger().error("Unable to get the control metrics, ERROR: " + str(e)) + + def fetch_vehicle_dimensions(self): + # /api/vehicle/dimensions サービスから wheelbase を1回だけ取得する。 + # サービス未起動なら次のタイマーで再試行し、応答待ち中は多重リクエストしない。 + try: + if self._dimensions_future is not None: + return + if not self._dimensions_client.service_is_ready(): + self._node.get_logger().warn( + "Waiting for /api/vehicle/dimensions service ...", + throttle_duration_sec=10, + ) + return + self._dimensions_future = self._dimensions_client.call_async( + GetVehicleDimensions.Request() + ) + self._dimensions_future.add_done_callback(self.on_vehicle_dimensions_response) + except Exception as e: + self._node.get_logger().error( + "Unable to request vehicle dimensions, ERROR: " + str(e) + ) + + def on_vehicle_dimensions_response(self, future): + try: + wheel_base = future.result().dimensions.wheel_base + if wheel_base > 0.0: + self.information.wheel_base = wheel_base + self._node.get_logger().info( + "Got wheelbase from /api/vehicle/dimensions: {:.3f} m".format(wheel_base) + ) + # 取得できたのでリトライタイマーを停止する + self._dimensions_timer.cancel() + else: + # 不正値のときは param のフォールバック値のまま次回リトライする + self._node.get_logger().warn( + "Invalid wheelbase from service ({}), keep fallback".format(wheel_base) + ) + self._dimensions_future = None + except Exception as e: + self._node.get_logger().error("Unable to get vehicle dimensions, ERROR: " + str(e)) + self._dimensions_future = None diff --git a/src/signage/src/signage/cvm_interface.py b/src/signage/src/signage/cvm_interface.py new file mode 100644 index 0000000..c9d0b57 --- /dev/null +++ b/src/signage/src/signage/cvm_interface.py @@ -0,0 +1,240 @@ +# !/usr/bin/env python3 +# -*- coding: utf-8 -*- +# This Python file uses the following encoding: utf-8 + +from threading import Lock + +from command_view_manager_msgs.msg import DeviceDisplayState, DisplayModeCommand +from command_view_manager_msgs.srv import GetManifest +from rclpy.qos import ( + DurabilityPolicy, + HistoryPolicy, + QoSProfile, + ReliabilityPolicy, +) + + +COMMAND_TOPIC_TEMPLATE = "/command_view_manager/devices/{device_id}/command" +STATE_TOPIC_TEMPLATE = "/command_view_manager/devices/{device_id}/state" +GET_MANIFEST_SERVICE = "/command_view_manager/get_manifest" + +# CVM コマンドを受けたときの内部 view_mode。 +# 個別の表示は QML 側で cvm_display_mode_id を見て分岐する。 +CVM_VIEW_MODE = "cvm_display" + +# CVM コマンドとして受け付ける display_mode_id（cvm_display ページ内で表示分岐するもの） +SUPPORTED_CVM_DISPLAY_MODES = {"remote_emergency_display"} + +# 受信すると override を解除し autoware 由来の view_mode に戻す display_mode_id +RELEASE_DISPLAY_MODES = {"idle_display"} + +# signage internal view_mode → 候補 CVM display_mode_id（state 報告用）。 +# マニフェストの supported_display_mode_ids に含まれる候補のみ採用し、 +# 含まれていない場合は DEFAULT_REPORTED_DISPLAY_MODE にフォールバックする。 +VIEW_MODE_TO_CVM = { + "emergency_slowing": "mrm_emergency_display", + "emergency_stopped": "mrm_emergency_display", + "disconnected": "disconnected_display", + "manual_driving": "manual_driving_display", + "auto_driving": "auto_driving_display", + "slowing": "slow_stop_display", + "slow_stop": "slow_stop_display", + "stopping": "arriving_display", + "driving": "destination_display", + "out_of_service": "idle_display", +} + +# 候補がマニフェストに含まれない／マニフェスト未取得の時のフォールバック +DEFAULT_REPORTED_DISPLAY_MODE = "idle_display" + +STATE_PUBLISH_INTERVAL_SEC = 1.0 +MANIFEST_FETCH_RETRY_SEC = 5.0 + + +class CvmInterface: + """CommandViewManager とのインタフェース。 + + - `/command_view_manager/devices//command` を購読し、 + 指定された display_mode_id に対応する view_mode を上書き要求として保持する + - `/command_view_manager/devices//state` に + 現在の view_mode を CVM 向けにマッピングして 1Hz で発信する + """ + + def __init__(self, node, device_id): + self._node = node + self._device_id = device_id + self._lock = Lock() + self._cvm_view_mode_override = None + self._cvm_display_mode_id_override = None + self._current_view_mode = "" + # マニフェスト取得状態 + # None: 未取得（DEFAULT_REPORTED_DISPLAY_MODE 固定で報告） + # set: 取得済み（含まれる候補のみ採用、それ以外は DEFAULT_REPORTED_DISPLAY_MODE） + self._supported_modes = None + self._manifest_request_pending = False + + command_qos = QoSProfile( + reliability=ReliabilityPolicy.RELIABLE, + history=HistoryPolicy.KEEP_LAST, + depth=10, + durability=DurabilityPolicy.VOLATILE, + ) + state_qos = QoSProfile( + reliability=ReliabilityPolicy.RELIABLE, + history=HistoryPolicy.KEEP_LAST, + depth=10, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + ) + + self._command_sub = node.create_subscription( + DisplayModeCommand, + COMMAND_TOPIC_TEMPLATE.format(device_id=device_id), + self._on_command, + command_qos, + ) + self._state_pub = node.create_publisher( + DeviceDisplayState, + STATE_TOPIC_TEMPLATE.format(device_id=device_id), + state_qos, + ) + self._timer = node.create_timer( + STATE_PUBLISH_INTERVAL_SEC, self._publish_state + ) + + self._manifest_client = node.create_client( + GetManifest, GET_MANIFEST_SERVICE + ) + self._manifest_timer = node.create_timer( + MANIFEST_FETCH_RETRY_SEC, self._try_fetch_manifest + ) + # 起動直後にも一度試みる（タイマー初回発火を待たない） + self._try_fetch_manifest() + + node.get_logger().info( + "CvmInterface started: device_id={}".format(device_id) + ) + + def _on_command(self, msg): + # オーバーライドの変更はクライアントから来た正当なコマンドでのみ行う。 + # サポート外の display_mode_id は WARN ログのみ出して既存の override は維持する + if msg.display_mode_id in RELEASE_DISPLAY_MODES: + self._node.get_logger().info( + "CVM override released: display_mode_id={}, issuer={}".format( + msg.display_mode_id, msg.issuer_client_id + ) + ) + with self._lock: + self._cvm_view_mode_override = None + self._cvm_display_mode_id_override = None + return + + if msg.display_mode_id not in SUPPORTED_CVM_DISPLAY_MODES: + self._node.get_logger().warning( + "CVM command with unsupported display_mode_id ignored: " + "display_mode_id={}, issuer={}".format( + msg.display_mode_id, msg.issuer_client_id + ) + ) + return + + self._node.get_logger().info( + "CVM command received: display_mode_id={}, issuer={}, priority={}".format( + msg.display_mode_id, msg.issuer_client_id, msg.priority, + ) + ) + with self._lock: + self._cvm_view_mode_override = CVM_VIEW_MODE + self._cvm_display_mode_id_override = msg.display_mode_id + + def get_view_mode_override(self): + with self._lock: + return self._cvm_view_mode_override + + def get_display_mode_id_override(self): + with self._lock: + return self._cvm_display_mode_id_override + + def set_current_view_mode(self, view_mode): + with self._lock: + self._current_view_mode = view_mode + + def _publish_state(self): + with self._lock: + view_mode = self._current_view_mode + supported = self._supported_modes + cvm_override_id = self._cvm_display_mode_id_override + + # CVM override 中は受け付けた display_mode_id をそのまま CVM に返す + preferred = cvm_override_id if cvm_override_id else VIEW_MODE_TO_CVM.get(view_mode) + if supported is not None and preferred is not None and preferred in supported: + display_mode_id = preferred + else: + display_mode_id = DEFAULT_REPORTED_DISPLAY_MODE + + msg = DeviceDisplayState() + msg.stamp = self._node.get_clock().now().to_msg() + msg.device_id = self._device_id + msg.display_mode_id = display_mode_id + msg.summary_format = "" + msg.summary_text = "" + self._state_pub.publish(msg) + + def _try_fetch_manifest(self): + with self._lock: + already_loaded = self._supported_modes is not None + pending = self._manifest_request_pending + if already_loaded: + self._manifest_timer.cancel() + return + if pending: + return + if not self._manifest_client.service_is_ready(): + self._node.get_logger().info( + "CVM manifest service not available yet, " + "retrying in {}s".format(MANIFEST_FETCH_RETRY_SEC), + throttle_duration_sec=30, + ) + return + + with self._lock: + self._manifest_request_pending = True + future = self._manifest_client.call_async(GetManifest.Request()) + future.add_done_callback(self._on_manifest_response) + + def _on_manifest_response(self, future): + with self._lock: + self._manifest_request_pending = False + + try: + response = future.result() + except Exception as e: + self._node.get_logger().warning( + "Failed to fetch CVM manifest: {}".format(e) + ) + return + + if response is None: + return + + for device in response.manifest.devices: + if device.device_id == self._device_id: + supported = set(device.supported_display_mode_ids) + with self._lock: + self._supported_modes = supported + self._node.get_logger().info( + "CVM manifest loaded: device={}, supported_modes={}".format( + self._device_id, sorted(supported) + ) + ) + self._manifest_timer.cancel() + return + + self._node.get_logger().warning( + "CVM manifest does not contain device '{}'; " + "all states will be reported as '{}'".format( + self._device_id, DEFAULT_REPORTED_DISPLAY_MODE + ) + ) + with self._lock: + self._supported_modes = set() + self._manifest_timer.cancel() diff --git a/src/signage/src/signage/heartbeat.py b/src/signage/src/signage/heartbeat.py index ce21347..fdece6f 100644 --- a/src/signage/src/signage/heartbeat.py +++ b/src/signage/src/signage/heartbeat.py @@ -4,6 +4,7 @@ from signage.autoware_diagnostic import AutowareDiagnostic from diagnostic_msgs.msg import DiagnosticStatus + class Heartbeat: def __init__(self, node): self._node = node @@ -17,4 +18,4 @@ def __init__(self, node): def handle_heartbeat_diagnostics(self, stat): stat.summary(DiagnosticStatus.OK, "signage is working") - return stat \ No newline at end of file + return stat diff --git a/src/signage/src/signage/packet_tools.py b/src/signage/src/signage/packet_tools.py deleted file mode 100644 index 37d485a..0000000 --- a/src/signage/src/signage/packet_tools.py +++ /dev/null @@ -1,267 +0,0 @@ -import datetime -import operator -from functools import reduce - -# Packet start and end markers -SOT = 0xAA -EOT = 0x55 - - -def dump_packet(packet, timestamp=None, color=None): - """ - Prints a packet's contents in a formatted manner, optionally with color and timestamp. - - Parameters: - - packet: List[int], the packet data including start and end markers. - - timestamp: datetime.datetime, optional timestamp to prepend to the output. - - color: str, an optional terminal color code to apply to the output. - """ - verified = verify_sum(packet) - if color: - print(color, end="") - if timestamp: - print(timestamp, end=" ") - for p in packet: - print(f"{p:02x}", end=" ") - if color: - print("\033[0m", end="") - print() - - -def calc_sum(payload): - """ - Calculates the checksum of a given payload. - - Parameters: - - payload: List[int], the payload data from which to calculate the checksum. - - Returns: - - List[int], a two-element list containing the checksum bytes. - """ - s = reduce(operator.add, payload) - return [s & 0xFF, (s & 0xFF00) >> 8] - - -def verify_sum(packet): - """ - Verifies the checksum of a packet. - - Parameters: - - packet: List[int], the complete packet including checksum and markers. - - Returns: - - bool, True if the checksum is correct, False otherwise. - """ - sum = calc_sum(packet[1:-3]) - return (packet[-3] == sum[0]) and (packet[-2] == sum[1]) - - -def gen_data_packet(data, seq, addr1, addr2): - """ - Generates a single data packet from given data and sequence number. - - Parameters: - - data: List[int], the data to include in the packet. - - seq: int, the sequence number of the packet. - - addr1: int, the first part of the address. - - addr2: int, the second part of the address. - - Returns: - - List[int], the assembled packet. - """ - length = len(data) + 8 - cmd = 0x20 - payload = [addr1, addr2, length, cmd, seq, 0x00] + list(data) - checksum = calc_sum(payload) - return [SOT] + payload + checksum + [EOT] - - -def gen_data_packets(data, addr1, addr2): - """ - Generates a sequence of data packets from a larger dataset. - - Parameters: - - data: List[int], the complete set of data to be sent in packets. - - addr1: int, the first part of the address for the packets. - - addr2: int, the second part of the address for the packets. - - Returns: - - List[List[int]], a list of assembled packets. - """ - packets = [] - seq = 0 - for i in range(0, len(data), 128): - packet_data = data[i : i + 128] - packets.append(gen_data_packet(packet_data, seq, addr1, addr2)) - seq += 1 - return packets - - -def gen_name_time_packet(linename, timestamp, nightmode): - """ - Generates a packet containing a line name and the current time. - - Parameters: - - linename: str, the name of the line, must be 16 characters. - - timestamp: datetime.datetime, the current time. - - nightmode: bool, whether night mode is enabled. - - Returns: - - List[int], the assembled packet. - """ - addr1 = 0x60 - addr2 = 0x9F - length = 0x24 - cmd = 0x01 - if len(linename) != 16: - raise ValueError("Line Name length invalid") - payload = [addr1, addr2, length, cmd] + list(linename) - payload += map(lambda x: int(x, 16), timestamp.strftime("%M %H %d %w %m %y").split()) - payload.append(0x10 if nightmode else 0x00) - payload.extend([0x00] * 7) - checksum = calc_sum(payload) - return [SOT] + payload + checksum + [EOT] - - -def lists_match(l1, l2): - """ - Compares two lists for equality. - - Parameters: - - l1: List, the first list to compare. - - l2: List, the second list to compare. - - Returns: - - bool, True if the lists are equal, False otherwise. - """ - return len(l1) == len(l2) and all(x == y for x, y in zip(l1, l2)) - - -class TD5Data: - """ - Represents data extracted from a .td5 file, including line name, data packets, and heartbeat packet. - - Attributes: - - width: int, the width of the display. - - height: int, the height of the display. - - linename: bytes, the name of the line extracted from the file. - - data_packets: List[List[int]], the data packets ready for transmission. - - heartbeat_packet: List[int], the heartbeat packet for maintaining connection. - """ - - def __init__(self, filename, addr1, addr2, height, width): - self.width = width - self.height = height - with open(filename, "rb") as f: - f.seek(0x400) - self.linename = f.read(16) - f.seek(0x61C) - f.seek(0x607) - raw_linedatalen = f.read(2) - self.linedatalen = int.from_bytes(raw_linedatalen, "little") - f.seek(0x600) - linedata = f.read(self.linedatalen) - self.data_packets = gen_data_packets(linedata, addr1, addr2) - f.seek(0x603) - data = f.read(2) - self.heartbeat_packet = self.gen_heartbeat_packet( - data, raw_linedatalen, height, width, addr1, addr2 - ) - - def gen_heartbeat_packet(self, data, linedatalen, height, width, addr1, addr2): - """ - Generates a heartbeat packet based on the given parameters. - - Parameters: - - data: bytes, part of the data extracted from the .td5 file. - - linedatalen: bytes, the length of the line data. - - height: int, the height of the display. - - width: int, the width of the display. - - addr1: int, the first part of the display address. - - addr2: int, the second part of the display address. - - Returns: - - List[int], the assembled heartbeat packet. - """ - cmd1 = 0x16 - cmd2 = 0x12 - payload = [addr1, addr2, cmd1, cmd2] - payload.extend([0x00, 0x00, height, 0x00]) - payload.extend(linedatalen) - payload.extend([0x00, 0x00]) - payload.extend(data) - payload.extend([0x00, 0x00, 0x53, 0x30, 0x30, 0x35]) - checksum = calc_sum(payload) - return [SOT] + payload + checksum + [EOT] - - -class Parser: - """ - Parses incoming data from a serial bus into complete packets. - - Attributes: - - bus: serial.Serial, the serial bus from which to read data. - """ - - def __init__(self, bus): - self.state = 0 - self.buf = [] - self.i = 0 - self.datalen = 0 - self.bus = bus - - def read(self): - """ - Reads a single byte from the serial bus. - - Returns: - - int, the byte read, or None if no data is available. - """ - return self.bus.read(1) - - def parse(self, c): - """ - Parse a single byte and update the state machine. - - Parameters: - - c: int, the byte to parse. - - Returns: - - int: 1 if a complete packet is parsed successfully, 0 otherwise. - """ - if self.state == 0 and c == 0xAA: # Start of packet - self.buf = [c] - self.state = 1 - elif self.state == 1: # Reading packet header - self.buf.append(c) - self.i += 1 - if self.i == 2: # Length byte next - self.state = 2 - elif self.state == 2: # Reading packet length - self.buf.append(c) - self.datalen = int(c) - self.state = 3 - self.i = 0 - elif self.state == 3: # Reading packet payload - self.buf.append(c) - self.i += 1 - if self.i == (self.datalen - 2): # Packet complete - self.state = 0 - return 1 # Indicate a complete packet has been parsed - return 0 # Indicate parsing is ongoing - - def wait_ack(self): - """ - Wait for an acknowledgement packet. - - Returns: - - list: The received acknowledgement packet, or an empty list if none is received. - """ - while True: - received = self.read() - if not received or len(received) == 0: - break # No more data available - c = received[0] - if self.parse(c): - return self.buf # Return the complete packet - return [] # No acknowledgement received diff --git a/src/signage/src/signage/parameter_interface.py b/src/signage/src/signage/parameter_interface.py index b3b359f..a862816 100644 --- a/src/signage/src/signage/parameter_interface.py +++ b/src/signage/src/signage/parameter_interface.py @@ -8,6 +8,7 @@ @dataclass class SignageParameter: debug_mode: bool = False + use_external_signage: bool = False signage_stand_alone: bool = False ignore_manual_driving: bool = False ignore_disconnected: bool = False @@ -17,10 +18,21 @@ class SignageParameter: goal_distance: float = 1.0 check_fms_time: float = 5.0 accept_start: float = 5.0 + arriving_distance: float = 10.0 # UC-05 到着接近の車内安全配慮しきい値 (m) emergency_ignore_period: float = 5.0 emergency_repeat_period: float = 180.0 monitor_width: int = 1920 monitor_height: int = 540 + cvm_device_id: str = "in_vehicle_signage" + standing_mode_default: bool = False + sudden_decel_threshold: float = 0.8 + sudden_decel_jerk_threshold: float = 0.6 + # 急操舵は横加速度 (metric) と横ジャーク (計算) のいずれか超過で判定する (SYS2-UC03-01「いずれか」) + sudden_lateral_accel_threshold: float = 0.8 + sudden_lateral_jerk_threshold: float = 0.5 + # 横ジャーク計算 (v^2 * steering_rate / wheel_base) 用のフォールバック値。 + # 起動時に /api/vehicle/dimensions サービスから取得でき次第そちらで上書きされる。 + wheel_base: float = 2.75 @dataclass @@ -35,20 +47,37 @@ class AnnounceParameter: in_emergency: bool = True going_to_depart: bool = True going_to_arrive: bool = True + arrive_caution: bool = True + temporary_stop: bool = True + standing_depart: bool = True + standing_sudden: bool = True + + +@dataclass +class AnnounceIntervalParameter: + # 発話後にその期間だけ再発話を抑止する (秒)。VVAS の announce_interval と同方式。 + stop_reason: float = 20.0 # UC-04 停止案内 (SYS-HMI-04/05/06) + arrived: float = 5.0 # UC-05 到着表示の表示秒数 (SYS-HMI-07) + # standing_sudden は急減速/急操舵/複合の3種で共有する (VVAS の turn_signal と同様)。 + standing_depart: float = 5.0 # UC-02 発車警告 (SYS2-UC02-01) + standing_sudden: float = 10.0 # UC-03 急減速・急操舵警告 (SYS2-UC03-06) class ParameterInterface: def __init__(self, node): self.parameter = SignageParameter() self.announce_settings = AnnounceParameter() + self.announce_interval = AnnounceIntervalParameter() node.declare_parameter("debug_mode", False) + node.declare_parameter("use_external_signage", False) node.declare_parameter("signage_stand_alone", False) node.declare_parameter("ignore_disconnected", False) node.declare_parameter("ignore_manual_driving", False) node.declare_parameter("freeze_emergency", True) node.declare_parameter("check_fms_time", 5.0) node.declare_parameter("accept_start", 5.0) + node.declare_parameter("arriving_distance", 10.0) node.declare_parameter("ignore_emergency_stoppped", False) node.declare_parameter("set_goal_by_distance", False) node.declare_parameter("goal_distance", 1.0) @@ -56,10 +85,20 @@ def __init__(self, node): node.declare_parameter("emergency_repeat_period", 180.0) node.declare_parameter("monitor_width", 1920) node.declare_parameter("monitor_height", 540) + node.declare_parameter("cvm_device_id", "in_vehicle_signage") + node.declare_parameter("standing_mode_default", False) + node.declare_parameter("sudden_decel_threshold", 0.8) + node.declare_parameter("sudden_decel_jerk_threshold", 0.6) + node.declare_parameter("sudden_lateral_accel_threshold", 0.8) + node.declare_parameter("sudden_lateral_jerk_threshold", 0.5) + node.declare_parameter("wheel_base", 2.75) self.parameter.debug_mode = ( node.get_parameter("debug_mode").get_parameter_value().bool_value ) + self.parameter.use_external_signage = ( + node.get_parameter("use_external_signage").get_parameter_value().bool_value + ) self.parameter.signage_stand_alone = ( node.get_parameter("signage_stand_alone").get_parameter_value().bool_value ) @@ -78,6 +117,9 @@ def __init__(self, node): self.parameter.accept_start = ( node.get_parameter("accept_start").get_parameter_value().double_value ) + self.parameter.arriving_distance = ( + node.get_parameter("arriving_distance").get_parameter_value().double_value + ) self.parameter.ignore_emergency = ( node.get_parameter("ignore_emergency_stoppped").get_parameter_value().bool_value ) @@ -99,6 +141,27 @@ def __init__(self, node): self.parameter.monitor_height = ( node.get_parameter("monitor_height").get_parameter_value().integer_value ) + self.parameter.cvm_device_id = ( + node.get_parameter("cvm_device_id").get_parameter_value().string_value + ) + self.parameter.standing_mode_default = ( + node.get_parameter("standing_mode_default").get_parameter_value().bool_value + ) + self.parameter.sudden_decel_threshold = ( + node.get_parameter("sudden_decel_threshold").get_parameter_value().double_value + ) + self.parameter.sudden_decel_jerk_threshold = ( + node.get_parameter("sudden_decel_jerk_threshold").get_parameter_value().double_value + ) + self.parameter.sudden_lateral_accel_threshold = ( + node.get_parameter("sudden_lateral_accel_threshold").get_parameter_value().double_value + ) + self.parameter.sudden_lateral_jerk_threshold = ( + node.get_parameter("sudden_lateral_jerk_threshold").get_parameter_value().double_value + ) + self.parameter.wheel_base = ( + node.get_parameter("wheel_base").get_parameter_value().double_value + ) node.declare_parameter("announce.emergency", True) node.declare_parameter("announce.restart_engage", True) @@ -109,6 +172,10 @@ def __init__(self, node): node.declare_parameter("announce.in_emergency", True) node.declare_parameter("announce.going_to_depart", True) node.declare_parameter("announce.going_to_arrive", True) + node.declare_parameter("announce.arrive_caution", True) + node.declare_parameter("announce.temporary_stop", True) + node.declare_parameter("announce.standing_depart", True) + node.declare_parameter("announce.standing_sudden", True) announce_prefix = node.get_parameters_by_prefix("announce") @@ -118,3 +185,17 @@ def __init__(self, node): key, announce_prefix[key].get_parameter_value().bool_value, ) + + node.declare_parameter("announce_interval.stop_reason", 20.0) + node.declare_parameter("announce_interval.arrived", 5.0) + node.declare_parameter("announce_interval.standing_depart", 5.0) + node.declare_parameter("announce_interval.standing_sudden", 10.0) + + announce_interval_prefix = node.get_parameters_by_prefix("announce_interval") + + for key in announce_interval_prefix.keys(): + setattr( + self.announce_interval, + key, + announce_interval_prefix[key].get_parameter_value().double_value, + ) diff --git a/src/signage/src/signage/ros_service_interface.py b/src/signage/src/signage/ros_service_interface.py index f390fe2..d813c14 100644 --- a/src/signage/src/signage/ros_service_interface.py +++ b/src/signage/src/signage/ros_service_interface.py @@ -35,7 +35,10 @@ def __init__(self, node, parameter_interface): self._cli_accept_start = self.__create_client( AcceptStart, "/api/motion/accept_start" ) - self._cli_trigger_external = self.__create_client(SetBool, "/signage/trigger_external") + if self._parameter.use_external_signage: + self._cli_trigger_external = self.__create_client( + SetBool, "/signage/trigger_external" + ) # service call function def accept_start(self): @@ -47,7 +50,7 @@ def accept_start(self): # service call function def trigger_external_signage(self, on): - if self._parameter.debug_mode: + if self._parameter.debug_mode or not self._parameter.use_external_signage: return request = SetBool.Request() diff --git a/src/signage/src/signage/route_handler.py b/src/signage/src/signage/route_handler.py index d7dd41b..360b075 100644 --- a/src/signage/src/signage/route_handler.py +++ b/src/signage/src/signage/route_handler.py @@ -25,6 +25,8 @@ def __init__( autoware_interface, parameter_interface, ros_service_interface, + cvm_interface, + standing_mode_interface, ): self._node = node self._viewController = viewController @@ -32,10 +34,12 @@ def __init__( self._autoware = autoware_interface self._parameter = parameter_interface.parameter self._service_interface = ros_service_interface + self._cvm = cvm_interface + self._standing_mode = standing_mode_interface self._schedule_details = utils.init_ScheduleDetails() self._display_details = utils.init_DisplayDetails() self._current_task_details = utils.init_CurrentTask() - self._task_list = utils.init_TaskList() + self.task_list = utils.init_TaskList() self._display_phrase = "" self._in_emergency_state = False self._emergency_trigger_time = self._node.get_clock().now() @@ -54,9 +58,21 @@ def __init__( self._in_slowing_state = False self._announced_depart = False self._announced_arrive = False + # UC-05: 接近10mの安全配慮アナウンス発話済みフラグ・直近到着停留所 (到着表示用) + self._announced_arrive_caution = False + self._arrived_station = ["", ""] + # UC-04: 停止1回につき停止案内は1回。再発進でリセットする。 + # 初期値 True (disarm) = 未発進では発話しない。最初の実発進で arm される。 + self._stop_announce_executed = True self._trigger_external_signage = False self._processing_thread = False + # 立席運行 転倒防止アナウンスの状態 (UC-02/UC-03) + # 発車警告と急減速・急操舵警告はそれぞれ独立したクールダウンを持つ + # 再発話抑止・警告表示時間は announce_controller の announce_interval で管理する + # (VVAS と同方式)。ここでは表示に使う直近の急制動種別のみ保持する。 + self._sudden_warning_type = "" + self.process_station_list_from_fms() self._node.create_timer(0.2, self.route_checker_callback) @@ -65,8 +81,14 @@ def __init__( self._node.create_timer(0.2, self.calculate_time_callback) self._node.create_timer(0.2, self.door_status_callback) self._node.create_timer(0.2, self.announce_engage_when_starting) + self._node.create_timer(0.2, self.stop_reason_checker_callback) + self._node.create_timer(0.2, self.sudden_motion_checker_callback) def emergency_checker_callback(self): + # MRM の状態更新と緊急アナウンスを、責務ごとにヘルパーへ分割して実行する。 + # - _update_comfortable_stop_state: comfortable stop の減速中/停止後判定 + # - _update_emergency_state: 緊急停止中かどうかの判定 (_in_emergency_state) + # - _announce_emergency: 緊急アナウンスの発話 if ( self._parameter.ignore_emergency or self._autoware.information.operation_mode == OperationModeState.STOP @@ -78,18 +100,25 @@ def emergency_checker_callback(self): return current_time = self._node.get_clock().now() - in_emergency = self._autoware.information.mrm_behavior == 12 - in_comfortable_stop = self._autoware.information.mrm_behavior not in [ - 1, - 12, - ] - + mrm_behavior = self._autoware.information.mrm_behavior + in_emergency = mrm_behavior == 12 + # comfortable stop = MRM 挙動が緊急(12)でも通常(1)でもないもの + in_comfortable_stop = mrm_behavior not in [1, 12] + + self._update_comfortable_stop_state(in_comfortable_stop, current_time) + # 発話は初回/繰り返し判定に更新前の _in_emergency_state を使うため、 + # 状態更新 (_update_emergency_state) より先に呼ぶ。 + self._announce_emergency(in_emergency, current_time) + self._update_emergency_state(in_emergency, current_time) + + def _update_comfortable_stop_state(self, in_comfortable_stop, current_time): + # comfortable stop 中は motion_state から減速中(slowing)/停止後(slow_stop)を判定する。 + # comfortable stop を抜けたら freeze 期間(emergency_ignore_period)経過後に解除する。 if in_comfortable_stop: - self._in_slowing_state = self._autoware.information.motion_state == MotionState.MOVING - self._in_slow_stop_state = ( - self._autoware.information.motion_state == MotionState.STOPPED - ) - self._emergency_trigger_time = self._node.get_clock().now() + motion_state = self._autoware.information.motion_state + self._in_slowing_state = motion_state == MotionState.MOVING + self._in_slow_stop_state = motion_state == MotionState.STOPPED + self._emergency_trigger_time = current_time elif ( utils.check_timeout( current_time, self._emergency_trigger_time, self._parameter.emergency_ignore_period @@ -99,35 +128,37 @@ def emergency_checker_callback(self): self._in_slowing_state = False self._in_slow_stop_state = False + def _update_emergency_state(self, in_emergency, current_time): + # 緊急停止中かどうか(_in_emergency_state)を判定する。 + # 緊急に入ったら即 True。抜けたら freeze 期間(emergency_ignore_period)経過後に False。 + if in_emergency: + self._in_emergency_state = True + elif ( + utils.check_timeout( + current_time, self._emergency_trigger_time, self._parameter.emergency_ignore_period + ) + or not self._parameter.freeze_emergency + ): + self._in_emergency_state = False + + def _announce_emergency(self, in_emergency, current_time): + # 緊急停止中の車内アナウンスを発話する。 + # 初回は "emergency"、以降は emergency_repeat_period ごとに "in_emergency" を繰り返す。 + # 初回/繰り返しの判定には更新前の _in_emergency_state を使う (False = 緊急突入の初回tick)。 if not in_emergency: - if ( - self._in_emergency_state - and utils.check_timeout( - current_time, - self._emergency_trigger_time, - self._parameter.emergency_ignore_period, - ) - or not self._parameter.freeze_emergency - ): - # only change back to false state after the emergency is on for a specific time - self._in_emergency_state = in_emergency return - # Emergency is trigger, check whether is already trigger before audio = "" if not self._in_emergency_state: audio = "emergency" - elif self._in_emergency_state and utils.check_timeout( - current_time, - self._emergency_trigger_time, - self._parameter.emergency_repeat_period, + elif utils.check_timeout( + current_time, self._emergency_trigger_time, self._parameter.emergency_repeat_period ): audio = "in_emergency" if audio: self._announce_interface.announce_emergency(audio) - self._emergency_trigger_time = self._node.get_clock().now() - self._in_emergency_state = in_emergency + self._emergency_trigger_time = current_time def door_status_callback(self): door_status = self._autoware.information.door_status @@ -162,6 +193,8 @@ def announce_engage_when_starting(self): in [MotionState.STARTING, MotionState.MOVING] and self._prev_motion_state == 1 ): + # 再発進したので次の停止で停止案内を再度出せるようにする (UC-04) + self._stop_announce_executed = False if self._announce_engage and not self._skip_announce: self._skip_announce = True elif utils.check_timeout( @@ -194,6 +227,45 @@ def announce_engage_when_starting(self): except Exception as e: self._node.get_logger().error("not able to play the announce, ERROR: {}".format(str(e))) + def stop_reason_checker_callback(self): + # UC-04 (SYS-HMI-04/05/06): 走行中の予定外停止で車内に「停車します」と案内する。 + # 停止種別 (障害物/横断歩道/一般) は車内では区別しないため停止理由は参照しない。 + # e2e プランナでは停止理由 (velocity_factors の behavior) が出ない場合があるため、 + # 停止理由に依存せず運行状態から一時停止を判定する: + # AUTONOMOUS 走行中 (route 継続中) にゴール手前で停止 = 障害物/信号/横断歩道等の一時停止。 + # 到着停止 (AUTONOMOUS 離脱 & ゴール近傍) と発進待ちは除外される。 + try: + if not self._parameter.signage_stand_alone: + return + if not self._autoware.information.autoware_control: + return + # MRM (緊急停止) 中は停止案内より緊急案内を優先するためスキップ + if self._in_emergency_state: + return + # 発進前 (発進待ち) は disarm 状態。最初の STOPPED->MOVING (実発進) で + # announce_engage_when_starting が arm (False) する。engage 直後の + # 「AUTONOMOUS だが未発進で停止中」での誤発話を防ぐ。 + if self._stop_announce_executed: + return + + info = self._autoware.information + is_temporary_stop = ( + info.operation_mode == OperationModeState.AUTONOMOUS + and info.route_state == RouteState.SET + and info.motion_state == MotionState.STOPPED + and info.goal_distance > self._parameter.arriving_distance + ) + if is_temporary_stop: + if self._announce_interface.in_interval("stop_reason"): + return + self._announce_interface.send_announce("temporary_stop") + self._announce_interface.set_timeout("stop_reason") + self._stop_announce_executed = True + except Exception as e: + self._node.get_logger().error( + "not able to check the stop reason, ERROR: {}".format(str(e)) + ) + def process_station_list_from_fms(self, force_update=False): try: data = json.loads(self._autoware.information.active_schedule) @@ -259,13 +331,25 @@ def process_station_list_from_fms(self, force_update=False): def arrived_goal(self): try: + # UC-05: 音声は終点・通常停留所とも thank_you で共通 (announce_arrived)。 + # 通常停留所のみ乗客サイネージに到着表示 (arrived) を出す。終点は _reach_final + # 経由で「終点です」表示になるため到着表示 (set_timeout) は行わない。 + is_final = not self.task_list.todo_list + arrived_station = self._current_task_details.arrival_station self._announce_interface.announce_arrived() + # 次の接近で再度安全配慮を出せるようリセット + self._announced_arrive_caution = False if self._current_task_details == utils.init_CurrentTask(): raise Exception("No current task details") self._display_details.previous_station = self._current_task_details.departure_station + if not is_final: + # 通常停留所: 到着表示(SYS-HMI-07)の表示時間を起算し到着停留所名を保持する + self._arrived_station = arrived_station + self._announce_interface.set_timeout("arrived") + if not self.task_list.todo_list: # Reach final station self._current_task_details.departure_station = ( @@ -309,7 +393,13 @@ def route_checker_callback(self): and self._parameter.signage_stand_alone and self._autoware.information.autoware_control ): - self._announce_interface.send_announce("engage") + # UC-02: バス停発車 (STOP->AUTONOMOUS) のタイミング。 + # 立席モードON時は standing_depart (発進します+転倒防止の注意) のみ再生し、 + # engage との「発進します」二重発話を避ける。 + # 立席モードOFF、またはクールダウン等で standing_depart を再生しなかった + # 場合は通常の engage (発進します) を再生する。 + if not self.trigger_depart_warning(): + self._announce_interface.send_announce("engage") self._service_interface.trigger_external_signage(True) self._trigger_external_signage = True self._announce_engage = True @@ -369,6 +459,19 @@ def route_checker_callback(self): def calculate_time_callback(self): try: + # UC-05 (SYS-HMI-07): 接近10mの車内安全配慮は FMS スケジュールに依存しない + # (_is_driving と goal_distance だけで判定できる) ため、FMS タスク未取得でも + # 発話できるよう以降の FMS ゲートより前で評価する。 + if ( + self._is_driving + and 0 + < self._autoware.information.goal_distance + < self._parameter.arriving_distance + and not self._announced_arrive_caution + ): + self._announce_interface.send_announce("arrive_caution") + self._announced_arrive_caution = True + if self._current_task_details == utils.init_CurrentTask(): return @@ -376,9 +479,13 @@ def calculate_time_callback(self): self._current_task_details.depart_time, self._node.get_clock().now().to_msg().sec ) - if self._reach_final: - # display arrive to final station - self._display_phrase = utils.handle_phrase("final") + if self._announce_interface.in_interval("arrived"): + # UC-05: 停止後 announce_interval.arrived の間「‹停留所名›に到着しました」を表示 + self._display_phrase = utils.handle_phrase("arrived", self._arrived_station[0]) + elif self._reach_final: + # 終点では「終点です」の文言表示は行わない (ユーザー方針 2026-07-14)。 + # is_stopping ブランチに落として発車待ち文言を出さないよう、ここで空表示に固定する。 + self._display_phrase = "" elif self._is_stopping: # handle text and announce while bus is stopping if remain_minute > 2: @@ -396,8 +503,11 @@ def calculate_time_callback(self): self._announced_depart = True elif self._is_driving: # handle text and announce while bus is running + # 到着予告(表示・音声とも)は going_to_arrive 設定で一括制御する。 + # Phase1方針ではデフォルト除外のため、無効時は「もうすぐ到着」表示も出さない。 if ( - self._autoware.information.goal_distance < 100 + self._announce_interface.check_announce_or_not("going_to_arrive") + and self._autoware.information.goal_distance < 100 and self._autoware.information.goal_distance > 0 ): # display text and announce if the goal is within 100m @@ -414,6 +524,111 @@ def calculate_time_callback(self): except Exception as e: self._node.get_logger().error("Error in getting calculate the time: " + str(e)) + def trigger_depart_warning(self): + # standing_depart を再生したら True を返す (発進アナウンスを兼ねるため呼び出し側で利用) + # 立席運用モードOFF時は提供しない (SYS2-UC02-02) + if not self._standing_mode.is_standing_mode: + return False + # SYS2-UC02-05: インターバル(5s)中の再発車はスキップ (キューに積まない) + # スキップ分はログに残さず、実際に発話するときのみ記録する (UC-03 と方針統一) + if self._announce_interface.in_interval("standing_depart"): + return False + self._announce_interface.set_timeout("standing_depart") + # 提供結果 (played/queued/failed 等) は send_announce 側で記録される + self._announce_interface.send_announce("standing_depart") + return True + + def trigger_sudden_warning(self, warning_type, reasons): + # SYS2-UC03-06: インターバル(10s)中の同種イベントはスキップ (キューに積まない) + # スキップ分はログに残さず、実際に発話するときのみ記録する + if self._announce_interface.in_interval("standing_sudden"): + return + self._announce_interface.set_timeout("standing_sudden") + self._sudden_warning_type = warning_type + # 閾値チューニング用: どの値がどの閾値を超えて発話したかを記録する + # (提供結果 played/queued/failed 等は send_announce 側で別途記録される) + self._node.get_logger().info( + "UC-03: sudden motion triggered [{}: {}]".format( + warning_type, ", ".join(reasons) + ) + ) + self._announce_interface.send_announce("standing_" + warning_type) + + def sudden_motion_checker_callback(self): + # UC-03: 急減速・急操舵の事後検知。立席運用モードON・走行中・MRM非作動のときのみ評価する。 + try: + if not self._standing_mode.is_standing_mode: + return + if not self._is_driving: + return + # MRMが最優先。MRM(緊急停止/快適減速)作動中は本警告を出さず、 + # 直前にトリガー済みで再生中の立席アナウンスがあれば停止する (SYS2-UC03-01) + if self._in_emergency_state or self._in_slowing_state or self._in_slow_stop_state: + self._announce_interface.stop_standing_announce() + return + + info = self._autoware.information + param = self._parameter + + # 閾値を超えた項目を記録する (どの値が閾値以上だったかをログに残す) + decel_reasons = [] + if info.longitudinal_acceleration <= -param.sudden_decel_threshold: + decel_reasons.append( + "accel={:.3f} <= -{:.3f}".format( + info.longitudinal_acceleration, param.sudden_decel_threshold + ) + ) + # jerk は減速方向 (負値) のみを対象とする。加速方向 (正のjerk) は + # 発進・加速時の挙動で転倒リスクが低いため対象外とし、jerk <= -閾値 で判定する。 + if info.longitudinal_jerk <= -param.sudden_decel_jerk_threshold: + decel_reasons.append( + "jerk={:.3f} <= -{:.3f}".format( + info.longitudinal_jerk, param.sudden_decel_jerk_threshold + ) + ) + + # 急操舵は横加速度・横ジャークの「いずれか」超過で判定する (SYS2-UC03-01)。 + turn_reasons = [] + # 横加速度: metric をそのまま使用 (検討案 最大横加速度 >= 0.8) + if info.lateral_acceleration_abs >= param.sudden_lateral_accel_threshold: + turn_reasons.append( + "lateral_accel_abs={:.3f} >= {:.3f}".format( + info.lateral_acceleration_abs, param.sudden_lateral_accel_threshold + ) + ) + # 横ジャーク: metric 未配信のため計算で近似する。横加速 a_y ≒ v^2*δ/L の + # 時間微分 (速度一定近似) より 横ジャーク ≒ v^2 * steering_rate / wheel_base。 + # 左右どちらの向きでも転倒リスクとなるため絶対値で閾値判定する (検討案 最大横ジャーク >= 0.5)。 + if info.wheel_base > 0.0: + lateral_jerk = info.velocity**2 * info.steering_rate / info.wheel_base + if abs(lateral_jerk) >= param.sudden_lateral_jerk_threshold: + turn_reasons.append( + "lateral_jerk={:.3f} (v={:.2f}, steering_rate={:.3f}, L={:.2f}) >= {:.3f}".format( + lateral_jerk, + info.velocity, + info.steering_rate, + info.wheel_base, + param.sudden_lateral_jerk_threshold, + ) + ) + + is_sudden_decel = bool(decel_reasons) + is_sudden_turn = bool(turn_reasons) + + if not is_sudden_decel and not is_sudden_turn: + return + + if is_sudden_decel and is_sudden_turn: + warning_type = "sudden_stop_turn" + elif is_sudden_decel: + warning_type = "sudden_stop" + else: + warning_type = "sudden_turn" + + self.trigger_sudden_warning(warning_type, decel_reasons + turn_reasons) + except Exception as e: + self._node.get_logger().error("Error in sudden motion checker: " + str(e)) + def view_mode_callback(self): try: self._viewController.clock_string = datetime.now().strftime("%H:%M") @@ -426,23 +641,44 @@ def view_mode_callback(self): self._viewController.next_station_list = self._display_details.next_station_list self._viewController.display_phrase = self._display_phrase + cvm_override = self._cvm.get_view_mode_override() + if ( self._autoware.is_disconnected and not self._parameter.ignore_disconnected and not self._parameter.ignore_emergency ): view_mode = "disconnected" + elif cvm_override is not None: + view_mode = cvm_override elif ( not self._autoware.information.autoware_control and not self._parameter.ignore_manual_driving ): view_mode = "manual_driving" elif self._in_emergency_state: - view_mode = "emergency_stopped" + # comfortable stop (slowing/slow_stop) と揃えて、緊急停止も減速中と停止後で + # 表示を固定する。減速中 (emergency_slowing) は EmergencyStop、 + # 停止後 (emergency_stopped) は EmergencyStopping を表示する。 + if self._autoware.information.motion_state == MotionState.STOPPED: + view_mode = "emergency_stopped" + else: + view_mode = "emergency_slowing" elif self._in_slowing_state: view_mode = "slowing" elif self._in_slow_stop_state: view_mode = "slow_stop" + elif self._announce_interface.in_interval("standing_sudden"): + # UC-03: 急減速・急操舵警告 (MRMの直下・発車警告より上位) + # 表示時間＝再発話抑止インターバル (announce_interval.standing_sudden) で連動 + view_mode = "standing_sudden_warning" + elif self._announce_interface.in_interval("standing_depart"): + # UC-02: 発車時警告 (表示時間＝announce_interval.standing_depart) + view_mode = "standing_depart_warning" + elif self._announce_interface.in_interval("arrived"): + # UC-05 (SYS-HMI-07): 到着直後 announce_interval.arrived 秒間は + # 「‹停留所名›に到着しました」を専用画面で表示する (stopping より優先) + view_mode = "arrived" elif self._is_stopping and self._current_task_details.departure_station != ["", ""]: view_mode = "stopping" elif self._is_driving and self._current_task_details.arrival_station != ["", ""]: @@ -455,6 +691,17 @@ def view_mode_callback(self): view_mode = "out_of_service" self._announced_depart = False + if view_mode == "standing_sudden_warning": + self._viewController.standing_warning_type = self._sudden_warning_type + elif view_mode == "standing_depart_warning": + self._viewController.standing_warning_type = "depart" + else: + self._viewController.standing_warning_type = "" + + self._cvm.set_current_view_mode(view_mode) + self._viewController.cvm_display_mode_id = ( + self._cvm.get_display_mode_id_override() or "" + ) self._viewController.view_mode = view_mode except Exception as e: self._node.get_logger().error("Error in updating the view mode: " + str(e)) diff --git a/src/signage/src/signage/settings_store.py b/src/signage/src/signage/settings_store.py new file mode 100644 index 0000000..d9f33a7 --- /dev/null +++ b/src/signage/src/signage/settings_store.py @@ -0,0 +1,138 @@ +# !/usr/bin/env python3 +# -*- coding: utf-8 -*- +# This Python file uses the following encoding: utf-8 + +import json +import os + +# signage の永続設定を集約する単一 JSON ファイル +SETTINGS_PATH = "/opt/autoware/signage_settings.json" + +# 旧: 設定項目ごとの個別ファイル。JSON へ統合したため移行にのみ使用する。 +LEGACY_VOLUME_PATH = "/opt/autoware/volume.txt" +LEGACY_STANDING_MODE_PATH = "/opt/autoware/standing_mode.txt" + +# 設定キー +KEY_VOLUME = "volume" +KEY_STANDING_MODE = "standing_mode" + + +class SettingsStore: + """signage の永続設定を単一 JSON (/opt/autoware/signage_settings.json) で保持する。 + + 従来は設定項目ごとに個別ファイル (volume.txt / standing_mode.txt) へ保存していたが、 + ファイルが増えて管理しづらくなるため 1 ファイルへ統合した。初回起動時に旧ファイルが + 残っていれば読み込んで JSON へ移行し、移行できた旧ファイルは削除する。 + + 複数のインターフェース (音量 / 立席モード) が同じファイルを読み書きするため、 + ファイルアクセスは本クラスに集約し、書き込み競合を避ける。 + """ + + def __init__(self, node): + self._node = node + self._data = self._load() + self._migrate_legacy_files() + + def _load(self): + try: + if os.path.isfile(SETTINGS_PATH): + with open(SETTINGS_PATH, "r") as f: + data = json.load(f) + if isinstance(data, dict): + return data + except Exception as e: + self._node.get_logger().error( + "not able to load settings, ERROR: {}".format(str(e)) + ) + return {} + + def _save(self): + os.makedirs(os.path.dirname(SETTINGS_PATH), exist_ok=True) + with open(SETTINGS_PATH, "w") as f: + json.dump(self._data, f, indent=2) + + def get(self, key, default=None): + return self._data.get(key, default) + + def set(self, key, value): + # 永続化に成功した場合のみメモリ上の値を確定させる。失敗時は旧値へロールバックし、 + # 例外を呼び出し元へ伝播する (呼び出し側で失敗を通知できるようにするため)。 + sentinel = object() + previous = self._data.get(key, sentinel) + self._data[key] = value + try: + self._save() + except Exception: + if previous is sentinel: + self._data.pop(key, None) + else: + self._data[key] = previous + raise + + def _migrate_legacy_files(self): + migrated = False + if KEY_VOLUME not in self._data: + value = self._read_legacy_volume() + if value is not None: + self._data[KEY_VOLUME] = value + migrated = True + if KEY_STANDING_MODE not in self._data: + value = self._read_legacy_standing_mode() + if value is not None: + self._data[KEY_STANDING_MODE] = value + migrated = True + + if migrated: + try: + self._save() + except Exception as e: + # 移行値の保存に失敗した場合は旧ファイルを残し、次回起動で再試行する。 + self._node.get_logger().error( + "not able to migrate legacy settings, ERROR: {}".format(str(e)) + ) + return + + # 移行済み (= JSON 側に値がある) の旧ファイルは削除する。 + self._remove_legacy_files() + + def _read_legacy_volume(self): + try: + if os.path.isfile(LEGACY_VOLUME_PATH): + with open(LEGACY_VOLUME_PATH, "r") as f: + value = f.readline().strip() + if value != "": + return float(value) + except Exception as e: + self._node.get_logger().error( + "not able to read legacy volume, ERROR: {}".format(str(e)) + ) + return None + + def _read_legacy_standing_mode(self): + try: + if os.path.isfile(LEGACY_STANDING_MODE_PATH): + with open(LEGACY_STANDING_MODE_PATH, "r") as f: + value = f.readline().strip().lower() + if value in ["true", "false"]: + return value == "true" + except Exception as e: + self._node.get_logger().error( + "not able to read legacy standing mode, ERROR: {}".format(str(e)) + ) + return None + + def _remove_legacy_files(self): + for path, key in ( + (LEGACY_VOLUME_PATH, KEY_VOLUME), + (LEGACY_STANDING_MODE_PATH, KEY_STANDING_MODE), + ): + # 値が JSON へ取り込めている旧ファイルのみ削除する。 + if key not in self._data: + continue + try: + if os.path.isfile(path): + os.remove(path) + except Exception as e: + self._node.get_logger().warning( + "not able to remove legacy file {}, ERROR: {}".format(path, str(e)) + ) diff --git a/src/signage/src/signage/signage.py b/src/signage/src/signage/signage.py index d56e8ed..7a7eae4 100644 --- a/src/signage/src/signage/signage.py +++ b/src/signage/src/signage/signage.py @@ -11,8 +11,11 @@ from signage.view_controller import ViewControllerProperty from signage.announce_controller import AnnounceControllerProperty from signage.autoware_interface import AutowareInterface +from signage.cvm_interface import CvmInterface from signage.parameter_interface import ParameterInterface from signage.route_handler import RouteHandler +from signage.settings_store import SettingsStore +from signage.standing_mode_interface import StandingModeInterface from signage.ros_service_interface import RosServiceInterface from ament_index_python.packages import get_package_share_directory @@ -28,10 +31,17 @@ def main(args=None): heartbeat = Heartbeat(node) parameter_interface = ParameterInterface(node) + settings_store = SettingsStore(node) autoware_interface = AutowareInterface(node, parameter_interface) ros_service_interface = RosServiceInterface(node, parameter_interface) viewController = ViewControllerProperty(node, parameter_interface) - announceController = AnnounceControllerProperty(node, autoware_interface, parameter_interface) + announceController = AnnounceControllerProperty( + node, autoware_interface, parameter_interface, settings_store + ) + cvm_interface = CvmInterface(node, parameter_interface.parameter.cvm_device_id) + standing_mode_interface = StandingModeInterface( + node, autoware_interface, parameter_interface, settings_store + ) route_handler = RouteHandler( node, viewController, @@ -39,6 +49,8 @@ def main(args=None): autoware_interface, parameter_interface, ros_service_interface, + cvm_interface, + standing_mode_interface, ) ctx = engine.rootContext() diff --git a/src/signage/src/signage/signage_utils.py b/src/signage/src/signage/signage_utils.py index 2b95615..2db6cbb 100644 --- a/src/signage/src/signage/signage_utils.py +++ b/src/signage/src/signage/signage_utils.py @@ -168,12 +168,12 @@ def get_remain_minute(depart_time, current_time): return (depart_time - current_time) / 60 -def handle_phrase(phrase_type, remain_minute=0): +def handle_phrase(phrase_type, value=0): return { - "final": "終点です。\nご乗車ありがとうございました", - "remain_minute": "このバスはあと{}分程で出発します".format(str(remain_minute)), + "remain_minute": "このバスはあと{}分程で出発します".format(str(value)), "departing": "間もなく発車時刻です", "arriving": "間もなく到着します", + "arrived": "{}に到着しました".format(value), }.get(phrase_type, "") diff --git a/src/signage/src/signage/standing_mode_interface.py b/src/signage/src/signage/standing_mode_interface.py new file mode 100644 index 0000000..1456440 --- /dev/null +++ b/src/signage/src/signage/standing_mode_interface.py @@ -0,0 +1,79 @@ +# !/usr/bin/env python3 +# -*- coding: utf-8 -*- +# This Python file uses the following encoding: utf-8 + +from std_msgs.msg import Bool +from std_srvs.srv import SetBool +from autoware_adapi_v1_msgs.msg import OperationModeState + +from signage.settings_store import KEY_STANDING_MODE + + +class StandingModeInterface: + """立席運用モード(着席/立席)の保持・永続化・切替を担う (UC-01)。 + + 立席運用モードは UC-02/UC-03 の転倒防止アナウンスのゲート条件となる。 + 切替は ~/set/standing_mode (SetBool) で行い、停車中(operation_mode == STOP) + のときのみ受け付ける。設定値は SettingsStore (統合 JSON) に保存し、再起動後も保持する。 + """ + + def __init__(self, node, autoware_interface, parameter_interface, settings_store): + self._node = node + self._autoware = autoware_interface + self._parameter = parameter_interface.parameter + self._settings = settings_store + + self._standing_mode = self._load_standing_mode() + + self._set_srv = self._node.create_service( + SetBool, "~/set/standing_mode", self.set_standing_mode + ) + self._get_pub = self._node.create_publisher(Bool, "~/get/standing_mode", 1) + self._node.create_timer(1.0, self.publish_standing_mode_callback) + + @property + def is_standing_mode(self): + return self._standing_mode + + def _load_standing_mode(self): + value = self._settings.get(KEY_STANDING_MODE) + if isinstance(value, bool): + return value + # 設定が無い場合は既定値(パラメータ)を使用する + return self._parameter.standing_mode_default + + def _save_standing_mode(self, standing_mode): + self._settings.set(KEY_STANDING_MODE, standing_mode) + + def set_standing_mode(self, request, response): + # SYS2-UC01-01: 停車中(operation_mode == STOP)のときのみ受付 + if self._autoware.information.operation_mode != OperationModeState.STOP: + response.success = False + response.message = "Standing mode can only be changed while the vehicle is stopped" + self._node.get_logger().warning(response.message) + return response + + # 切替不要(同値)の場合も成功として扱う + if request.data == self._standing_mode: + response.success = True + response.message = "Standing mode is already {}".format(request.data) + return response + + # SYS2-UC01-02/03: 不揮発化に成功した場合のみモードを更新する。 + # 書込失敗時は切替前のモードを保持し、失敗を通知する。 + try: + self._save_standing_mode(request.data) + except Exception as e: + response.success = False + response.message = "Failed to persist standing mode: {}".format(str(e)) + self._node.get_logger().error(response.message) + return response + + self._standing_mode = request.data + response.success = True + response.message = "Standing mode changed to {}".format(request.data) + self._node.get_logger().info(response.message) + return response + + def publish_standing_mode_callback(self): + self._get_pub.publish(Bool(data=self._standing_mode)) diff --git a/src/signage/src/signage/view_controller.py b/src/signage/src/signage/view_controller.py index c94690d..027ecfa 100644 --- a/src/signage/src/signage/view_controller.py +++ b/src/signage/src/signage/view_controller.py @@ -10,6 +10,8 @@ class ViewControllerProperty(QObject): _view_mode_changed_signal = pyqtSignal(str) + _cvm_display_mode_id_changed_signal = pyqtSignal(str) + _standing_warning_type_changed_signal = pyqtSignal(str) _route_name_signal = pyqtSignal(list) _get_departure_station_name_signal = pyqtSignal(list) _get_arrival_station_name_signal = pyqtSignal(list) @@ -25,6 +27,8 @@ def __init__(self, node, parameter_interface): super(ViewControllerProperty, self).__init__() self._node = node self._view_mode = "" + self._cvm_display_mode_id = "" + self._standing_warning_type = "" self._route_name = ["", ""] self._departure_station_name = ["", ""] self._arrival_station_name = ["", ""] @@ -50,6 +54,29 @@ def view_mode(self, view_mode): self._view_mode = view_mode self._view_mode_changed_signal.emit(view_mode) + @pyqtProperty(str, notify=_cvm_display_mode_id_changed_signal) + def cvm_display_mode_id(self): + return self._cvm_display_mode_id + + @cvm_display_mode_id.setter + def cvm_display_mode_id(self, cvm_display_mode_id): + if self._cvm_display_mode_id == cvm_display_mode_id: + return + self._cvm_display_mode_id = cvm_display_mode_id + self._cvm_display_mode_id_changed_signal.emit(cvm_display_mode_id) + + # 立席警告の種別 (depart / sudden_stop / sudden_turn / sudden_stop_turn) をQMLへ反映 + @pyqtProperty(str, notify=_standing_warning_type_changed_signal) + def standing_warning_type(self): + return self._standing_warning_type + + @standing_warning_type.setter + def standing_warning_type(self, standing_warning_type): + if self._standing_warning_type == standing_warning_type: + return + self._standing_warning_type = standing_warning_type + self._standing_warning_type_changed_signal.emit(standing_warning_type) + # QMLへroute_nameを反映させる @pyqtProperty(list, notify=_route_name_signal) def route_name(self): diff --git a/src/signage_fms_client/setup.py b/src/signage_fms_client/setup.py index f57c842..6caa499 100644 --- a/src/signage_fms_client/setup.py +++ b/src/signage_fms_client/setup.py @@ -7,7 +7,7 @@ def package_files(directory): paths = [] - for (path, directories, filenames) in os.walk(directory): + for path, directories, filenames in os.walk(directory): for filename in filenames: paths.append(os.path.join(path, filename)) return paths diff --git a/src/signage_fms_client/src/signage_fms_client/signage_fms_client.py b/src/signage_fms_client/src/signage_fms_client/signage_fms_client.py index 21aa40c..9d263b4 100644 --- a/src/signage_fms_client/src/signage_fms_client/signage_fms_client.py +++ b/src/signage_fms_client/src/signage_fms_client/signage_fms_client.py @@ -18,11 +18,12 @@ def __init__(self, node): self._fms_payload = { "method": "get", "url": "https://" - + os.getenv("FMS_URL", "fms.web.auto") + + os.getenv("FMS_URL", "") + "/v1/projects/{project_id}/environments/{environment_id}/vehicles/{vehicle_id}/active_schedule", "body": {}, } self.AUTOWARE_IP = os.getenv("AUTOWARE_IP", "localhost") + self.AUTOWARE_PORT = os.getenv("AUTOWARE_PORT", "") self.schedule_pub_ = node.create_publisher(String, "/signage/active_schedule", 10) self.timer = node.create_timer(self._post_request_time + 0.5, self.pub_schedule) @@ -30,7 +31,7 @@ def pub_schedule(self): try: msg = String() respond = requests.post( - "http://{}:4711/v1/services/order".format(self.AUTOWARE_IP), + "http://{}:{}/v1/services/order".format(self.AUTOWARE_IP, self.AUTOWARE_PORT), json=self._fms_payload, timeout=self._post_request_time, ) diff --git a/tests/conftest.py b/tests/conftest.py new file mode 100644 index 0000000..79e3e98 --- /dev/null +++ b/tests/conftest.py @@ -0,0 +1,16 @@ +import sys +from pathlib import Path +from unittest.mock import MagicMock + +# Mock rclpy module before any signage module imports it +rclpy_mock = MagicMock() +sys.modules["rclpy"] = rclpy_mock +sys.modules["rclpy.duration"] = rclpy_mock.duration +sys.modules["rclpy.node"] = rclpy_mock.node +sys.modules["rclpy.qos"] = rclpy_mock.qos + +# Add source directories to sys.path for module resolution +# (pythonpath in pytest.ini requires pytest >= 7.0) +_base = Path(__file__).resolve().parent.parent +sys.path.insert(0, str(_base / "src" / "signage" / "src")) +sys.path.insert(0, str(_base / "src" / "external_signage" / "src")) diff --git a/tests/external_signage/test_ext_packet_tools.py b/tests/external_signage/test_ext_packet_tools.py new file mode 100644 index 0000000..e317f42 --- /dev/null +++ b/tests/external_signage/test_ext_packet_tools.py @@ -0,0 +1,105 @@ +# tests/external_signage/test_packet_tools.py +import datetime +import pytest +from external_signage.packet_tools import ( + SOT, + EOT, + calc_sum, + verify_sum, + gen_data_packet, + gen_data_packets, + gen_name_time_packet, + lists_match, + Parser, +) + + +class TestCalcSum: + def test_single_byte(self): + assert calc_sum([0x10]) == [0x10, 0x00] + + def test_multiple_bytes(self): + assert calc_sum([0x01, 0x02, 0x03]) == [0x06, 0x00] + + def test_overflow(self): + assert calc_sum([0xFF, 0x01]) == [0x00, 0x01] + + +class TestVerifySum: + def test_valid(self): + payload = [0x01, 0x02, 0x03] + checksum = calc_sum(payload) + packet = [SOT] + payload + checksum + [EOT] + assert verify_sum(packet) is True + + def test_invalid(self): + packet = [SOT, 0x01, 0x02, 0x03, 0xFF, 0xFF, EOT] + assert verify_sum(packet) is False + + +class TestGenDataPacket: + def test_structure(self): + result = gen_data_packet([0x01], seq=0, addr1=0x70, addr2=0x8F) + assert result[0] == SOT + assert result[-1] == EOT + assert verify_sum(result) is True + + def test_length_field(self): + data = [0x01, 0x02, 0x03] + result = gen_data_packet(data, seq=0, addr1=0x70, addr2=0x8F) + assert result[3] == len(data) + 8 + + +class TestGenDataPackets: + def test_single_packet(self): + assert len(gen_data_packets(list(range(10)), 0x70, 0x8F)) == 1 + + def test_multiple_packets(self): + assert len(gen_data_packets(list(range(256)), 0x70, 0x8F)) == 2 + + def test_all_valid_checksums(self): + for p in gen_data_packets(list(range(300)), 0x70, 0x8F): + assert verify_sum(p) is True + + +class TestGenNameTimePacket: + def test_valid(self): + result = gen_name_time_packet( + b"1234567890123456", datetime.datetime(2026, 1, 15, 10, 30), False + ) + assert result[0] == SOT + assert result[-1] == EOT + assert verify_sum(result) is True + + def test_invalid_length(self): + with pytest.raises(ValueError): + gen_name_time_packet(b"short", datetime.datetime.now(), False) + + +class TestListsMatch: + def test_equal(self): + assert lists_match([1, 2], [1, 2]) is True + + def test_not_equal(self): + assert lists_match([1, 2], [1, 3]) is False + + def test_different_length(self): + assert lists_match([1], [1, 2]) is False + + def test_empty(self): + assert lists_match([], []) is True + + +class TestParser: + def test_complete_packet(self): + payload = [0x70, 0x8F, 0x07, 0x12, 0x02] + checksum = calc_sum(payload) + packet = [SOT] + payload + checksum + [EOT] + parser = Parser(bus=None) + results = [parser.parse(b) for b in packet] + assert 1 in results + + def test_non_sot_ignored(self): + parser = Parser(bus=None) + assert parser.parse(0x00) == 0 + assert parser.state == 0 diff --git a/tests/signage/test_signage_utils.py b/tests/signage/test_signage_utils.py new file mode 100644 index 0000000..434386e --- /dev/null +++ b/tests/signage/test_signage_utils.py @@ -0,0 +1,253 @@ +# tests/signage/test_signage_utils.py +import pytest +from signage.signage_utils import ( + DEFAULT_ROUTE_NAME, + DEFAULT_DEPARTURE_NAME, + DEFAULT_ARRIVAL_NAME, + NEXT_STATION_DISPLAY_AMOUNT, + TaskList, + CurrentTask, + ScheduleDetails, + DisplayDetails, + init_TaskList, + init_CurrentTask, + init_ScheduleDetails, + init_DisplayDetails, + check_schedule_update, + update_schedule_details, + process_tag, + split_name, + get_route_name, + separate_task_list, + process_current_task, + get_previous_station_name_from_fms, + repeat_task_for_loop, + auto_add_empty_list, + create_next_station_list, + get_remain_minute, + handle_phrase, +) + + +class TestInitFunctions: + def test_init_task_list(self): + result = init_TaskList() + assert result == TaskList([], [], []) + + def test_init_current_task(self): + result = init_CurrentTask() + assert result == CurrentTask(["", ""], ["", ""], 0) + + def test_init_schedule_details(self): + result = init_ScheduleDetails() + assert result == ScheduleDetails("", "", "") + + def test_init_display_details(self): + result = init_DisplayDetails() + assert result.route_name == DEFAULT_ROUTE_NAME + assert result.previous_station == ["", ""] + + +class TestCheckScheduleUpdate: + def test_same_schedule(self): + details = ScheduleDetails("2026-01-01", "schedule-1", "loop") + data = {"updated_at": "2026-01-01", "schedule_id": "schedule-1"} + assert check_schedule_update(details, data) is True + + def test_different_time(self): + details = ScheduleDetails("2026-01-01", "schedule-1", "loop") + data = {"updated_at": "2026-01-02", "schedule_id": "schedule-1"} + assert check_schedule_update(details, data) is False + + def test_different_id(self): + details = ScheduleDetails("2026-01-01", "schedule-1", "loop") + data = {"updated_at": "2026-01-01", "schedule_id": "schedule-2"} + assert check_schedule_update(details, data) is False + + +class TestUpdateScheduleDetails: + def test_basic(self): + data = { + "updated_at": "2026-01-01", + "schedule_id": "schedule-1", + "schedule_type": "loop", + } + result = update_schedule_details(data) + assert result == ScheduleDetails("2026-01-01", "schedule-1", "loop") + + +class TestProcessTag: + def test_found(self): + tags = [{"key": "route_name", "value": "Line A"}] + assert process_tag(tags, "route_name") == "Line A" + + def test_not_found(self): + tags = [{"key": "other", "value": "value"}] + assert process_tag(tags, "route_name") == "" + + def test_empty_list(self): + assert process_tag([], "route_name") == "" + + +class TestSplitName: + def test_with_semicolon(self): + assert split_name("Tokyo;東京") == ["Tokyo", "東京"] + + def test_without_semicolon(self): + assert split_name("Tokyo") == ["Tokyo", ""] + + def test_multiple_semicolons(self): + result = split_name("A;B;C") + assert len(result) == 3 + + +class TestGetRouteName: + def test_with_route_name(self): + tags = [{"key": "route_name", "value": "Route A;ルートA"}] + assert get_route_name(tags) == ["Route A", "ルートA"] + + def test_without_route_name(self): + tags = [{"key": "other", "value": "value"}] + assert get_route_name(tags) == DEFAULT_ROUTE_NAME + + def test_empty_route_name(self): + tags = [{"key": "route_name", "value": ""}] + assert get_route_name(tags) == DEFAULT_ROUTE_NAME + + +class TestSeparateTaskList: + def test_mixed_tasks(self): + tasks = [ + {"task_type": "move", "status": "doing"}, + {"task_type": "move", "status": "todo"}, + {"task_type": "move", "status": "done"}, + {"task_type": "charge", "status": "doing"}, + ] + result = separate_task_list(tasks) + assert len(result.doing_list) == 1 + assert len(result.todo_list) == 1 + assert len(result.done_list) == 1 + + def test_empty_list(self): + result = separate_task_list([]) + assert result == TaskList([], [], []) + + def test_only_move_tasks(self): + tasks = [ + {"task_type": "move", "status": "doing"}, + {"task_type": "move", "status": "doing"}, + ] + result = separate_task_list(tasks) + assert len(result.doing_list) == 2 + + +class TestProcessCurrentTask: + def test_with_origin_and_destination(self): + task = { + "origin": {"name": "Station A;駅A"}, + "destination": {"name": "Station B;駅B"}, + "plan_start_time": "2026-01-15T10:00:00Z", + } + result = process_current_task(task) + assert result.departure_station == ["Station A", "駅A"] + assert result.arrival_station == ["Station B", "駅B"] + assert result.depart_time > 0 + + def test_without_origin(self): + task = { + "origin": {"name": ""}, + "destination": {"name": "Station B;駅B"}, + "plan_start_time": "2026-01-15T10:00:00Z", + } + result = process_current_task(task) + assert result.departure_station == split_name(DEFAULT_DEPARTURE_NAME) + + def test_invalid_time(self): + task = { + "origin": {"name": "A;B"}, + "destination": {"name": "C;D"}, + "plan_start_time": "invalid", + } + result = process_current_task(task) + assert result.depart_time == 0 + + +class TestGetPreviousStationNameFromFms: + def test_basic(self): + done_list = [ + {"origin": {"name": "First;最初"}}, + {"origin": {"name": "Second;2番目"}}, + ] + result = get_previous_station_name_from_fms(done_list) + assert result == ["Second", "2番目"] + + +class TestRepeatTaskForLoop: + def test_short_list(self): + stations = [["A", "a"], ["B", "b"]] + result = repeat_task_for_loop(stations) + assert len(result) == NEXT_STATION_DISPLAY_AMOUNT + + def test_already_full(self): + stations = [["S", "s"]] * NEXT_STATION_DISPLAY_AMOUNT + result = repeat_task_for_loop(stations) + assert len(result) == NEXT_STATION_DISPLAY_AMOUNT + + +class TestAutoAddEmptyList: + def test_pads_to_display_amount(self): + stations = [["A", "a"]] + auto_add_empty_list(stations) + assert len(stations) == NEXT_STATION_DISPLAY_AMOUNT + assert stations[-1] == ["", ""] + + def test_already_full(self): + stations = [["S", "s"]] * NEXT_STATION_DISPLAY_AMOUNT + auto_add_empty_list(stations) + assert len(stations) == NEXT_STATION_DISPLAY_AMOUNT + + +class TestCreateNextStationList: + def test_basic(self): + current = CurrentTask(["Dep", "出発"], ["Arr", "到着"], 0) + todo = [{"destination": {"name": "Next;次"}}] + result = create_next_station_list(current, todo, "fms") + assert result[0] == ["Arr", "到着"] + assert len(result) == NEXT_STATION_DISPLAY_AMOUNT - 1 + + def test_empty_todo(self): + current = CurrentTask(["Dep", "出発"], ["Arr", "到着"], 0) + result = create_next_station_list(current, [], "fms") + assert result[0] == ["Arr", "到着"] + + +class TestGetRemainMinute: + def test_positive(self): + assert get_remain_minute(660, 60) == 10.0 + + def test_zero(self): + assert get_remain_minute(100, 100) == 0.0 + + def test_negative(self): + assert get_remain_minute(0, 60) == -1.0 + + +class TestHandlePhrase: + def test_final(self): + result = handle_phrase("final") + assert "終点" in result + + def test_remain_minute(self): + result = handle_phrase("remain_minute", 5) + assert "5" in result + + def test_departing(self): + result = handle_phrase("departing") + assert "発車" in result + + def test_arriving(self): + result = handle_phrase("arriving") + assert "到着" in result + + def test_unknown(self): + assert handle_phrase("unknown") == ""