hotfix: FMSのアクセスノードを分離する (#132)

* Fix/seperate fms client (#130)

* seperate fms client

Signed-off-by: tkhmy <[email protected]>

* add ignore disconnect

Signed-off-by: tkhmy <[email protected]>

* remove some colcon build warning

Signed-off-by: tkhmy <[email protected]>

* change naming

Signed-off-by: tkhmy <[email protected]>

* change time out, minor fix

Signed-off-by: tkhmy <[email protected]>

* remove old logger

Signed-off-by: tkhmy <[email protected]>

* fix

Signed-off-by: tkhmy <[email protected]>

---------

Signed-off-by: tkhmy <[email protected]>

* add handle negative goal distance (#131)

Signed-off-by: tkhmy <[email protected]>

---------

Signed-off-by: tkhmy <[email protected]>
Co-authored-by: Kah Hooi Tan <[email protected]>

src/external_signage/setup.cfg

@@ -1,4 +1,4 @@
 [develop]
-script-dir=$base/lib/external_signage
+script_dir=$base/lib/external_signage
 [install]
-install-scripts=$base/lib/external_signage
+install_scripts=$base/lib/external_signage

src/signage/config/signage_param.yaml

@@ -2,11 +2,12 @@ signage:
   ros__parameters:
     signage_stand_alone: true
     ignore_emergency_stoppped: false
+    ignore_disconnected: false
     ignore_manual_driving: false
     set_goal_by_distance: false
     freeze_emergency: true
     goal_distance: 1.0 # meter
-    check_fms_time: 5.0 # second
+    check_fms_time: 2.0 # second
     emergency_ignore_period: 5.0 # second
     emergency_repeat_period: 180.0 # second
     accept_start: 5.0 # second

src/signage/launch/signage.launch.xml

@@ -1,10 +1,15 @@
 <?xml version="1.0"?>
 <launch>
   <arg name="signage_param" default="$(find-pkg-share signage)/config/signage_param.yaml" />
+  <arg name="fms_client_param" default="$(find-pkg-share signage_fms_client)/config/fms_client_param.yaml" />
   <arg name="debug_mode" default="false" />
   <node pkg="signage" exec="signage"  output="screen">
     <param from="$(var signage_param)"/>
     <param name="debug_mode" value="$(var debug_mode)" />
   </node>
   <node pkg="external_signage" exec="external_signage"  output="screen" />
+
+  <node pkg="signage_fms_client" exec="signage_fms_client"  output="screen" >
+    <param from="$(var fms_client_param)"/>
+  </node>
 </launch>

src/signage/package.xml

@@ -12,14 +12,15 @@
 
   <depend>autoware_auto_system_msgs</depend>
   <depend>diagnostic_updater</depend>
+  <depend>external_signage</depend>
   <depend>python-pulsectl-pip</depend>
   <depend>rclpy</depend>
+  <depend>signage_fms_client</depend>
   <depend>std_srvs</depend>
   <depend>tier4_api_msgs</depend>
   <depend>tier4_debug_msgs</depend>
   <depend>tier4_external_api_msgs</depend>
   <depend>tier4_hmi_msgs</depend>
-  <depend>external_signage</depend>
 
   <export>
     <build_type>ament_python</build_type>

src/signage/setup.cfg

@@ -1,4 +1,4 @@
 [develop]
-script-dir=$base/lib/signage
+script_dir=$base/lib/signage
 [install]
-install-scripts=$base/lib/signage
+install_scripts=$base/lib/signage

src/signage/src/signage/autoware_interface.py

@@ -11,6 +11,7 @@
     LocalizationInitializationState,
     VelocityFactorArray,
 )
+from std_msgs.msg import String
 import signage.signage_utils as utils
 from tier4_debug_msgs.msg import Float64Stamped
 from tier4_external_api_msgs.msg import DoorStatus
@@ -28,6 +29,7 @@ class AutowareInformation:
     goal_distance: float = 1000.0
     motion_state: int = 0
     localization_init_state: int = 0
+    active_schedule: str = ""
 
 
 class AutowareInterface:
@@ -92,6 +94,12 @@ def __init__(self, node, parameter_interface):
             self.sub_velocity_factors_callback,
             sub_qos,
         )
+        self._sub_active_schedule = node.create_subscription(
+            String,
+            "/signage/active_schedule",
+            self.sub_active_schedule_callback,
+            sub_qos,
+        )
         if not self._parameter.debug_mode:
             self._autoware_connection_time = self._node.get_clock().now()
             self._node.create_timer(1, self.reset_timer)
@@ -158,3 +166,9 @@ def sub_velocity_factors_callback(self, msg):
             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))
+
+    def sub_active_schedule_callback(self, msg):
+        try:
+            self.information.active_schedule = msg.data
+        except Exception as e:
+            self._node.get_logger().error("Unable to get the active schedule, ERROR: " + str(e))

