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()