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 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 8d8a77d..64c774f 100755 --- a/src/signage/launch/signage.launch.xml +++ b/src/signage/launch/signage.launch.xml @@ -1,8 +1,13 @@ + + + + + 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 6fd9479..e9c9e01 100644 --- a/src/signage/src/signage/autoware_interface.py +++ b/src/signage/src/signage/autoware_interface.py @@ -11,12 +11,14 @@ 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 DISCONNECT_THRESHOLD = 2 + @dataclass class AutowareInformation: autoware_control: bool = False @@ -27,6 +29,7 @@ class AutowareInformation: goal_distance: float = 1000.0 motion_state: int = 0 localization_init_state: int = 0 + active_schedule: str = "" class AutowareInterface: @@ -90,13 +93,23 @@ def __init__(self, node): 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, + ) self._autoware_connection_time = self._node.get_clock().now() 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): - self.information = AutowareInformation() - self._node.get_logger().error("Autoware disconnected", throttle_duration_sec=DISCONNECT_THRESHOLD) + if utils.check_timeout( + self._node.get_clock().now(), self._autoware_connection_time, DISCONNECT_THRESHOLD + ): + self.information.mrm_behavior = MrmState.NONE + self._node.get_logger().error( + "Autoware disconnected", throttle_duration_sec=DISCONNECT_THRESHOLD + ) self.is_disconnected = True else: self.is_disconnected = False @@ -151,3 +164,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 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 8bc1aaa..06d72cd 100644 --- a/src/signage/src/signage/route_handler.py +++ b/src/signage/src/signage/route_handler.py @@ -3,9 +3,9 @@ # This Python file uses the following encoding: utf-8 import os -import requests import json from datetime import datetime + import signage.signage_utils as utils from tier4_external_api_msgs.msg import DoorStatus from autoware_adapi_v1_msgs.msg import ( @@ -33,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() @@ -169,15 +161,7 @@ def announce_engage_when_starting(self): def process_station_list_from_fms(self, force_update=False): try: - respond = requests.post( - "http://{}:4711/v1/services/order".format(self.AUTOWARE_IP), - json=self._fms_payload, - timeout=5, - ) - - data = json.loads(respond.text) - 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() @@ -187,6 +171,8 @@ def process_station_list_from_fms(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( @@ -384,7 +370,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()