feat: 路肩退避のMRMの表示に対応する (#104)

* fix delete code * add pull over mrm
tier4 · Jun 26, 2024 · f5d8ac6 · f5d8ac6
1 parent 71824a9
commit f5d8ac6
Show file tree

Hide file tree

Showing 4 changed files with 12 additions and 148 deletions.
diff --git a/src/signage/src/signage/autoware_interface.py b/src/signage/src/signage/autoware_interface.py
@@ -17,6 +17,7 @@
 
 DISCONNECT_THRESHOLD = 2
 
+
 @dataclass
 class AutowareInformation:
     autoware_control: bool = False
@@ -94,9 +95,13 @@ def __init__(self, node):
         self._node.create_timer(1, self.reset_timer)
 
     def reset_timer(self):
-        if utils.check_timeout(self._node.get_clock().now(), self._autoware_connection_time, DISCONNECT_THRESHOLD):
+        if utils.check_timeout(
+            self._node.get_clock().now(), self._autoware_connection_time, DISCONNECT_THRESHOLD
+        ):
             self.information = AutowareInformation()
-            self._node.get_logger().error("Autoware disconnected", throttle_duration_sec=DISCONNECT_THRESHOLD)
+            self._node.get_logger().error(
+                "Autoware disconnected", throttle_duration_sec=DISCONNECT_THRESHOLD
+            )
             self.is_disconnected = True
         else:
             self.is_disconnected = False

diff --git a/src/signage/src/signage/external_signage.py b/src/signage/src/signage/external_signage.py
diff --git a/src/signage/src/signage/route_handler.py b/src/signage/src/signage/route_handler.py
@@ -26,15 +26,13 @@ def __init__(
         autoware_interface,
         parameter_interface,
         ros_service_interface,
-        external_signage,
     ):
         self._node = node
         self._viewController = viewController
         self._announce_interface = announceController
         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",
@@ -84,13 +82,13 @@ def emergency_checker_callback(self):
 
         self._in_slowing_state = (
             self._autoware.information.mrm_behavior == MrmState.COMFORTABLE_STOP
-            and self._autoware.information.motion_state == MotionState.MOVING
-        )
+            or self._autoware.information.mrm_behavior == MrmState.PULL_OVER
+        ) and self._autoware.information.motion_state == MotionState.MOVING
 
         self._in_slow_stop_state = (
             self._autoware.information.mrm_behavior == MrmState.COMFORTABLE_STOP
-            and self._autoware.information.motion_state == MotionState.STOPPED
-        )
+            or self._autoware.information.mrm_behavior == MrmState.PULL_OVER
+        ) and self._autoware.information.motion_state == MotionState.STOPPED
 
         if in_emergency and not self._in_emergency_state:
             self._announce_interface.announce_emergency("emergency")
@@ -285,7 +283,7 @@ def route_checker_callback(self):
                     self._trigger_external_signage = True
                 if not self._announce_engage and self._parameter.signage_stand_alone:
                     self._announce_interface.send_announce("engage")
-                    self._external_signage.trigger()
+                    self._service_interface.trigger_external_signage(True)
                     self._trigger_external_signage = True
                     self._announce_engage = True
             elif self._autoware.information.route_state == RouteState.ARRIVED:

diff --git a/src/signage/src/signage/signage.py b/src/signage/src/signage/signage.py
@@ -14,7 +14,6 @@
 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
 
 
@@ -34,15 +33,13 @@ def main(args=None):
     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,
         announceController,
         autoware_interface,
         parameter_interface,
         ros_service_interface,
-        external_signage,
     )
 
     ctx = engine.rootContext()