From 82cef78ff7e599b0d354d22e14bca1042a848ff4 Mon Sep 17 00:00:00 2001 From: Kah Hooi Tan <41041286+tkhmy@users.noreply.github.com> Date: Fri, 1 Nov 2024 17:53:36 +0900 Subject: [PATCH 1/3] Fix/seperate fms client (#130) * seperate fms client Signed-off-by: tkhmy * add ignore disconnect Signed-off-by: tkhmy * remove some colcon build warning Signed-off-by: tkhmy * change naming Signed-off-by: tkhmy * change time out, minor fix Signed-off-by: tkhmy * remove old logger Signed-off-by: tkhmy * fix Signed-off-by: tkhmy --------- Signed-off-by: tkhmy --- src/signage/config/signage_param.yaml | 5 +- src/signage/launch/signage.launch.xml | 5 ++ src/signage/package.xml | 4 ++ src/signage/setup.cfg | 4 +- src/signage/src/signage/autoware_interface.py | 14 +++++ .../src/signage/parameter_interface.py | 8 ++- src/signage/src/signage/route_handler.py | 32 +---------- .../config/fms_client_param.yaml | 3 + .../launch/signage_fms_client.launch.xml | 7 +++ src/signage_fms_client/package.xml | 17 ++++++ .../resource/signage_fms_client | 0 src/signage_fms_client/setup.cfg | 4 ++ src/signage_fms_client/setup.py | 47 +++++++++++++++ .../src/signage_fms_client/__init__.py | 4 ++ .../signage_fms_client/signage_fms_client.py | 57 +++++++++++++++++++ 15 files changed, 177 insertions(+), 34 deletions(-) create mode 100644 src/signage_fms_client/config/fms_client_param.yaml create mode 100755 src/signage_fms_client/launch/signage_fms_client.launch.xml create mode 100644 src/signage_fms_client/package.xml create mode 100644 src/signage_fms_client/resource/signage_fms_client create mode 100644 src/signage_fms_client/setup.cfg create mode 100644 src/signage_fms_client/setup.py create mode 100644 src/signage_fms_client/src/signage_fms_client/__init__.py create mode 100644 src/signage_fms_client/src/signage_fms_client/signage_fms_client.py diff --git a/src/signage/config/signage_param.yaml b/src/signage/config/signage_param.yaml index 41661b6..912b230 100644 --- a/src/signage/config/signage_param.yaml +++ b/src/signage/config/signage_param.yaml @@ -2,10 +2,11 @@ signage: ros__parameters: signage_stand_alone: true ignore_emergency_stoppped: false + ignore_disconnected: false ignore_manual_driving: false set_goal_by_distance: false goal_distance: 1.0 # meter - check_fms_time: 5.0 # second + check_fms_time: 2.0 # second emergency_repeat_period: 180.0 # second accept_start: 5.0 # second monitor_width: 1920 @@ -19,4 +20,4 @@ signage: thank_you: true in_emergency: true going_to_depart: true - going_to_arrive: true \ No newline at end of file + going_to_arrive: true diff --git a/src/signage/launch/signage.launch.xml b/src/signage/launch/signage.launch.xml index bdb2d3b..78bb16f 100755 --- a/src/signage/launch/signage.launch.xml +++ b/src/signage/launch/signage.launch.xml @@ -1,7 +1,12 @@ + + + + + diff --git a/src/signage/package.xml b/src/signage/package.xml index b60e554..e4af9e4 100644 --- a/src/signage/package.xml +++ b/src/signage/package.xml @@ -11,7 +11,11 @@ signage_version autoware_auto_system_msgs + python-pulsectl-pip rclpy + rclpy + signage_fms_client + std_srvs tier4_api_msgs tier4_debug_msgs tier4_external_api_msgs diff --git a/src/signage/setup.cfg b/src/signage/setup.cfg index 7d50ab1..40e86c5 100644 --- a/src/signage/setup.cfg +++ b/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 diff --git a/src/signage/src/signage/autoware_interface.py b/src/signage/src/signage/autoware_interface.py index 5152b2c..35af124 100644 --- a/src/signage/src/signage/autoware_interface.py +++ b/src/signage/src/signage/autoware_interface.py @@ -10,6 +10,7 @@ MotionState, LocalizationInitializationState, ) +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 @@ -25,6 +26,7 @@ class AutowareInformation: goal_distance: float = 1000.0 motion_state: int = 0 localization_init_state: int = 0 + active_schedule: str = "" class AutowareInterface: @@ -83,6 +85,12 @@ def __init__(self, node): ) self._autoware_connection_time = self._node.get_clock().now() self._node.create_timer(2, self.reset_timer) + self._sub_active_schedule = node.create_subscription( + String, + "/signage/active_schedule", + self.sub_active_schedule_callback, + sub_qos, + ) def reset_timer(self): if utils.check_timeout(self._node.get_clock().now(), self._autoware_connection_time, 10): @@ -134,3 +142,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_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)) diff --git a/src/signage/src/signage/parameter_interface.py b/src/signage/src/signage/parameter_interface.py index c9f1dc8..396e3a4 100644 --- a/src/signage/src/signage/parameter_interface.py +++ b/src/signage/src/signage/parameter_interface.py @@ -9,6 +9,7 @@ class SignageParameter: signage_stand_alone: bool = False ignore_manual_driving: bool = False + ignore_disconnected: bool = False ignore_emergency: bool = False set_goal_by_distance: bool = False goal_distance: float = 1.0 @@ -18,6 +19,7 @@ class SignageParameter: monitor_width: int = 1920 monitor_height: int = 540 + @dataclass class AnnounceParameter: emergency: bool = True @@ -31,12 +33,14 @@ class AnnounceParameter: going_to_depart: bool = True going_to_arrive: bool = True + class ParameterInterface: def __init__(self, node): self.parameter = SignageParameter() self.announce_settings = AnnounceParameter() node.declare_parameter("signage_stand_alone", False) + node.declare_parameter("ignore_disconnected", False) node.declare_parameter("ignore_manual_driving", False) node.declare_parameter("check_fms_time", 5.0) node.declare_parameter("accept_start", 5.0) @@ -50,6 +54,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 ) @@ -96,4 +103,3 @@ def __init__(self, node): key, announce_prefix[key].get_parameter_value().bool_value, ) - diff --git a/src/signage/src/signage/route_handler.py b/src/signage/src/signage/route_handler.py index 30229d7..581568d 100644 --- a/src/signage/src/signage/route_handler.py +++ b/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() @@ -171,25 +160,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() @@ -199,6 +171,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( diff --git a/src/signage_fms_client/config/fms_client_param.yaml b/src/signage_fms_client/config/fms_client_param.yaml new file mode 100644 index 0000000..3836d2c --- /dev/null +++ b/src/signage_fms_client/config/fms_client_param.yaml @@ -0,0 +1,3 @@ +signage: + ros__parameters: + post_request_time: 8.0 # second diff --git a/src/signage_fms_client/launch/signage_fms_client.launch.xml b/src/signage_fms_client/launch/signage_fms_client.launch.xml new file mode 100755 index 0000000..f342fe8 --- /dev/null +++ b/src/signage_fms_client/launch/signage_fms_client.launch.xml @@ -0,0 +1,7 @@ + + + + + + + diff --git a/src/signage_fms_client/package.xml b/src/signage_fms_client/package.xml new file mode 100644 index 0000000..bdcb852 --- /dev/null +++ b/src/signage_fms_client/package.xml @@ -0,0 +1,17 @@ + + + signage_fms_client + 0.1.0 + The fms client for signage + + tkhmy + + Apache License 2.0 + ament_index_python + + rclpy + + + ament_python + + diff --git a/src/signage_fms_client/resource/signage_fms_client b/src/signage_fms_client/resource/signage_fms_client new file mode 100644 index 0000000..e69de29 diff --git a/src/signage_fms_client/setup.cfg b/src/signage_fms_client/setup.cfg new file mode 100644 index 0000000..9c777d2 --- /dev/null +++ b/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 diff --git a/src/signage_fms_client/setup.py b/src/signage_fms_client/setup.py new file mode 100644 index 0000000..f57c842 --- /dev/null +++ b/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="kahhooi.tan@tier4.jp", + 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", + ] + }, +) diff --git a/src/signage_fms_client/src/signage_fms_client/__init__.py b/src/signage_fms_client/src/signage_fms_client/__init__.py new file mode 100644 index 0000000..003f841 --- /dev/null +++ b/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 diff --git a/src/signage_fms_client/src/signage_fms_client/signage_fms_client.py b/src/signage_fms_client/src/signage_fms_client/signage_fms_client.py new file mode 100644 index 0000000..21aa40c --- /dev/null +++ b/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() From d3661ab426160ccb00823b49f826ef7c3dfd1144 Mon Sep 17 00:00:00 2001 From: Kah Hooi Tan <41041286+tkhmy@users.noreply.github.com> Date: Tue, 5 Nov 2024 12:57:57 +0900 Subject: [PATCH 2/3] add handle negative goal distance (#131) Signed-off-by: tkhmy --- src/signage/src/signage/route_handler.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/signage/src/signage/route_handler.py b/src/signage/src/signage/route_handler.py index 581568d..7ccbc4c 100644 --- a/src/signage/src/signage/route_handler.py +++ b/src/signage/src/signage/route_handler.py @@ -334,7 +334,10 @@ def calculate_time_callback(self): self._announce_interface.announce_going_to_depart_and_arrive("going_to_depart") 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") self._announce_interface.announce_going_to_depart_and_arrive("going_to_arrive") From d25664812986ac119c3411828f0dda9417525909 Mon Sep 17 00:00:00 2001 From: tkhmy Date: Tue, 5 Nov 2024 13:50:15 +0900 Subject: [PATCH 3/3] fix package Signed-off-by: tkhmy --- src/signage/package.xml | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/signage/package.xml b/src/signage/package.xml index e4af9e4..eac1eb8 100644 --- a/src/signage/package.xml +++ b/src/signage/package.xml @@ -11,11 +11,8 @@ signage_version autoware_auto_system_msgs - python-pulsectl-pip - rclpy rclpy signage_fms_client - std_srvs tier4_api_msgs tier4_debug_msgs tier4_external_api_msgs