sync stage

src/signage/config/signage_param.yaml

@@ -1,7 +1,7 @@
 signage:
   ros__parameters:
     signage_stand_alone: true
     ignore_emergency_stoppped: false
     ignore_manual_driving: false
     set_goal_by_distance: false
     goal_distance: 1.0 # meter
@@ -10,3 +10,13 @@
     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

src/signage/package.xml

@@ -4,13 +4,15 @@
   <version>0.1.0</version>
   <description>The signage package</description>
 
   <maintainer email="[email protected]">yabuta</maintainer>
 
   <license>Apache License 2.0</license>
   <exec_depend>ament_index_python</exec_depend>
   <exec_depend>signage_version</exec_depend>
 
   <depend>autoware_auto_system_msgs</depend>
+  <depend>diagnostic_updater</depend>
+  <depend>python-pulsectl-pip</depend>
   <depend>rclpy</depend>
   <depend>tier4_api_msgs</depend>
   <depend>tier4_debug_msgs</depend>

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"),

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,20 +27,35 @@
     "going_to_arrive": 1,
 }
 
+CURRENT_VOLUME_PATH = "/opt/autoware/volume.txt"
+
 
 class AnnounceControllerProperty:
     def __init__(self, node, autoware_interface, parameter_interface):
         super(AnnounceControllerProperty, self).__init__()
 
         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("")
         self._prev_depart_and_arrive_type = ""
         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

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

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,13 +82,22 @@ 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)
 
     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))

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

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