From 1ba4d4844ec712fd9a762273ff48bd326b2f9e66 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/2] 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/external_signage/setup.cfg | 4 +- src/signage/config/signage_param.yaml | 3 +- src/signage/launch/signage.launch.xml | 5 ++ src/signage/package.xml | 3 +- src/signage/setup.cfg | 4 +- src/signage/src/signage/autoware_interface.py | 14 +++++ .../src/signage/parameter_interface.py | 5 ++ src/signage/src/signage/route_handler.py | 41 +++---------- .../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 +++++++++++++++++++ 16 files changed, 179 insertions(+), 39 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/external_signage/setup.cfg b/src/external_signage/setup.cfg index c66e1f6..661806c 100644 --- a/src/external_signage/setup.cfg +++ b/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 diff --git a/src/signage/config/signage_param.yaml b/src/signage/config/signage_param.yaml index 76774b6..6848dad 100644 --- a/src/signage/config/signage_param.yaml +++ b/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 diff --git a/src/signage/launch/signage.launch.xml b/src/signage/launch/signage.launch.xml index b3c695e..fafc03f 100755 --- a/src/signage/launch/signage.launch.xml +++ b/src/signage/launch/signage.launch.xml @@ -1,10 +1,15 @@ + + + + + diff --git a/src/signage/package.xml b/src/signage/package.xml index da4a857..26931a6 100644 --- a/src/signage/package.xml +++ b/src/signage/package.xml @@ -12,14 +12,15 @@ autoware_auto_system_msgs diagnostic_updater + external_signage python-pulsectl-pip rclpy + signage_fms_client std_srvs tier4_api_msgs tier4_debug_msgs tier4_external_api_msgs tier4_hmi_msgs - external_signage ament_python 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 4531e55..0e49c64 100644 --- a/src/signage/src/signage/autoware_interface.py +++ b/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)) diff --git a/src/signage/src/signage/parameter_interface.py b/src/signage/src/signage/parameter_interface.py index 6c6eb50..b3b359f 100644 --- a/src/signage/src/signage/parameter_interface.py +++ b/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 ) diff --git a/src/signage/src/signage/route_handler.py b/src/signage/src/signage/route_handler.py index 718f2b8..5583c3f 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() @@ -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") @@ -453,7 +424,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 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 2821bd3ab359c64654f1705c9b87ba39f4e8c31b 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/2] 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 5583c3f..1d7b0ba 100644 --- a/src/signage/src/signage/route_handler.py +++ b/src/signage/src/signage/route_handler.py @@ -397,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: