Merge pull request #74 from tier4/develop

stage release for test

signage.repos

@@ -19,3 +19,4 @@ repositories:
     type: git
     url: https://github.com/tier4/boot_shutdown_tools.git
     version: main
+

src/signage/config/signage_param.yaml

@@ -1,6 +1,6 @@
 signage:
   ros__parameters:
-    signage_stand_alone: false
+    signage_stand_alone: true
     ignore_emergency_stoppped: false
     ignore_manual_driving: false
     set_goal_by_distance: false

src/signage/src/signage/announce_controller.py

@@ -89,7 +89,7 @@ def send_announce(self, message):
 
     def announce_arrived(self):
         if self._parameter.signage_stand_alone:
-            self.send_announce("arrived")
+            self.send_announce("thank_you")
 
     def announce_emergency(self, message):
         if self._parameter.signage_stand_alone:

src/signage/src/signage/autoware_interface.py

@@ -10,6 +10,7 @@
     MotionState,
     LocalizationInitializationState,
 )
+import signage.signage_utils as utils
 from tier4_debug_msgs.msg import Float64Stamped
 from tier4_external_api_msgs.msg import DoorStatus
 
@@ -60,7 +61,7 @@ def __init__(self, node):
             MrmState,
             "/api/fail_safe/mrm_state",
             self.sub_mrm_callback,
-            sub_qos,
+            api_qos,
         )
         self._sub_vehicle_door = node.create_subscription(
             DoorStatus, "/api/external/get/door", self.sub_vehicle_door_callback, sub_qos
@@ -80,6 +81,13 @@ def __init__(self, node):
             self.sub_localization_initialization_state_callback,
             api_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)
 
     def sub_operation_mode_callback(self, msg):
         try:
@@ -108,6 +116,7 @@ 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))

src/signage/src/signage/route_handler.py

@@ -56,21 +56,26 @@ def __init__(
         self._fms_check_time = 0
         self._prev_motion_state = 0
         self._prev_route_state = 0
+        self._skip_announce = False
+        self._announce_engage = False
 
         self.process_station_list_from_fms()
 
-        self._node.create_timer(1, self.route_checker_callback)
-        self._node.create_timer(1, self.emergency_checker_callback)
-        self._node.create_timer(1, self.view_mode_callback)
-        self._node.create_timer(1, self.calculate_time_callback)
-        self._node.create_timer(1, self.door_status_callback)
+        self._node.create_timer(0.2, self.route_checker_callback)
+        self._node.create_timer(0.2, self.emergency_checker_callback)
+        self._node.create_timer(0.2, self.view_mode_callback)
+        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)
 
     def emergency_checker_callback(self):
         if self._parameter.ignore_emergency:
             in_emergency = False
         else:
-            in_emergency = self._autoware.information.mrm_behavior == MrmState.EMERGENCY_STOP
+            in_emergency = (
+                self._autoware.information.mrm_behavior == MrmState.EMERGENCY_STOP
+                or self._autoware.information.mrm_behavior == MrmState.COMFORTABLE_STOP
+            )
 
         if in_emergency and not self._in_emergency_state:
             self._announce_interface.announce_emergency("emergency")
@@ -117,7 +122,10 @@ def announce_engage_when_starting(self):
                 in [MotionState.STARTING, MotionState.MOVING]
                 and self._prev_motion_state == 1
             ):
-                self._announce_interface.send_announce("engage")
+                if self._announce_engage and not self._skip_announce:
+                    self._skip_announce = True
+                else:
+                    self._announce_interface.send_announce("engage")
 
                 if self._autoware.information.motion_state == MotionState.STARTING:
                     self._service_interface.accept_start()
@@ -150,6 +158,7 @@ def process_station_list_from_fms(self, force_update=False):
             )
 
             data = json.loads(respond.text)
+            self._fms_check_time = self._node.get_clock().now()
 
             if not data:
                 self._schedule_details = utils.init_ScheduleDetails()
@@ -168,7 +177,7 @@ def process_station_list_from_fms(self, force_update=False):
 
             self.task_list = utils.seperate_task_list(data.get("tasks", []))
 
-            if not self.task_list.doing_list:
+            if not self.task_list.doing_list and not self.task_list.todo_list:
                 self._schedule_details = utils.init_ScheduleDetails()
                 self._display_details = utils.init_DisplayDetails()
                 self._current_task_details = utils.init_CurrentTask()
@@ -211,6 +220,8 @@ def process_station_list_from_fms(self, force_update=False):
 
     def arrived_goal(self):
         try:
+            self._announce_interface.announce_arrived()
+
             if self._current_task_details == utils.init_CurrentTask():
                 raise Exception("No current task details")
 
@@ -235,7 +246,6 @@ def arrived_goal(self):
                 "local",
                 self._schedule_details.schedule_type,
             )
-            self._announce_interface.announce_arrived()
         except Exception as e:
             self._node.get_logger().error("Unable to update the goal, ERROR: " + str(e))
 
@@ -247,10 +257,15 @@ def route_checker_callback(self):
                 # Check whether the vehicle is move in autonomous
                 self._is_driving = True
                 self._is_stopping = False
+                if not self._announce_engage and self._parameter.signage_stand_alone:
+                    self._announce_interface.send_announce("engage")
+                    self._announce_engage = True
             elif self._autoware.information.route_state == RouteState.ARRIVED:
                 # Check whether the vehicle arrive to goal
                 self._is_driving = False
                 self._is_stopping = True
+                self._skip_announce = False
+                self._announce_engage = False
 
             if self._prev_route_state != RouteState.SET:
                 if self._autoware.information.route_state == RouteState.SET:
@@ -304,7 +319,7 @@ def calculate_time_callback(self):
                 self._display_phrase = utils.handle_phrase("final")
             elif self._is_stopping:
                 # handle text and announce while bus is stopping
-                if remain_minute > 1:
+                if remain_minute > 2:
                     # display the text with the remaining time for departure
                     self._display_phrase = utils.handle_phrase(
                         "remain_minute", round(remain_minute)

src/signage/src/signage/signage_utils.py

@@ -172,7 +172,7 @@ def handle_phrase(phrase_type, remain_minute=0):
     return {
         "final": "終点です。\nご乗車ありがとうございました",
         "remain_minute": "このバスはあと{}分程で出発します".format(str(remain_minute)),
-        "departing": "間もなく出発します",
+        "departing": "間もなく発車時刻です",
         "arriving": "間もなく到着します",
     }.get(phrase_type, "")