src/signage/src/signage/parameter_interface.py

@@ -10,6 +10,7 @@ class SignageParameter:
     debug_mode: bool = False
     signage_stand_alone: bool = False
     ignore_manual_driving: bool = False
+    ignore_disconnected: bool = False
     ignore_emergency: bool = False
     set_goal_by_distance: bool = False
     freeze_emergency: bool = True
@@ -43,6 +44,7 @@ def __init__(self, node):
 
         node.declare_parameter("debug_mode", False)
         node.declare_parameter("signage_stand_alone", False)
+        node.declare_parameter("ignore_disconnected", False)
         node.declare_parameter("ignore_manual_driving", False)
         node.declare_parameter("freeze_emergency", True)
         node.declare_parameter("check_fms_time", 5.0)
@@ -61,6 +63,9 @@ def __init__(self, node):
         self.parameter.signage_stand_alone = (
             node.get_parameter("signage_stand_alone").get_parameter_value().bool_value
         )
+        self.parameter.ignore_disconnected = (
+            node.get_parameter("ignore_disconnected").get_parameter_value().bool_value
+        )
         self.parameter.ignore_manual_driving = (
             node.get_parameter("ignore_manual_driving").get_parameter_value().bool_value
         )

src/signage/src/signage/route_handler.py

@@ -5,9 +5,6 @@
 import os
 import json
 from datetime import datetime
-import aiohttp
-import asyncio
-from threading import Thread
 
 import signage.signage_utils as utils
 from tier4_external_api_msgs.msg import DoorStatus
