diff --git a/src/signage/config/signage_param.yaml b/src/signage/config/signage_param.yaml
index e5d57df..41661b6 100644
--- a/src/signage/config/signage_param.yaml
+++ b/src/signage/config/signage_param.yaml
@@ -10,3 +10,13 @@ signage:
accept_start: 5.0 # second
monitor_width: 1920
monitor_height: 540
+ announce:
+ emergency: true
+ restart_engage: true
+ door_close: true
+ door_open: true
+ engage: true
+ thank_you: true
+ in_emergency: true
+ going_to_depart: true
+ going_to_arrive: true
\ No newline at end of file
diff --git a/src/signage/package.xml b/src/signage/package.xml
index b60e554..d31d720 100644
--- a/src/signage/package.xml
+++ b/src/signage/package.xml
@@ -11,6 +11,8 @@
signage_version
autoware_auto_system_msgs
+ diagnostic_updater
+ python-pulsectl-pip
rclpy
tier4_api_msgs
tier4_debug_msgs
diff --git a/src/signage/resource/sound/arrived.wav b/src/signage/resource/sound/arrived.wav
deleted file mode 100644
index 0fb821a..0000000
Binary files a/src/signage/resource/sound/arrived.wav and /dev/null differ
diff --git a/src/signage/resource/sound/restart_engage.wav b/src/signage/resource/sound/restart_engage.wav
new file mode 100644
index 0000000..96a52ff
Binary files /dev/null and b/src/signage/resource/sound/restart_engage.wav differ
diff --git a/src/signage/resource/sound/wait_for_oncoming_car.wav b/src/signage/resource/sound/wait_for_oncoming_car.wav
deleted file mode 100644
index 200089c..0000000
Binary files a/src/signage/resource/sound/wait_for_oncoming_car.wav and /dev/null differ
diff --git a/src/signage/resource/sound/wait_for_walker.wav b/src/signage/resource/sound/wait_for_walker.wav
deleted file mode 100644
index 1d9ea17..0000000
Binary files a/src/signage/resource/sound/wait_for_walker.wav and /dev/null differ
diff --git a/src/signage/resource/td5_file/automatic_128x16.td5 b/src/signage/resource/td5_file/automatic_128x16.td5
new file mode 100644
index 0000000..0297cef
Binary files /dev/null and b/src/signage/resource/td5_file/automatic_128x16.td5 differ
diff --git a/src/signage/resource/td5_file/automatic_80x24.td5 b/src/signage/resource/td5_file/automatic_80x24.td5
new file mode 100644
index 0000000..4d5077f
Binary files /dev/null and b/src/signage/resource/td5_file/automatic_80x24.td5 differ
diff --git a/src/signage/resource/td5_file/automonus.td5 b/src/signage/resource/td5_file/automonus.td5
new file mode 100644
index 0000000..1926e4a
Binary files /dev/null and b/src/signage/resource/td5_file/automonus.td5 differ
diff --git a/src/signage/resource/td5_file/null_128x16.td5 b/src/signage/resource/td5_file/null_128x16.td5
new file mode 100644
index 0000000..1cf7414
Binary files /dev/null and b/src/signage/resource/td5_file/null_128x16.td5 differ
diff --git a/src/signage/resource/td5_file/null_80x24.td5 b/src/signage/resource/td5_file/null_80x24.td5
new file mode 100644
index 0000000..6e53de0
Binary files /dev/null and b/src/signage/resource/td5_file/null_80x24.td5 differ
diff --git a/src/signage/setup.py b/src/signage/setup.py
index 2a89093..084fcdb 100644
--- a/src/signage/setup.py
+++ b/src/signage/setup.py
@@ -23,6 +23,7 @@ def package_files(directory):
("share/ament_index/resource_index/packages", ["resource/" + package_name]),
("share/" + package_name + "/resource/page", package_files("resource/page")),
("share/" + package_name + "/resource/sound", package_files("resource/sound")),
+ ("share/" + package_name + "/resource/td5_file", package_files("resource/td5_file")),
(
"share/" + package_name + "/resource/page/BusStopView",
package_files("resource/page/BusStopView"),
diff --git a/src/signage/src/signage/announce_controller.py b/src/signage/src/signage/announce_controller.py
index aeeff49..535af08 100644
--- a/src/signage/src/signage/announce_controller.py
+++ b/src/signage/src/signage/announce_controller.py
@@ -2,9 +2,16 @@
# -*- coding: utf-8 -*-
# This Python file uses the following encoding: utf-8
+import os
+
from PyQt5.QtMultimedia import QSound
from rclpy.duration import Duration
from ament_index_python.packages import get_package_share_directory
+from pulsectl import Pulse
+
+from std_msgs.msg import Float32
+from tier4_hmi_msgs.srv import SetVolume
+from tier4_external_api_msgs.msg import ResponseStatus
# The higher the value, the higher the priority
PRIORITY_DICT = {
@@ -20,6 +27,8 @@
"going_to_arrive": 1,
}
+CURRENT_VOLUME_PATH = "/opt/autoware/volume.txt"
+
class AnnounceControllerProperty:
def __init__(self, node, autoware_interface, parameter_interface):
@@ -27,6 +36,7 @@ def __init__(self, node, autoware_interface, parameter_interface):
self._node = node
self._parameter = parameter_interface.parameter
+ self._announce_settings = parameter_interface.announce_settings
self._current_announce = ""
self._pending_announce_list = []
self._sound = QSound("")
@@ -34,6 +44,18 @@ def __init__(self, node, autoware_interface, parameter_interface):
self._package_path = get_package_share_directory("signage") + "/resource/sound/"
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:
+ self._sink = self._pulse.get_sink_by_name(
+ self._pulse.server_info().default_sink_name
+ )
+ self._pulse.volume_set_all_chans(self._sink, float(f.readline()))
+
+ self._get_volume_pub = self._node.create_publisher(Float32, "~/get/volume", 1)
+ self._node.create_timer(1.0, self.publish_volume_callback)
+ self._node.create_service(SetVolume, "~/set/volume", self.set_volume)
+
def process_pending_announce(self):
try:
for play_sound in self._pending_announce_list:
@@ -62,10 +84,21 @@ def play_sound(self, message):
self._sound = QSound(self._package_path + message + ".wav")
self._sound.play()
+ # skip announce by setting
+ def check_announce_or_not(self, message):
+ try:
+ return getattr(self._announce_settings, message)
+ except Exception as e:
+ self._node.get_logger().error("check announce or not: " + str(e))
+ return False
+
def send_announce(self, message):
priority = PRIORITY_DICT.get(message, 0)
previous_priority = PRIORITY_DICT.get(self._current_announce, 0)
+ if not self.check_announce_or_not(message):
+ return
+
if priority == 3:
self._sound.stop()
self.play_sound(message)
@@ -100,3 +133,18 @@ def announce_going_to_depart_and_arrive(self, message):
# To stop repeat announcement
self.send_announce(message)
self._prev_depart_and_arrive_type = message
+
+ def publish_volume_callback(self):
+ self._sink = self._pulse.get_sink_by_name(self._pulse.server_info().default_sink_name)
+ self._get_volume_pub.publish(Float32(data=self._sink.volume.value_flat))
+
+ 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")
+ response.status.code = ResponseStatus.SUCCESS
+ except Exception:
+ response.status.code = ResponseStatus.ERROR
+ return response
diff --git a/src/signage/src/signage/autoware_diagnostic.py b/src/signage/src/signage/autoware_diagnostic.py
new file mode 100644
index 0000000..ee8650a
--- /dev/null
+++ b/src/signage/src/signage/autoware_diagnostic.py
@@ -0,0 +1,28 @@
+# -*- coding: utf-8 -*-
+# Copyright (c) 2020 Tier IV, Inc. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+# ========================================================================
+# This code is used to public the diagnostic to autoware and let autoware
+# to decide the hazard level
+# ========================================================================
+
+import diagnostic_updater
+
+class AutowareDiagnostic():
+ def init_updater(self, node, name, update_function, hardware_id):
+ updater = diagnostic_updater.Updater(node, 1)
+ updater.setHardwareID(hardware_id)
+ updater.add(name, update_function)
+ return updater
diff --git a/src/signage/src/signage/autoware_interface.py b/src/signage/src/signage/autoware_interface.py
index 5152b2c..3286249 100644
--- a/src/signage/src/signage/autoware_interface.py
+++ b/src/signage/src/signage/autoware_interface.py
@@ -9,12 +9,12 @@
OperationModeState,
MotionState,
LocalizationInitializationState,
+ VelocityFactorArray,
)
import signage.signage_utils as utils
from tier4_debug_msgs.msg import Float64Stamped
from tier4_external_api_msgs.msg import DoorStatus
-
@dataclass
class AutowareInformation:
autoware_control: bool = False
@@ -31,6 +31,7 @@ class AutowareInterface:
def __init__(self, node):
self._node = node
self.information = AutowareInformation()
+ self.is_disconnected = False
sub_qos = rclpy.qos.QoSProfile(
history=rclpy.qos.QoSHistoryPolicy.KEEP_LAST,
@@ -81,6 +82,12 @@ def __init__(self, node):
self.sub_localization_initialization_state_callback,
api_qos,
)
+ self._sub_velocity_factors = node.create_subscription(
+ VelocityFactorArray,
+ "/api/planning/velocity_factors",
+ self.sub_velocity_factors_callback,
+ sub_qos,
+ )
self._autoware_connection_time = self._node.get_clock().now()
self._node.create_timer(2, self.reset_timer)
@@ -88,6 +95,9 @@ def reset_timer(self):
if utils.check_timeout(self._node.get_clock().now(), self._autoware_connection_time, 10):
self.information = AutowareInformation()
self._node.get_logger().error("Autoware disconnected", throttle_duration_sec=10)
+ self.is_disconnected = True
+ else:
+ self.is_disconnected = False
def sub_operation_mode_callback(self, msg):
try:
@@ -116,7 +126,6 @@ def sub_vehicle_door_callback(self, msg):
def sub_path_distance_callback(self, msg):
try:
- self._autoware_connection_time = self._node.get_clock().now()
self.information.goal_distance = msg.data
except Exception as e:
self._node.get_logger().error("Unable to get the goal distance, ERROR: " + str(e))
@@ -134,3 +143,9 @@ def sub_localization_initialization_state_callback(self, msg):
self._node.get_logger().error(
"Unable to get the localization init state, ERROR: " + str(e)
)
+
+ def sub_velocity_factors_callback(self, msg):
+ try:
+ self._autoware_connection_time = self._node.get_clock().now()
+ except Exception as e:
+ self._node.get_logger().error("Unable to get the velocity factors, ERROR: " + str(e))
diff --git a/src/signage/src/signage/external_signage.py b/src/signage/src/signage/external_signage.py
new file mode 100644
index 0000000..e72bdb3
--- /dev/null
+++ b/src/signage/src/signage/external_signage.py
@@ -0,0 +1,136 @@
+from dataclasses import dataclass
+import datetime
+import time
+import serial
+from ament_index_python.packages import get_package_share_directory
+import signage.packet_tools as packet_tools
+
+
+@dataclass
+class Display:
+ address1: int
+ address2: int
+ height: int
+ width: int
+ ack_query_ack: list
+ ack_data_chunk: list
+
+
+class Protocol:
+ SOT = 0xAA
+ EOT = 0x55
+ SEND_COLOR = "\x1B[34;1m"
+ RECV_COLOR = "\x1B[32;1m"
+ ERR_COLOR = "\x1B[31;1m"
+
+ def __init__(self):
+ self.front = Display(
+ address1=0x70,
+ address2=0x8F,
+ height=16,
+ width=128,
+ ack_query_ack=[0xAA, 0x70, 0x8F, 0x07, 0x12, 0x02, 0x1A, 0x01, 0x55],
+ ack_data_chunk=[0xAA, 0x70, 0x8F, 0x07, 0x20, 0x30, 0x56, 0x01, 0x55],
+ )
+ self.back = Display(
+ address1=0x80,
+ address2=0x7F,
+ height=16,
+ width=128,
+ ack_query_ack=[0xAA, 0x80, 0x7F, 0x07, 0x12, 0x02, 0x1A, 0x01, 0x55],
+ ack_data_chunk=[0xAA, 0x80, 0x7F, 0x07, 0x20, 0x30, 0x56, 0x01, 0x55],
+ )
+ self.side = Display(
+ address1=0x90,
+ address2=0x6F,
+ height=24,
+ width=80,
+ ack_query_ack=[0xAA, 0x90, 0x6F, 0x07, 0x12, 0x02, 0x1A, 0x01, 0x55],
+ ack_data_chunk=[0xAA, 0x90, 0x6F, 0x07, 0x20, 0x30, 0x56, 0x01, 0x55],
+ )
+
+
+class DataSender:
+ def __init__(self, bus, parser, protocol, node_logger):
+ self._bus = bus
+ self._parser = parser
+ self._protocol = protocol
+ self._logger = node_logger
+ self._delay_time = 0.02
+
+ def _send_heartbeat(self, data, ACK_QueryACK):
+ timestamp = datetime.datetime.now()
+ name_time_packet = packet_tools.gen_name_time_packet(data.linename, timestamp, False)
+ self._bus.write(name_time_packet)
+ packet_tools.dump_packet(name_time_packet, None, self._protocol.SEND_COLOR)
+ time.sleep(self._delay_time)
+ self._bus.write(data.heartbeat_packet)
+ packet_tools.dump_packet(data.heartbeat_packet, None, self._protocol.SEND_COLOR)
+ buf = self._parser.wait_ack()
+ if not packet_tools.lists_match(buf, ACK_QueryACK):
+ if len(buf) == 0:
+ self._logger.error("No ACK received for heartbeat.")
+ else:
+ packet_tools.dump_packet(buf, None, self._protocol.ERR_COLOR)
+
+ def _send_data_packets(self, data, ACK_DataChunk):
+ for packet in data.data_packets:
+ packet_tools.dump_packet(packet, None, self._protocol.SEND_COLOR)
+ self._bus.write(packet)
+ buf = self._parser.wait_ack()
+ if not packet_tools.lists_match(buf, ACK_DataChunk):
+ if len(buf) == 0:
+ self._logger.error("No ACK received for data packet.")
+ else:
+ packet_tools.dump_packet(buf, None, self._protocol.ERR_COLOR)
+
+ def send(self, data, ACK_QueryACK, ACK_DataChunk):
+ self._send_heartbeat(data, ACK_QueryACK)
+ self._send_data_packets(data, ACK_DataChunk)
+ return # Exit after sending all data packets
+
+
+class ExternalSignage:
+ def __init__(self, node):
+ self.node = node
+ self.protocol = Protocol()
+ package_path = get_package_share_directory("signage") + "/resource/td5_file/"
+ self.bus = serial.Serial(
+ "/dev/ttyUSB0", baudrate=38400, parity=serial.PARITY_EVEN, timeout=0.2, exclusive=False
+ )
+ self.parser = packet_tools.Parser(self.bus)
+
+ self.displays = {
+ "front": self._load_display_data(self.protocol.front, package_path),
+ "back": self._load_display_data(self.protocol.back, package_path),
+ "side": self._load_display_data(self.protocol.side, package_path),
+ }
+
+ def _load_display_data(self, display, package_path):
+ auto_path = package_path + f"/automatic_{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
+ ),
+ "null": packet_tools.TD5Data(
+ null_path, display.address1, display.address2, display.height, display.width
+ ),
+ }
+
+ def send_data(self, display_key, data_key):
+ display = self.displays[display_key]
+ data = display[data_key]
+ ack_query_ack = self.protocol.__dict__[display_key].ack_query_ack
+ ack_data_chunk = self.protocol.__dict__[display_key].ack_data_chunk
+ sender = DataSender(self.bus, self.parser, self.protocol, self.node.get_logger())
+ sender.send(data, ack_query_ack, ack_data_chunk)
+
+ def trigger(self):
+ for display_key in self.displays:
+ self.send_data(display_key, "auto")
+ time.sleep(1)
+
+ def close(self):
+ for display_key in self.displays:
+ self.send_data(display_key, "null")
diff --git a/src/signage/src/signage/heartbeat.py b/src/signage/src/signage/heartbeat.py
new file mode 100644
index 0000000..7c83269
--- /dev/null
+++ b/src/signage/src/signage/heartbeat.py
@@ -0,0 +1,20 @@
+# !/usr/bin/env python3
+# -*- coding: utf-8 -*-
+
+from signage.autoware_diagnostic import AutowareDiagnostic
+from diagnostic_msgs.msg import DiagnosticStatus
+
+class Heartbeat:
+ def __init__(self, node):
+ self._node = node
+
+ self._diagnostic_updater = AutowareDiagnostic().init_updater(
+ self._node,
+ "/system/signage_connection : signage heartbeat",
+ self.handle_heartbeat_diagnostics,
+ "none",
+ )
+
+ def handle_heartbeat_diagnostics(self, stat):
+ stat.summary(DiagnosticStatus.OK, "signage is working")
+ return stat
\ No newline at end of file
diff --git a/src/signage/src/signage/packet_tools.py b/src/signage/src/signage/packet_tools.py
new file mode 100644
index 0000000..37d485a
--- /dev/null
+++ b/src/signage/src/signage/packet_tools.py
@@ -0,0 +1,267 @@
+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 a8ead50..c9f1dc8 100644
--- a/src/signage/src/signage/parameter_interface.py
+++ b/src/signage/src/signage/parameter_interface.py
@@ -18,10 +18,23 @@ class SignageParameter:
monitor_width: int = 1920
monitor_height: int = 540
+@dataclass
+class AnnounceParameter:
+ emergency: bool = True
+ restart_engage: bool = True
+ door_close: bool = True
+ door_open: bool = True
+ engage: bool = True
+ arrived: bool = True
+ thank_you: bool = True
+ in_emergency: bool = True
+ going_to_depart: bool = True
+ going_to_arrive: bool = True
class ParameterInterface:
def __init__(self, node):
self.parameter = SignageParameter()
+ self.announce_settings = AnnounceParameter()
node.declare_parameter("signage_stand_alone", False)
node.declare_parameter("ignore_manual_driving", False)
@@ -64,3 +77,23 @@ def __init__(self, node):
self.parameter.monitor_height = (
node.get_parameter("monitor_height").get_parameter_value().integer_value
)
+
+ node.declare_parameter("announce.emergency", True)
+ node.declare_parameter("announce.restart_engage", True)
+ node.declare_parameter("announce.door_close", True)
+ node.declare_parameter("announce.door_open", True)
+ node.declare_parameter("announce.engage", True)
+ node.declare_parameter("announce.thank_you", True)
+ node.declare_parameter("announce.in_emergency", True)
+ node.declare_parameter("announce.going_to_depart", True)
+ node.declare_parameter("announce.going_to_arrive", True)
+
+ announce_prefix = node.get_parameters_by_prefix("announce")
+
+ for key in announce_prefix.keys():
+ setattr(
+ self.announce_settings,
+ key,
+ announce_prefix[key].get_parameter_value().bool_value,
+ )
+
diff --git a/src/signage/src/signage/route_handler.py b/src/signage/src/signage/route_handler.py
index 1aa7260..465b1ae 100644
--- a/src/signage/src/signage/route_handler.py
+++ b/src/signage/src/signage/route_handler.py
@@ -26,6 +26,7 @@ def __init__(
autoware_interface,
parameter_interface,
ros_service_interface,
+ external_signage,
):
self._node = node
self._viewController = viewController
@@ -33,6 +34,7 @@ def __init__(
self._autoware = autoware_interface
self._parameter = parameter_interface.parameter
self._service_interface = ros_service_interface
+ self._external_signage = external_signage
self.AUTOWARE_IP = os.getenv("AUTOWARE_IP", "localhost")
self._fms_payload = {
"method": "get",
@@ -48,6 +50,7 @@ def __init__(
self._display_phrase = ""
self._in_emergency_state = False
self._emergency_trigger_time = self._node.get_clock().now()
+ self._engage_trigger_time = self._node.get_clock().now()
self._is_stopping = True
self._is_driving = False
self._previous_driving_status = False
@@ -60,6 +63,7 @@ def __init__(
self._announce_engage = False
self._in_slow_stop_state = False
self._in_slowing_state = False
+ self._trigger_external_signage = False
self.process_station_list_from_fms()
@@ -73,6 +77,8 @@ def __init__(
def emergency_checker_callback(self):
if self._parameter.ignore_emergency:
in_emergency = False
+ if self._autoware.information.operation_mode == OperationModeState.STOP:
+ in_emergency = False
else:
in_emergency = self._autoware.information.mrm_behavior == MrmState.EMERGENCY_STOP
@@ -133,8 +139,15 @@ def announce_engage_when_starting(self):
):
if self._announce_engage and not self._skip_announce:
self._skip_announce = True
- else:
- self._announce_interface.send_announce("engage")
+ elif utils.check_timeout(
+ self._node.get_clock().now(),
+ self._engage_trigger_time,
+ self._parameter.accept_start,
+ ):
+ self._announce_interface.send_announce("restart_engage")
+ self._external_signage.trigger()
+ self._trigger_external_signage = True
+ self._engage_trigger_time = self._node.get_clock().now()
if self._autoware.information.motion_state == MotionState.STARTING:
self._service_interface.accept_start()
@@ -268,6 +281,8 @@ def route_checker_callback(self):
self._is_stopping = False
if not self._announce_engage and self._parameter.signage_stand_alone:
self._announce_interface.send_announce("engage")
+ self._external_signage.trigger()
+ self._trigger_external_signage = True
self._announce_engage = True
elif self._autoware.information.route_state == RouteState.ARRIVED:
# Check whether the vehicle arrive to goal
@@ -276,6 +291,13 @@ def route_checker_callback(self):
self._skip_announce = False
self._announce_engage = False
+ if (
+ self._autoware.information.operation_mode != OperationModeState.AUTONOMOUS
+ and self._trigger_external_signage
+ ):
+ self._external_signage.close()
+ self._trigger_external_signage = False
+
if self._prev_route_state != RouteState.SET:
if self._autoware.information.route_state == RouteState.SET:
self.process_station_list_from_fms(force_update=True)
@@ -362,7 +384,9 @@ def view_mode_callback(self):
self._viewController.next_station_list = self._display_details.next_station_list
self._viewController.display_phrase = self._display_phrase
- if (
+ if self._autoware.is_disconnected:
+ view_mode = "emergency_stopped"
+ elif (
not self._autoware.information.autoware_control
and not self._parameter.ignore_manual_driving
):
diff --git a/src/signage/src/signage/signage.py b/src/signage/src/signage/signage.py
index 146e666..4020678 100644
--- a/src/signage/src/signage/signage.py
+++ b/src/signage/src/signage/signage.py
@@ -7,12 +7,14 @@
import rclpy
from rclpy.node import Node
+from signage.heartbeat import Heartbeat
from signage.view_controller import ViewControllerProperty
from signage.announce_controller import AnnounceControllerProperty
from signage.autoware_interface import AutowareInterface
from signage.parameter_interface import ParameterInterface
from signage.route_handler import RouteHandler
from signage.ros_service_interface import RosServiceInterface
+from signage.external_signage import ExternalSignage
from ament_index_python.packages import get_package_share_directory
@@ -25,11 +27,14 @@ def main(args=None):
app = QApplication(sys.argv)
engine = QQmlApplicationEngine()
+ heartbeat = Heartbeat(node)
+ autoware_interface = AutowareInterface(node)
autoware_interface = AutowareInterface(node)
parameter_interface = ParameterInterface(node)
ros_service_interface = RosServiceInterface(node, parameter_interface)
viewController = ViewControllerProperty(node, parameter_interface)
announceController = AnnounceControllerProperty(node, autoware_interface, parameter_interface)
+ external_signage = ExternalSignage(node)
route_handler = RouteHandler(
node,
viewController,
@@ -37,6 +42,7 @@ def main(args=None):
autoware_interface,
parameter_interface,
ros_service_interface,
+ external_signage,
)
ctx = engine.rootContext()