fix(arrived goal): fix arrived goal condition (#49)

* fix arrived goal condition

* revert value name

src/signage/src/signage/autoware_state_interface.py

@@ -6,6 +6,7 @@
 from tier4_api_msgs.msg import AwapiAutowareStatus, AwapiVehicleStatus
 from tier4_external_api_msgs.msg import DoorStatus
 from autoware_auto_system_msgs.msg import HazardStatusStamped
+from autoware_adapi_v1_msgs.msg import RouteState
 
 
 class AutowareStateInterface:
@@ -18,6 +19,7 @@ def __init__(self, node):
         self.turn_signal_callback_list = []
         self.velocity_callback_list = []
         self.distance_callback_list = []
+        self.routing_state_callback_list = []
         self._node = node
 
         self._sub_autoware_state = node.create_subscription(
@@ -38,6 +40,9 @@ def __init__(self, node):
             self.path_distance_callback,
             10,
         )
+        self._sub_routing_state = node.create_subscription(
+            RouteState, "/api/routing/state", self.sub_routing_state_callback, 10
+        )
         self._autoware_status_time = self._node.get_clock().now()
         self._vehicle_status_time = self._node.get_clock().now()
         self._distance_time = self._node.get_clock().now()
@@ -85,6 +90,9 @@ def set_door_status_callback(self, callback):
     def set_distance_callback(self, callback):
         self.distance_callback_list.append(callback)
 
+    def set_routing_state_callback(self, callback):
+        self.routing_state_callback_list.append(callback)
+
     # ros subscriber
     # autoware stateをsubしたときの処理
     def autoware_state_callback(self, topic):
@@ -143,3 +151,11 @@ def sub_hazard_status_callback(self, topic):
                 callback(emergency_stopped)
         except Exception as e:
             self._node.get_logger().error("Unable to get the hazard_status, ERROR: " + str(e))
+
+    def sub_routing_state_callback(self, topic):
+        try:
+            routing_state = topic.state
+            for callback in self.routing_state_callback_list:
+                callback(routing_state)
+        except Exception as e:
+            self._node.get_logger().error("Unable to get the routing state, ERROR: " + str(e))

src/signage/src/signage/route_handler.py

@@ -93,6 +93,7 @@ def __init__(self, node, viewController, announceController, autoware_state_inte
         autoware_state_interface.set_emergency_stopped_callback(self.sub_emergency)
         autoware_state_interface.set_distance_callback(self.sub_distance)
         autoware_state_interface.set_door_status_callback(self.sub_door_status)
+        autoware_state_interface.set_routing_state_callback(self.sub_routing_state)
 
         route_checker = self._node.create_timer(1, self.route_checker_callback)
         view_mode_update = self._node.create_timer(1, self.view_mode_callback)
@@ -111,23 +112,10 @@ def sub_autoware_state(self, autoware_state):
         if not self._is_auto_mode:
             return
 
-        stop_condition = [
-            "WaitingForRoute",
-            "ArrivedGoal",
-            "Planning",
-        ]
+        if self._prev_autoware_state == "WaitingForEngage" and  autoware_state == "Driving":
+            self._is_driving = True
+            self._is_stopping = False
 
-        buffer_flag = (
-            autoware_state == self._prev_prev_autoware_state
-            and autoware_state == self._prev_autoware_state
-        )
-
-        self._is_stopping = (autoware_state in stop_condition) or (
-            buffer_flag and autoware_state == "WaitingForEngage"
-        )
-        self._is_driving = autoware_state == "Driving"
-
-        self._prev_prev_autoware_state = self._prev_autoware_state
         self._prev_autoware_state = autoware_state
 
     def sub_control_mode(self, control_mode):
@@ -172,6 +160,11 @@ def sub_door_status(self, door_status):
         else:
             self._pre_door_announce_status = door_status
 
+    def sub_routing_state(self, routing_state):
+        if routing_state == 3:
+            self._is_stopping = True
+            self._is_driving = False
+
     # ============== Subsciber callback ==================
 
     def process_station_list_from_fms(self):