@@ -36,14 +33,6 @@ def __init__(
         self._autoware = autoware_interface
         self._parameter = parameter_interface.parameter
         self._service_interface = ros_service_interface
-        self.AUTOWARE_IP = os.getenv("AUTOWARE_IP", "localhost")
-        self._fms_payload = {
-            "method": "get",
-            "url": "https://"
-            + os.getenv("FMS_URL", "fms.web.auto")
-            + "/v1/projects/{project_id}/environments/{environment_id}/vehicles/{vehicle_id}/active_schedule",
-            "body": {},
-        }
         self._schedule_details = utils.init_ScheduleDetails()
         self._display_details = utils.init_DisplayDetails()
         self._current_task_details = utils.init_CurrentTask()
@@ -207,25 +196,8 @@ def announce_engage_when_starting(self):
             self._node.get_logger().error("not able to play the announce, ERROR: {}".format(str(e)))
 
     def process_station_list_from_fms(self, force_update=False):
-        if not self._processing_thread:
-            self._processing_thread = True
-            thread = Thread(target=asyncio.run(self.fms_thread()), args=(force_update,))
-            thread.setDaemon(True)
-            thread.start()
-            self._processing_thread = False
-
-    async def fms_thread(self, force_update=False):
         try:
-            async with aiohttp.ClientSession() as session:
-                async with session.post(
-                    f"http://{self.AUTOWARE_IP}:4711/v1/services/order",
-                    json=self._fms_payload,
-                    timeout=10,
-                ) as response:
-                    data = await response.json()
-
-            self._fms_check_time = self._node.get_clock().now()
-
+            data = json.loads(self._autoware.information.active_schedule)
             if not data:
                 self._schedule_details = utils.init_ScheduleDetails()
                 self._display_details = utils.init_DisplayDetails()
@@ -235,6 +207,8 @@ async def fms_thread(self, force_update=False):
                 self._fms_check_time = self._node.get_clock().now()
                 raise Exception("same schedule, skip")
 
+            self._fms_check_time = self._node.get_clock().now()
+
             self._schedule_details = utils.update_schedule_details(data)
 
             self._display_details.route_name = utils.get_route_name(
@@ -403,9 +377,6 @@ def calculate_time_callback(self):
                 self._current_task_details.depart_time, self._node.get_clock().now().to_msg().sec
             )
 
-            self._node.get_logger().info("_reach_final: " + str(self._reach_final))
-            self._node.get_logger().info("remain_minute: " + str(remain_minute))
-
             if self._reach_final:
                 # display arrive to final station
                 self._display_phrase = utils.handle_phrase("final")
@@ -426,7 +397,10 @@ def calculate_time_callback(self):
                         self._announced_depart = True
             elif self._is_driving:
                 # handle text and announce while bus is running
-                if self._autoware.information.goal_distance < 100:
+                if (
+                    self._autoware.information.goal_distance < 100
+                    and self._autoware.information.goal_distance > 0
+                ):
                     # display text and announce if the goal is within 100m
                     self._display_phrase = utils.handle_phrase("arriving")
                     if not self._announced_arrive:
@@ -453,7 +427,11 @@ def view_mode_callback(self):
             self._viewController.next_station_list = self._display_details.next_station_list
             self._viewController.display_phrase = self._display_phrase
 
-            if self._autoware.is_disconnected:
+            if (
+                self._autoware.is_disconnected
+                and not self._parameter.ignore_disconnected
+                and not self._parameter.ignore_emergency
+            ):
                 view_mode = "emergency_stopped"
             elif (
                 not self._autoware.information.autoware_control

src/signage_fms_client/config/fms_client_param.yaml

@@ -0,0 +1,3 @@
+signage:
+  ros__parameters:
+    post_request_time: 8.0 # second

src/signage_fms_client/launch/signage_fms_client.launch.xml

@@ -0,0 +1,7 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="fms_client_param" default="$(find-pkg-share signage)/config/fms_client_param.yaml" />
+  <node pkg="signage_fms_client" exec="signage_fms_client"  output="screen" >
+    <param from="$(var signage_param)"/>
+  </node>
+</launch>

src/signage_fms_client/package.xml

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>signage_fms_client</name>
+  <version>0.1.0</version>
+  <description>The fms client for signage</description>
+
+  <maintainer email="[email protected]">tkhmy</maintainer>
+
+  <license>Apache License 2.0</license>
+  <exec_depend>ament_index_python</exec_depend>
+
+  <depend>rclpy</depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

src/signage_fms_client/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/signage_fms_client
+[install]
+install_scripts=$base/lib/signage_fms_client

src/signage_fms_client/setup.py

@@ -0,0 +1,47 @@
+#!/usr/bin/env python3
+
+import os
+
+from setuptools import setup
+
+
+def package_files(directory):
+    paths = []
+    for (path, directories, filenames) in os.walk(directory):
+        for filename in filenames:
+            paths.append(os.path.join(path, filename))
+    return paths
+
+
+package_name = "signage_fms_client"
+setup(
+    name=package_name,
+    version="0.1.0",
+    package_dir={"": "src"},
+    packages=[package_name],
+    data_files=[
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+        ("share/" + package_name, ["package.xml"]),
+        ("share/" + package_name + "/launch", ["launch/signage_fms_client.launch.xml"]),
+        ("share/" + package_name + "/config", ["config/fms_client_param.yaml"]),
+    ],
+    install_requires=["setuptools"],
+    zip_safe=True,
+    author="Kah Hooi Tan",
+    maintainer="Kah Hooi Tan",
+    maintainer_email="[email protected]",
+    keywords=["ROS"],
+    classifiers=[
+        "Intended Audience :: Developers",
+        "License :: OSI Approved :: Apache Software License",
+        "Programming Language :: Python",
+        "Topic :: Software Development",
+    ],
+    description=("for fms client with signage"),
+    license="TODO",
+    entry_points={
+        "console_scripts": [
+            "signage_fms_client = signage_fms_client.signage_fms_client:main",
+        ]
+    },
+)

src/signage_fms_client/src/signage_fms_client/__init__.py

@@ -0,0 +1,4 @@
+# This Python file uses the following encoding: utf-8
+
+# if__name__ == "__main__":
+#     pass

src/signage_fms_client/src/signage_fms_client/signage_fms_client.py

@@ -0,0 +1,57 @@
+# This Python file uses the following encoding: utf-8
+import os
+
+import rclpy
+from rclpy.node import Node
+
+import requests
+from std_msgs.msg import String
+
+
+class FMSClient(Node):
+    def __init__(self, node):
+        self._node = node
+        node.declare_parameter("post_request_time", 8.0)
+        self._post_request_time = (
+            node.get_parameter("post_request_time").get_parameter_value().double_value
+        )
+        self._fms_payload = {
+            "method": "get",
+            "url": "https://"
+            + os.getenv("FMS_URL", "fms.web.auto")
+            + "/v1/projects/{project_id}/environments/{environment_id}/vehicles/{vehicle_id}/active_schedule",
+            "body": {},
+        }
+        self.AUTOWARE_IP = os.getenv("AUTOWARE_IP", "localhost")
+        self.schedule_pub_ = node.create_publisher(String, "/signage/active_schedule", 10)
+        self.timer = node.create_timer(self._post_request_time + 0.5, self.pub_schedule)
+
+    def pub_schedule(self):
+        try:
+            msg = String()
+            respond = requests.post(
+                "http://{}:4711/v1/services/order".format(self.AUTOWARE_IP),
+                json=self._fms_payload,
+                timeout=self._post_request_time,
+            )
+            msg.data = respond.text
+            self.schedule_pub_.publish(msg)
+        except Exception as e:
+            self._node.get_logger().warning(
+                "Unable to get the task from FMS, ERROR: " + str(e), throttle_duration_sec=5
+            )
+
+
+def main(args=None):
+
+    rclpy.init(args=args)
+    node = Node("signage_fms_client")
+
+    signage_fms_client = FMSClient(node)
+
+    while True:
+        rclpy.spin_once(node, timeout_sec=0.01)
+
+
+if __name__ == "__main__":
+    main()