From fac9114899f00a6829e1ed04e3347742bcfd8c46 Mon Sep 17 00:00:00 2001 From: Sebastian Lindholm Date: Mon, 13 Apr 2026 12:48:37 +0200 Subject: [PATCH 01/33] changes so build works --- scripts/installation/install_functions.sh | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/scripts/installation/install_functions.sh b/scripts/installation/install_functions.sh index 9f8601a07..4b85d6876 100755 --- a/scripts/installation/install_functions.sh +++ b/scripts/installation/install_functions.sh @@ -41,10 +41,10 @@ add_source_line_if_needed() { local shell_type="$2" local source_line="$3$shell_type" - if [ ! grep -qF "$source_line" "$file" ]; then - # Ask the user if they want to add the source line. - # First check for noninteractive shell with DEBAIN_FRONTEND=noninteractive - if [ -z "$DEBIAN_FRONTEND" && ! -z $GITHUB_ACTION ]; then + if ! grep -qF "$source_line" "$file"; then + # Ask the user only for interactive local shells. + # In non-interactive environments (e.g. CI), append automatically. + if [ -t 0 ] && [ -z "$DEBIAN_FRONTEND" ] && [ -z "$GITHUB_ACTION" ]; then echo "Do you want to add the following line to your $shell_type config file $file:" echo "$source_line" echo "y/n" @@ -58,4 +58,4 @@ add_source_line_if_needed() { echo "$source_line" >> "$file" fi fi -} \ No newline at end of file +} From 8bf9645d440199340ced612e37a6bdcfab5c409b Mon Sep 17 00:00:00 2001 From: Sebastian Lindholm Date: Mon, 13 Apr 2026 13:57:52 +0200 Subject: [PATCH 02/33] ATOSMini scaffolding --- ATOSMini_architecture_and_dev_guide.pdf | Bin 0 -> 4248 bytes README.md | 21 ++++ atos/launch/launch_atosmini.py | 113 ++++++++++++++++++ .../modules/TruckObjectControl/CMakeLists.txt | 1 - atos_gui/atos_gui/main.py | 90 +++++++++----- atos_gui/atos_gui/objectpanel/objectpanel.py | 48 +++----- atos_gui/setup.py | 5 +- 7 files changed, 219 insertions(+), 59 deletions(-) create mode 100644 ATOSMini_architecture_and_dev_guide.pdf create mode 100644 atos/launch/launch_atosmini.py diff --git a/ATOSMini_architecture_and_dev_guide.pdf b/ATOSMini_architecture_and_dev_guide.pdf new file mode 100644 index 0000000000000000000000000000000000000000..88e320e32d6c747e66e1a14280dd5fa6ea88496d GIT binary patch literal 4248 zcmdT|*}9@g62A8-aNk)31UC+XqJj&!?}`G7$|B%Y)m+RA%-qfWJnN*VPj#KDQ!`zC z*?fMI8Ae8AWc+^^5SJT59)pQc{NMlj=YOD~?mAx3RS!^0w{=eqbPWO;kmej8mTm{o zISAY`f_(b)3HDxfZ|VU)I#vjSP{=u4Le**r5iqC#rb7r33K0G_!Xe^03NcJbx3#D0 zQ|IdvPW5%c0S=J1e*2pqm`4>tn)cDrO#1{OGt;iyzWGhiS1lcw#~@_%I0Ro@Fd2hy zFn`{_FTmD2SIL6eo~I7R(!z{%XNcKMm%M4S%&@=y$<9lh$)IK(dLi_Z=Gyo+1FdHw^v8Kwb^h zn{#@X-qJfG^+#}aJSeqJskUQ(v7Cp$PNyM+_jENd9lP-aPO4EZpoJnzU^s>o;L4(C z5`4bu{bt}{`>uX84O7=%O-TBePmZVgPzvaQjsB4-0Br4X1hh!`KXNVE;cx2Wa5Bprj&#dh_2S|Mlp;LH_}02P%MMf+$wd zljB3^Nsb}(MAh)g+usiP_2Azf@hkJc+7ZlK0Kx)VHUXx( zx_t`HpgfeGmiz#uzzVfg|1d50WwMdrh|IAk>1F#C4=dNsCEQIyxZN5Za3O`*NVCEsFNrh_Al0McD=D^cXqhg*$tD=Y0-qN6^F#!F$S_}c;c0sr?}2F zHppah&78{>tEK=gI-jZMT z)XO}oKFn5%oQz6WKDwW-Gc7~&_Xtc?TUzpvr4x%G%nxHQe^T6gqAY9|Y^zZVpYqYw zCV0pj<5+5XJvkgyuH9CJxvP%_mAxkiY3Zg^P9HyM#2HF`h^_C3#iKAbP)ixg-QGjYW5@L0g$|y%;~=ceQX$aLyZJXLpog^SxoWQvn|w0hM6TN=lO|#qGGV&$0##82=~Vo{16ZGc94@MO_z;u_38^?)vd~A zx~X$S*ju~6I3~!%ICJK$?bvVj$5cmV%J+p2`9y2 z-!~RVOKvvEU4WbN!BjW$B$?OS6yGWpMjQ9M#NykwCivO#prVhC|6ssh{~N&GngHy5 z7w{($OVQMKhz%yFaf|B78n zWshpMuGix!+@nt0<@hRa$T*_V(E|+Q?KESIS4vEIa5T}Xxl%68zC;SjEm%hek?X{S zS8y$!NaI?2xs#2Wo^=I|pN@811upG5XOIOFJX4qX$#`lDa7sc|XXfB4$BT-yk)~8I4T^s%oE}zex$aY#W6msBN9VYom#Yb>|8g7pH z0YYp}`c~cG=S1U9_oz`Ob-4&4OwRL6Y?^Pw`}JN>CydweoVBYU?7eo|g7pff*CWJC zi^LE&7mp>XqXC-QE9KlY>s1+Ib7W6!hp#tDw@b&!G~x9^sfn9; zWw>_mhBmp}JeP}0dNFJ-k5up4IWOUB3@cSKwW^f6?^`Z*Nm5rS#yxO@FUYHy-QR?> zJ#ml8hq}?uS7a`QNwYj39ysl;nQji6LNfiB!OiJRfN5*iT*M=sC9=}}n$mqr9OyTW zwMu?ry1sh(t{GKY2Wn#uhVY=iO>^Fri`>q=%MjUKE?SJdTzXy9ow^Amn!R?L%X_Wg zmOZ@^p6yV{f3SwyIwLu8eJn0~?9!4tOmu(P$}Uy;8Qb1B!he!&pnd$FZC3G6R}!a0 zovJQf1dnWAJb|q?y8C0>K(>aSo~KH!aYr1dO5-Eb%3ehKlt-fNSw8S{W~5KkeBFyg zO4)t^bGD;1ObhU#9W`1RtGG~`?B#5q6tN@C#o>@}s7RB+w&`PTw~u1uwJ1g(Cv}!p z=nTJbve{Z`HEO4ld2za5x($(HnB2VH*llHxF_@lWinH|GDdolqq~VA0>z&9W(mqD_ zyv1E$V@x|TfKwKlzYHGToGaX1snMEIEIc9{cS^&PYK)BB^5d=*_-0XF*^OzTI~v1B z0ogj_CTAz4tl?NZ-HoC1T7S3Moy(2IDmgoK=NM};f*5CKYrfNu)JF96Sdx{(sco^H zBR#8{?8=jetG#{L5(>O_{ASnnoC%pFTC1jUDpB*ayRUarOuv}yiBqM|*6PYme3|Qm zQ9e${<+QM^yK1i$Df|SjpFQ0Op%6xd(BChpNRl`S8PM-C3SjzqLH6%5(29JJ6>#t% z^Mf8vd|XQqpxOOFuSkB>qsb3?7+U Building from source manually You can find instructions on how to manually install ATOS and its dependencies from source [here](https://atos.readthedocs.io/en/latest/Installation/installation/). +## ATOSMini development launch +ATOSMini is a lightweight development stack built around `truck_object_control` and `truck_object_gui` and does not start OpenScenarioGateway, JournalControl, or EsminiAdapter. + +After building, start it with: +```bash +ros2 launch atos launch_atosmini.py insecure:=True +``` + +The placeholder COT input topic for TruckObjectControl is: +```text +/atos/truck_objects/cot +``` +Expected temporary payload format: +```text +id=;distance_m=;tcp_connected=<0|1> +``` +TruckObjectControl publishes speed commands on: +```text +/atos/truck_objects/speed_command +``` + # Using ATOS with a Graphical User Interface (GUI) Please click [here](https://atos.readthedocs.io/en/latest/Usage/GUI/foxglove/) for instructions on how to use ATOS with a GUI. diff --git a/atos/launch/launch_atosmini.py b/atos/launch/launch_atosmini.py new file mode 100644 index 000000000..a950225f2 --- /dev/null +++ b/atos/launch/launch_atosmini.py @@ -0,0 +1,113 @@ +import os +import sys + +from ament_index_python.packages import get_package_prefix +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import Node + +# Need to modify sys.path since we launch from the ros2 installed path. +sys.path.insert(0, os.path.join(get_package_prefix('atos'), 'share', 'atos', 'launch')) + + +def generate_launch_description(): + insecure_websockets = LaunchConfiguration('insecure') + foxbridge = LaunchConfiguration('foxbridge') + + insecure_launch_arg = DeclareLaunchArgument('insecure', default_value='False') + foxbridge_launch_arg = DeclareLaunchArgument('foxbridge', default_value='True') + + fox_tls_bridge_params = [ + {'port': 8765}, + {'retry_startup_delay': 5.0}, + {'tls': True}, + {'fragment_timeout': 600}, + {'max_message_size': 10000000}, + {'unregister_timeout': 10.0}, + {'use_compression': False}, + ] + ros_tls_bridge_params = [ + {'port': 9090}, + {'retry_startup_delay': 5.0}, + {'tls': True}, + {'fragment_timeout': 600}, + {'max_message_size': 10000000}, + {'unregister_timeout': 10.0}, + {'use_compression': False}, + ] + + fox_bridge_params = [dict(item) for item in fox_tls_bridge_params] + fox_bridge_params[2] = {'tls': False} + ros_bridge_params = [dict(item) for item in ros_tls_bridge_params] + ros_bridge_params[1] = {'retry_startup_delay': 5.0} + ros_bridge_params[2] = {'tls': False} + + return LaunchDescription([ + foxbridge_launch_arg, + insecure_launch_arg, + Node( + condition=IfCondition(PythonExpression(['not ', LaunchConfiguration('insecure')])), + package='atos_gui', + namespace='atos', + executable='truck_object_gui', + name='truck_object_gui', + output='screen', + arguments=['True', 'atosmini'], + ), + Node( + condition=IfCondition(LaunchConfiguration('insecure')), + package='atos_gui', + namespace='atos', + executable='truck_object_gui', + name='truck_object_gui', + output='screen', + arguments=['False', 'atosmini'], + ), + Node( + package='atos', + namespace='atos', + executable='truck_object_control', + name='truck_object_control', + output='screen', + ), + Node( + condition=IfCondition(PythonExpression(['not ', foxbridge])), + name='rosapi', + package='rosapi', + executable='rosapi_node', + ), + Node( + condition=IfCondition(PythonExpression([insecure_websockets, ' and ', foxbridge])), + package='foxglove_bridge', + executable='foxglove_bridge', + name='foxglove_bridge', + output={'both': 'log'}, + parameters=fox_bridge_params, + ), + Node( + condition=IfCondition(PythonExpression(['not ', insecure_websockets, ' and ', foxbridge])), + package='foxglove_bridge', + executable='foxglove_bridge', + name='foxglove_bridge', + output={'both': 'log'}, + parameters=fox_tls_bridge_params, + ), + Node( + condition=IfCondition(PythonExpression([insecure_websockets, ' and not ', foxbridge])), + package='rosbridge_server', + executable='rosbridge_websocket', + name='ros_bridge', + output={'both': 'log'}, + parameters=ros_bridge_params, + ), + Node( + condition=IfCondition(PythonExpression(['not ', insecure_websockets, ' and not ', foxbridge])), + package='rosbridge_server', + executable='rosbridge_websocket', + name='ros_bridge', + output={'both': 'log'}, + parameters=ros_tls_bridge_params, + ), + ]) diff --git a/atos/modules/TruckObjectControl/CMakeLists.txt b/atos/modules/TruckObjectControl/CMakeLists.txt index 1da34fbbc..2096276c2 100644 --- a/atos/modules/TruckObjectControl/CMakeLists.txt +++ b/atos/modules/TruckObjectControl/CMakeLists.txt @@ -18,7 +18,6 @@ find_package(OpenSSL REQUIRED) include(GNUInstallDirs) set(COREUTILS_LIBRARY ATOSCoreUtil) - add_executable(${PROJECT_NAME} ${CMAKE_CURRENT_SOURCE_DIR}/src/main.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/truckobjectcontrol.cpp diff --git a/atos_gui/atos_gui/main.py b/atos_gui/atos_gui/main.py index dd9c3a9bf..9ab6c9200 100644 --- a/atos_gui/atos_gui/main.py +++ b/atos_gui/atos_gui/main.py @@ -15,63 +15,99 @@ from atos_gui.configpanel.configpanel import ConfigPanelNode from atos_gui.controlpanel.controlpanel import ControlPanelNode from atos_gui.objectpanel.objectpanel import ObjectPanelNode -from atos_interfaces.srv import * -USE_SSL = sys.argv[1] == "True" +USE_SSL = len(sys.argv) > 1 and sys.argv[1] == "True" +ATOSMINI_MODE = len(sys.argv) > 2 and sys.argv[2].lower() == "atosmini" + def main() -> None: - # NOTE: This function is defined as the ROS entry point in setup.py, but it's empty to enable NiceGUI auto-reloading + # NOTE: This function is defined as the ROS entry point in setup.py, + # but it's empty to enable NiceGUI auto-reloading. pass +def render_atosmini_pages() -> None: + nicegui.ui.link("ATOSMini Home", "/") + + @ui.page(path="/", title="TruckObjectGUI") + def render_home() -> None: + ui.label("TruckObjectGUI (ATOSMini mode)").classes("text-h4") + ui.markdown( + """ +This GUI is running in **ATOSMini mode**. + +Active runtime components: +- `truck_object_control` +- `foxglove_bridge` / `rosbridge` + +Expected COT topic: +- `/atos/truck_objects/cot` + +Speed command topic: +- `/atos/truck_objects/speed_command` + +Placeholder COT payload format: +- `id=;distance_m=;tcp_connected=<0|1>` + """.strip() + ) + + def ros_main() -> None: - nicegui.ui.link("Control Panel", "/control") - nicegui.ui.link("Config Panel", "/config") - nicegui.ui.link("Object Panel", "/object") rclpy.init() - control_panel = ControlPanelNode() - config_panel = ConfigPanelNode() - object_panel = ObjectPanelNode() executor = MultiThreadedExecutor() - executor.add_node(control_panel) - executor.add_node(config_panel) - executor.add_node(object_panel) + nodes = [] + + if ATOSMINI_MODE: + render_atosmini_pages() + else: + nicegui.ui.link("Control Panel", "/control") + nicegui.ui.link("Config Panel", "/config") + nicegui.ui.link("Object Panel", "/object") + + control_panel = ControlPanelNode() + config_panel = ConfigPanelNode() + object_panel = ObjectPanelNode() + nodes = [control_panel, config_panel, object_panel] + + for node in nodes: + executor.add_node(node) + try: - executor.spin() + if nodes: + executor.spin() except ExternalShutdownException: pass finally: executor.shutdown() - control_panel.destroy_node() - config_panel.destroy_node() - object_panel.destroy_node() + for node in nodes: + node.destroy_node() + rclpy.shutdown() + +def print_access_hint() -> None: + scheme = "https" if USE_SSL else "http" + print(f"TruckObjectGUI ready. Open {scheme}://localhost:3000", flush=True) -# Starting the ros node in a thread managed by nicegui. It will be restarted with "on_startup" after a reload. -# It has to be in a thread, since NiceGUI wants the main thread for itself. -app.on_startup(lambda: threading.Thread(target=ros_main).start()) +# Start the ROS node logic in a thread managed by nicegui. +app.on_startup(lambda: threading.Thread(target=ros_main, daemon=True).start()) +app.on_startup(print_access_hint) -ui_run.APP_IMPORT_STRING = f"{__name__}:app" # ROS2 uses a non-standard module name, so we need to specify it here +ui_run.APP_IMPORT_STRING = f"{__name__}:app" # ROS2 uses non-standard module naming. -# Prepare the arguments for ui.run() uvicorn_args = { "uvicorn_reload_dirs": str(Path(__file__).parent.resolve()), - # 'favicon': '/images/favicon.ico', "port": 3000, - "show": False, # Disable auto-opening the browser - "title": "ATOS GUI", + "show": False, + "title": "TruckObjectGUI" if ATOSMINI_MODE else "ATOS GUI", } -# If use_ssl is True, add the SSL arguments if USE_SSL: uvicorn_args["ssl_keyfile"] = Path.home() / ".astazero/ATOS/certs/selfsigned.key" uvicorn_args["ssl_certfile"] = Path.home() / ".astazero/ATOS/certs/selfsigned.crt" -# Call ui.run() with the prepared arguments ui.run(**uvicorn_args) -# If print is above ui.run(), it will be printed twice for some reason if USE_SSL: print( "ATTENTION: Using SSL, use https://localhost:3000 to access the GUI instead", diff --git a/atos_gui/atos_gui/objectpanel/objectpanel.py b/atos_gui/atos_gui/objectpanel/objectpanel.py index 86c256af1..111ec6d91 100644 --- a/atos_gui/atos_gui/objectpanel/objectpanel.py +++ b/atos_gui/atos_gui/objectpanel/objectpanel.py @@ -7,8 +7,14 @@ class ObjectPanelNode(Node): - """This node is responsible for rendering the object panel and visualize the IP address for each object ID in the scenario.""" + def _safe_notify(self, message: str) -> None: + try: + ui.notify(message) + except RuntimeError: + self.get_logger().warning(message) + + """This node is responsible for rendering the object panel and visualize the IP address for each object ID in the scenario.""" def __init__(self) -> None: """Initializes the node, fetches all object IDs and their IPs in the current scenario and visualizes them.""" super().__init__("object_panel") @@ -49,12 +55,8 @@ def get_object_ids(self): "Get object ID service not available, waiting again..." ) if service_timeout_counter > MAX_TIMEOUT: - ui.notify( - f"Get object ID service not available after {MAX_TIMEOUT} seconds" - ) - self.get_logger().info( - f"Get object ID service not available after {MAX_TIMEOUT} seconds" - ) + self._safe_notify(f'Get object ID service not available after {MAX_TIMEOUT} seconds') + self.get_logger().info(f'Get object ID service not available after {MAX_TIMEOUT} seconds') return future = self.get_id_client.call_async(self.object_ids_req) future.add_done_callback( @@ -75,12 +77,8 @@ def get_object_ips(self, object_ids): "Get object IP service not available, waiting again..." ) if service_timeout_counter > MAX_TIMEOUT: - ui.notify( - f"Get object IP service not available after {MAX_TIMEOUT} seconds" - ) - self.get_logger().info( - f"Get object IP service not available after {MAX_TIMEOUT} seconds" - ) + self._safe_notify(f'Get object IP service not available after {MAX_TIMEOUT} seconds') + self.get_logger().info(f'Get object IP service not available after {MAX_TIMEOUT} seconds') return for object_id in object_ids: self.object_id_ip_map[object_id] = "" @@ -107,19 +105,15 @@ def update_object_ip(self, object_id, object_ip): object_ip (str): New IP address for the given object ID. """ - ui.notify(f"Setting object {object_id} IP: {object_ip}") + self._safe_notify(f'Setting object {object_id} IP: {object_ip}') service_timeout_counter = 0 while not self.set_ip_client.wait_for_service(timeout_sec=1.0): self.get_logger().debug( "Set object IP service not available, waiting again..." ) if service_timeout_counter > MAX_TIMEOUT: - ui.notify( - f"Set object IP service not available after {MAX_TIMEOUT} seconds" - ) - self.get_logger().info( - f"Set object IP service not available after {MAX_TIMEOUT} seconds" - ) + self._safe_notify(f'Set object IP service not available after {MAX_TIMEOUT} seconds') + self.get_logger().info(f'Set object IP service not available after {MAX_TIMEOUT} seconds') return future = self.set_ip_client.call_async( SetObjectIp.Request(id=object_id, ip=object_ip) @@ -137,19 +131,13 @@ def update_object_ip_result(self, result): """ if result.success: with self.refresh_row: - ui.notify( - f"IP set to {result.ip} for object {result.id} was successful" - ) - self.get_logger().info( - f"IP set to {result.ip} for object {result.id} was successful" - ) + self._safe_notify(f'IP set to {result.ip} for object {result.id} was successful') + self.get_logger().info(f'IP set to {result.ip} for object {result.id} was successful') self.object_id_ip_map[result.id] = result.ip else: with self.refresh_row: - ui.notify(f"Failed to set object {result.id} IP to {result.ip}") - self.get_logger().info( - f"Failed to set object {result.id} IP to {result.ip}" - ) + self._safe_notify(f'Failed to set object {result.id} IP to {result.ip}') + self.get_logger().info(f'Failed to set object {result.id} IP to {result.ip}') def refresh(self): """Refreshes the object panel by fetching all object IDs and their IPs in the current scenario.""" diff --git a/atos_gui/setup.py b/atos_gui/setup.py index ea3f8726a..2648aae69 100644 --- a/atos_gui/setup.py +++ b/atos_gui/setup.py @@ -20,6 +20,9 @@ install_requires=["setuptools"], zip_safe=True, entry_points={ - "console_scripts": ["gui = atos_gui.main:main"], + "console_scripts": [ + "gui = atos_gui.main:main", + "truck_object_gui = atos_gui.main:main", + ], }, ) From f58c30542c1d8deb7d0fe02b0fbdea687f1660cd Mon Sep 17 00:00:00 2001 From: Sebastian Lindholm Date: Mon, 13 Apr 2026 14:19:05 +0200 Subject: [PATCH 03/33] renamed to ATOSFleetManagement --- ...tManagement_architecture_and_dev_guide.pdf | Bin 4248 -> 4089 bytes README.md | 6 +++--- ROS2_ATOSFleetManagement_cheatsheet.pdf | Bin 0 -> 2548 bytes ...smini.py => launch_atosfleetmanagement.py} | 4 ++-- atos_gui/atos_gui/main.py | 16 ++++++++-------- 5 files changed, 13 insertions(+), 13 deletions(-) rename ATOSMini_architecture_and_dev_guide.pdf => ATOSFleetManagement_architecture_and_dev_guide.pdf (61%) create mode 100644 ROS2_ATOSFleetManagement_cheatsheet.pdf rename atos/launch/{launch_atosmini.py => launch_atosfleetmanagement.py} (97%) diff --git a/ATOSMini_architecture_and_dev_guide.pdf b/ATOSFleetManagement_architecture_and_dev_guide.pdf similarity index 61% rename from ATOSMini_architecture_and_dev_guide.pdf rename to ATOSFleetManagement_architecture_and_dev_guide.pdf index 88e320e32d6c747e66e1a14280dd5fa6ea88496d..f3629912224644778042e98f7a1675cc05d29fd7 100644 GIT binary patch delta 1250 zcmaiyORu5^07f&JY0|i8CQY|ZuL>xFf`AAJK6t6uE8zPL0)j6(8P>Yso6p#EcDzJB=r-IFHhOtYDJq={#T znXdhC=Ios&6sR#+>%$38l0w{zIz_FqA1!pY#0>A7{S-Hb>rPc-4oYx_>E%5X27)4= zp=b^#^K-llY7i*F`W_gd4k7!fX#+%;1f9)fxwJ%Yl7^|%cTs?Q!mka0%Xh~;thKmu zx$&Sa%kb$n8c%GXjTX{E-t4xF1GAY`;dK^g52q^U2E)cI#)^*;m6Cc*c%|_HobS8q z>>7M4tmQ}-to^%Yulzc8!8n@^) zI#kj_;KR4D>vWFuqp=a3FZ0ICd&S*{)LET?YZ zt~F#?2EO>S`V6p)rYXKcD`BHmZm%Otg#2G-n@&ZVL&%X`lO*sY|3v4 zew>z1Yo-qY-Pvu~Nsr}9+&Upk(7AGh&P+A-#p{-@>^#KHRop_GeeY`em+0F+-ajdv z{m&6+Yyd9vV4k9Tg;YP4Y5YDejTeTCwrM^nXw8HW)vG5cwMO3*@9o@e8p_rTSx;~5 zqbDW)oQ6u&2qRY5IJ+nW{^i2A)j~pxV*ZXA31x;_Y?PoXi(&E;G6_~8vX&xSSt&rl zt5MFwgj8bZ;40XmEiR<_2IoVaErYZ|OGh}VKzi|2;f40}6jij;9glKR${^d>q6jTu z!_Xq6bv2^}X{(F%iCKb;hZG3dS91EM#l@aEgkha^TU`1WMdcA7 z)cIHBXc8OXEfzbkN|xo~c-Z>#^LIbIdlESHg8#)!^pg2oa1v*Te_NW^=fF!cJfFe( z@=IpJ*D2Sfe7|~d4}|TrxZ_gZfuXCmL)H$SPc;93vCDujWAA=TKL7YJ&eRwp1A>aK GX8r^M$z(kM delta 1409 zcmaiyNzbAN0ERQ^G-*tlNt)jK;jXxVf`|*EARr)%;))w6AS#QX*Lv=GF8vF=G)?B* zU({bPw@%wb(^H?*lQ-}2`TWDrz;D02|GJKR;p4CGnSbol=l9>ed!aagb-!g!D9bEuP$6L5R0~cEo;t*>GH3KN@z%^tf*Fl=J1Sp z=z06a^+rPC!o2w4mME7_sjPBpOps+@%Zh79{1sc~2cv6RE*49}QaGq7V9eDk7BdA4 zV85j!5JKY|WM0#Jk1KA9ur?=?dT%?eHPls}pY&O!cffkfl<{<5Y#yCW@hg=&%r`~W zKE;P>1?#YVZ@r~_8f2UTa)1d*Jm-p=zV|TaHWE_qL_~ziYe7VOb0{b}UaVSD z>vG95g94n|vofbH2IDlfJcuWt%LM1Gz4u4zWO!dw#Ga22-ObEGJ0J^rIe=IXlrlB5 z94uY2<22@`WI)o|bu$A{{-Y$!h9Q|8^gBJM*0^=ZqL@D?nS-}~IQB7%X0_s(@^)H0 zlr(Il5Uu;_)ZzroPt%-ov8(qB*3#WtJ;j z97eHtZMqH5YdEvxPD~5}a@(37KmGL0?_Yh14gF7S53gbN)vS~4W1V$attWTAh#@<) zcsO!)b!S_JWgcg>5wGQO?MSfTh4-CuHN`E;kGSL}rDl`rN691(N;NpxP0y7kQ!%-e z%@)1J${3QDv+t<A%je4c z{ICvb9wUg-vQO`KN<`SiSatbDVHw~hEnB5ypnkn0<*INOUjkJ2bC}K zQ!IsoHD7NxPpCS73Pg9E9o*s9I6c`J<2)zVF-coN6tUR{Ku`h3l~ABEVy|7>=(Bn; zZm;x{zQg6RqUuy9qrzgQrqP!Z-skpu>}*P7VuJ{-HozugJ|}LtR9k4^w((4+qY5D7 z@Rhxo^z3r3@?krO>x=r`E{}s3khi9D;oe(7-RL1>O^8U5E?^w9BI<|m0a>}8UQqa( zuww`7cDH%58?|S0FmMW$StpGqZ)HvN9m!)M6jVndCjWOv%U9&tsjIh;q1j?G2KCG|*|=m+22A!=n4 z#6vNRf*A@TtUk0I99tH`Lw}Sfq(*Mc&sB-+_SHS#TUzq8oI#XUf!Xy#W6UONpZ@sv z`*$xG!``63c=O*9e+yKHQ1svJ5gG2yiV~epqB^4A5_$)LYgWyM5EQ};?1UJojX`Lw k-n3yGF)hR-{{OK}WZouL7(^d`{BTcT2%N}dhBA}*6F!)kMgRZ+ diff --git a/README.md b/README.md index 21a60183c..d1c10ec3b 100644 --- a/README.md +++ b/README.md @@ -46,12 +46,12 @@ ATOS comes with an installation script that automates the installation process. ## Building from source manually You can find instructions on how to manually install ATOS and its dependencies from source [here](https://atos.readthedocs.io/en/latest/Installation/installation/). -## ATOSMini development launch -ATOSMini is a lightweight development stack built around `truck_object_control` and `truck_object_gui` and does not start OpenScenarioGateway, JournalControl, or EsminiAdapter. +## ATOSFleetManagement development launch +ATOSFleetManagement is a lightweight development stack built around `truck_object_control` and `truck_object_gui` and does not start OpenScenarioGateway, JournalControl, or EsminiAdapter. After building, start it with: ```bash -ros2 launch atos launch_atosmini.py insecure:=True +ros2 launch atos launch_atosfleetmanagement.py insecure:=True ``` The placeholder COT input topic for TruckObjectControl is: diff --git a/ROS2_ATOSFleetManagement_cheatsheet.pdf b/ROS2_ATOSFleetManagement_cheatsheet.pdf new file mode 100644 index 0000000000000000000000000000000000000000..87e68a8b8f845ecc061d757149a8f9fd67efa77a GIT binary patch literal 2548 zcma)8*Lvbc6u##vhHAPx2#|!SMy7XcFd(E+!9oI+>~1dd0?A$O^Q;-M_J(*Z z>C>mYbJaDaLteH|Ap+pQJ{&_F7sA>EAklOJur>*(w-E*CU=$LB5VVv)HPCrCsY@1i+^L!TWMXlNtI)^WcTPAYV73 zf<^pCs9fDPl^=;m{$RXdtKY|K*uG;x=SKojKNyI)3DiJ)MjW}IgoFtEK2!{;Zhr=A zG{*r`V)A5>DFBux@+@0o!3)4fk`vI%ezllD7cEhSF1XSd@DcEee2pAiolKB0S$F z$Wsqmnx}(O5f2=>9s>OodXW76*j#jqdZN>a?OGiB+Fvwca%!>@% zFf(58ys1$VrHpX5iPy|RrZmqt9?OaCWScK;Y*#sx4IAd}H0#5>uSXHn$4>V2_p zQF5-Jk>h1f<}l%TaJR2k)j>2$J(Uvv6S2v4eP6Cd&&=ek9rSrOz1Sxc=f_y&HLcCD z*;;pwW7UpQIXZgLRz{(i-(-Pr^JHGI` zwydsOCCgYvG>4hVy-BZ@yo?%gclsE^^3BGoR5(YtK`t)I?FyGZM=|y;)2!?yrOIKQ z%?Uo}>rr_;KQorWYmvdK-NO6ckm^Y8MSb)-!t-%*Vqq-bv&w^1wcN{_eVN{by>-nI z_A`5<6#D(c{YBuMYHV+=PP<6*oV4;vpO>Eb=%L=gg%yNLerjfzwEti&{h=I5Mr}vU z)C@yu8Hd+nG?tLIc4|F}cO@Uj_l59sshwOxR#Wm)xQtH~D{OkjCr;og4MLr4FEhea z829(i#M!0#gW-P5=Nq-@^7(SCcXJa_+49&Sxn3tr%hhh!4&%D{#MSmh*WG1nHm2Xl zUaWgM(M|E2g1k}RcYWl62 ze|Df6!U2JY@NW+&Fbu^26?~HwQ6ml>V11JjBx;r)WF&>!&IcJqe{W0E=v4clS75%k zEfVMm`@0^2w^_h)ob$b1Fj}DD@zzs-)nutP~0~MX406(z1%P zEFt{A#!D!EbIH>F`I42(0mVSw4xRh|cHc^cLYQc&HV8W2B!rr+?SY_!qVd%D83sgL U&t1v!uGEkONrd9@*03G=6C(85lmGw# literal 0 HcmV?d00001 diff --git a/atos/launch/launch_atosmini.py b/atos/launch/launch_atosfleetmanagement.py similarity index 97% rename from atos/launch/launch_atosmini.py rename to atos/launch/launch_atosfleetmanagement.py index a950225f2..2efb71efd 100644 --- a/atos/launch/launch_atosmini.py +++ b/atos/launch/launch_atosfleetmanagement.py @@ -54,7 +54,7 @@ def generate_launch_description(): executable='truck_object_gui', name='truck_object_gui', output='screen', - arguments=['True', 'atosmini'], + arguments=['True', 'atosfleetmanagement'], ), Node( condition=IfCondition(LaunchConfiguration('insecure')), @@ -63,7 +63,7 @@ def generate_launch_description(): executable='truck_object_gui', name='truck_object_gui', output='screen', - arguments=['False', 'atosmini'], + arguments=['False', 'atosfleetmanagement'], ), Node( package='atos', diff --git a/atos_gui/atos_gui/main.py b/atos_gui/atos_gui/main.py index 9ab6c9200..82ff66187 100644 --- a/atos_gui/atos_gui/main.py +++ b/atos_gui/atos_gui/main.py @@ -17,7 +17,7 @@ from atos_gui.objectpanel.objectpanel import ObjectPanelNode USE_SSL = len(sys.argv) > 1 and sys.argv[1] == "True" -ATOSMINI_MODE = len(sys.argv) > 2 and sys.argv[2].lower() == "atosmini" +FLEET_MODE = len(sys.argv) > 2 and sys.argv[2].lower() == "atosfleetmanagement" @@ -27,15 +27,15 @@ def main() -> None: pass -def render_atosmini_pages() -> None: - nicegui.ui.link("ATOSMini Home", "/") +def render_atosfleetmanagement_pages() -> None: + nicegui.ui.link("ATOSFleetManagement Home", "/") @ui.page(path="/", title="TruckObjectGUI") def render_home() -> None: - ui.label("TruckObjectGUI (ATOSMini mode)").classes("text-h4") + ui.label("TruckObjectGUI (ATOSFleetManagement mode)").classes("text-h4") ui.markdown( """ -This GUI is running in **ATOSMini mode**. +This GUI is running in **ATOSFleetManagement mode**. Active runtime components: - `truck_object_control` @@ -58,8 +58,8 @@ def ros_main() -> None: executor = MultiThreadedExecutor() nodes = [] - if ATOSMINI_MODE: - render_atosmini_pages() + if FLEET_MODE: + render_atosfleetmanagement_pages() else: nicegui.ui.link("Control Panel", "/control") nicegui.ui.link("Config Panel", "/config") @@ -99,7 +99,7 @@ def print_access_hint() -> None: "uvicorn_reload_dirs": str(Path(__file__).parent.resolve()), "port": 3000, "show": False, - "title": "TruckObjectGUI" if ATOSMINI_MODE else "ATOS GUI", + "title": "TruckObjectGUI" if FLEET_MODE else "ATOS GUI", } if USE_SSL: From fd14786cd48fd36eaa9942f97de5f62242a147bb Mon Sep 17 00:00:00 2001 From: Sebastian Lindholm Date: Mon, 13 Apr 2026 14:57:24 +0200 Subject: [PATCH 04/33] Use atos fleet management instead of ATOSMini --- ...tManagement_architecture_and_dev_guide.pdf | Bin 4089 -> 4075 bytes ROS2_ATOSFleetManagement_cheatsheet.pdf | Bin 2548 -> 2525 bytes scripts/build_atosfleetmanagement.sh | 70 ++++++++++++++++++ 3 files changed, 70 insertions(+) create mode 100755 scripts/build_atosfleetmanagement.sh diff --git a/ATOSFleetManagement_architecture_and_dev_guide.pdf b/ATOSFleetManagement_architecture_and_dev_guide.pdf index f3629912224644778042e98f7a1675cc05d29fd7..2dfd6ee55d288fc5d910c01a3aac35dd05dd124e 100644 GIT binary patch delta 523 zcmajV%Z{2*007W5F==Yjbl=TH92HSSM3JiqN`b3`_@41G+mR!`ny1{Cwj;DJn_PAiQ97ib52uAJn%dQ8^$w;)pc`-V*@n zrSr_98xRFXUb2!+&4?)xY!@rJM5k))Ypw%`e$kOiHwGZP@0uApc^)i}Hl5yZ{v?|* z#?}*|d4fU%II}CL%qHAi!?YIerU;kHP@cqzavkZs(XH-xRw{MY-Nwob#cCEC`Jz4x z5!EB54MpaZCb_Fhe%)u1Y>mNECO!NSkB2Jzbe!QErhTtxlZHyZ9Lp3<1tVfRjl~`; z1e&6&x5n1Ec|R#vw#oh>Uc0JStxb27E#w6`>yDbR(HS;+E$HiVOC0DTXZg?1 z3|MDtqMFi_m&0hF?_?RqR_VPTc1;jy^GD8y!=3rPKi`2)Z2X|FoC~b@;lb{PQ}Xmw zRo~vf!cPjn0eQi~EQi(ti&>s8p)%r-h1fg>7;}>NzwsJ=`}u_zqKSmo)uVp_dMcTK delta 537 zcmajVOOBdg007`LF==Yj^av9lfC7pjf-oowf;b%kt&i#`3dr9XUdm&3O+3WhqKl@< zoTn$~31-)%7wGO=e1C4$9XU>uf zp|2IFLNgkY8;=d9b(y2{wdvvmMhFqTpHh+fBG868S;h(m$dsMIML zbX0gmE*jN?Uw5rysn$39%DlQ z7{lNPpxW3EcTNtxrw7Htgqm3nI8lh@BfKwZ^g+AA&s$)sL8p8UWfcVutDbG2JmS)P zbMRtmghc7C*#ksqPoBFg(DdsS@3ZI5;XIDw{0sICAR2hxlKlNuQ^0 z(WKWVeSzM-!}s^MzrOwP>&uTMMbY1Xdr|+xpYF>~KRhX>BnYLi$9?agOR8UZAA(?( z-yrKsX|L!;a~2Z7<=qFkrR+sMsx99`7$>+YL$4Dd%7cx=j`IyUGd&W4rbxA(v!r|j z*28M^d{DPYe`uwv8-aic?}PYok((ymUb=a$+zhqIc(!ek2_U)3hHGU~VGzx>c7!YE zK`OObADVj^ZjKf-r6U5vJU*D9E!~@MnJq(f5-CpNiTIo>Fcvg+NvIgUSlO!t3m7Lr z?KoM4oioMbfat(8Egc`ASnZ$~XEd}HYrNBpxMxABjAx;B!T78due>&%6|-$en=>3Y z#+#Zubz-~wATGUW^4Xr)N5|IyS@his`w-IXyaV3BO0-rFc&B~g4EdVtU%&#}MU#QH zj=Hm6&~lcc6Q37WSv|d>^J;uC5@mL021G4^IyYh)yOq20Y|(W5fy z`|vjH0BhPPr~3z%6T*DH##{E9dM-exB@n4whF%wqh7?+A-3;1EI#FHlBWJbU zSYamftps0uJIG>*yCp15(iS!0TDJ0LOW}@5KjUp`U&rhtZ8iqNjjvoc!>mu6Q4HPW ztnvykyugzZ9fy&qacFB7Z#&!${7>F_?g_5zALfCoKqGhy7Z6lU5mK`fUl6;ge0WTK zX;VDjyUr%x_bUxyd&Km|Yk3Y-HWr;(sV(@i(>_e-!deSV&C2{DIZ2Wn; zM2VPHxp-7waW?{9o~GS+HtOv+23$Xd?ryMre5Zf@=iMjhh@hw-Cg}a1;TSe#LvkPO cX`CgQh-L-;|Hk+0FTa;?jxE({vZ<8*1(_{5^Z)<= delta 1101 zcmaiz$*!sf06^2+HfeA27R~#BMj0GXnH3owE(#*fiV9ajWKtAGS#{w@ydP=O@9AGO z>9#NZf$q-YBxiO0`ug|RKfb*Eh|@Il?bo;dfAFXE_R|lq2KFwB?~iBdIhXgoliAaF zr8GiV>4JNiXU_Vn3$h?Ml$geH(}1LvL<2*tX$lAy4mfwFRwnsNde=3LukRwlkw{beIH`lZb75PXBqD>#+I-l4~p0gUu9mE{^P<-I951m*Lgyi90n~H zb1vgK?y*^+f_u8s6$npSc79pAuMVsT$TM|_ka!|aWVk(@%Op16HAAYw50b#tYD>_k zr_f0E3^gIt<)oMTbX-zC#|lZ?Wxgk@5mAT!~hXlqEi_c^E zA@?(n#6gs>AA{H#D9oLcOveq52@NTxNHLT=u zyb;~Rs}=(Eydf~9^0`%2LGxRU&Xshl^?;ZfRhLJ_*LGB}CCMKF0J&+x*#@_&T9&yI zaHD6p;1Djg^2#p4w*i_P zA;;};eE9~TOab}e_Rdk_QN_A={m`7S)8&o6MnaGut$Q-2KgXovJnsJBsfgRt?810p zZFprzC5au@V%?>z+|z8QqPp8;+E)U%GORgc_rx)&; zi=T}RFA8Mfc_Gm=5OFR}bvvD<`;tBy+dHAwZO@I@_I~PU)tP$S3Fx6{n#HEE-j91Z zEQ~jN+s3uWzS2(6umygic7fp#xj4&cB|GF*=+GKCrrqIFah%U_-|OdfbqZKiH73Hk zu#76yR`pT=cJ}*KxdJbmDU~bt#%$4u3?6JoVWwCIza_>!bB)J25?p*=s7DkdMXf1yF&ZPZRSztNs}E#PYBESlEbMSf*^TMlv?RHVagK(* z^X<2v|LOQNc_96R Date: Mon, 13 Apr 2026 17:08:52 +0200 Subject: [PATCH 05/33] simulation along path --- atos/launch/launch_atosfleetmanagement.py | 11 + atos_gui/atos_gui/main.py | 157 +- atos_gui/atos_gui/static/fleet_map.js | 240 + atos_gui/setup.py | 3 + ...ralRoad_center_of_driving_lane_ccw.geojson | 4643 +++++++++++++++++ 5 files changed, 5043 insertions(+), 11 deletions(-) create mode 100644 atos_gui/atos_gui/static/fleet_map.js create mode 100644 conf/conf/RuralRoad_center_of_driving_lane_ccw.geojson diff --git a/atos/launch/launch_atosfleetmanagement.py b/atos/launch/launch_atosfleetmanagement.py index 2efb71efd..b96fef9d1 100644 --- a/atos/launch/launch_atosfleetmanagement.py +++ b/atos/launch/launch_atosfleetmanagement.py @@ -15,9 +15,11 @@ def generate_launch_description(): insecure_websockets = LaunchConfiguration('insecure') foxbridge = LaunchConfiguration('foxbridge') + with_simulator = LaunchConfiguration('with_truck_simulator') insecure_launch_arg = DeclareLaunchArgument('insecure', default_value='False') foxbridge_launch_arg = DeclareLaunchArgument('foxbridge', default_value='True') + simulator_launch_arg = DeclareLaunchArgument('with_truck_simulator', default_value='False') fox_tls_bridge_params = [ {'port': 8765}, @@ -47,6 +49,7 @@ def generate_launch_description(): return LaunchDescription([ foxbridge_launch_arg, insecure_launch_arg, + simulator_launch_arg, Node( condition=IfCondition(PythonExpression(['not ', LaunchConfiguration('insecure')])), package='atos_gui', @@ -72,6 +75,14 @@ def generate_launch_description(): name='truck_object_control', output='screen', ), + Node( + condition=IfCondition(with_simulator), + package='atos', + namespace='atos', + executable='atos_truck_simulator', + name='atos_truck_simulator', + output='screen', + ), Node( condition=IfCondition(PythonExpression(['not ', foxbridge])), name='rosapi', diff --git a/atos_gui/atos_gui/main.py b/atos_gui/atos_gui/main.py index 82ff66187..48b926e18 100644 --- a/atos_gui/atos_gui/main.py +++ b/atos_gui/atos_gui/main.py @@ -3,6 +3,7 @@ file, You can obtain one at https://mozilla.org/MPL/2.0/. """ +import json import sys import threading from pathlib import Path @@ -11,6 +12,8 @@ import rclpy from nicegui import app, ui, ui_run from rclpy.executors import ExternalShutdownException, MultiThreadedExecutor +from rclpy.node import Node +from std_msgs.msg import String from atos_gui.configpanel.configpanel import ConfigPanelNode from atos_gui.controlpanel.controlpanel import ControlPanelNode @@ -18,6 +21,11 @@ USE_SSL = len(sys.argv) > 1 and sys.argv[1] == "True" FLEET_MODE = len(sys.argv) > 2 and sys.argv[2].lower() == "atosfleetmanagement" +GEOJSON_NAME = "RuralRoad_center_of_driving_lane_ccw.geojson" +FLEET_STATIC_ROUTE = "/atos_gui_static" +FLEET_STATIC_DIR = Path(__file__).parent / "static" +FLEET_STATE_LOCK = threading.Lock() +FLEET_TRUCK_STATES: dict[str, dict] = {} @@ -27,14 +35,86 @@ def main() -> None: pass +def _candidate_geojson_paths() -> list[Path]: + candidates = [ + Path.home() / ".astazero/ATOS/conf" / GEOJSON_NAME, + Path.home() / "atos_ws/src/atos/conf/conf" / GEOJSON_NAME, + Path.home() / "Documents/repos/ATOS/conf/conf" / GEOJSON_NAME, + ] + + try: + from ament_index_python.packages import get_package_prefix + + atos_prefix = Path(get_package_prefix("atos")) + candidates.append(atos_prefix / "etc/conf" / GEOJSON_NAME) + except Exception: + pass + + unique_candidates = [] + seen = set() + for path in candidates: + key = str(path) + if key in seen: + continue + seen.add(key) + unique_candidates.append(path) + + return unique_candidates + + +def _load_fleet_geojson() -> tuple[dict | None, Path | None]: + for path in _candidate_geojson_paths(): + if path.exists(): + try: + return json.loads(path.read_text()), path + except Exception: + continue + return None, None + + +def _fleet_snapshot_payload() -> str: + with FLEET_STATE_LOCK: + trucks = list(FLEET_TRUCK_STATES.values()) + return json.dumps(trucks).replace(" None: + super().__init__("truck_object_gui_state_bridge") + self._state_sub = self.create_subscription(String, "truck_objects/state", self._on_state, 100) + + def _on_state(self, msg: String) -> None: + try: + payload = json.loads(msg.data) + except Exception: + self.get_logger().warning("Failed to parse truck_objects/state payload as JSON") + return + + uid = payload.get("uid") + if not uid: + return + + with FLEET_STATE_LOCK: + FLEET_TRUCK_STATES[str(uid)] = payload + + def render_atosfleetmanagement_pages() -> None: - nicegui.ui.link("ATOSFleetManagement Home", "/") + if FLEET_STATIC_DIR.exists(): + app.add_static_files(FLEET_STATIC_ROUTE, str(FLEET_STATIC_DIR)) @ui.page(path="/", title="TruckObjectGUI") def render_home() -> None: + ui.add_head_html(f'') ui.label("TruckObjectGUI (ATOSFleetManagement mode)").classes("text-h4") - ui.markdown( - """ + + with ui.tabs().classes("w-full") as tabs: + home_tab = ui.tab("Overview", icon="dashboard") + road_tab = ui.tab("RuralRoad Map", icon="map") + + with ui.tab_panels(tabs, value=home_tab).classes("w-full"): + with ui.tab_panel(home_tab): + ui.markdown( + """ This GUI is running in **ATOSFleetManagement mode**. Active runtime components: @@ -44,13 +124,68 @@ def render_home() -> None: Expected COT topic: - `/atos/truck_objects/cot` +Expected COT TCP endpoint: +- `tcp://0.0.0.0:8114` (TruckObjectControl listener) + Speed command topic: - `/atos/truck_objects/speed_command` -Placeholder COT payload format: -- `id=;distance_m=;tcp_connected=<0|1>` - """.strip() - ) +Live truck state topic for map overlay: +- `/atos/truck_objects/state` + """.strip() + ) + + with ui.tab_panel(road_tab): + geojson, source_path = _load_fleet_geojson() + ui.label("RuralRoad centerline visualization").classes("text-h5") + + if not geojson: + ui.label("Could not load geojson file.").classes("text-red-600") + ui.markdown("Searched:\n" + "\n".join([f"- `{p}`" for p in _candidate_geojson_paths()])) + return + + ui.label(f"Source: {source_path}").classes("text-sm text-gray-600") + map_id = "rural-road-map" + ui.html(f'
') + + geojson_payload = json.dumps(geojson).replace(" {{ + const payload = {geojson_payload}; + const containerId = "{map_id}"; + let attempts = 0; + const maxAttempts = 60; + const timer = setInterval(() => {{ + attempts += 1; + const hasFn = typeof window.renderFleetRoadMap === "function"; + const hasEl = !!document.getElementById(containerId); + if (hasFn && hasEl) {{ + clearInterval(timer); + window.renderFleetRoadMap(containerId, payload, {_fleet_snapshot_payload()}); + return; + }} + if (attempts >= maxAttempts) {{ + clearInterval(timer); + const el = document.getElementById(containerId); + if (el) {{ + el.innerHTML = "
Map renderer not available. Check browser console.
"; + }} + }} + }}, 250); +}})(); + """.strip() + ), + once=True, + ) + ui.timer( + 0.5, + lambda: ui.run_javascript( + f"window.updateFleetTruckStates('{map_id}', {_fleet_snapshot_payload()});" + ), + ) def ros_main() -> None: @@ -60,6 +195,7 @@ def ros_main() -> None: if FLEET_MODE: render_atosfleetmanagement_pages() + nodes = [FleetStateNode()] else: nicegui.ui.link("Control Panel", "/control") nicegui.ui.link("Config Panel", "/config") @@ -70,12 +206,11 @@ def ros_main() -> None: object_panel = ObjectPanelNode() nodes = [control_panel, config_panel, object_panel] - for node in nodes: - executor.add_node(node) + for node in nodes: + executor.add_node(node) try: - if nodes: - executor.spin() + executor.spin() except ExternalShutdownException: pass finally: diff --git a/atos_gui/atos_gui/static/fleet_map.js b/atos_gui/atos_gui/static/fleet_map.js new file mode 100644 index 000000000..3ed8ff0ba --- /dev/null +++ b/atos_gui/atos_gui/static/fleet_map.js @@ -0,0 +1,240 @@ +(function() { + function toRad(value) { + return value * Math.PI / 180.0; + } + + // Constants aligned with atos/common/util.c: + // EARTH_EQUATOR_RADIUS_M = 6378137.0 and INVERSE_FLATTENING = 298.257223563 + function vincentyDistanceMeters(lat1, lon1, lat2, lon2) { + const a = 6378137.0; + const f = 1.0 / 298.257223563; + const b = (1.0 - f) * a; + + const phi1 = toRad(lat1); + const phi2 = toRad(lat2); + const L = toRad(lon2 - lon1); + + const U1 = Math.atan((1.0 - f) * Math.tan(phi1)); + const U2 = Math.atan((1.0 - f) * Math.tan(phi2)); + const sinU1 = Math.sin(U1); + const cosU1 = Math.cos(U1); + const sinU2 = Math.sin(U2); + const cosU2 = Math.cos(U2); + + let lambda = L; + let lambdaPrev = 0.0; + let iterLimit = 100; + let sinSigma = 0.0; + let cosSigma = 0.0; + let sigma = 0.0; + let sinAlpha = 0.0; + let cosSqAlpha = 0.0; + let cos2SigmaM = 0.0; + + while (iterLimit > 0) { + iterLimit -= 1; + const sinLambda = Math.sin(lambda); + const cosLambda = Math.cos(lambda); + sinSigma = Math.sqrt( + (cosU2 * sinLambda) * (cosU2 * sinLambda) + + (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda) * + (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda) + ); + + if (sinSigma === 0.0) { + return 0.0; + } + + cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda; + sigma = Math.atan2(sinSigma, cosSigma); + sinAlpha = (cosU1 * cosU2 * sinLambda) / sinSigma; + cosSqAlpha = 1.0 - sinAlpha * sinAlpha; + if (cosSqAlpha !== 0.0) { + cos2SigmaM = cosSigma - 2.0 * sinU1 * sinU2 / cosSqAlpha; + } else { + cos2SigmaM = 0.0; + } + + const C = f / 16.0 * cosSqAlpha * (4.0 + f * (4.0 - 3.0 * cosSqAlpha)); + lambdaPrev = lambda; + lambda = L + (1.0 - C) * f * sinAlpha * + (sigma + C * sinSigma * + (cos2SigmaM + C * cosSigma * (-1.0 + 2.0 * cos2SigmaM * cos2SigmaM))); + + if (Math.abs(lambda - lambdaPrev) < 1e-12) { + break; + } + } + + if (iterLimit <= 0) { + return Number.NaN; + } + + const uSq = cosSqAlpha * (a * a - b * b) / (b * b); + const A = 1.0 + uSq / 16384.0 * + (4096.0 + uSq * (-768.0 + uSq * (320.0 - 175.0 * uSq))); + const B = uSq / 1024.0 * + (256.0 + uSq * (-128.0 + uSq * (74.0 - 47.0 * uSq))); + const deltaSigma = B * sinSigma * + (cos2SigmaM + B / 4.0 * + (cosSigma * (-1.0 + 2.0 * cos2SigmaM * cos2SigmaM) - + B / 6.0 * cos2SigmaM * (-3.0 + 4.0 * sinSigma * sinSigma) * + (-3.0 + 4.0 * cos2SigmaM * cos2SigmaM))); + + return b * A * (sigma - deltaSigma); + } + + function findPathCoordinates(geojson) { + if (!geojson || !Array.isArray(geojson.features)) { + return []; + } + + const preferred = geojson.features.find(function(feature) { + return feature && feature.id === "PathSection" && + feature.geometry && feature.geometry.type === "LineString"; + }); + if (preferred && preferred.geometry && preferred.geometry.coordinates) { + return preferred.geometry.coordinates; + } + + const fallback = geojson.features.find(function(feature) { + return feature && feature.geometry && feature.geometry.type === "LineString"; + }); + if (fallback && fallback.geometry && fallback.geometry.coordinates) { + return fallback.geometry.coordinates; + } + + return []; + } + + function computeTotalLengthMeters(coords) { + if (!Array.isArray(coords) || coords.length < 2) { + return 0.0; + } + + let total = 0.0; + for (let index = 1; index < coords.length; index += 1) { + const previous = coords[index - 1]; + const current = coords[index]; + const distance = vincentyDistanceMeters(previous[1], previous[0], current[1], current[0]); + if (Number.isFinite(distance)) { + total += distance; + } + } + return total; + } + + function normalizeTruckStates(trucks) { + if (!Array.isArray(trucks)) { + return []; + } + return trucks.filter(function(item) { + return item && + Number.isFinite(Number(item.lat)) && + Number.isFinite(Number(item.lon)); + }); + } + + function renderSvg(container, coords, trucks) { + const width = Math.max(container.clientWidth, 700); + const height = Math.max(container.clientHeight, 420); + const padding = 30; + + let minLon = Infinity; + let maxLon = -Infinity; + let minLat = Infinity; + let maxLat = -Infinity; + + coords.forEach(function(item) { + const lon = item[0]; + const lat = item[1]; + if (lon < minLon) minLon = lon; + if (lon > maxLon) maxLon = lon; + if (lat < minLat) minLat = lat; + if (lat > maxLat) maxLat = lat; + }); + + const lonSpan = Math.max(maxLon - minLon, 1e-9); + const latSpan = Math.max(maxLat - minLat, 1e-9); + + function toSvgXY(lon, lat) { + const x = padding + ((lon - minLon) / lonSpan) * (width - 2 * padding); + const y = height - padding - ((lat - minLat) / latSpan) * (height - 2 * padding); + return {x: x, y: y}; + } + + const linePoints = coords.map(function(item) { + const p = toSvgXY(item[0], item[1]); + return p.x.toFixed(2) + "," + p.y.toFixed(2); + }).join(" "); + + const totalMeters = computeTotalLengthMeters(coords); + const totalKm = totalMeters / 1000.0; + + const truckCircles = trucks.map(function(item) { + const p = toSvgXY(Number(item.lon), Number(item.lat)); + const uid = String(item.uid || "truck"); + const speedKmh = Number(item.speed_kmh || 0); + const courseDeg = Number(item.course_deg || 0); + const color = item.tcp_connected ? "#dc2626" : "#6b7280"; + return ( + "" + + "" + + "" + + uid + " " + speedKmh.toFixed(1) + " km/h @" + courseDeg.toFixed(0) + "°" + + "" + + "" + ); + }).join(""); + + container.innerHTML = + "" + + "" + + truckCircles + + "" + + "
" + + "
Path points: " + coords.length + "
" + + "
Total length (Vincenty): " + totalMeters.toFixed(2) + " m
" + + "
Total length: " + totalKm.toFixed(3) + " km
" + + "
Live trucks: " + trucks.length + "
" + + "
"; + } + + function renderInternal(containerId) { + const container = document.getElementById(containerId); + if (!container) { + return; + } + const state = window.__fleetRoadMapState[containerId]; + if (!state) { + return; + } + + const coords = findPathCoordinates(state.geojson); + if (coords.length === 0) { + container.innerHTML = "
No LineString found in geojson.
"; + return; + } + renderSvg(container, coords, normalizeTruckStates(state.trucks)); + } + + window.__fleetRoadMapState = window.__fleetRoadMapState || {}; + + window.renderFleetRoadMap = function(containerId, geojson, trucks) { + window.__fleetRoadMapState[containerId] = { + geojson: geojson, + trucks: normalizeTruckStates(trucks), + }; + renderInternal(containerId); + }; + + window.updateFleetTruckStates = function(containerId, trucks) { + if (!window.__fleetRoadMapState[containerId]) { + return; + } + window.__fleetRoadMapState[containerId].trucks = normalizeTruckStates(trucks); + renderInternal(containerId); + }; +})(); diff --git a/atos_gui/setup.py b/atos_gui/setup.py index 2648aae69..55469add6 100644 --- a/atos_gui/setup.py +++ b/atos_gui/setup.py @@ -10,6 +10,9 @@ name=package_name, version=package_xml.find("version").text, packages=find_packages(), + package_data={ + package_name: ["images/*", "static/*.js"], + }, maintainer=package_xml.find("license").text, maintainer_email=package_xml.find("maintainer").attrib["email"], license=package_xml.find("license").text, diff --git a/conf/conf/RuralRoad_center_of_driving_lane_ccw.geojson b/conf/conf/RuralRoad_center_of_driving_lane_ccw.geojson new file mode 100644 index 000000000..2aa13164b --- /dev/null +++ b/conf/conf/RuralRoad_center_of_driving_lane_ccw.geojson @@ -0,0 +1,4643 @@ +{ + "type": "FeatureCollection", + "id": "rural_road_center_of_driving_lane_cw", + "features": [ + { + "type": "Feature", + "id": "Origin", + "geometry": { + "type": "Point", + "coordinates": [ + 12.763825093586895, + 57.78361108438431 + ] + } + }, + { + "type": "Feature", + "id": "PathSection", + "geometry": { + "type": "LineString", + "coordinates": [ + [ + 12.763825093586895, + 57.78361108438431 + ], + [ + 12.763869604990875, + 57.78357295286575 + ], + [ + 12.76391326219355, + 57.78353454115728 + ], + [ + 12.763957426455423, + 57.783496295831064 + ], + [ + 12.764001406854304, + 57.783457990144235 + ], + [ + 12.76404561722036, + 57.783419759991816 + ], + [ + 12.764089460317054, + 57.78338140921858 + ], + [ + 12.764133490705342, + 57.7833431185619 + ], + [ + 12.7641775697746, + 57.78330484154378 + ], + [ + 12.764221557741665, + 57.78326653501719 + ], + [ + 12.764265534513587, + 57.78322822522374 + ], + [ + 12.764309541579657, + 57.783189925821006 + ], + [ + 12.764353645584041, + 57.78315165876879 + ], + [ + 12.764397479995528, + 57.783113303543985 + ], + [ + 12.764441565953646, + 57.783075031975656 + ], + [ + 12.764485805076967, + 57.78303681170221 + ], + [ + 12.764529899891183, + 57.7829985447664 + ], + [ + 12.764574105643865, + 57.78296031519215 + ], + [ + 12.764618278450682, + 57.78292207559071 + ], + [ + 12.764662265333069, + 57.78288377565136 + ], + [ + 12.76470659250614, + 57.78284558837803 + ], + [ + 12.764750786794815, + 57.782807358131336 + ], + [ + 12.764794930780436, + 57.78276911187658 + ], + [ + 12.764839003585056, + 57.78273084131617 + ], + [ + 12.764882826978493, + 57.78269248779769 + ], + [ + 12.764926829320043, + 57.78265419173216 + ], + [ + 12.764970777491547, + 57.78261587676199 + ], + [ + 12.765014544920438, + 57.78257750148958 + ], + [ + 12.76505869619121, + 57.78253925079246 + ], + [ + 12.76510265604523, + 57.78250093636566 + ], + [ + 12.76514663529317, + 57.78246262726265 + ], + [ + 12.765190567193452, + 57.78242430162654 + ], + [ + 12.765234953073085, + 57.78238612399818 + ], + [ + 12.765279130996792, + 57.78234787731711 + ], + [ + 12.765323240209106, + 57.782309607189326 + ], + [ + 12.765367181758913, + 57.782271281668145 + ], + [ + 12.76541135327786, + 57.78223303583056 + ], + [ + 12.765455533138415, + 57.78219479727294 + ], + [ + 12.765499488426926, + 57.78215648914894 + ], + [ + 12.765543357603068, + 57.78211815720217 + ], + [ + 12.765587460850032, + 57.78207990670966 + ], + [ + 12.76563162443135, + 57.78204168001723 + ], + [ + 12.765675693555089, + 57.78200342593981 + ], + [ + 12.765719367234452, + 57.781965046308294 + ], + [ + 12.76576305784547, + 57.78192667542468 + ], + [ + 12.765806691928555, + 57.78188828857738 + ], + [ + 12.765849927629043, + 57.78184977946514 + ], + [ + 12.76589284717455, + 57.78181118003626 + ], + [ + 12.76593558062845, + 57.78177253375103 + ], + [ + 12.76597807242795, + 57.781733823975145 + ], + [ + 12.766020148451531, + 57.78169499748817 + ], + [ + 12.766061598484786, + 57.78165599211569 + ], + [ + 12.76610235965459, + 57.781616792430555 + ], + [ + 12.766142501147124, + 57.78157742182691 + ], + [ + 12.766182152652627, + 57.78153791816231 + ], + [ + 12.766221289718745, + 57.78149827078914 + ], + [ + 12.766259634456386, + 57.781458401906285 + ], + [ + 12.766297114553286, + 57.781418300970635 + ], + [ + 12.766333876604898, + 57.78137801499578 + ], + [ + 12.766369881371237, + 57.7813375397649 + ], + [ + 12.766404962626426, + 57.781296840590706 + ], + [ + 12.766438908886364, + 57.781255876191885 + ], + [ + 12.76647209519634, + 57.781214743741664 + ], + [ + 12.766504270808223, + 57.78117339115735 + ], + [ + 12.76653515522517, + 57.78113176687818 + ], + [ + 12.7665651168051, + 57.78108995914094 + ], + [ + 12.76659452905811, + 57.781048042600176 + ], + [ + 12.76662303466407, + 57.78100594278374 + ], + [ + 12.76665024168174, + 57.780963594255525 + ], + [ + 12.766676562373124, + 57.780921081965374 + ], + [ + 12.766701500147164, + 57.78087832897636 + ], + [ + 12.766725501947475, + 57.78083542135327 + ], + [ + 12.766748616577708, + 57.780792372384006 + ], + [ + 12.766770627169464, + 57.78074915621179 + ], + [ + 12.766792218321442, + 57.78070587543127 + ], + [ + 12.766812678531815, + 57.78066243410807 + ], + [ + 12.766831805480573, + 57.78061881826662 + ], + [ + 12.766850110762883, + 57.780575101519375 + ], + [ + 12.766866876596534, + 57.780531210948666 + ], + [ + 12.766882255437997, + 57.78048717980774 + ], + [ + 12.766897532490232, + 57.78044314379541 + ], + [ + 12.766911801745945, + 57.78039901166852 + ], + [ + 12.766924895472545, + 57.78035477630366 + ], + [ + 12.766937033961506, + 57.78031046374898 + ], + [ + 12.766947918686702, + 57.78026605900944 + ], + [ + 12.766957566303567, + 57.780221574136 + ], + [ + 12.766966540159679, + 57.78017705005956 + ], + [ + 12.766974519915616, + 57.78013247087012 + ], + [ + 12.766981697152248, + 57.780087850737395 + ], + [ + 12.766987334225957, + 57.780043167309096 + ], + [ + 12.766992329911035, + 57.77999841528871 + ], + [ + 12.7669961728862, + 57.77995352438256 + ], + [ + 12.766998499740245, + 57.779908604183305 + ], + [ + 12.76700017434802, + 57.77986375513407 + ], + [ + 12.767000490875628, + 57.7798189725322 + ], + [ + 12.766999949548161, + 57.77977419185406 + ], + [ + 12.766998677345864, + 57.77972941468014 + ], + [ + 12.766995796240113, + 57.77968465929169 + ], + [ + 12.766992666017751, + 57.77963990817603 + ], + [ + 12.766988025493704, + 57.779595194072215 + ], + [ + 12.766982283377624, + 57.779550516770705 + ], + [ + 12.766975746394163, + 57.77950586905005 + ], + [ + 12.766968399868736, + 57.779461253450336 + ], + [ + 12.76696047688341, + 57.77941665826751 + ], + [ + 12.766950904709525, + 57.77937215264088 + ], + [ + 12.766939463448974, + 57.77932768990905 + ], + [ + 12.766927031773257, + 57.77928324844121 + ], + [ + 12.7669134900977, + 57.77923889896582 + ], + [ + 12.76689933283628, + 57.7791946368232 + ], + [ + 12.766884434587311, + 57.77915053350407 + ], + [ + 12.766868502936092, + 57.779106540561635 + ], + [ + 12.766851413488189, + 57.77906267839872 + ], + [ + 12.766833413444237, + 57.77901892933629 + ], + [ + 12.766814177641802, + 57.77897534203104 + ], + [ + 12.766794082513748, + 57.7789318799302 + ], + [ + 12.766773345871155, + 57.778888516138416 + ], + [ + 12.766751553329385, + 57.77884530677036 + ], + [ + 12.766728796579525, + 57.77880223401071 + ], + [ + 12.766704886197074, + 57.77875933219503 + ], + [ + 12.766680045046215, + 57.778716572121766 + ], + [ + 12.766654107756032, + 57.77867398873547 + ], + [ + 12.766626419782291, + 57.77863171535208 + ], + [ + 12.766597874374021, + 57.77858960208095 + ], + [ + 12.766568153916992, + 57.77854771947236 + ], + [ + 12.766537519512198, + 57.77850602590333 + ], + [ + 12.766506663507005, + 57.7784643761067 + ], + [ + 12.766474352181945, + 57.77842304005362 + ], + [ + 12.766441318577455, + 57.77838186540534 + ], + [ + 12.766407555928327, + 57.778340857013134 + ], + [ + 12.766372625492089, + 57.77830012765996 + ], + [ + 12.766336668542891, + 57.77825965532387 + ], + [ + 12.766300268301006, + 57.77821928817295 + ], + [ + 12.766263149130202, + 57.77817908573195 + ], + [ + 12.766224905647727, + 57.77813916047989 + ], + [ + 12.766186307879307, + 57.77809930828466 + ], + [ + 12.766147535935026, + 57.77805947928181 + ], + [ + 12.76610862400696, + 57.778019664902246 + ], + [ + 12.766069620120385, + 57.777979854056646 + ], + [ + 12.766030702545782, + 57.77794000635167 + ], + [ + 12.765991437281725, + 57.77790026156987 + ], + [ + 12.765952309636738, + 57.77786048411182 + ], + [ + 12.765913266669129, + 57.777820688292245 + ], + [ + 12.76587415683342, + 57.77778091601065 + ], + [ + 12.76583474134854, + 57.77774123391578 + ], + [ + 12.765795760147823, + 57.77770143329714 + ], + [ + 12.765756486810169, + 57.77766171343552 + ], + [ + 12.765717055217445, + 57.77762202527001 + ], + [ + 12.765677850965647, + 57.777582259565655 + ], + [ + 12.765638532584775, + 57.777542513395424 + ], + [ + 12.76559980346394, + 57.777502589519315 + ], + [ + 12.76555993365045, + 57.77746297097388 + ], + [ + 12.765522178467865, + 57.777422752934136 + ], + [ + 12.765483685353546, + 57.777382728200024 + ], + [ + 12.765445202770753, + 57.77734268978897 + ], + [ + 12.765407947865953, + 57.777302314793175 + ], + [ + 12.765371392544859, + 57.7772617517112 + ], + [ + 12.765334908891754, + 57.77722116472382 + ], + [ + 12.765298605099614, + 57.77718052160295 + ], + [ + 12.765263607924133, + 57.777139543516775 + ], + [ + 12.765228487528292, + 57.77709858520017 + ], + [ + 12.76519513644151, + 57.77705720223811 + ], + [ + 12.765161615261801, + 57.77701585527619 + ], + [ + 12.765129332803696, + 57.77697422505929 + ], + [ + 12.765098226409068, + 57.77693233903226 + ], + [ + 12.76506785668889, + 57.77689030038837 + ], + [ + 12.765037356469893, + 57.77684828589248 + ], + [ + 12.765008194283306, + 57.77680599798954 + ], + [ + 12.764980022192447, + 57.77676351560418 + ], + [ + 12.764952627170414, + 57.776720885037875 + ], + [ + 12.764926140413227, + 57.7766780894085 + ], + [ + 12.764900954109654, + 57.77663506985845 + ], + [ + 12.764876618892504, + 57.77659191194258 + ], + [ + 12.76485290186406, + 57.77654865618268 + ], + [ + 12.764830349162292, + 57.776505222999155 + ], + [ + 12.764808403364368, + 57.776461701371645 + ], + [ + 12.764787300342707, + 57.77641805997034 + ], + [ + 12.764767402824358, + 57.77637425736969 + ], + [ + 12.764748599238091, + 57.77633031861294 + ], + [ + 12.764730276640371, + 57.77628632329882 + ], + [ + 12.764713170406935, + 57.77624218955199 + ], + [ + 12.764697041973317, + 57.77619795342327 + ], + [ + 12.764681499905548, + 57.776153658994886 + ], + [ + 12.764666861922992, + 57.77610927664189 + ], + [ + 12.764653032271845, + 57.77606481928664 + ], + [ + 12.764640684969255, + 57.776020237614155 + ], + [ + 12.764628053355295, + 57.775975674244904 + ], + [ + 12.764617011178078, + 57.775930989263195 + ], + [ + 12.764607432119014, + 57.77588620755714 + ], + [ + 12.764598063633292, + 57.77584141241624 + ], + [ + 12.764590465649738, + 57.77579652100272 + ], + [ + 12.764584223046443, + 57.775751571459715 + ], + [ + 12.764577937359492, + 57.77570662818657 + ], + [ + 12.764572300472347, + 57.77566165761715 + ], + [ + 12.764567278886638, + 57.77561666223021 + ], + [ + 12.76456431025704, + 57.77557161435029 + ], + [ + 12.764561592415237, + 57.77552656454408 + ], + [ + 12.764560260676452, + 57.77548149749557 + ], + [ + 12.76455973336343, + 57.77543642736747 + ], + [ + 12.76456017790282, + 57.77539135731339 + ], + [ + 12.764561525032565, + 57.775346291477305 + ], + [ + 12.764563840866575, + 57.77530123757356 + ], + [ + 12.764566623479473, + 57.77525619113421 + ], + [ + 12.764570370360357, + 57.775211163093665 + ], + [ + 12.76457581075555, + 57.77516618596817 + ], + [ + 12.764582041618047, + 57.775121240599944 + ], + [ + 12.764589159454104, + 57.775076335688205 + ], + [ + 12.76459705842993, + 57.7750314711853 + ], + [ + 12.76460611730902, + 57.77498666576564 + ], + [ + 12.764615984325943, + 57.774941906646426 + ], + [ + 12.764626336643143, + 57.77489717631626 + ], + [ + 12.764638444798035, + 57.77485256937612 + ], + [ + 12.764650552354267, + 57.7748079611217 + ], + [ + 12.764663948735942, + 57.774763452050635 + ], + [ + 12.764678197010234, + 57.77471901643978 + ], + [ + 12.764693798405036, + 57.77467471653223 + ], + [ + 12.764709798532248, + 57.77463046005962 + ], + [ + 12.764727029608588, + 57.774586340250025 + ], + [ + 12.764745110866633, + 57.77454232402224 + ], + [ + 12.764764399274503, + 57.7744984642862 + ], + [ + 12.764784773026925, + 57.77445474871684 + ], + [ + 12.764805695276445, + 57.7744111072754 + ], + [ + 12.764827238116734, + 57.77436754968651 + ], + [ + 12.764849446018136, + 57.774324081738555 + ], + [ + 12.764872817858734, + 57.774280779704426 + ], + [ + 12.764896812261266, + 57.77423770357922 + ], + [ + 12.764921940138228, + 57.774194820710214 + ], + [ + 12.764948296888736, + 57.774152012527594 + ], + [ + 12.764975625376332, + 57.77410937362588 + ], + [ + 12.765003985675223, + 57.77406693612339 + ], + [ + 12.765033000716862, + 57.77402463369228 + ], + [ + 12.765063056880425, + 57.77398254646352 + ], + [ + 12.765093630310925, + 57.77394057180454 + ], + [ + 12.765124929430872, + 57.773898755702085 + ], + [ + 12.765157009678427, + 57.7738571500852 + ], + [ + 12.765190185930328, + 57.77381586025222 + ], + [ + 12.765224239320403, + 57.77377476944434 + ], + [ + 12.765258747563953, + 57.77373378578466 + ], + [ + 12.765293786936502, + 57.77369292989298 + ], + [ + 12.765330226541227, + 57.77365242514152 + ], + [ + 12.765367705854503, + 57.77361219112755 + ], + [ + 12.765405457487917, + 57.77357203613442 + ], + [ + 12.765444298525464, + 57.77353199125536 + ], + [ + 12.76548432295945, + 57.77349228212309 + ], + [ + 12.765525069579573, + 57.773452799109805 + ], + [ + 12.765566462659804, + 57.77341351529537 + ], + [ + 12.765608963013703, + 57.77337457450914 + ], + [ + 12.765652446175363, + 57.773335948421824 + ], + [ + 12.765696718528782, + 57.7732975823001 + ], + [ + 12.765741678632894, + 57.77325944734898 + ], + [ + 12.765787431852702, + 57.773221584834396 + ], + [ + 12.765833703075304, + 57.77318390432087 + ], + [ + 12.76588082410231, + 57.773146527961075 + ], + [ + 12.765928806600995, + 57.773109464982674 + ], + [ + 12.765977370752443, + 57.77307261748496 + ], + [ + 12.766026781094418, + 57.77303609127473 + ], + [ + 12.766077107691032, + 57.77299992342417 + ], + [ + 12.766128424770878, + 57.77296415532568 + ], + [ + 12.766180365022974, + 57.772928646394234 + ], + [ + 12.766232918433774, + 57.772893395686026 + ], + [ + 12.766286008574085, + 57.772858374191216 + ], + [ + 12.766339741276184, + 57.77282363112053 + ], + [ + 12.766394208254408, + 57.77278921345703 + ], + [ + 12.766449572849366, + 57.77275520577525 + ], + [ + 12.766505638277662, + 57.772721526875095 + ], + [ + 12.766562553123492, + 57.7726882573986 + ], + [ + 12.766620037019246, + 57.772655270160016 + ], + [ + 12.766678054335767, + 57.772622550422106 + ], + [ + 12.76673678695227, + 57.772590196493695 + ], + [ + 12.766795987726224, + 57.77255808474227 + ], + [ + 12.76685610932523, + 57.77252646530835 + ], + [ + 12.766916833752221, + 57.77249517724325 + ], + [ + 12.766978485392922, + 57.77246441446919 + ], + [ + 12.767040762503198, + 57.77243401853392 + ], + [ + 12.767103485869567, + 57.77240388857846 + ], + [ + 12.76716678692756, + 57.772374105845245 + ], + [ + 12.767230506373428, + 57.77234457673797 + ], + [ + 12.767295185363887, + 57.772315648488586 + ], + [ + 12.767360089893643, + 57.772286864071965 + ], + [ + 12.767425995195333, + 57.772258731391 + ], + [ + 12.767492197387107, + 57.77223079730472 + ], + [ + 12.767559316396207, + 57.77220349341198 + ], + [ + 12.767626675779914, + 57.772176364361634 + ], + [ + 12.767694541201243, + 57.77214959924045 + ], + [ + 12.767763077178046, + 57.77212332860877 + ], + [ + 12.767832127474314, + 57.772097448685955 + ], + [ + 12.767901639646594, + 57.772071925517906 + ], + [ + 12.767971525082256, + 57.77204669100574 + ], + [ + 12.768042009616012, + 57.77202192964782 + ], + [ + 12.768113178379897, + 57.77199773237698 + ], + [ + 12.768185066312604, + 57.77197415289877 + ], + [ + 12.76825740308934, + 57.771950971681726 + ], + [ + 12.768330407543962, + 57.771928397961496 + ], + [ + 12.768403640761711, + 57.77190603982785 + ], + [ + 12.768477244847556, + 57.771884030185575 + ], + [ + 12.76855129113822, + 57.77186244706609 + ], + [ + 12.768625708869907, + 57.77184123206143 + ], + [ + 12.768700483471552, + 57.77182038062208 + ], + [ + 12.76877576126462, + 57.771800056032674 + ], + [ + 12.768851461427104, + 57.771780182783615 + ], + [ + 12.76892763273769, + 57.771760829553514 + ], + [ + 12.769004308085915, + 57.77174205327659 + ], + [ + 12.76908127318242, + 57.77172361906315 + ], + [ + 12.769158603210467, + 57.771705621520425 + ], + [ + 12.769236305333012, + 57.771688081490225 + ], + [ + 12.76931432240206, + 57.77167094122568 + ], + [ + 12.769392733545907, + 57.77165431972075 + ], + [ + 12.7694715330724, + 57.77163823189063 + ], + [ + 12.769550536867238, + 57.77162243292376 + ], + [ + 12.769629966229624, + 57.77160725619568 + ], + [ + 12.769709679884025, + 57.7715925160643 + ], + [ + 12.769789674656023, + 57.77157822198577 + ], + [ + 12.769870029807837, + 57.77156452101242 + ], + [ + 12.769950595780198, + 57.77155117927644 + ], + [ + 12.770031438121052, + 57.771538320562875 + ], + [ + 12.770112570249793, + 57.77152599047079 + ], + [ + 12.770194019787223, + 57.7715142716768 + ], + [ + 12.770275647954946, + 57.77150291176349 + ], + [ + 12.770357536553616, + 57.77149209673517 + ], + [ + 12.770439590800992, + 57.771481641104735 + ], + [ + 12.770521836157487, + 57.77147161421457 + ], + [ + 12.770604303627769, + 57.77146211543379 + ], + [ + 12.770687024545653, + 57.771453267935854 + ], + [ + 12.770769952642382, + 57.77144499611156 + ], + [ + 12.770852992708395, + 57.77143705854853 + ], + [ + 12.770936215403806, + 57.77142969321952 + ], + [ + 12.771019551646374, + 57.77142271609099 + ], + [ + 12.7711029708595, + 57.771416029327646 + ], + [ + 12.77118653643765, + 57.77140988669155 + ], + [ + 12.771270266486896, + 57.77140443253271 + ], + [ + 12.771354084454117, + 57.771399398282995 + ], + [ + 12.771437978031052, + 57.771394744883814 + ], + [ + 12.77152201181441, + 57.77139089783265 + ], + [ + 12.77160608625195, + 57.77138734324094 + ], + [ + 12.77169021073359, + 57.771384235455656 + ], + [ + 12.77177440287452, + 57.771381868129104 + ], + [ + 12.771858612312542, + 57.7713798606187 + ], + [ + 12.771942817090235, + 57.77137790399914 + ], + [ + 12.772027055493552, + 57.77137660889622 + ], + [ + 12.77211128649618, + 57.771375419087285 + ], + [ + 12.772195521113481, + 57.77137477062636 + ], + [ + 12.772279732882483, + 57.77137410149296 + ], + [ + 12.77236392986367, + 57.77137377915346 + ], + [ + 12.772448109385367, + 57.77137340314209 + ], + [ + 12.772532275404076, + 57.771373313090294 + ], + [ + 12.772616426055311, + 57.77137346430706 + ], + [ + 12.77270056128031, + 57.77137373566584 + ], + [ + 12.772784682535505, + 57.771374019131066 + ], + [ + 12.772868789625681, + 57.77137439304765 + ], + [ + 12.772952884536895, + 57.771374749807656 + ], + [ + 12.77303697777643, + 57.7713751525673 + ], + [ + 12.773121073813755, + 57.77137565937934 + ], + [ + 12.773205175945995, + 57.7713760098742 + ], + [ + 12.77328928262298, + 57.77137634103743 + ], + [ + 12.773373391855266, + 57.77137679195601 + ], + [ + 12.77345750703711, + 57.77137715407886 + ], + [ + 12.773541627287113, + 57.77137751434133 + ], + [ + 12.773625750548563, + 57.771377989134805 + ], + [ + 12.773709876951147, + 57.771378564368405 + ], + [ + 12.77379400801251, + 57.771379182157645 + ], + [ + 12.773878143534642, + 57.77137986472204 + ], + [ + 12.773962288991841, + 57.77138062439621 + ], + [ + 12.774046449830943, + 57.77138145876656 + ], + [ + 12.774130627035145, + 57.771382316450534 + ], + [ + 12.774214795182987, + 57.771383665295325 + ], + [ + 12.774298966277232, + 57.77138517575388 + ], + [ + 12.77438312704849, + 57.77138701809158 + ], + [ + 12.774467273806614, + 57.771389187603525 + ], + [ + 12.774551409867271, + 57.77139162902864 + ], + [ + 12.774635538101338, + 57.77139433361965 + ], + [ + 12.774719666774407, + 57.771397234203704 + ], + [ + 12.774803736705234, + 57.77140070974403 + ], + [ + 12.774887771255033, + 57.77140452748652 + ], + [ + 12.77497169493373, + 57.77140905791163 + ], + [ + 12.775055484341694, + 57.77141430097888 + ], + [ + 12.77513907707188, + 57.77142012520245 + ], + [ + 12.775222307665757, + 57.771426548264635 + ], + [ + 12.77530549273549, + 57.7714332010418 + ], + [ + 12.775388378500928, + 57.77144083231828 + ], + [ + 12.775471067276163, + 57.7714490515333 + ], + [ + 12.775553549668846, + 57.77145784306243 + ], + [ + 12.77563579706634, + 57.771467414872795 + ], + [ + 12.775718027743663, + 57.77147772569187 + ], + [ + 12.77579988093308, + 57.771488849449405 + ], + [ + 12.77588146822149, + 57.77150050722104 + ], + [ + 12.77596265659458, + 57.771512928081 + ], + [ + 12.77604354024669, + 57.77152589853144 + ], + [ + 12.776124021663248, + 57.77153956314057 + ], + [ + 12.776204000051628, + 57.77155404330066 + ], + [ + 12.776283785307985, + 57.77156883228801 + ], + [ + 12.77636310889683, + 57.7715843259399 + ], + [ + 12.776441744517937, + 57.771600792420365 + ], + [ + 12.776519840045003, + 57.77161797314121 + ], + [ + 12.77659737876555, + 57.77163585269551 + ], + [ + 12.776674362525405, + 57.771654395180086 + ], + [ + 12.776751076356827, + 57.77167325016839 + ], + [ + 12.776827246175738, + 57.77169272788194 + ], + [ + 12.776902823696302, + 57.771712855454425 + ], + [ + 12.776977793419428, + 57.77173362245821 + ], + [ + 12.777052414019623, + 57.77175475236041 + ], + [ + 12.77712603155524, + 57.77177686472118 + ], + [ + 12.777199282684396, + 57.77179932303827 + ], + [ + 12.77727142811115, + 57.77182278094687 + ], + [ + 12.777343158449511, + 57.77184659487394 + ], + [ + 12.777414010500596, + 57.77187114830082 + ], + [ + 12.77748429078751, + 57.77189616639169 + ], + [ + 12.77755368759311, + 57.77192188000302 + ], + [ + 12.777622243414179, + 57.77194822730252 + ], + [ + 12.77769002657633, + 57.771975134980394 + ], + [ + 12.777756778830195, + 57.77200276392108 + ], + [ + 12.777823267123138, + 57.77203056873733 + ], + [ + 12.777889161181381, + 57.77205878254836 + ], + [ + 12.777954045827231, + 57.772087663128985 + ], + [ + 12.778018090608127, + 57.77211708136212 + ], + [ + 12.77808101056586, + 57.77214718703165 + ], + [ + 12.77814294066376, + 57.772177872424535 + ], + [ + 12.778204091198822, + 57.772209001008555 + ], + [ + 12.778264202021798, + 57.77224070190335 + ], + [ + 12.778323515167743, + 57.77227282822829 + ], + [ + 12.778381876754459, + 57.77230544726188 + ], + [ + 12.778439243242339, + 57.77233856122763 + ], + [ + 12.778495518647528, + 57.77237219660351 + ], + [ + 12.778550788674586, + 57.77240629794685 + ], + [ + 12.778605031398154, + 57.772440861335376 + ], + [ + 12.778658146322215, + 57.77247591487525 + ], + [ + 12.778710673989348, + 57.772511219300654 + ], + [ + 12.778762245120697, + 57.77254694320722 + ], + [ + 12.778813261109597, + 57.77258291061316 + ], + [ + 12.77886281466082, + 57.77261947262795 + ], + [ + 12.77891098063406, + 57.772656575350865 + ], + [ + 12.778957966183757, + 57.77269409939732 + ], + [ + 12.779003639424827, + 57.77273206595663 + ], + [ + 12.779048376795092, + 57.772770335537395 + ], + [ + 12.779091857949089, + 57.77280899956988 + ], + [ + 12.779134876632433, + 57.772847797735 + ], + [ + 12.779176464494885, + 57.772887046556235 + ], + [ + 12.779216909129024, + 57.77292664310098 + ], + [ + 12.779256275114134, + 57.77296655744342 + ], + [ + 12.779294408796481, + 57.773006823771766 + ], + [ + 12.779330840061355, + 57.773047541782205 + ], + [ + 12.779366064750082, + 57.7730885565706 + ], + [ + 12.779400369261937, + 57.77312978894489 + ], + [ + 12.77943367435043, + 57.773171253036395 + ], + [ + 12.77946598373449, + 57.77321294201596 + ], + [ + 12.779496886855396, + 57.77325493483536 + ], + [ + 12.779527031270813, + 57.77329708998629 + ], + [ + 12.779554862704169, + 57.773339695581114 + ], + [ + 12.77958172618852, + 57.7733824750061 + ], + [ + 12.779606654512945, + 57.77342558454037 + ], + [ + 12.779630915688745, + 57.77346879678464 + ], + [ + 12.779653711174573, + 57.773512234530315 + ], + [ + 12.77967591882718, + 57.77355575993052 + ], + [ + 12.77969662139476, + 57.773599498593114 + ], + [ + 12.779716048675509, + 57.7736434052563 + ], + [ + 12.779734304540643, + 57.773687457301484 + ], + [ + 12.779751321570703, + 57.773731653257165 + ], + [ + 12.779766630069021, + 57.77377602761215 + ], + [ + 12.779780656609192, + 57.77382052120753 + ], + [ + 12.779793358797855, + 57.77386512633681 + ], + [ + 12.779804910036432, + 57.77390981888798 + ], + [ + 12.779814858508047, + 57.773954619382295 + ], + [ + 12.779824021540392, + 57.77399946551659 + ], + [ + 12.779832362595256, + 57.77404436099548 + ], + [ + 12.779838774105292, + 57.77408934786981 + ], + [ + 12.779844054619153, + 57.774134377161396 + ], + [ + 12.779848049180965, + 57.77417944589067 + ], + [ + 12.779850359514496, + 57.774224548916415 + ], + [ + 12.77985150802352, + 57.77426966481883 + ], + [ + 12.779851046005774, + 57.774314784075074 + ], + [ + 12.779849366169053, + 57.77435989340205 + ], + [ + 12.779846444080231, + 57.77440498371698 + ], + [ + 12.779842272421876, + 57.77445004559278 + ], + [ + 12.779836872928035, + 57.77449507044457 + ], + [ + 12.77983014094051, + 57.77454004442495 + ], + [ + 12.7798220920945, + 57.77458495681038 + ], + [ + 12.779812576842241, + 57.77462978676884 + ], + [ + 12.779802372698583, + 57.77467457573621 + ], + [ + 12.779790698409897, + 57.77471926504619 + ], + [ + 12.779776942395786, + 57.77476378442081 + ], + [ + 12.779762315146899, + 57.7748082220043 + ], + [ + 12.779746179211887, + 57.77485250951704 + ], + [ + 12.779728814102162, + 57.77489665799973 + ], + [ + 12.779710182153927, + 57.77494064779227 + ], + [ + 12.779690804007748, + 57.774984536935385 + ], + [ + 12.779670078260684, + 57.775028240118104 + ], + [ + 12.779648997990646, + 57.77507188482301 + ], + [ + 12.779626089618635, + 57.77511525225323 + ], + [ + 12.779602683289246, + 57.7751585310187 + ], + [ + 12.779578198697942, + 57.775201628424576 + ], + [ + 12.779553053967748, + 57.77524460524086 + ], + [ + 12.779527477543343, + 57.775287496501846 + ], + [ + 12.779502134208002, + 57.77533041678369 + ], + [ + 12.779475625387773, + 57.775373124176085 + ], + [ + 12.77944919099785, + 57.77541583414943 + ], + [ + 12.779422309545273, + 57.77545845366719 + ], + [ + 12.779395310017817, + 57.77550103491977 + ], + [ + 12.779368146712885, + 57.77554356254465 + ], + [ + 12.779341130097585, + 57.775586116852445 + ], + [ + 12.779314263834497, + 57.77562870032595 + ], + [ + 12.779287252401579, + 57.775671264778126 + ], + [ + 12.77926016238024, + 57.77571381443399 + ], + [ + 12.779233331422512, + 57.77575640996979 + ], + [ + 12.77920650622253, + 57.77579900611393 + ], + [ + 12.779179312930829, + 57.775841535466874 + ], + [ + 12.779152487103103, + 57.77588413065709 + ], + [ + 12.779125484403266, + 57.77592669351667 + ], + [ + 12.779098614976172, + 57.77596928014614 + ], + [ + 12.779071534586295, + 57.776011828143474 + ], + [ + 12.779044420669528, + 57.77605436947154 + ], + [ + 12.779017362055413, + 57.77609692026657 + ], + [ + 12.77899019959765, + 57.776139451567985 + ], + [ + 12.778963082398546, + 57.77618199048854 + ], + [ + 12.778936049258816, + 57.77622454410156 + ], + [ + 12.77890899758052, + 57.776267093688176 + ], + [ + 12.778881741974063, + 57.77630960539159 + ], + [ + 12.7788545718639, + 57.77635213197538 + ], + [ + 12.778827700556668, + 57.77639471232875 + ], + [ + 12.778800687740198, + 57.77643726608284 + ], + [ + 12.778773661070009, + 57.776479816492774 + ], + [ + 12.778746407991516, + 57.776522325145464 + ], + [ + 12.778719367406254, + 57.77656487259993 + ], + [ + 12.778692360324007, + 57.77660742627593 + ], + [ + 12.77866513053154, + 57.77664993959388 + ], + [ + 12.778637915066192, + 57.77669245576075 + ], + [ + 12.778610892599271, + 57.77673500731439 + ], + [ + 12.778583591295872, + 57.77677750847549 + ], + [ + 12.778556442203165, + 57.77682003768679 + ], + [ + 12.778529380292158, + 57.77686258315968 + ], + [ + 12.778502377131042, + 57.776905139768886 + ], + [ + 12.778475319350663, + 57.77694768694738 + ], + [ + 12.77844832117381, + 57.77699024551377 + ], + [ + 12.77842132929062, + 57.777032805823886 + ], + [ + 12.77839423624596, + 57.777075348375256 + ], + [ + 12.778367085581653, + 57.77711788112557 + ], + [ + 12.778339933170862, + 57.77716041428465 + ], + [ + 12.778312743510895, + 57.77720294143117 + ], + [ + 12.778285494509143, + 57.777245457731006 + ], + [ + 12.77825821685125, + 57.77728796463342 + ], + [ + 12.778231310655972, + 57.777330534026895 + ], + [ + 12.778204304262424, + 57.77737307959808 + ], + [ + 12.778177585730123, + 57.7774156712179 + ], + [ + 12.778150681678497, + 57.77745822315899 + ], + [ + 12.7781243196242, + 57.777500865593495 + ], + [ + 12.778098080767805, + 57.77754352281711 + ], + [ + 12.778071849128983, + 57.77758617465394 + ], + [ + 12.778045966501658, + 57.77762888113337 + ], + [ + 12.778020227606033, + 57.77767160673715 + ], + [ + 12.777994740897446, + 57.777714372119725 + ], + [ + 12.777969497780383, + 57.777757181144125 + ], + [ + 12.777944529276493, + 57.77780003928675 + ], + [ + 12.777919788430745, + 57.7778429385999 + ], + [ + 12.777895623503857, + 57.77788593415639 + ], + [ + 12.777871941831958, + 57.777929008987485 + ], + [ + 12.777848402739126, + 57.77797210851939 + ], + [ + 12.777825401757976, + 57.778015293300925 + ], + [ + 12.777802396535971, + 57.778058479192254 + ], + [ + 12.77777926362983, + 57.77810164839704 + ], + [ + 12.777757063142433, + 57.77814495864764 + ], + [ + 12.777734612301087, + 57.77818823287204 + ], + [ + 12.777712480506896, + 57.77823155415312 + ], + [ + 12.777690077230957, + 57.77827483585973 + ], + [ + 12.777668317885034, + 57.77831820977721 + ], + [ + 12.777646674934863, + 57.778361599870706 + ], + [ + 12.777625937782531, + 57.77840511588409 + ], + [ + 12.777605090275795, + 57.77844861524421 + ], + [ + 12.777584719670587, + 57.77849217879251 + ], + [ + 12.777564213700037, + 57.778535722735825 + ], + [ + 12.777543886283246, + 57.778579289809045 + ], + [ + 12.777524046279863, + 57.77862292006847 + ], + [ + 12.77750468585809, + 57.77866663695523 + ], + [ + 12.777485546834596, + 57.77871039310265 + ], + [ + 12.777466465003647, + 57.778754156692 + ], + [ + 12.777447829069096, + 57.77879797503893 + ], + [ + 12.777429341487577, + 57.77884181132396 + ], + [ + 12.777411111791025, + 57.77888567830164 + ], + [ + 12.777393329008943, + 57.778929608209175 + ], + [ + 12.777375713151864, + 57.778973560736596 + ], + [ + 12.777358408107398, + 57.77901754119288 + ], + [ + 12.777341459421923, + 57.779061554322915 + ], + [ + 12.777324505210553, + 57.77910556837118 + ], + [ + 12.777308255827643, + 57.77914965742645 + ], + [ + 12.777292193327112, + 57.77919370637156 + ], + [ + 12.777276679675465, + 57.779237780144726 + ], + [ + 12.777261043342316, + 57.779281858115574 + ], + [ + 12.777245727175025, + 57.77932598437307 + ], + [ + 12.7772308992667, + 57.779370174216496 + ], + [ + 12.777215938010007, + 57.77941436724149 + ], + [ + 12.77720151417386, + 57.77945861239074 + ], + [ + 12.777187255359866, + 57.779502868716946 + ], + [ + 12.777173430650581, + 57.77954715997555 + ], + [ + 12.777159821189395, + 57.77959146568048 + ], + [ + 12.777146201470371, + 57.77963576637076 + ], + [ + 12.777133241887514, + 57.77968012001261 + ], + [ + 12.77712051175529, + 57.779724493178165 + ], + [ + 12.777107877382157, + 57.77976887606738 + ], + [ + 12.777095671942456, + 57.77981329520801 + ], + [ + 12.777083496477562, + 57.77985771892806 + ], + [ + 12.777071950005537, + 57.77990219248352 + ], + [ + 12.777060297664171, + 57.7799466599302 + ], + [ + 12.77704880237779, + 57.77999113875093 + ], + [ + 12.777037900490093, + 57.780035657922014 + ], + [ + 12.777027192254087, + 57.780080188069284 + ], + [ + 12.77701682601549, + 57.780124738824256 + ], + [ + 12.7770066835849, + 57.78016930179835 + ], + [ + 12.776996793289753, + 57.78021387871125 + ], + [ + 12.776987424399117, + 57.7802584877497 + ], + [ + 12.776978025090507, + 57.7803031003644 + ], + [ + 12.776969377766587, + 57.78034776198708 + ], + [ + 12.776960869253486, + 57.78039243648244 + ], + [ + 12.776952368572484, + 57.780437116893715 + ], + [ + 12.77694411569819, + 57.78048181603329 + ], + [ + 12.776936128664085, + 57.78052653410299 + ], + [ + 12.776928289309113, + 57.78057126244626 + ], + [ + 12.77692040092486, + 57.78061598996946 + ], + [ + 12.776912321099267, + 57.78066070968135 + ], + [ + 12.77690440465305, + 57.780705439716016 + ], + [ + 12.776896548216811, + 57.78075017479845 + ], + [ + 12.77688894204078, + 57.780794924474286 + ], + [ + 12.77688068849382, + 57.7808396437259 + ], + [ + 12.77687285096592, + 57.78088438574808 + ], + [ + 12.7768649715612, + 57.780929125550465 + ], + [ + 12.776856892452516, + 57.780973854585895 + ], + [ + 12.77684876122238, + 57.78101858036169 + ], + [ + 12.776840732639638, + 57.78106331079086 + ], + [ + 12.776832479620118, + 57.781108029440965 + ], + [ + 12.77682449414065, + 57.78115276126359 + ], + [ + 12.776816688167843, + 57.781197501870416 + ], + [ + 12.776808705124605, + 57.78124223335891 + ], + [ + 12.776800765951897, + 57.781286970499394 + ], + [ + 12.77679268757441, + 57.781331708298325 + ], + [ + 12.776784837121786, + 57.781376464934915 + ], + [ + 12.776776488955297, + 57.78142120279522 + ], + [ + 12.776768280650368, + 57.78146595459302 + ], + [ + 12.776759755574123, + 57.78151069626775 + ], + [ + 12.776750883624782, + 57.781555425697306 + ], + [ + 12.776741651900325, + 57.78160014188688 + ], + [ + 12.776732343621106, + 57.78164485999096 + ], + [ + 12.776722257109695, + 57.781689531332844 + ], + [ + 12.77671177343911, + 57.78173417763949 + ], + [ + 12.77670109700981, + 57.7817788123481 + ], + [ + 12.77668972333689, + 57.78182339876433 + ], + [ + 12.776678021784326, + 57.78186796171676 + ], + [ + 12.776665755698566, + 57.78191248194407 + ], + [ + 12.776653154328722, + 57.7819569760123 + ], + [ + 12.776640225654107, + 57.78200144418786 + ], + [ + 12.776626738443484, + 57.78204586499682 + ], + [ + 12.77661264011464, + 57.78209023114784 + ], + [ + 12.77659843098979, + 57.782134587089466 + ], + [ + 12.776583605706753, + 57.78217888520664 + ], + [ + 12.776568233151218, + 57.78222312951386 + ], + [ + 12.776552633107181, + 57.78226735064647 + ], + [ + 12.77653614497159, + 57.782311478161816 + ], + [ + 12.77651972887298, + 57.78235561020978 + ], + [ + 12.776502872279188, + 57.782399692764145 + ], + [ + 12.776485139191353, + 57.78244367491299 + ], + [ + 12.776467073810725, + 57.78248761518066 + ], + [ + 12.77644903346231, + 57.78253155507215 + ], + [ + 12.776430540553353, + 57.78257544091364 + ], + [ + 12.77641188604907, + 57.782619311704195 + ], + [ + 12.776393095820813, + 57.782663171434024 + ], + [ + 12.77637359415752, + 57.78270694753546 + ], + [ + 12.776353535613096, + 57.78275065489374 + ], + [ + 12.776333043582259, + 57.7827943045692 + ], + [ + 12.77631215640239, + 57.78283790046849 + ], + [ + 12.776291028812585, + 57.78288146339444 + ], + [ + 12.77626911776674, + 57.78292491546374 + ], + [ + 12.776246965304548, + 57.7829683317937 + ], + [ + 12.776224191659054, + 57.78301165530681 + ], + [ + 12.77620105393371, + 57.783054922268526 + ], + [ + 12.776177669979246, + 57.7830981505776 + ], + [ + 12.776153812418048, + 57.783141304425556 + ], + [ + 12.776129446033835, + 57.78318437640247 + ], + [ + 12.776104726942375, + 57.78322739024107 + ], + [ + 12.776079735315978, + 57.78327035865128 + ], + [ + 12.776054355634026, + 57.78331326171203 + ], + [ + 12.776028522676153, + 57.78335608736258 + ], + [ + 12.776002390451705, + 57.783398861763054 + ], + [ + 12.775975905045945, + 57.7834415748268 + ], + [ + 12.77594891023299, + 57.78348419718499 + ], + [ + 12.775921393259356, + 57.783526724259225 + ], + [ + 12.775893498588825, + 57.78356918093963 + ], + [ + 12.775865321545973, + 57.783611584438624 + ], + [ + 12.775836682065165, + 57.783653899598846 + ], + [ + 12.775807689301113, + 57.78369614621661 + ], + [ + 12.775778471515286, + 57.78373834927183 + ], + [ + 12.775748422204522, + 57.78378038569933 + ], + [ + 12.775718089020595, + 57.783822363100526 + ], + [ + 12.775687301560048, + 57.783864245429875 + ], + [ + 12.775656333858153, + 57.783906089351134 + ], + [ + 12.77562510911029, + 57.7839478790826 + ], + [ + 12.775593226731617, + 57.783989526881506 + ], + [ + 12.775561018774093, + 57.78403110286583 + ], + [ + 12.775528650429983, + 57.7840726436273 + ], + [ + 12.77549591488736, + 57.784114102770715 + ], + [ + 12.775462489075233, + 57.784155404385466 + ], + [ + 12.77542892559342, + 57.784196674021466 + ], + [ + 12.77539468742995, + 57.78423778511176 + ], + [ + 12.775360048058081, + 57.784278800097816 + ], + [ + 12.77532547223485, + 57.784319830925085 + ], + [ + 12.77529046801669, + 57.78436075889968 + ], + [ + 12.77525490849515, + 57.78440155051341 + ], + [ + 12.775218847510459, + 57.78444221647918 + ], + [ + 12.775182441136254, + 57.78448279450397 + ], + [ + 12.775145840885216, + 57.784523323218615 + ], + [ + 12.775108737449594, + 57.784563721734266 + ], + [ + 12.775071010518573, + 57.78460395453319 + ], + [ + 12.77503471091665, + 57.78464457239655 + ], + [ + 12.775001974300425, + 57.78468617761267 + ], + [ + 12.774963364932184, + 57.78472619043032 + ], + [ + 12.774923955516662, + 57.784765978928405 + ], + [ + 12.774884550488167, + 57.784805765942714 + ], + [ + 12.774844906587205, + 57.78484548408661 + ], + [ + 12.774804735986416, + 57.78488505453944 + ], + [ + 12.774764419642613, + 57.78492458758651 + ], + [ + 12.774723477621873, + 57.78496394130172 + ], + [ + 12.77468276021243, + 57.78500336735861 + ], + [ + 12.7746416314428, + 57.78504267789275 + ], + [ + 12.774599565486435, + 57.785081709239584 + ], + [ + 12.774557005400927, + 57.78512059216905 + ], + [ + 12.774514086514001, + 57.78515935577122 + ], + [ + 12.774470925710444, + 57.785198032698126 + ], + [ + 12.774427464527873, + 57.785236603761945 + ], + [ + 12.774383149158428, + 57.78527488503108 + ], + [ + 12.774338869963854, + 57.78531316527641 + ], + [ + 12.774294551483907, + 57.78535142226587 + ], + [ + 12.77424971489857, + 57.785389496990184 + ], + [ + 12.774204801279232, + 57.785427524326195 + ], + [ + 12.774159666169105, + 57.78546544647581 + ], + [ + 12.77411392297487, + 57.78550316023477 + ], + [ + 12.774067612849715, + 57.78554068235993 + ], + [ + 12.774021205345958, + 57.78557822788299 + ], + [ + 12.773974643103275, + 57.78561568380746 + ], + [ + 12.773927844319248, + 57.78565300310456 + ], + [ + 12.773881205773426, + 57.78569033390591 + ], + [ + 12.773834787457476, + 57.78572778390891 + ], + [ + 12.77378794075068, + 57.78576514753825 + ], + [ + 12.773741160546413, + 57.78580258518795 + ], + [ + 12.773694625911876, + 57.78583999051623 + ], + [ + 12.773648157443281, + 57.785877272289056 + ], + [ + 12.773601682459207, + 57.785914835965215 + ], + [ + 12.773554718594143, + 57.78595219344733 + ], + [ + 12.773508400818706, + 57.785989454254185 + ], + [ + 12.773461393360188, + 57.786027049727714 + ], + [ + 12.77341481884996, + 57.7860642207357 + ], + [ + 12.773368368890448, + 57.78610167589786 + ], + [ + 12.77332146180046, + 57.7861391823309 + ], + [ + 12.773274258044617, + 57.7861764690394 + ], + [ + 12.773227782236653, + 57.78621369369649 + ], + [ + 12.773181773751222, + 57.78625119867822 + ], + [ + 12.773135273761008, + 57.78628880651199 + ], + [ + 12.773088582951422, + 57.786326277147104 + ], + [ + 12.773042091330174, + 57.78636368352191 + ], + [ + 12.772995540139979, + 57.786401117259935 + ], + [ + 12.772948731443279, + 57.78643849583995 + ], + [ + 12.772901940910701, + 57.78647581456223 + ], + [ + 12.77285536065802, + 57.78651314280109 + ], + [ + 12.772808846751264, + 57.78655053913305 + ], + [ + 12.772762129776648, + 57.78658794600542 + ], + [ + 12.772715312860907, + 57.78662534813464 + ], + [ + 12.772668534932563, + 57.7866626978411 + ], + [ + 12.772621933454634, + 57.78670011049862 + ], + [ + 12.7725754764398, + 57.786737582012705 + ], + [ + 12.77252901507228, + 57.78677505429717 + ], + [ + 12.77248256124596, + 57.78681252814787 + ], + [ + 12.772436095220579, + 57.786849996539566 + ], + [ + 12.772389599101485, + 57.78688745239942 + ], + [ + 12.77234305838576, + 57.786924890317664 + ], + [ + 12.77229649365217, + 57.78696231758163 + ], + [ + 12.772249959962997, + 57.78699975372943 + ], + [ + 12.772203446944877, + 57.787037195072685 + ], + [ + 12.77215683289123, + 57.787074600334606 + ], + [ + 12.772110124620452, + 57.78711197448031 + ], + [ + 12.772063461406866, + 57.7871493668781 + ], + [ + 12.77201689925942, + 57.78718679737246 + ], + [ + 12.77197016637753, + 57.78722416950198 + ], + [ + 12.771923609838394, + 57.78726160646053 + ], + [ + 12.771876985583496, + 57.787299021624925 + ], + [ + 12.771830328485107, + 57.78733642680202 + ], + [ + 12.771783735215388, + 57.78737385088303 + ], + [ + 12.771736834375046, + 57.787411160026856 + ], + [ + 12.771690420616778, + 57.78744863758787 + ], + [ + 12.771644011720602, + 57.78748611141431 + ], + [ + 12.771597419374084, + 57.787523514337 + ], + [ + 12.771550017136075, + 57.787560626408975 + ], + [ + 12.77149669619379, + 57.78759564409164 + ], + [ + 12.771450317328293, + 57.78763313065704 + ], + [ + 12.771403855653306, + 57.787670591273525 + ], + [ + 12.77135726609922, + 57.78770801001414 + ], + [ + 12.77131066925945, + 57.78774542907742 + ], + [ + 12.771264341255643, + 57.78778293798008 + ], + [ + 12.77121778110149, + 57.787820356003074 + ], + [ + 12.771171250416142, + 57.78785777603469 + ], + [ + 12.77112483753672, + 57.787895229177096 + ], + [ + 12.771078591828415, + 57.78793274879656 + ], + [ + 12.771032814605425, + 57.78797045232531 + ], + [ + 12.770987240913247, + 57.78800823020395 + ], + [ + 12.770942413629282, + 57.78804626044247 + ], + [ + 12.770898157495505, + 57.78808448058419 + ], + [ + 12.770854634573872, + 57.7881229407918 + ], + [ + 12.770811361416653, + 57.78816148018502 + ], + [ + 12.770768309350922, + 57.78820008769783 + ], + [ + 12.770725710886365, + 57.78823882095175 + ], + [ + 12.770683538006638, + 57.78827770405476 + ], + [ + 12.77064181795176, + 57.788316687192925 + ], + [ + 12.770600646764104, + 57.788355762799426 + ], + [ + 12.770559877927951, + 57.78839496585613 + ], + [ + 12.770519851320111, + 57.78843439158376 + ], + [ + 12.770480395190567, + 57.788473985374885 + ], + [ + 12.770441414156034, + 57.78851371792745 + ], + [ + 12.770402894861446, + 57.78855358370314 + ], + [ + 12.7703646089169, + 57.788593518907994 + ], + [ + 12.770326952365489, + 57.78863362749818 + ], + [ + 12.770289557324581, + 57.78867380449155 + ], + [ + 12.770252671456252, + 57.7887141139615 + ], + [ + 12.770216238680982, + 57.788754539156145 + ], + [ + 12.77018030923323, + 57.788795090993325 + ], + [ + 12.770144912470043, + 57.78883577458626 + ], + [ + 12.770109898110462, + 57.788876551216234 + ], + [ + 12.770075637519547, + 57.788917509960235 + ], + [ + 12.770041882649357, + 57.7889585876184 + ], + [ + 12.770008438232265, + 57.788999736843 + ], + [ + 12.769975119907704, + 57.789040915890226 + ], + [ + 12.76994267822644, + 57.789082294069466 + ], + [ + 12.769910581991118, + 57.78912375528341 + ], + [ + 12.769878812702261, + 57.7891652992957 + ], + [ + 12.769847406087312, + 57.7892069329199 + ], + [ + 12.769816179598463, + 57.78924860725316 + ], + [ + 12.769785987327676, + 57.7892904915747 + ], + [ + 12.769755753923295, + 57.78933235865979 + ], + [ + 12.76972561649406, + 57.7893742530806 + ], + [ + 12.769696109254747, + 57.789416295463425 + ], + [ + 12.769666828260963, + 57.78945840126653 + ], + [ + 12.769637489089561, + 57.78950050162289 + ], + [ + 12.769608379102104, + 57.789542651076594 + ], + [ + 12.769579083106368, + 57.7895847673486 + ], + [ + 12.769549890329714, + 57.7896269056941 + ], + [ + 12.769520988763675, + 57.78966910244765 + ], + [ + 12.769491727422887, + 57.789711229141375 + ], + [ + 12.769462498032013, + 57.78975336465171 + ], + [ + 12.769433700931518, + 57.789795590116675 + ], + [ + 12.769404413886672, + 57.7898377245434 + ], + [ + 12.769375199712343, + 57.789879879277166 + ], + [ + 12.769345952084887, + 57.78992203879852 + ], + [ + 12.769316657266996, + 57.78996421687425 + ], + [ + 12.769286146947968, + 57.79000617566493 + ], + [ + 12.769255493742438, + 57.790048128394496 + ], + [ + 12.769224034348513, + 57.79008993625272 + ], + [ + 12.769192074463016, + 57.79013164442804 + ], + [ + 12.769159109379357, + 57.79017313097768 + ], + [ + 12.769125082593236, + 57.79021437317686 + ], + [ + 12.76909042493878, + 57.790255465706124 + ], + [ + 12.76905508452627, + 57.7902964038535 + ], + [ + 12.769018412783282, + 57.790337025430155 + ], + [ + 12.76898075108172, + 57.79037740579056 + ], + [ + 12.768942288444094, + 57.790417594425875 + ], + [ + 12.76890223242696, + 57.790457359534095 + ], + [ + 12.768860772349672, + 57.79049672824535 + ], + [ + 12.76881795529241, + 57.790535695212306 + ], + [ + 12.768773747679909, + 57.79057421624685 + ], + [ + 12.768727923711358, + 57.790612161581535 + ], + [ + 12.768681357074692, + 57.79064981554232 + ], + [ + 12.768633725722616, + 57.79068706763296 + ], + [ + 12.768584648154553, + 57.790723793050496 + ], + [ + 12.76853502579568, + 57.79076032308573 + ], + [ + 12.768483974089264, + 57.79079630427944 + ], + [ + 12.768432020651643, + 57.79083192302553 + ], + [ + 12.768378870560248, + 57.79086704047761 + ], + [ + 12.768324765438436, + 57.79090174454039 + ], + [ + 12.768269153504278, + 57.79093576146685 + ], + [ + 12.768212413683681, + 57.79096923899234 + ], + [ + 12.768154849525981, + 57.79100230102278 + ], + [ + 12.768096291202179, + 57.79103484007413 + ], + [ + 12.768036920823278, + 57.791066867278154 + ], + [ + 12.767976705364909, + 57.791098253558395 + ], + [ + 12.767915932225362, + 57.791129333552114 + ], + [ + 12.767853756953146, + 57.79115976664574 + ], + [ + 12.767790407104497, + 57.79118973542888 + ], + [ + 12.767726056126463, + 57.791219088936565 + ], + [ + 12.767660521183133, + 57.791247680189926 + ], + [ + 12.767593875460957, + 57.791275480944186 + ], + [ + 12.767526435662468, + 57.791302671901555 + ], + [ + 12.767458182846141, + 57.791329223182 + ], + [ + 12.76738987658773, + 57.79135574474196 + ], + [ + 12.767320652723994, + 57.79138161062102 + ], + [ + 12.76725099991972, + 57.79140718765282 + ], + [ + 12.767180324196133, + 57.79143198444343 + ], + [ + 12.767108341135897, + 57.79145570817166 + ], + [ + 12.767035722628238, + 57.79147885882315 + ], + [ + 12.76696224177138, + 57.79150118256025 + ], + [ + 12.76688860151115, + 57.79152332562945 + ], + [ + 12.766813736225934, + 57.79154426905142 + ], + [ + 12.76673899925709, + 57.79156538062029 + ], + [ + 12.766663237407435, + 57.791585464890176 + ], + [ + 12.76658661013989, + 57.79160462875888 + ], + [ + 12.766509096553843, + 57.7916227198198 + ], + [ + 12.766431127285731, + 57.791640188624925 + ], + [ + 12.766352780222352, + 57.791657118511225 + ], + [ + 12.766274128071599, + 57.79167363089832 + ], + [ + 12.766194895267404, + 57.79168933323462 + ], + [ + 12.76611531062078, + 57.791704527745125 + ], + [ + 12.766035074694614, + 57.79171871649306 + ], + [ + 12.765954696277882, + 57.791732694352405 + ], + [ + 12.765873534121924, + 57.79174532458464 + ], + [ + 12.76579217542967, + 57.7917575867762 + ], + [ + 12.76571041622678, + 57.79176907723525 + ], + [ + 12.765628287108695, + 57.791779799524186 + ], + [ + 12.765545925724513, + 57.7917900162873 + ], + [ + 12.765463185961123, + 57.791799336462745 + ], + [ + 12.765380158206522, + 57.79180790882884 + ], + [ + 12.765296847189013, + 57.791815664736454 + ], + [ + 12.765213191867904, + 57.791822250516105 + ], + [ + 12.765129456774646, + 57.791828484727034 + ], + [ + 12.765045528885265, + 57.79183390610628 + ], + [ + 12.764961491299143, + 57.79183880609693 + ], + [ + 12.764877259198464, + 57.791842629388135 + ], + [ + 12.764792967785574, + 57.791846060900866 + ], + [ + 12.764708566049377, + 57.79184863827791 + ], + [ + 12.76462410143674, + 57.79185054956172 + ], + [ + 12.76453957831984, + 57.791851302561355 + ], + [ + 12.764455056018605, + 57.79185144173099 + ], + [ + 12.764370544773117, + 57.79185099534169 + ], + [ + 12.764286054777054, + 57.791849912916355 + ], + [ + 12.764201596472589, + 57.79184822917389 + ], + [ + 12.76411756032787, + 57.79184553216098 + ], + [ + 12.764033623036473, + 57.791842270412005 + ], + [ + 12.763949890671505, + 57.791837758234735 + ], + [ + 12.763866279157645, + 57.79183246059031 + ], + [ + 12.763782524020357, + 57.79182678365757 + ], + [ + 12.76369896217939, + 57.79182020023812 + ], + [ + 12.76361544822577, + 57.79181327973845 + ], + [ + 12.763532097554085, + 57.79180566169836 + ], + [ + 12.763448983827349, + 57.79179721240923 + ], + [ + 12.763366151620806, + 57.79178802556912 + ], + [ + 12.763283885712408, + 57.79177754417472 + ], + [ + 12.763202014879584, + 57.79176628025398 + ], + [ + 12.76312030372866, + 57.79175474839738 + ], + [ + 12.763038985340893, + 57.79174244606461 + ], + [ + 12.762958079055442, + 57.79172936733432 + ], + [ + 12.762877467214263, + 57.79171575469964 + ], + [ + 12.762797270377972, + 57.79170142518566 + ], + [ + 12.762717793056053, + 57.791685999436275 + ], + [ + 12.762638862035107, + 57.79166983006478 + ], + [ + 12.762560361857933, + 57.791653103265986 + ], + [ + 12.76248219484691, + 57.7916359605211 + ], + [ + 12.762404714695778, + 57.79161795446981 + ], + [ + 12.762327712476534, + 57.79159934516484 + ], + [ + 12.76225116377439, + 57.79158018087218 + ], + [ + 12.76217534532898, + 57.79156017350395 + ], + [ + 12.76210024201112, + 57.7915394090606 + ], + [ + 12.762025607117534, + 57.79151818222351 + ], + [ + 12.761952197687377, + 57.791495789990755 + ], + [ + 12.7618791336809, + 57.79147309984544 + ], + [ + 12.761806506975052, + 57.79145000822652 + ], + [ + 12.76173474475167, + 57.7914261512249 + ], + [ + 12.761663718653915, + 57.791401645393066 + ], + [ + 12.76159379971293, + 57.79137621970792 + ], + [ + 12.761524826450398, + 57.79135008750846 + ], + [ + 12.761456339807108, + 57.7913236434581 + ], + [ + 12.76138869299625, + 57.79129662767259 + ], + [ + 12.76132200058023, + 57.79126890549385 + ], + [ + 12.761255709484455, + 57.791240855699854 + ], + [ + 12.761190526080531, + 57.79121205804936 + ], + [ + 12.76112739441517, + 57.79118200619629 + ], + [ + 12.7610644680079, + 57.79115190799362 + ], + [ + 12.76100237965744, + 57.79112131276291 + ], + [ + 12.760941018109898, + 57.79109029692679 + ], + [ + 12.760880559428267, + 57.79105877916693 + ], + [ + 12.760820925812993, + 57.79102681784613 + ], + [ + 12.760762456446457, + 57.79099425508445 + ], + [ + 12.760704768832264, + 57.790961304426204 + ], + [ + 12.760647993232743, + 57.790927910113325 + ], + [ + 12.76059184831621, + 57.79089421320058 + ], + [ + 12.760536751181538, + 57.79086002618981 + ], + [ + 12.760482708870885, + 57.79082536340702 + ], + [ + 12.760429381149562, + 57.790790387175385 + ], + [ + 12.760377312125083, + 57.79075487556178 + ], + [ + 12.760325838188225, + 57.79071911896039 + ], + [ + 12.760275481603543, + 57.79068291189888 + ], + [ + 12.76022585645425, + 57.79064641800924 + ], + [ + 12.760177203939435, + 57.79060955103201 + ], + [ + 12.760129484774936, + 57.79057233607226 + ], + [ + 12.760082771625592, + 57.79053475725546 + ], + [ + 12.76003706507595, + 57.790496826165445 + ], + [ + 12.759992538481464, + 57.79045849750998 + ], + [ + 12.759949425253845, + 57.79041971351104 + ], + [ + 12.759906841881241, + 57.790380770112534 + ], + [ + 12.759865261870075, + 57.790341522597174 + ], + [ + 12.759824459277509, + 57.79030204519567 + ], + [ + 12.759785042660601, + 57.79026216994617 + ], + [ + 12.759746282759604, + 57.7902221141677 + ], + [ + 12.759708060832892, + 57.79018190690237 + ], + [ + 12.759671071359856, + 57.790141367733995 + ], + [ + 12.75963515430922, + 57.7901005506778 + ], + [ + 12.759600486709427, + 57.7900594256682 + ], + [ + 12.759567187928344, + 57.790017980501496 + ], + [ + 12.75953473050769, + 57.78997634825659 + ], + [ + 12.759502903240058, + 57.789934577201336 + ], + [ + 12.759472238031847, + 57.789892557613165 + ], + [ + 12.759442590686746, + 57.78985032887995 + ], + [ + 12.759414156165885, + 57.789807861912614 + ], + [ + 12.759386976458345, + 57.7897651610253 + ], + [ + 12.759360852442672, + 57.789722273000216 + ], + [ + 12.759335802146817, + 57.78967920246799 + ], + [ + 12.759311442521712, + 57.7896360160378 + ], + [ + 12.759288733540423, + 57.78959257191324 + ], + [ + 12.759267008903, + 57.78954898395025 + ], + [ + 12.759246353180083, + 57.78950524875766 + ], + [ + 12.759226839811271, + 57.78946136611754 + ], + [ + 12.759208874757409, + 57.78941729830436 + ], + [ + 12.75919151947676, + 57.78937316459083 + ], + [ + 12.759175175467234, + 57.789328921746595 + ], + [ + 12.759160042926611, + 57.78928455694319 + ], + [ + 12.759146268206248, + 57.789240068079565 + ], + [ + 12.759133493273326, + 57.78919549671037 + ], + [ + 12.75912140669761, + 57.789150870125425 + ], + [ + 12.75911116448698, + 57.789106112632545 + ], + [ + 12.75910155252349, + 57.78906131764738 + ], + [ + 12.75909280770865, + 57.78901646800801 + ], + [ + 12.759085397925654, + 57.78897154534686 + ], + [ + 12.759079587150662, + 57.78892655260018 + ], + [ + 12.759073964140768, + 57.788881550992826 + ], + [ + 12.759069450784217, + 57.7888365085175 + ], + [ + 12.759067208571654, + 57.78879141652567 + ], + [ + 12.759065520572404, + 57.78874632027497 + ], + [ + 12.75906546969063, + 57.78870121580007 + ], + [ + 12.759066074924068, + 57.78865611439494 + ], + [ + 12.759068226185626, + 57.788611027854174 + ], + [ + 12.759071875569742, + 57.788565972181544 + ], + [ + 12.759075846441945, + 57.78852092732815 + ], + [ + 12.759081409600517, + 57.7884759318172 + ], + [ + 12.759087670341152, + 57.78843096284223 + ], + [ + 12.759095555707775, + 57.78838606632867 + ], + [ + 12.759104293042437, + 57.78834121489172 + ], + [ + 12.759113811221509, + 57.78829640515879 + ], + [ + 12.759124809885002, + 57.78825168896557 + ], + [ + 12.759136938817653, + 57.788207053757574 + ], + [ + 12.759150631665099, + 57.788162551211286 + ], + [ + 12.75916561722988, + 57.78811817578001 + ], + [ + 12.759180951171423, + 57.78807384013438 + ], + [ + 12.75919692554129, + 57.78802956093983 + ], + [ + 12.759214154405965, + 57.787985407459054 + ], + [ + 12.759233879885265, + 57.787941548530554 + ], + [ + 12.759254023922969, + 57.787897750502005 + ], + [ + 12.759275249328287, + 57.787854100071485 + ], + [ + 12.759297039019536, + 57.7878105270558 + ], + [ + 12.759321196485946, + 57.78776731013565 + ], + [ + 12.759345214396612, + 57.78772407263489 + ], + [ + 12.759370688837127, + 57.787681071267905 + ], + [ + 12.759397475578783, + 57.787638300976305 + ], + [ + 12.75942563722643, + 57.78759578807032 + ], + [ + 12.759453958602334, + 57.787553307628706 + ], + [ + 12.759484686566237, + 57.787511309866694 + ], + [ + 12.759514995315838, + 57.78746922866038 + ], + [ + 12.759546274072372, + 57.787427346066565 + ], + [ + 12.75957932926544, + 57.78738585453777 + ], + [ + 12.75961319863215, + 57.787344552126555 + ], + [ + 12.759648359912108, + 57.78730356288936 + ], + [ + 12.759683943817878, + 57.787262682287015 + ], + [ + 12.759720508631002, + 57.787222052187865 + ], + [ + 12.75975782893971, + 57.787181622020384 + ], + [ + 12.759796443163093, + 57.78714155244367 + ], + [ + 12.759835608880007, + 57.7871016508155 + ], + [ + 12.759875454871375, + 57.787061955504576 + ], + [ + 12.75991609058063, + 57.78702250337276 + ], + [ + 12.759956893713955, + 57.78698310559935 + ], + [ + 12.759998359109481, + 57.786943907121845 + ], + [ + 12.76004035945034, + 57.78690487321918 + ], + [ + 12.760082911241776, + 57.78686601802423 + ], + [ + 12.760125736052737, + 57.78682726649186 + ], + [ + 12.760168625945008, + 57.78678855227171 + ], + [ + 12.760212169925046, + 57.786750065644846 + ], + [ + 12.760255710757454, + 57.78671158929788 + ], + [ + 12.76029930296897, + 57.78667313144181 + ], + [ + 12.7603429839603, + 57.78663470399919 + ], + [ + 12.760387140339, + 57.78659643336867 + ], + [ + 12.760431330596163, + 57.78655817580958 + ], + [ + 12.760475426477816, + 57.78651989006853 + ], + [ + 12.7605196495283, + 57.786481652049225 + ], + [ + 12.76056391549661, + 57.78644343428538 + ], + [ + 12.760608085236676, + 57.78640519091226 + ], + [ + 12.760652222373693, + 57.78636694274024 + ], + [ + 12.760696325546048, + 57.78632868846934 + ], + [ + 12.76074036893234, + 57.78629041951039 + ], + [ + 12.760784576082166, + 57.78625219009755 + ], + [ + 12.7608285785296, + 57.786213890642365 + ], + [ + 12.760872719167518, + 57.786175636584034 + ], + [ + 12.760916930326545, + 57.786137405748626 + ], + [ + 12.76096101563735, + 57.78609913349372 + ], + [ + 12.761005163791202, + 57.78606088192409 + ], + [ + 12.761049370209136, + 57.78602264954092 + ], + [ + 12.76109344095731, + 57.78598437250195 + ], + [ + 12.761137523320693, + 57.78594609930069 + ], + [ + 12.761181483767611, + 57.78590778605302 + ], + [ + 12.761225637527495, + 57.78586953936352 + ], + [ + 12.761269642818002, + 57.78583124513898 + ], + [ + 12.761313640655258, + 57.7857929478893 + ], + [ + 12.761357769377781, + 57.78575469296397 + ], + [ + 12.761401803335753, + 57.785716406351035 + ], + [ + 12.761445946441848, + 57.7856781548964 + ], + [ + 12.761489913982295, + 57.78563984531785 + ], + [ + 12.761533747335532, + 57.78560149119643 + ], + [ + 12.761578062525178, + 57.78556329432446 + ], + [ + 12.761621981777697, + 57.785524967172336 + ], + [ + 12.761666029853124, + 57.78548668164482 + ], + [ + 12.761710201138552, + 57.785448435933006 + ], + [ + 12.76175411433116, + 57.78541010505744 + ], + [ + 12.761797994811822, + 57.7853717628438 + ], + [ + 12.761841846667991, + 57.78533341059696 + ], + [ + 12.76188609221864, + 57.785295187626026 + ], + [ + 12.761930101241491, + 57.78525688777344 + ], + [ + 12.761974194533863, + 57.78521861638848 + ], + [ + 12.762018393759098, + 57.78518038061852 + ], + [ + 12.762062179129941, + 57.785142009567515 + ], + [ + 12.762106195500492, + 57.7851037152605 + ], + [ + 12.762150421529135, + 57.78506549068515 + ], + [ + 12.762194556338779, + 57.78502723691285 + ], + [ + 12.762238498811822, + 57.78498892074474 + ], + [ + 12.762282843177111, + 57.78495073737113 + ], + [ + 12.762326897190235, + 57.78491245940951 + ], + [ + 12.76237070670655, + 57.78487410207147 + ], + [ + 12.762414769806853, + 57.78483582871071 + ], + [ + 12.762458907749075, + 57.78479758058431 + ], + [ + 12.76250295155895, + 57.784759300245874 + ], + [ + 12.762546722028539, + 57.78472092533556 + ], + [ + 12.76259078830107, + 57.78468264726889 + ], + [ + 12.76263497677592, + 57.78464440974428 + ], + [ + 12.76267889701466, + 57.784606084579266 + ], + [ + 12.762723219645075, + 57.784567891715206 + ], + [ + 12.762767146558648, + 57.78452956857447 + ], + [ + 12.762811378743791, + 57.78449134576076 + ], + [ + 12.762855571914635, + 57.78445311002297 + ], + [ + 12.762899807831172, + 57.7844148882414 + ], + [ + 12.762943888078874, + 57.7843766151696 + ], + [ + 12.762987982800656, + 57.784338346765985 + ], + [ + 12.763033067382414, + 57.78430040380251 + ], + [ + 12.763076276956271, + 57.78426184411832 + ], + [ + 12.76312066848461, + 57.78422367305345 + ], + [ + 12.763164256583991, + 57.78418523765524 + ], + [ + 12.763208416662238, + 57.78414699028122 + ], + [ + 12.763252905195689, + 57.784108850970945 + ], + [ + 12.763296544786778, + 57.78407043273466 + ], + [ + 12.763340696242407, + 57.78403218323086 + ], + [ + 12.763384543643053, + 57.78399383407041 + ], + [ + 12.763428790514752, + 57.78395561666366 + ], + [ + 12.76347264147169, + 57.78391726941852 + ], + [ + 12.763516709276631, + 57.783878993854536 + ], + [ + 12.763561046983886, + 57.78384080735327 + ], + [ + 12.76360518520187, + 57.783802555618415 + ], + [ + 12.763648989777, + 57.783764194658765 + ], + [ + 12.763692807125386, + 57.78372583832328 + ], + [ + 12.763737113050723, + 57.7836876427023 + ], + [ + 12.763780968404449, + 57.78364929961327 + ] + ] + }, + "properties": {} + }, + { + "type": "Feature", + "id": "Goal", + "geometry": { + "type": "Point", + "coordinates": [ + 12.763780968404449, + 57.78364929961327 + ] + }, + "properties": {} + } + ] +} From 6b8946ac21f5bd22ac2e9e4c52d441f8455e681e Mon Sep 17 00:00:00 2001 From: Sebastian Lindholm Date: Wed, 15 Apr 2026 08:32:16 +0200 Subject: [PATCH 06/33] Simulate with 3 trucks --- ROS2_ATOSFleetManagement_cheatsheet.pdf | Bin 2525 -> 22888 bytes atos/launch/launch_atosfleetmanagement.py | 37 +++++++++++- atos_gui/atos_gui/static/fleet_map.js | 67 +++++++++++++++++++++- 3 files changed, 102 insertions(+), 2 deletions(-) diff --git a/ROS2_ATOSFleetManagement_cheatsheet.pdf b/ROS2_ATOSFleetManagement_cheatsheet.pdf index 8a3589439c495735e873b4d981ca36ceaf5e80d0..280387a65af8bf3229cdf4bb0f040ac1f04f83b4 100644 GIT binary patch literal 22888 zcmdSAQ?w|-vMso5+qP}nwr$(SUbb!RW!tuG+g9)M#=WCof1c0&sacsBH8W>aMO0>v zDpCazF96=%`!*B~u4`Cl?t*BLZ<#J5wh^7gH00e-;EL_QtNZrgkm_7A`Ih zT=ewr?(TF>|HszG(1_01-WH0HfPui?$O?*w2Z~;dk$~x6QHg+FjES0ofa5>sP<(t) zrgkR(G5Js1|C$OJI-82w|0AZCFtu?rb+I%yB%l|yGqyLev@<85SGTkiv~#xne=Fn+ zZB75tG7``$yBfK8I{YiB{5$?D{|ElH{Nwt+x?=vnx)T1^^iP5_0ptG+PS(`K(oo3W zgFySA6$1e)Co3HjJ1ZLj2MZ$|2L~qyfi3~Pf}zuYWHbIp-akD`rq1@RPR6Fr1pi0p ze+K?PEecNd#>%EH|G4P?<1l&^Q;&b5ByA1NO@;mkh5rX733UGr@gJGJ%RdMIBc@kz zGPL_Yl>e!RBKTi13jZ$tzr?WoGyPw`{l8-VBk|wJ{{JS1_5TthW@+PM>hvGiiP`+~ zh=}RG+i&_G@5`9lnY&mJuro6M#|_RdPNs&oP#)Q?%d*y3h8Usy-MvPsw7Ti1+<=IQ z=>lk$(zM&yzOuBg$@4T1x0m-JXj0tyTZ2MgbHx<109|;z%8#HnD?x(lMZuR&v^5Lho;l`VMPnX*kOy?u|a`5n@`_10&zCC)!4s} zYk6rsJvZ8Ar``n~?b^+Qe)Tqkh5mL$;_V&xH(c+33;uKXci`WE|2-_^-I0Go z`v2E3uYZUB4eI}2f<5$o3&AqrnFz=u(B z&%bo|KacePBu2LXA|@Ew7#P|AH$wj9TUHKErvFcnYYH-SNs)=pnQ>vB5XlgAE zJ7(HU5MA0XNg00rRyV)y8?8(|_Wa(y=bj!q%xHSwHv3(=_g=$75Qp&0S&7UoeGRmQ zq~?Od4P?aa^L95?7OOD}TZ=-(1+>baCxUIJxTW9`(xLC%n>IsNnK?(%6At}Co!shc ztgF0aMc`c`B83qpKbf>w4V|Mee$oh4nD2dzIdK&54M9T0oa}o@c&ZLIlrt9C4=_%N z>!z@lkj9Ksll42YjUfs_t1Jt>Uo4$Liuyy?@PDR6TX8JPxU-Y3~8}x#r(ALV>l9k z;@zPpKvg7wSth`_-3fr1h>)3xp&dnZ#j`&MsvSjr#q&c61X4hN79p4j7f`^277>oc z5-E_(A%@-v&QpNF3fEJ>4HxZG0Fo9WQb5Ltj9Z{c307FJU~Mx7LVVu0eFv3TDO zYVUVO=dAZt19fcG!npv|INRuf^Tn|54>IgTdF?sVe!y9eUvglI<&-0_zK~ z11#{_(Wrwx$^kXuT7c_C(dB#L1IhOu zTp@I;>$sfe7!e~ycL;Ns8Fld3CFmyh~1LFAy){n0B|MUB-O zX4)E>HLpcY=Ch;Z$G;U;2RH$G0LBRiGULV^hUo4?h42)@o6`Y1b!(Vadsk*DXdRQ? z`&5$~Kz_GL1XEYLYd=$L0#_3QGDoN+l&tfH)grV>)wjU!3WLm&#O^cS>WkRYXiB0U zoDR5#-7|w72hpV#793D*g6jqFAYPVIq1O?}AEU1j2_(l6V`N(doXI`p*=uWEdl{LU zGh}qf*^r^y1Y~&*5KpQ;e7UX|_6cBk3_$P*SX&17@O~rbcfU_EWK1eU+vKWarB(*a zV&zAcLqi*k$LqJ`$V(Zonb(DfaC@81@b6R8vh(hH67aYHCR4v)i1%z?8cBne4~nT& z7^D6(Dgcw-yGr;MZN-Tv@{((p7bj!JJ?}R5l_fm$y5MBa3GaC+DIj;Ym@Q>L{Ptpq z!is#)aS!+iqNPTV5RQ1Aphg^fnMcu(h^YGfEBz)UIeDMr#!23tM~)@Q!AR~2?(}VO zN8Tfkk5=cbTi*+&(~FyAZV0F$EI#l?Gw1HeIa%f9c&# z#~rw?s#n)@0)=hL?#Wr8X{3JT5yCQJQC=4aCDHc>w_1**W#rAQa2rU>4Xmr>t9H;< zETLf8E)Ee~VuBouJxZ8n%mBxJHDbl#tQqZ57j0y(5tl44I(<+tJ?XO!ufxjo3v2e8 zOj^nll*VDGTNHyMj%*epq6M429X&n)HJVr%s1Y@1nrys`>|~2jV0#WrxG>1VNbiCV z^=5x+`@dYbL#AbD9P(*B_a~j=IUN4-^G)QXKgYG|dizWzJPUi-op(5X*|jT&j|29f@kFg z7jYf?#|4E4fnoqf9^)CW3XW`r@gujbw#4urU}64%X+~<<<1SoWZy(iFxV^UJwL~AX37-DFJ)hncU7+e zqnLDYD6UCqgld3U_qD{KRhJq$Iwg%BflNgro z+@v+iyLsED8+4BY!;DfSgr2SVIJuVGEdm&mUsd3nL%_;;WdQ5ZZWT-8Iy=2OWNOD{ z3P1WWK&gb@UpJ1i)ptLkbrM}iY-A(ATZ@=5d%_HN)9zS(2OZ4P!IVoP2P0IB|L!M% z;oyapb1{|_UV+2_?F5Zfr0)Q>CXMc!(Ba(1Miab^OD4VQj#2Y;oz*Vz<6Fr6@&ArH zw{8@#ouaCc#pN`A!H(NzCMP4<63O(Qg|(+{HLW7bdsDwRS6$CoJDe(|&q2mdpL_XR z`&=UNc|}>Ynmzr#o&B=vTP;=vJ}tw6ygus6gm3H#Sro(Lfxa`ewcSj2qShuiTu>(i zcAL~g98(BNgVdGPhRkbrr$A_gm{7X{aEa70tuiXJ)ESZ8nl_Au*H0Q#N0Kz+YQ0*s zx&3oDcwKnh!19roCt2~^6)FGqjdXJX*5(XgSBzZ~IiPBogZ2J^hTC_2E_W85b!L43 za-gzLOe;yu!4uxU1v@7y-O6&ZK)#TELw{$Tr#DhNKV?nN9uWt&%~-@E9z8A`?$Ip< z%2GLg7^L~zhK9XCW7VB%A;R1rBJ3xoS9br1Ks%-C4`Wa543|v!B!kWiIC{SZTy@ol zPw0dphc*>q#*I&V-DgU<5a@L-07aMNR=ij7>_O#!{uTjRRs!30{uG2fDlLCVW;tk? zii%eB`A+7Cz2;s#2+dp2-x*4-A~O5_NTG0Di~5WLey7RuQU?e@vc!$T2Fh-IqSx=A zI>JwDdZ7Mht8~K0wHB@qsAreNx8F19xF_g?%k{wBBu>HuCqxuuV7?{u1Qr)#|CGI@ z7^Kn{lj3rBzCd^)-IWn)+hk+i9YS4`@UI1;Gp!9tH!L2@ngZ(|P{D z`o`zax^u6G6FKOpgA;Lh7{b9Qhguf9IAnR~Vo3p8=4eS0Bagf=#G;zXC7V<#yLepW zlRbNE{pk8y@OkIE<-76e(m6-h6}2hqcHbGIdqB5L_e?iUcTKnLe8%~O^NQmc>o4aa z=Og!={lPiyym9IxKCkGL%q;pXC9ZP{?fiBX&I-92fd{szcl0GHx+2q}PU!EgO zMjPJTzruiwiDYsS*d2=tJX>YsaUFf-588UzOg076IJhIJ09WrIcc`|&k)7VB3~$rd zmf+n|b`S9)>ttrH&HEX%5n7w(6XMF8Q$%;Q*?LP7i`&}8>wlaYeQzE{uJiu*Wej`% zTl{7RVkQ18dlXQd>HX3P2VAeGCd^J!&0aa zKwsd^r!|8!!A?QGGN0ugfL!R10$!HUM)|DDQctfCmvHp{=!_x7Rk164R2uVtOj%W*i# z2`988rWTOXY0s1yhj1;}gFuLf31Pls15}31s!lH|Xt~Q`37#?C%d33QnDCYfS2LMo zG!KX@uh0;-fj_gGDSSvbvGDG*K)>hbqhbeENMTm}s5*Cuyd-DMGx?zg)q@@81y0Bq z*RYFftXJ36j?MPo>UD2pUM%jq(zl-kLr|{(R=70Yfa-{DIG3LKE3pFrSpEWlQa!#E z9czeVqPB6Xd8g(KlTqIw>FNAcTK;63Q|DnRN<6p29sKN;&$4QqLif8t`(wq-o7(U# zkhc?lb4Xh=*xm3#vfkctEKHH zotxBd*!QLWy9NHdq^~XP@xF z`qkp_k|=9DKac5;80=7VxJ}{`>4npo;f2@V4>+U~u`X2l(JWyvYIy$lqkaDG`*q+c zzf8g0rLsth@qO|&&H+Sj`a4V@hhzwlM-G_Ka)4DjG@ip6*G|~b9`i6jdc(^(vxeT+ zNq0_H`$PxBk7-sQ5wD;XAo+*%TiEa|@2g-o%o!K6W68K8*q~E5Oh!blTWoI3^iW z<4jl$0tSOOgI^xKMF7F?C8m{7E0ayQmZ0F+fDRGgP)XhONXz&3rSjbwrdsGtQ z=S%kc(e5iEs0mI3V4`(d*g0gNK}&fFQ1GM@c?giCHG({JxX8`uvI*Zbpt#U486tX6 zpo2dAeG^ALnN#3Pq|ybN%;+FgU2xdpfP9qYolOkVPJ2^$sC1Fxx<}e+M87R?q8xdA zCu9%-%d?n@? zg>>`x?~Nw#4?Gm*Cx&8V#b&?Ue+c$u-^l*49JTVISg9UQj>dw*H>ufXs*U{0s z>H&@}-M#7SZutH&vXMwGmoKLEMx(C6;N{2~G4I`oUz z=vl8|{==|dQh`V?lqBZd2^nnanoC~ZRrkyYY**aAYtXp4PD`g+>$-izy17;25q;RJ zZ;f{_sr*`mA_ajMow1CQX&p-4<{@x6i~X-&m&bJGaFlv0kA=SW-B9N(AEWDcJg~OC z1o}OR4W?!WQeF#@s+UY=RN6k{shv}SHJ-Q4v^}1;%ts*^?PI69-aDPQ&Blz}n7v-A zUSg`Zr~TdI@a-O27v8UE$G!Vr1g=SM-mo~W*HS|4Rs2_w)h4JOsQo-0MAwL1la+^Y zXR1X)PVier=v#%=C^&%+R6Y&SWsXG%M>U)W3A=n>`BuO3v`?+n4=#gJFh5^85yq_R zn{@(W^FS22#^}AsthGv^7w^tc#c5%CdPsu^XL5-1F?v4U=c236g^ug(^MtdI2fci5 zH)hPLBy~CbF7(qD{8M?sNB#WRN%%%{rrV_0R?WBaAI($rgNhRn6Nnc?B%p9>g*sBc zWUQLR>bms#=?2sd3`HDfNBFA(;4OEQkyn4`{OeYE|Hoxv>s1~a*@1BWPs8gCTX{+L zV*C=D8eLOg}({Ll)AHUh6V7}|2 zL(gwP<<8O=o7Y#FxuSieE`HH!{-YrF*w)v}a3$f%_Pa3?zqe%G#uE8rWDZ#H=^bX% zg`RWr&JH;={-h)n)X+@_+M1EgqQaS2R{^N6)FJ864d_a<}7Y3GL5XTQp$H3a@Qqkw8bVVqnjn;-c-9n3vM zEzCNZQGvRBbqU#9oSEg%HA^VRgPt9elGxAWJg5dJA!D2Zzn`)2w^netM0JM%{W`pu zCmDpKAj+~13J+0>1<&^F7wAOhaPrUx4;TFv(Oy<%b7uAj;eP<^aL z7~!h5O2#hn_Ux6L&Db%Uv{ER<^JFgPTHRe_K= zQFBlzhyKi0Lby_m6Cw{mR<4_bDbS!*nU$j_9h+oKK*7O<1R}G>r&p#kr21`phh+K5 zK5Pn-D$Ve2d~W7W>%1m)<3{Jc2IhLL*uT1-WjGxgFyX`?$kI4Qb<#+$azA}9Znfi; zIRvWaVaSHN%GSxnt#BGG#dB3b>^G$kV#N+(YN3k9W33ecr@{T`%taF6m!T?lh1-1D zrMf)3{)Q+!nI6@|!eKRIOuND$E@{P}ej-(Es3Qj!NrZ4oF(Z}5CtN6E1uKHm(*`E) zFr!RR&I#mi;_%@b@qnLq-v4ni?sWNPK&UzKYr~`!dGV3uvG>bA`dOc6Oo)`ePuzt`{52S{DWQ~yK({4zwA+C>-Rnw?=wm_r_J{MP zCuUH2(T?PL$9CkpheD8FNv4kEovC9y%JDLdHtu$d=J){J&PcAq_i~awv~8ym zzE-={_O=XP_e=bC(aCn6!gthytr2Q9EJ)VOSb+Q9jBXys=7se*! zq(pWZpj`37U#IBjIcKmOUJeqC>U|Y0ZwU#9Fg0|X0pr`#?%W4k_QTV8WuMOO=`LT3 zIyH2pd?7?Y%CLRC%H+DYf+n$>^Bx)d__~H>`r$Tf=e!QS*vuHUP!$6!kzvEkjAz$- z;hTlkh{2{4Tp&)mcDB=F&uZ@~Jk6&W#`Eyp$j7M5U8>!lT1V^S&02#v=|%2PUi%ol z#+#po2Z!DL+b)m&+rhBzyrgzZ(QBkEeVx-A_8)E}i#TA;8$ixVh5SK+iCB76BCkq; zTbKqj9eoI(u+jVHg}Oy07LnX6bddJ6xE8?Qk%nBE{{J&RaL+9Sj>7HuzHl zAdpv0DiM|m82LIsdaqyc9`OVbcuVP+1+1vnoE2!M>doTSj^ z>+WDkchDeAHsx&X9E7>48EWmY90JEn`kjkc8HR3|)Q2K9IJ@q0nLfIiK6ZHsu(8R< zx)SxEydOgekd6vP%XeC^^A}Bfa;{u;(5NW>Zh8t;s@XXXUd5ws#j|rl3AhM!_!DE zYPulq@b!szSeo?eRc&b0i16_)fU(#ga;R zmuhJG(&SQ!yH3*o)&uh|^+Do(JDox+7;b9XhHwV@A^~E+ol`{9>;+#F5&^vJEaymC?JvgGlzPa>L@#)loPvi z$W|F+ML(mD3)QUEv{54+L}OCgp>cX}!HEo86E8oP9At*N$R9w%a|=v~tN$V3C1Lf2 zpdWnqftI$GHSZj%z1jKiHCOmyCCHk2{---hja5lmYZ;SeOUOzX1g{E#UkpObSOv?> zU+VcYYGQn@@MrFauVyHUKVHZ(QhC_+l5@E;;FGp7Dkor64ehiMC(x>W6kIE#pp>`A z4bTZ?YLqZx#8aYa+Lv@ZUu;*f8LKHeHO<*_Hm?i&W$sM8_f)p528k>ptFX7@~rd+|!{>i^>d>=Ia@9S2j z`m^N^WBJQ}$bzI8WJkN>M$_2{^pJhALub}H?St0^RWJJ3ncl*>P?IbbG$2gP(x zZ`d9uxkU;dcgP;#a(8%IiI_e59gOl1Klyy8!U(z|_z5DqyI4Ck*w*3fc4Sgzsa(o! zhUR)uQy6Xd=?EDdMSmF9G{h-NcdB!%=K6?Ew-1#33GTWia zWIPv+-k_C$@(o9L5j6j-oz-%oa359L)H$=w`TSJEoYddnK(&5lj?)?GXQL7lDD zC`q}(Q)Qv)S}ZwDfQIwLjGOo_E8G4I^C#QSovZ{~*+m>w)IBWBq=!VBH6`=h7yK2HVMc}9Ct?0ZSae*(1d zt+QCOMDv6-Z0`GNKbk$EnpWZ=>eUT7|8z} zg*&$=f8}vDAWy-68<8Hqnw0(%bj`3)rx|*i% zrrI*>>Og6}>;HST4$qfN_aea?2pDc;{DPxUo6QbhW`wGCxOCpwGSQBHtPgOgA3dmU zOzjX~KZQvIP@Xp-Uu{CU3-1 zxmhH$YLq0XKA~E9<``3fq>U2j&V0i8%~0NtZf3y0RUET6?@jC25|}C zb!VVkH3kq*T)+fA;x*y7q?1HIF>XYM4y>atzY-!m*c}>aFe*$t!{92Xu&QgkuR+nY^{(WVBv)Jt`Tt#;OMsul?hvA z*&ojn%8@RCUCJNLeU`Ti!naJ#yXRhCo298-MqVc;u?()02UfS>td;#fz0nX4KD|Ha zhpP7%9fR3&C9LJwadM% z`K)!U<*Xbyv!;RLqww$y@qW_xX6WtYgH&X;<(^!W-WO+O%4EmIJkKf?mRdDYIyvdC zave*Z$=Z*u-;uLk>W>~6-uO4kI|)C4T+eiPVV2K0(y!OarH`U@^fBQPv3)%rYnE%V^6`g) z2+-tpF+SLC!!xy_n`;+Al@N-Rn*7jQVI#`9yUu{##(g zZ)J|dBfxS}%~R7@m*dcZH=D^UxMuoN#$CZ#%(i98y0aJ!1;(Uv%zR$~dQ$tl?@ypQ z>t!}sI=^yhY4)_-S!L_@&0Rv(fpf4&8FiJ3Vbbe3FQK-E zZGkK+yTn89;1=tgcFD2jiDJHgRiw1Nim)0r_M_do&5PY98aEwFgBzkyEhf$V%0-)B z0+=mAZM&gyTpFWlx1j;fVjKIbvv{&G%WivQ7t`svxVmXSZ|R{60o5;<%@29>9U>qb zu%eG%7$<8lQ(y2s%7)M7QJ_w5Q#hC;i0(E@$ogEf+bwI14wIlmdOkyF`YQ= z!r#yr!sO4LU(Y1*VGb;q_P9zI_43p`vXt_HfT$Jitckd}nS(OxWr3nFX!B<7atMC; z%6?q*Vz~2T=r-X>&fp>k68_%Ts2#Yk%H$E*BcGDtH^ht;r$40D0cy}2*JBTLSUlwy zXB;uPW(Hqmxn3|g>YaWF|`;7k@;D&$jPhq?bg*9ms>p|z5ZC7et` z+j(I2of=RAYwiYLoX{Gjk;OJObbs?)L_#iOtD^Pu;*Wz64slH89L+Y)I4>yiHBlU{ zfYFfSNTMu>zZ{jLfQ|Asm>E=koyj0L&g*OU7=YB-@lg37WTm$s?V{tWJvCIKT=dH1 z8yuhy`07|glqlyduf~~XhA%jbkE(z?aD+o@Y+7n1`_h@UK)J@`>=I8@yr5x{BOUZoce3`5b+Gov-$&=!estXAGcxzkNrp?bjt7SqG>s+UFKU%* zvi_3Y5#;JelZwBI2g4nxwbXm%J9;nFt(}Jw_W4qXSGzmvil4UsyF#({iCeZMuly6z zzeK#+jR3d>f4T^L5AnWZg%N{Ot>S(q7)pTUJI*>9H%nuO$!_;pXzu4?(khds ztIKSyC4CwUum4rrUC>-}y&1KsWp4Ane^2WNsO|PM$CblNZl4g;CdVk@DfcrBI6MBD zt(cp%OAq!Pur#6n^}N_KmVjdfe>ac449I2APfWiC@xYA|02kVecGqzq|1RpCpV!-( zz;}Igi)*)^;Q!}U19{;-`U7M`y5)=K+i9KRkzl71AqOYwvuzLgXr>Mmz3_f`YI@Op zS!Fi2htqj0PM&_}%E3nO>nZM#D~kQM&RIHOH+9s1O=qj)`k>X;wHo$%SI0-Av<$=j zQC1`lh@iIB0M}jw%&!$+BZ&?HlL%P$>g4G?m+|!6x$uhK(jpWeP4X5B=8Ui(+TkdR zto3BJV^^Hr)X?bXB)cmdDk(eZX$RK1ksa~7u_y_-*{A&WtjT;d`Uw)TY1}wt_w>Zo z=UG*1#CL>oRq@Z%Nu2qPbKd9Y%7`9~OQrI@g6!{wWCE~~s-z%`+17@~ru3Q0Pei}tMH5T3w$fZZk<7E zD%R1a!j*#Rl@`n1InQ&P4$_X^bGr-k^Sl-ReZ-Ucp+nH7y(8 zxsTl2TA$uW>etO%`}^L}U&3E^6U~BmgqosxqcIkav5!_|Ya}xRW+a5~P9uDqv!q?$ zvvj7B_;E7|V>q5o9aEXTFMmp7yb*dzV?d-Je+pwnOb&R{zhY)H1<|e)e>MxFd095Q zW~q&Vu@E2N3VGc>$hC58(78rf;?zcPln#ad!vJJDHTZ^TN<*Ok6~=_7NbnEDp)A3BYVk z0c^@r;Q6jbm`()<3l-ShSwLG#ktTXUk+jREJRe?-m)oho6?%mxP@a5hsZa)k76@Wd zFw3^C17~EpZe&<3nceuR6e!O+z2}GquiID`aGKiEUYxhQ_`QmNuq= zbZC@(1C3>!i3;li`$C6Qm9ZuJVnIXO%e16AV5YJawe3RJRa$38!?x5~ar{Y4+tO3= zSxCLRx_HL4s=Q^HjjsGJMKh;$r4&`AVQ_ys8WRs?tTI5H)rWd5RQf>Th1X_srWHpL zV41bRyMmbP^uaG;$Zvn*dGz3OUmDg(GsvDZkyIFTO~*%Mrj@pkjnRX_m|dH@K#9O@ zm>6aj*<_PoLN9hgL#EZ9EwBR)V>^4ml2X6;aA21ShBSZ}&EM+4Tdw%xsQxNrrZ~_x zI*s57Y%D9~!-H%97fE9qUs+?_imJb*b_LWe8=z^8;0Ajuh^;k0D(vU1mF{$xT= zSWPG=O0wU-;=ko&*0&vvG%-jRutV=2NBH zHFROltOW1=;m{>ERKe=Q`OlZlFrC@D&Xg{+CY>tG)h`_N2y z%eY@2LvkxZXR+2w686{QVv-c+`dn-Qb!-jPx4pl<;20xmQ-I83U?QiIN@Tz%GUP23 zIFyHHV2j6Kd+p-YCf3-P@J3+b;c(R%Jv&Ud?R%f=xK7N^{+t&v$G($d2h(f@!*mMm zUl^SKGA-N9jIXV{Sh@`2YOTu8D~-fe{#t;yvL-X;X;mwnVbv8S`7ZDpt+5{u5v40m z?2S8vT{xB;kZvPUnR;_$j-W!7R!nh4C99GG)Q5S7`C-C5iGJg}RgmWkmI>B}eF!>l zw=B%o2l5&0abXv=izF-s@%3J>ux4O4kkh||dal%Am?)d*5BGe2kNOiuf#r9l4mTr( z7On|qf0Q&Qlgs+PsWVV@?Y!Z=Ig)C=G#SpzovC$o-_ju+S25!gjQw`qhm|c3cEZwxbi6){O-fahyG@vY+7+(qBCZ!8| zp7%Y4;@ZV^9R#z;vf*xbAIbooy0He^+bCZP)<%-vvl`arI1!q}$g4t)stHZQ8J0>n zcYUtLpzy~$)o)36Iytw!>eTC>A7;5YeHJafvE$3uvo&c1bAGeaX@=d~62SbCw{6Mw zly{%HY2|7aorfuGpa-u|R)Y!Cm}K!{^r2HBAsZdG8rkH2jQd5#yF9JX%3qTRMna$b z;9x~Gt@nLU#KW2Qeb_IDm=5r12+*AjbY>enh1#2N;VAqSq)Hkm&CxAb9ZYAP31AFO zk1J)=V2TEME3Pviz=YZyi9Me;NzVhXJKLa9nR*!mvIw;?0}SQ`ojDnBPi2a$!GJd} z?PtW57hu6oc`f=Kt^BhNrz`H5z(^bP_}vLNLKS{}Hev&Xg9`8!F`@dw-{^$F6X_3g zh=P0iyq{5RkU8TaLa3a62w0@!CNe7*d|Sa#8-LH7Gf!ijHEJ;ycF9Dm2?MNXjO z{(F)+ZZ`M3p*mET&y7mSY>W|_oGa6kd&8BHlfpQuUMw^Etzd+zH{qf1BY(Tu9zUN4 zO^;pF`{!Gqfhx--;5O`XB?l5_AeoOhbL33g+-J<3+oaAqIVw4?-Io~}Ov04s5IqHZ zm?!Bvd4@yDLtScX4ov#gZexGP5U7Yg%8@E8%(2i41`H8s~zOnzybu+oozVB*v{K172{qOCh)X|&bVp;}`srsk!zkL$5 zdQ9NUZm*VF8O@@G133*Rzkitzv_tz_x9wdT@nQz5K6TjJGp)pCE8m1Hxkt>Dj<@$K zwzBOew(??kyiazr_s}0V$-P=hS(O29(T7PbP&<60dD$lD5W!fcvjW44%gzDKXz)}njTRC}-TuO=3%^(TBsUcHv>@4*HFYcDoF~Bvgi8HWJSDT`eV^&puU4dbR6%BP*?2a`d^O))$vJti2HiLw!roh|; zPaQ}qNl2>d3O6+ct_$Vn4Wc%chJ3S#C`AHmMB#jS$VzK00hs3pO{$a)KiK%%^@~aA zfO1f}Q0o9N3eRx@F6&A``U@LXR@a)&L|bc%O4pQ>KCyZxrQlk%6c)udRo0ov2B=C4 z(iGTyF9v{oQaSP>9W&hK+C;9ts``fFSlc6)Wp$drW)?JmZ7OMN%c9D|Qj>w=y*dVB z`i8`YS?8nCt` zZfG&oCxWKXc?p){6YLVEKO}-_drI;OxTyZyjkj5?-IvogZdMlLu&L9BOrrcE9bZ{h zQJOR>EDEBkS5<-sdO66|&{pbbGa*C(YthhMJmvp!9>!`y<)9UqSJf0t$V@k#oSCI= zKs_->pO&B>YdA*PC`CJxpB%m7AOz&oY<{aP@o_ZQ07EJNfO!l&0>aKh zg4O_n(Oz-sXmc>~va6vbE-SAqC!qF)%8_noCZzX=I-YOpntCeC{^HRVLyJ$$O~@iW z1S^i7m7UE%GM<#lOXMT0s`E$Yk|iw5!*%Ho`P=n3=xGZ>dw=QgJ*NwR&lKetmi~|# z&}|@cJIwa|yJx88t6VTL!)!MwgozQ_CJ=-~`0hOnvzQ-Vy!1ZGF*?d~0TXc<1NBI% zUc9n|;uzf+xJw0(A!vys)?E@5y?lS##Gl;H3b#I*4lP0kW@=t^05l~QKPL1hYHQ7k zs+Pcu7Ne~OZ<898mpn!pYg2LnDxLx?n17i%EPvKOljdOzuV%N+LLydii{2GKe;Xmj zbL0Ek{gUKesr9M+Xgn))K6L>S|5rQ(5}uOz7rSk;GFU@uKYi0QMh1sofNyTIS- zssK{yODe3sj5R5&{eRO6CsLGE%7P0ys53hE1O3y=+E5AA`XSYI6lmv53m)Ti6_vE* zB~{8|=9d(<;tpSnU|T}q8;}9(3$&4qrr)g2LPr{CJ(ydLR8B5Ue!A3WG3))UJ-%J~ zUEvsQt@<{i8lbKHhP91{4US=Q>RHaZyiI@6#^f>zuX99A{9Ni{a3lMrFbJO- zv)@CFpnCGpCIV=m%=NV)z8DK|AFQ>t!##E65Et$j0qT8;xU0JgVaU4*(>oTMR4mLr z<>3*cEaE!G*%@TFFk|yN<5Cflh^HD7UZi%?ge=ITQR1WUl%r2<+JRT3_a6-Z!w;E# zG3fUC%w9iQp*FrG9rwFcqOsndDC8g4Bp-C~A8n#tsiQ0qr{yT0ZLto(VY>(y_btBD zwrPLXKp)RxN}qMH8jf*vr1!5pz=2bwcXNPu8KN}wBze+@IO3zE-cQK9EfW7%;~Pbac*2@*ICYA{VKn+1e@8AD@sfPuAZF#Ll}3orlO^ek1a5(g!E(a|>B? zy~HuXH!hc?E{Hn4__0Shr#Z&T%*x9Xo_p?i9{u_054`<9|1D2lO)6-trH|XUrr5pMf_z> z`b`|uiufAICj2Um|C=y&VQR8ms}5B~@^G1AV#@HIBBF*d<`hAbGKLhv!gdxaMr?W2 zH+E0Bn@Z2iLFI zF-TC1E$Ic$2PPiN(!IxkAP%kvD{ko(6JJVhiMzJ%@Nh`NgrgCG(;@SH)kc{PkCoue^Atb-Vb3{o7yv zkH7lWAKZ37|K3^#JJZ*m3%va5?R#J2zVz}Be+PN`3!nP50{qkMXVK5R2;ckC-+%ep zSEKj#cbE6>-;JLfM5qkNlKdSxyZ@&8{Fa0_D+X3a5pZUApKfj%A{O9*Q zZ_U9szHsMVic`O51*`>)_LKl#1j*I#?>iOcTfjmz$N zXL+G)kT@MD375c;D9Q&;Ggj-HGFH2<1tAz5M^FsA#zm|g4}Kr}QMixtfN-3O<1|Wn zAe6-7c+$NlPbSGYNuro12_sY-MJ{ZK3n|0}PiTnea_zo*jorA&?h{7x*H4tyHuB&s zxO5KQGA)ky$=6%ZE9l3p(i`NQ%DU zRWGb0{(=ok^c}5fnQr4Ea%oQw08RHjUQu>ExV5@5NbKZ0moNKg$!v1R)O59E0Q zn&oz?YcG7>vQ@_ua$nr_|GY_D%#?awy0D~QWR^<=aB*21ZF%y9&ZcwFmWFOq28~Qi{%7wJv}(S{$|6 zW!B`HEE2OpytbNtI%O{4_1@_X6Pu*R}w!NX_BgU$$CXO6ShqU(f?>n6 zfa*O0n1Uv-{&A5!1%1Yy!AVJQ)bM6FeW zB1hnM6i#(U9cIXz;$j#JrxIcB6fTjAraf<5SwcCyb?AWCnguHnO~}#fGHTS0R-|aP zPpd^|c-;Ft?Xk(C$5X%IUmi9Inks?tuwN_E*f>)t^&6Uq>y3M0EUbMkUiG4MXr*L)P2#IuB{9^u^o@#*giXv z)yPsYt2VSn`c|~9kYg}!Q?yWtc;|diIAOjy*A@(DS%AE7aGf6Xe8FUB>=baY(=?~C z(8_K<0&+(9EAF9fPnQyjGYc9IR0Zlu#Ep$8i^c3*cMb;JPSng$-H4p{3fv>xYIR3cFuttAp zPd4oxfUepVDAP}c%aQq_Dkyaa@-Ft>wZBj~_f9q7+}l<|EM4^t#zq`Mrko^XWwce_ zNiq-zPiIMtWv}2y+iRQsXl}%$RuaiC@O)r3r)OCa1#k&Rmn_2wpt%SA9)28h@FXi7d~cuOr$ok z5_U*=ty}}2$~ii2!{b!UZ;~`j%!i4!f?PZ;J8zbThM~CfPYsgk+SS{8*SD`+?n{`nNVtw&r$Cza(icJ?# z2yE1M=}fK^> zL#IS*q;l5gli-Bz`cY&6-zVL+H{tIWz}wS71)FeH-P1-;E-FPe|gQmnp++aCY-n# z4b;MZ$X}S1rDK&+7WHDWgJz5LDL$X0rr%nJ^R=t7K!=8zQog9ittDJg2U~(e3Q=+- zH-Kc1QBqN)1{2XCu9p+l8Bqq#{M52XSx}rB;+~x;dQ+i&kXa1#eWE$S`~Z$skD*;9 zf7rxBAu(q$%4~z4Lrmb%tCTT$wM`7>R}H4l9f*TBTXv4-v|Wor1EMl*g@hp6nf1Nt zg|XD&Ey6A)?UarMqJxyUt99FAHP#mt3-3~^cr1?QiUJKcqGM*DtB3Aj2F2P_Z&9h$ zVM-2zX|`#H3)@bTLW}$g4qN?YpF_I>X2VH!VeoKjEY2~|PLGb(-V{f3ZHJFZAg=A! zm1C|x$>G~(>?{_;G}CM?hv5u2&$eP~T+M_yVqPBuxL|su}7vrp0Pc9_bEslxK%{kQTQM@nG`hs8+{pji00j{V_q1 zd~dO5MIK3v$1+C`NixGRovF7fpCqg!>rkq#h}G#JH}OVm3SK2R-5kmEa2qL=#Y3^S z8#Y8OG9D{(<2c@|BJ+}O%I`aPFgXeH5lpgO^u8|__VY=`#Wi>;z9KgWcV7aah8I@&YcAV-V$4Ce>n~-we zY)ff$9h8~buqk=z26RqG$Rp#r6PZ{02O~(VU~p5vnqcEX`7mq302%g|CL_h05ddRx zC2d#oQt6aZWDH|V76kft;?$h>cj9TN`(k=*K?LD!PsL+$*-wlkxCTMfgd5HQuMo0> zn6JQlnZW`{F-!LfgjAk@Rt=Bx?FdFtjJJ{BkewbMKw@NkQxeGhYCNtD{kiQh-)p9}oA53cf(sEc zajIzvc$}gJ6Cuy7u&6IJ31~t>thpaM!NGE}($`FU8XxqfNy*0^y~oya=eKMidFqX- z&8Cx_wQFh!wd#_?A?^C$a12;iL!uYp*G>Zrg$>5POdqxGhEaA#M)D2!G?!XZ-kEpE z6#32)F^;E_>fNYFzrYlCGgVzBcY$RoD-{nRnmyMyWT7d_AK--V+M0d~_9U@e@OzJk zz;P7!=$`jvr0c3LkH>sZmQ1=8s>fsq@}wSyxMilt^a%6`y(EUXDbf4%k|d70Y0zUb zl5nd{@0ZcZC&o+CZbI~g9`4qq9`EZGETZC90-w txi!Q~CUeWh_gW{I^B;fz7f|5Y_57~311;!4PxGMxkOnYZJvc$f2bS-j zOE~`Z>(_#J(>-dtg6^IoBp`shJ4DLm2;L+Cg{BI?n-nO%kBWc}M-fqsK*tDGL!A$k zXYC4`E((Cl@c0+#+qwquhNHWN?U(?c*-p*5+J6Z8+6f}}B)~_(&i{T!lQH^7^YDef zAYXT(!X*ArsQj~gG=3)@^_%g+rT!XEas$tX-tPn|{$?QNE|7%Qj5xXi0SOWKJ!sh4 zv-=G!(3}8_z!WHsVF6E*1)djpumbo{^Fmtr9~KI{q7-#_1veT)K0;oRuc7DaBj_Vn zcvuhk7=A-swNIJ}pWolo`@0QR2zh`ju8-;mnd4(mb8ap}c?5iLG(^UC9I;6C1GDf` z&ovN{lPm1I4jP1_>qnwLNR~8TJG$nb5AM!KE$}VZ1Lq@@MhFQPbz<}t;k<%hybo=>HOaH;sIHLXrBFUi@n zo+U^j&an$qG%mZ zSCYJ9-c}nJYZ}eDxvdftD!}Z;W1DUKo|p(~gnZ`XSeG3#+{9-mxS_!ZB$b!i^ZBd!j#rvAX`~%X3G0>r#&#DH96TWY-OF{ z*>oy5YQGUqtxhRyXWmO!J3Zd!AA_>d9F&(0)!Dc;RTB$5PAu{4?y&>$lXxhIIoUns zsMK{XEG_3WuN9Wlvi2I!U9bV0YnBx$jR$21w^G$|VeNHQX~)Q{JYm(>&eOP%@Y?6$ zV`9PfqZah>3n@+}3xZJ>^fWbRMABMDv4?3UBk4*jJ=m2@tJhklzFc^D(`|CS$y&-f zn!2<1l5jS8mSPpUCopO=uS)ucv&Q?irMg|-w~Cp%crM4>Rokwy17nY!8+Pm=JKgTK zlG!1$IU{o@$f5O_|KL3 z>H=%AjJSToJN`p-l8CJo)>bT4)b{hr@f3}wzPw#$uj@8v3n_`L3h6^Q9KM=wus)z7An`f^jB`F|5puYTXW?_K+w2lo*IkVJ&|`2dz-iVWC; zpE3@0-0%U;PZ>#}uJ}nt6;XfrBrDRt+R`*SzCP)(%&)c_iO#D3)FTN3T{J$+*gIR_ z)9fSkZdaFf9YhvojbsgyrgwXig^&|CUgvcR8jvM;iZfVV{Bw;}Bzbq6GXMK+DxU|2 zjoKPI;Q!afDiev|ZD;R-u(c@>)Ky&{gsl>dNA_P~NW|@c)ja=34MhliB$;dsnvs74 DUuV?0 diff --git a/atos/launch/launch_atosfleetmanagement.py b/atos/launch/launch_atosfleetmanagement.py index b96fef9d1..2d7f6c3f2 100644 --- a/atos/launch/launch_atosfleetmanagement.py +++ b/atos/launch/launch_atosfleetmanagement.py @@ -80,8 +80,43 @@ def generate_launch_description(): package='atos', namespace='atos', executable='atos_truck_simulator', - name='atos_truck_simulator', + name='atos_truck_simulator_1', output='screen', + parameters=[ + {'uid': 'L5S-TRUCK-SIM-1'}, + {'start_index': 0}, + {'target_speed_kmh': 80.0}, + {'acceleration_mps2': 2.0}, + {'ignore_warning_speed_commands': True}, + ], + ), + Node( + condition=IfCondition(with_simulator), + package='atos', + namespace='atos', + executable='atos_truck_simulator', + name='atos_truck_simulator_2', + output='screen', + parameters=[ + {'uid': 'L5S-TRUCK-SIM-2'}, + {'start_index': 250}, + {'target_speed_kmh': 40.0}, + {'acceleration_mps2': 2.0}, + ], + ), + Node( + condition=IfCondition(with_simulator), + package='atos', + namespace='atos', + executable='atos_truck_simulator', + name='atos_truck_simulator_3', + output='screen', + parameters=[ + {'uid': 'L5S-TRUCK-SIM-3'}, + {'start_index': 500}, + {'target_speed_kmh': 40.0}, + {'acceleration_mps2': 2.0}, + ], ), Node( condition=IfCondition(PythonExpression(['not ', foxbridge])), diff --git a/atos_gui/atos_gui/static/fleet_map.js b/atos_gui/atos_gui/static/fleet_map.js index 3ed8ff0ba..7807d02b4 100644 --- a/atos_gui/atos_gui/static/fleet_map.js +++ b/atos_gui/atos_gui/static/fleet_map.js @@ -135,6 +135,38 @@ }); } + function computeAheadDistanceMap(trucks, pathLengthMeters) { + const result = {}; + const candidates = trucks + .filter(function(item) { + return item.tcp_connected && Number.isFinite(Number(item.distance_m)); + }) + .map(function(item) { + return { + uid: String(item.uid || "truck"), + distance_m: Number(item.distance_m), + }; + }) + .sort(function(a, b) { return a.distance_m - b.distance_m; }); + + if (candidates.length < 2) { + return result; + } + + for (let i = 0; i < candidates.length; i += 1) { + const current = candidates[i]; + const next = candidates[(i + 1) % candidates.length]; + let gap = next.distance_m - current.distance_m; + if (gap <= 0 && pathLengthMeters > 0) { + gap += pathLengthMeters; + } + if (gap > 0) { + result[current.uid] = gap; + } + } + return result; + } + function renderSvg(container, coords, trucks) { const width = Math.max(container.clientWidth, 700); const height = Math.max(container.clientHeight, 420); @@ -171,23 +203,49 @@ const totalMeters = computeTotalLengthMeters(coords); const totalKm = totalMeters / 1000.0; + const aheadDistanceMap = computeAheadDistanceMap(trucks, totalMeters); + const truckCircles = trucks.map(function(item) { const p = toSvgXY(Number(item.lon), Number(item.lat)); const uid = String(item.uid || "truck"); const speedKmh = Number(item.speed_kmh || 0); const courseDeg = Number(item.course_deg || 0); const color = item.tcp_connected ? "#dc2626" : "#6b7280"; + const ahead = aheadDistanceMap[uid]; + const aheadText = + speedKmh > 0.1 && Number.isFinite(ahead) + ? (" next: " + ahead.toFixed(1) + " m") + : " next: -"; return ( "" + "" + "" + - uid + " " + speedKmh.toFixed(1) + " km/h @" + courseDeg.toFixed(0) + "°" + + uid + " " + speedKmh.toFixed(1) + " km/h @" + courseDeg.toFixed(0) + "°" + aheadText + "" + "" ); }).join(""); + const truckRows = trucks.map(function(item) { + const uid = String(item.uid || "truck"); + const speedKmh = Number(item.speed_kmh || 0); + const courseDeg = Number(item.course_deg || 0); + const ahead = aheadDistanceMap[uid]; + const aheadCell = + speedKmh > 0.1 && Number.isFinite(ahead) + ? ahead.toFixed(1) + " m" + : "-"; + return ( + "" + + "" + uid + "" + + "" + speedKmh.toFixed(1) + " km/h" + + "" + courseDeg.toFixed(0) + "°" + + "" + aheadCell + "" + + "" + ); + }).join(""); + container.innerHTML = "" + @@ -199,6 +257,13 @@ "
Total length (Vincenty): " + totalMeters.toFixed(2) + " m
" + "
Total length: " + totalKm.toFixed(3) + " km
" + "
Live trucks: " + trucks.length + "
" + + "
Distance To Next Truck Ahead
" + + "" + + "" + + "" + + "" + + "" + + "" + truckRows + "
TruckSpeedCourseNext ahead
" + ""; } From cd4c76c45de3a3dc8112d4395da0e1ea71a95079 Mon Sep 17 00:00:00 2001 From: Sebastian Lindholm Date: Wed, 15 Apr 2026 08:45:56 +0200 Subject: [PATCH 07/33] removed user path dependencies --- ROS2_ATOSFleetManagement_cheatsheet.pdf | Bin 22888 -> 22882 bytes 1 file changed, 0 insertions(+), 0 deletions(-) diff --git a/ROS2_ATOSFleetManagement_cheatsheet.pdf b/ROS2_ATOSFleetManagement_cheatsheet.pdf index 280387a65af8bf3229cdf4bb0f040ac1f04f83b4..86a5209e0b55e3ac5ca0622ab0acd700a51dc6de 100644 GIT binary patch delta 1429 zcmaixNwTU207dJkQmNG2M82S4_f!zD1D*ml2+BiH5l|38q!Gbp8W-{B8D^aNe-&9q zQW<0!dC3NHM`v*B)~z~!fBp35*H6Du!uJ@`#J&uK{eST7S0VT5yB7ZCck!p^tZC0A zS6dGa#&#gN&;Q06~`1Dg*vPS~xwHr?U1 zOC6(4cx4Wn#kgjL@|h&hFeh2m)T*n(DmYmfbf0y#s&-V`L}ddpjcjF)QZt@dcV z`!m)^O{qMvHrpF1eU(HxbGUL@BQflPc3)OIG2u!jg#LY391HogNMtB%tVd)IVl$;VL^zCSw{m>Y>x z;UJVQg0X;!x7_*qXjj+4nvrP9fiR5J{S2>5ZknFDH+S{s8yqN4;}>4aa9V7lUe6jk%CC0V3tC7XbPQm7efeCb<_UI78Qve= zu-+!SK~tbQ)zM4H-+ft{0A-G9FKV7CH>H|OdIMzu=K`gWib%X=1bim>b-ZGy?^Abl zSwIB>zzc#qn^AX5RqTPDFExz`l`#>K=ZD#35RMe{+O!^*6Pb%nFM<}_yfBX^? zp3dqwW^&quJ%BLzBYV6no26P;yE!VV18XSq*Jp=72#9juz>Gr43@J}CmjRCeBF)FdS=8O*yElQJ9L65FzMCKIKo!ci z_1y*I4rl2)>$z(2hSpIIza64^#@-xFE{8c$%30!v+b}j}_YBgmOBzt3y`HtnbHQk( zCm;yo;V2vk#vK{4o{-unS+*Hfc@Of?&$O4gxBJ!GpI<)c;(Yr{{^bX&@wxGDJ|~}F z?$><~d)F6|e5v!?=NE(ABiPYxwUCzc;db9RHrg$ujka5dHi6@qgV6um`0|JL{IU5` JC=8|1%RdR$tP%hK delta 1406 zcmajbORlO00D$4#rfHhHh9(UUQ4|yf6a-Na6h#z84lhAO@o^M<4_g;;w$OCk+`EeI zph;&Pbs25CfzJNHH~Rnj^6Bp{pMGm;-x4%g{SdX)fARHK4fym8Q~mgT^yACcuGx9x z*orSj!5GQ0#vtmg(IB@0t}qpo70S9UtC!Afxe9kQ#9-TiFFQLE zO{8t0J`rCr?|WEL0bcK2{QG-EJLGvcVj3=TbkXM2oeH*|IZGQun-vsT5HF>%9N)rG z`)u~m1y*sukCir zt8{izDxdt6IIz9<9n5WZO;ukV6gc$SGb zFzNb>@tPaC$J}QdeLtoNj?zIcqpyiM- z9ns`k76h}gs6R!{UvE2?q=syvDK<-~A$-oBgP~j#U{RXZE0H#LL&}u7q`8gnEqS1U zz>;)fYlCX!Lubr$d*MAzz`?$3Z)7v<PjFn)%sn^az$p4;-kD{j-o{oBQLItkSQ4On>?AQ~l#7 x=Ldgz2_(>0P@GwgQ53jF_xk3XH)_w46fPMP#R{{xUvn Date: Thu, 16 Apr 2026 09:38:52 +0200 Subject: [PATCH 08/33] Docker container supported --- Dockerfile | 7 +++- README.md | 53 +++++++++++++++++++++++- ROS2_ATOSFleetManagement_cheatsheet.pdf | Bin 22882 -> 24192 bytes atos_gui/atos_gui/main.py | 10 ++--- atos_gui/atos_gui/static/fleet_map.js | 8 +++- docker-compose-fleetmanagement.yml | 22 ++++++++++ scripts/atosfleetmanagement.env.example | 5 +++ scripts/atosfleetmanagement.service | 18 ++++++++ scripts/run_atosfleetmanagement.sh | 19 +++++++++ 9 files changed, 131 insertions(+), 11 deletions(-) create mode 100644 docker-compose-fleetmanagement.yml create mode 100644 scripts/atosfleetmanagement.env.example create mode 100644 scripts/atosfleetmanagement.service create mode 100644 scripts/run_atosfleetmanagement.sh diff --git a/Dockerfile b/Dockerfile index b90a777db..75a805678 100644 --- a/Dockerfile +++ b/Dockerfile @@ -24,4 +24,9 @@ RUN --mount=type=cache,target=/var/cache/apt \ ./scripts/installation/install_deps.sh ${REPO_DIR} COPY . . RUN ./scripts/installation/install_atos.sh ${REPO_DIR} -WORKDIR /root/atos_ws \ No newline at end of file +WORKDIR /root/atos_ws +RUN chmod +x /root/atos_git/scripts/run_atosfleetmanagement.sh + +EXPOSE 8420 8765 9090 8114 + +CMD ["/bin/bash", "-lc", "/root/atos_git/scripts/run_atosfleetmanagement.sh"] diff --git a/README.md b/README.md index d1c10ec3b..b8c478f54 100644 --- a/README.md +++ b/README.md @@ -30,7 +30,7 @@ docker run --network="host" --ipc=host --privileged -it -v ~/.astazero/ATOS/:/r ``` If you run Docker Desktop you will need to specify the ports to expose to the host computer. ```bash -docker run --ipc=host --privileged -it -v ~/.astazero/ATOS/:/root/.astazero/ATOS/ -p 80:80 -p 8080:8080 -p 8081:8081 -p 8082:8082 -p 3000:3000 -p 3443:3443 -p 55555:55555 -p 443:443 -p 9090:9090 astazero/atos_docker_env:latest bash -c "source /root/atos_ws/install/setup.sh ; ros2 launch atos launch_basic.py insecure:=True" +docker run --ipc=host --privileged -it -v ~/.astazero/ATOS/:/root/.astazero/ATOS/ -p 80:80 -p 8080:8080 -p 8081:8081 -p 8082:8082 -p 8420:8420 -p 3443:3443 -p 55555:55555 -p 443:443 -p 9090:9090 astazero/atos_docker_env:latest bash -c "source /root/atos_ws/install/setup.sh ; ros2 launch atos launch_basic.py insecure:=True" ``` See the [GUI](../Usage/GUI/foxglove.md) documentation on how to enable secure connections. @@ -67,6 +67,56 @@ TruckObjectControl publishes speed commands on: /atos/truck_objects/speed_command ``` +### ATOSFleetManagement in Docker (with or without simulators) +Build and start with docker compose: +```bash +docker compose -f docker-compose-fleetmanagement.yml up --build +``` + +Start with simulated trucks enabled: +```bash +WITH_TRUCK_SIMULATOR=True docker compose -f docker-compose-fleetmanagement.yml up --build +``` + +Start with simulated trucks disabled: +```bash +WITH_TRUCK_SIMULATOR=False docker compose -f docker-compose-fleetmanagement.yml up --build +``` + +Open TruckObjectGUI at: +```text +http://localhost:8420 +``` + +### Run as system service (Docker + systemd) +Example deployment folder on server: `/opt/atos`. + +1. Copy repository to server: +```bash +sudo mkdir -p /opt/atos +sudo rsync -a ./ /opt/atos/ +``` + +2. Install service env file and customize: +```bash +sudo cp /opt/atos/scripts/atosfleetmanagement.env.example /etc/default/atosfleetmanagement +sudo nano /etc/default/atosfleetmanagement +``` + +3. Install and enable service: +```bash +sudo cp /opt/atos/scripts/atosfleetmanagement.service /etc/systemd/system/atosfleetmanagement.service +sudo systemctl daemon-reload +sudo systemctl enable --now atosfleetmanagement +``` + +4. Service management: +```bash +sudo systemctl status atosfleetmanagement +sudo systemctl restart atosfleetmanagement +sudo journalctl -u atosfleetmanagement -f +``` + # Using ATOS with a Graphical User Interface (GUI) Please click [here](https://atos.readthedocs.io/en/latest/Usage/GUI/foxglove/) for instructions on how to use ATOS with a GUI. @@ -94,4 +144,3 @@ This project has partly been funded by the below organisations. The herein expre

- diff --git a/ROS2_ATOSFleetManagement_cheatsheet.pdf b/ROS2_ATOSFleetManagement_cheatsheet.pdf index 86a5209e0b55e3ac5ca0622ab0acd700a51dc6de..848d2719169757ae45e2e55ce7a313cf1c977bf5 100644 GIT binary patch delta 4228 zcmai%yU+7@R>$>@7_IbbBpM0|5Kk`7)sAn89mk0s$8mg1oO|Nf&c*R1wsUdfB+~98 z2oe<_q#Zrgwn)&!)KJj!Cs6HHNOTm`Jnt-GHM0dSe){^zKIf-n{m#eVU;opmzxd}* zzi>ZM-+I4z|JP6WC+xlW>6f3X-~KoDz5L7fpMCYsxBvP1Q|-5rzx^8g?(e_)kFVZ; z-u>pgUw-{7=2QLsPrHBhiG2TB*ngtl13~(T{G-tRM11#O!oU6AS19uSFADGn*ceyA^rs^71*_hbK0-hVZ2y>Rj~!+gF!63;$=7VXCzr%s9^=D7BO0m{H?qHmi= zt|I7Xr%NOTqK>oLB7XJu5YM_u`)Y$RoCqcn%** zuEfr}(KS7Q7?~{jR*#8?0|lJcx34=N(N<^J+;mDbFfFQ0w$a>&ke0 zD2=?b+}{iiw7i)gY>c=u3s>;l>s`B1Gxut1{n*XY*cpcSOqgxkWo(|^l_~))l)(V0yd%+rv9-)b?-yBn?qbywr6XSUNCy}VNW^tP`h4u0L1 zm9Uu2sd&CQ?Pju9Va|Mffe};WcAHD?Q4~a8@`^TdJlM-x_RZ9w^RFcbwyQ!qv zJM}UjEl2fc$jBfcnz`)q2>LW#tsDqPd_CxM#&IoT$@Xn`5l(rN+`_?PJSyil`?=|^ z^uS*tTLbm(JxJlZK#E#=9g^8S9e%eL=e)Wk$1t7o1@r37`UYciH8}s!@Yj1gwI z-s)>!%|Wy=K9%HQW9ODdvS?Ix&+>9~db!aEoConD&n$CygQwMF+TiMr#e!`t7;FAO zGIzBDJ$I^P#KEE@oWZethU(^q=;FuTdK zGqiWWVi=l%Rl{d)Q?|>51)}7iBqGe01C-e+v%qFeEWhZ^iU7{Z9HT)cCF@EW}YDq)i%(Yy@s)7#S zAj^nTic%}T+JhHLxjOanFi=G&9lD(wUf@u-R_u^ zyKJ_bR6)U=FSjPxh0=})FETZ8D9uw`Bwk8KAY=u(c$O*?_{k#7>0wuD$JMK^aIuV- zrrs^bV3r9sL#5Z+v%)fPt!sV$AFmRA`jCDI%|}TnEZ^?=4}M zm&PC2GQh1`6cGKq;M0~LLRIn7a!Zr!X;#~Xmv-32cLX+t8hi=ji{%R!T{f@kte__G zP_MRWT=F~q<%ZOWWtG1StA|eDH}6=}K90BB2A?qL^W&C@yU&tdT!J`My>;UaaeLg4 z0$53chpKjV8+v8CZ4y5psREYDw2P(jezB8!4zYzYntoUi!}(R9BbOY&EIG)$wE4C$ z9&}L-8P`Z5RpR;>zSjdQEbwwbKV(brS#dMYfzU}GHFa~a?%wEYFEwvX1OA?{rBPQn z8f$BkJGQ-4nq?Zy3a0?A*KbvF>Rtp;BO`;5Y@ID-6h*gUeI>A|(gfz7T#3!U`j)xX znv+RE)VE|nfRz!}5jvHDw~lZlS&!y2H@D*%<3j5}!6Xo&wdwcKj$0WPJA(i0y7QU* zq<1~FFKr7+ly(or-j=3~jK9q`aA|%W_v=9gc&g1VJROO%=3MuR6;K;_p?}o9*M4Xm z)B%v&FZmefnzRbnr}O^g5{J~?s`ty6;%GhfI`U}S_NUhE(M)=aK8iZP?VgWQD{tl9 zr%%}8az2}aHfpTN^Wq#UO-GUK7nVbcEoA(*yJ(JX)jF`xcGq|_hcHy)j}k%aQ!n_m zw+OW1m+E|&l7rdQ(+hV}Y?9#(NJ@-TM*;)mNvob4_*EjaRHe6#NKVY=3sW;)@B(ILc7_=Q*dcEASYUTe%iq*oj-&0 zHdnf7m)WKuj7v8enh(@W(U{HvmdZYq6ggx2_jP!Sq_6{=OY1D;fdv`4si&cmy!E6SCi0NmMljvXp1rb@U6nL8U&(o1m^3m=n7q+z6~RxS zyltd#6;m`M8sjE_Z48+b*SUtaTw`5j68a-^Y3JGbYKo3;ABcq)HKWzZcq{o*!d~s} zGhaQl#pvJ_z(UppMNBM+)P7!K(>ITpneh}^mIkh|Hmm5(1+aPL3Dn&uxk%k)a}>zU zk=X&_US{lcR#JZN>g86tr7G@0&0{m7iCUbgO81;}ax0CdCwJJ~R==CRs|9M!$i3=E zBh{&__DLqfXi?z#I_H#R=sXk!*X+^AdK1F-SN>a zJ!X>guDA%c8mK+XVqeOqq&-d_#nv?h_XDxoHa}oLaFe2j5Z3NQXnUKa>Xl{I9B+k^?}S(;EG-?g;dGapCQc4lT!4yN*0+*Vj59X2Zy z>|1hqYS*T4X0fCbDW|48^HD25b2_%GQ+Tpa;A>^T-7c#s=of}*$cX5*Gh7(jBvrYI z+}=8NPvMX*MW;ax9kzK>i8ax<_N#s>?Q@wsai!f5Y$)6saC$vrYrLocZDX=joilwf zY`4=6i#@(~Wx5LXIJ?d=aWARu?3k$bzbv5<6 z#NFFcsh{&zI2D(Rdx)y#D5fWmls=}*jq^=yj^IH$-@!Yqa}>jT&ktVBcJs!kwqS+L zAyT0fG?+NPp9lWIMt5=gxUh~Vo11Fz7YFf3Kx>e1nziiI@`%nRhz2Ktbjm(VpzdP= z0v~@NgBwe=4-Rn`ihF$lAW#k0deiZIvtgc5yNXM@W951up`&w4%v*)m4bAlW0v+27 zwR)P(3|4PPR0VUD(+RcHxe)5=ys1~l!mgjh$uy4!vdtZ9w{kXgy3=8Qi!d5lTbpOy zGEYmMp=;7jOu^(V!=2ZSMlP8g()IHB1mQnE1RrPMr^!z||1xId23a zU?_%vIOZP|!LT}pz5kj1M;)y0;3)1RRR?iuHM?flNuRPk%p*y6UneN0`Tsrf{_py< thJLZ+pW#UyKi*K}H#gJp=iWz?elsZS8|Md1B|M>R3 z^Vax{`~4g8`u-c_4Sp5g{_Z#bRsRwE_3PK){PGn_@2~jVKmG0X7ytRqi~B|Sw}12P zzfdpz+h4wv{&#Qq>)-nJTjPas{2TH5j_bZ*zx`+KU;p5ZeEqGE`xE#B`j~yx)L+N$ zM~&Itcc;jIJ%vAe{ku^5Lkw!5a28*A^8+ce!}oUky~ae3%l!lSs0o2{ z`&fU}e>@O|83U|Pjc?TlH1Q>7 z9UF47XRbC){F15KR@Nyzo?V0syFD|f1|=~HDSN3jz)cH*u&}yI@pIyl&b2or-ajcp zm%p&-8A~vk%=BOfa)A}+0N69b%VX)RQX&F0U27;63{$@X1+cX;u%KEzn0bvq>0~!c zZ}SFMShtSTnj*9>KAX|DPa}O{_Q#A8s*KiYVB7R#LE5C)y2e{`>TyXpA<;~V6Gy^c zC`7_~2I8No^YzgvFM}m5Q?d=9Xj}IatR_2gd}!XB#ize!GeV(owsTYZbfV6)1$N2q zSNd=PO$9a07u-yoqXT`^eYS(!kU?t*z?$M?zge#%Z$17xnoRwk)Mv;#+28Ay;JVTQkRNKr1njsW#I$u-5!s9|F+rJGkHA$> z#ZR)U3G;rva}ORkOgbu1DQSj??sFhnwZPfu0TOR5OwC(t1l8KPq&C)0kCVV_KK1PTTD%Junq0xmbix8(P35*{@+GE1rxT zE}8)ejNpsyFkA*odn!JqX)Plz4~mVc35TQA%rfq{%v3L5x{aB7DKdI$A$Ahn341M) zzFj)41|c=<`1i|mBx*s$9Q*THCJnDGu1g(14Sxn7&qC>8}J6+!}G>r2WxuYQbS7A zHiUi+uhkZz$1?0K&o&M}aiHUTW|9ulPT(S+ICREK!}555pGDpACN2-KD6?m~PL|td zR3O>Zwh_wi&+=7jIcolf)DRZC?T_Pxx!KyZ7^XxyWr|;K8=ltEJpr`qoJuRlot`w> z=iHiz59uH{^|`PotncuzIK~xX>oA>Kw5m*kEbtxWUYwug@HLmXCj$STKHhc_c^d5p zK(FglxpFCgugNQikHULrCdN&yxwi8r-8Wp{1uoqPY}9W+x0gVBst0MVEt><4rN>eD zsgOG>oA;Vk;`j(4XgZUJA}o$sI$q!xR_53p=Sa`Dv zE&Nu!lyaj&rjnsIoRth4wka^owBBLJdv&|5Uoz35&>MZ~3YlNF`#r|1Mtg{M7Dt>% z=n}EoiCI{s>}ClWL3nA2d0cI{!MW^1GNS9M187E|J`pO?v`AP$*5FW-~&U>D7lvOqN$xmvxzn=&lNkuW(~huu0HEsMj;`wq*DveiD!#Vr93fG zj6D*d#;yRg*zvVMAE$7m&PYB~i01n_U!N)H6@nczkf5fnZBgL*^>L%~MO$s%6Q&c{ zs(U&NlCH4sV#jg$p)m!L8q51-q0MhQa`}Kc?K~f>E$yjMt!saKo$yLdSCN&k=7bvv z3}*LaI+3SUXMu>6nhKaNPl-Rrc~vx+p}Jdnw??wt#8r^gj-nP{B(CHN=+%|h&Ba{D zaY9nfGpk8RP1kokxg>}-Pfd-~aCf)R>CUS(C$=^;7SHZFRT|mY5a+$iYDX@Q#XhIS z%;>ZfHMgki_D)Rn&@XQLX+t_sF%UKsIumRY9MEp&HR_bku!S}jxUS}pzLf3>N|#)g@qIH!%vw`_nZ+_P{PEh&d(%hzf{MUrA3y^JgN zBRuw7^#i!0Hp{VjB+4k;k19asq!|}ue$wtsFJ1P^CJsU!)p=s&9hbrNBIZ^qM$KH7vneHi z1<^DurKp98G^zclNm|?*h(_7U9*6MqC|6Rq0%>{8Ef^$IV+28YFMLSsBUh1xFP5iS z&MnmQ+w;h<7%%yxy7d{5hqr=5b^{QFb}M5T&1RsmgNGt*sp!>Kvj+EVs5SbX*NkE^8S>Gu37l5u7>0B z4c>g&QNA7t){RFd!Xi#`(dQCZbJF1^0{KEfW%jMaorOAOpMtiPUY<$5xo@O4sU^io z(>)R-_r8+~9b`^Z%xU8|K77jU`yGpk^CbHaLV_M^yWdgM_b*-jVJ`iPtLwc(A29mb z8_y?Iwi gAb-M2@R#ma?LT?S?t1^R8wgS->hHN+U*_Kb2SSaW2><{9 diff --git a/atos_gui/atos_gui/main.py b/atos_gui/atos_gui/main.py index 48b926e18..b25814483 100644 --- a/atos_gui/atos_gui/main.py +++ b/atos_gui/atos_gui/main.py @@ -221,7 +221,7 @@ def ros_main() -> None: def print_access_hint() -> None: scheme = "https" if USE_SSL else "http" - print(f"TruckObjectGUI ready. Open {scheme}://localhost:3000", flush=True) + print(f"TruckObjectGUI ready. Open {scheme}://localhost:8420", flush=True) # Start the ROS node logic in a thread managed by nicegui. @@ -232,7 +232,8 @@ def print_access_hint() -> None: uvicorn_args = { "uvicorn_reload_dirs": str(Path(__file__).parent.resolve()), - "port": 3000, + "host": "0.0.0.0", + "port": 8420, "show": False, "title": "TruckObjectGUI" if FLEET_MODE else "ATOS GUI", } @@ -244,7 +245,4 @@ def print_access_hint() -> None: ui.run(**uvicorn_args) if USE_SSL: - print( - "ATTENTION: Using SSL, use https://localhost:3000 to access the GUI instead", - flush=True, - ) + print("ATTENTION: Using SSL, use https://localhost:8420 to access the GUI instead", flush=True) diff --git a/atos_gui/atos_gui/static/fleet_map.js b/atos_gui/atos_gui/static/fleet_map.js index 7807d02b4..2db4e9642 100644 --- a/atos_gui/atos_gui/static/fleet_map.js +++ b/atos_gui/atos_gui/static/fleet_map.js @@ -208,7 +208,9 @@ const truckCircles = trucks.map(function(item) { const p = toSvgXY(Number(item.lon), Number(item.lat)); const uid = String(item.uid || "truck"); - const speedKmh = Number(item.speed_kmh || 0); + const speedKmh = Number.isFinite(Number(item.speed_mps)) + ? Number(item.speed_mps) * 3.6 + : Number(item.speed_kmh || 0); const courseDeg = Number(item.course_deg || 0); const color = item.tcp_connected ? "#dc2626" : "#6b7280"; const ahead = aheadDistanceMap[uid]; @@ -229,7 +231,9 @@ const truckRows = trucks.map(function(item) { const uid = String(item.uid || "truck"); - const speedKmh = Number(item.speed_kmh || 0); + const speedKmh = Number.isFinite(Number(item.speed_mps)) + ? Number(item.speed_mps) * 3.6 + : Number(item.speed_kmh || 0); const courseDeg = Number(item.course_deg || 0); const ahead = aheadDistanceMap[uid]; const aheadCell = diff --git a/docker-compose-fleetmanagement.yml b/docker-compose-fleetmanagement.yml new file mode 100644 index 000000000..49d35cb09 --- /dev/null +++ b/docker-compose-fleetmanagement.yml @@ -0,0 +1,22 @@ +version: "3.2" +services: + atos-fleetmanagement: + container_name: atos-fleetmanagement + image: astazero/atos_docker_env:latest + build: + context: . + dockerfile: ./Dockerfile + ipc: host + privileged: true + stdin_open: true + tty: true + restart: unless-stopped + environment: + - ROS_DOMAIN_ID=${ROS_DOMAIN_ID} + - INSECURE=${INSECURE:-True} + - WITH_TRUCK_SIMULATOR=${WITH_TRUCK_SIMULATOR:-False} + - FOXBRIDGE=${FOXBRIDGE:-True} + volumes: + - ~/.astazero/ATOS/:/root/.astazero/ATOS/ + network_mode: "host" + command: /bin/bash -lc "/root/atos_git/scripts/run_atosfleetmanagement.sh" diff --git a/scripts/atosfleetmanagement.env.example b/scripts/atosfleetmanagement.env.example new file mode 100644 index 000000000..07b91f8b6 --- /dev/null +++ b/scripts/atosfleetmanagement.env.example @@ -0,0 +1,5 @@ +# Copy this file to /etc/default/atosfleetmanagement and adjust values. +INSECURE=True +WITH_TRUCK_SIMULATOR=False +FOXBRIDGE=True +ROS_DOMAIN_ID=42 diff --git a/scripts/atosfleetmanagement.service b/scripts/atosfleetmanagement.service new file mode 100644 index 000000000..3c7c121f6 --- /dev/null +++ b/scripts/atosfleetmanagement.service @@ -0,0 +1,18 @@ +[Unit] +Description=ATOSFleetManagement Docker Service +After=docker.service network-online.target +Wants=network-online.target +Requires=docker.service + +[Service] +Type=oneshot +RemainAfterExit=yes +WorkingDirectory=/opt/atos +EnvironmentFile=-/etc/default/atosfleetmanagement +ExecStart=/usr/bin/docker compose -f /opt/atos/docker-compose-fleetmanagement.yml up -d --build +ExecStop=/usr/bin/docker compose -f /opt/atos/docker-compose-fleetmanagement.yml down +TimeoutStartSec=0 +TimeoutStopSec=120 + +[Install] +WantedBy=multi-user.target diff --git a/scripts/run_atosfleetmanagement.sh b/scripts/run_atosfleetmanagement.sh new file mode 100644 index 000000000..e73f3071a --- /dev/null +++ b/scripts/run_atosfleetmanagement.sh @@ -0,0 +1,19 @@ +#!/usr/bin/env bash + +set -euo pipefail + +INSECURE="${INSECURE:-True}" +WITH_TRUCK_SIMULATOR="${WITH_TRUCK_SIMULATOR:-False}" +FOXBRIDGE="${FOXBRIDGE:-True}" + +source /root/atos_ws/install/setup.sh + +echo "Starting ATOSFleetManagement with:" +echo " INSECURE=${INSECURE}" +echo " WITH_TRUCK_SIMULATOR=${WITH_TRUCK_SIMULATOR}" +echo " FOXBRIDGE=${FOXBRIDGE}" + +exec ros2 launch atos launch_atosfleetmanagement.py \ + insecure:="${INSECURE}" \ + with_truck_simulator:="${WITH_TRUCK_SIMULATOR}" \ + foxbridge:="${FOXBRIDGE}" From 5e8ac6b638d8ca5ce83303d826e1c6e998fdf9b0 Mon Sep 17 00:00:00 2001 From: Sebastian Lindholm Date: Thu, 16 Apr 2026 09:47:34 +0200 Subject: [PATCH 09/33] updated cheatsheet --- ROS2_ATOSFleetManagement_cheatsheet.pdf | Bin 24192 -> 24908 bytes 1 file changed, 0 insertions(+), 0 deletions(-) diff --git a/ROS2_ATOSFleetManagement_cheatsheet.pdf b/ROS2_ATOSFleetManagement_cheatsheet.pdf index 848d2719169757ae45e2e55ce7a313cf1c977bf5..85192d242a5538c141d191ef58c66fb9d6d4176f 100644 GIT binary patch delta 4510 zcmai%JM+4Xa>wVHN%cQPx>T7nk0J<&A|Mw*Pyq$GctH_S5JkMB;H^x|dw9CMip-ok zP3pXdyp5Fk<)k`Qw)z%Z?CkD*_p|@<@4x-$fBg1uf3p6rLDz{t{@&XDRs7{`m4Exv zx37Z5~w;2>K zopnCD4p{g&?%lM~Ar_r{!i_m_y7j=MduV7%N8P5ALOJMr)HXY8HXug(oG^Cn-N_hz z&E*>acm0Q>0vDNv#XkL}K%(~2_hBUxEjXByZ zeJxad*4(YU6Wtj-yz8mbK_%?9do($TG&F&OuE)Y7hmzzS&t)6n%PIl1kFeIQ#f*@{?vuFnf@ zNy_FVyM4F?qlL7(s4%1JzL0|^tMP^4AeZt-re#M=>z9mZK?ODWI33+D2FyP0=Z!T# zxnKY7H=1R=eD@@?z(;Jk*sLf{E+|3V+-zZZ%x3Z`8L|`3x3zKCY&mY>Su8v9f;d^# zHC8#WZJW$c-OxRf7xK?Fkk+ZOZQ7eRzp|IvJgq?PGsZAz&ZO0cW4OC%T_ZaX->$B@ zUp>p*DzF^CwNHa=Xbw?vnu#w3$+Iu(@WZWCH&WMLNaMJ_HWXktNAtUSpCtF_QGlPj z%WT|+D$Z-6&HI#{&EMQWp)P0ZGl1rjOSiPglnA&o00!#A_9J_FRg+Fz=qAN`wh58V z_cT$2dBsp+$J>~1+dbxTR?mM@~xzOd)8k~ZSuGp*V*8R=o^?QC0K8-JLPN&0EI5XO~pgJ z`5vzmoNv%hjug&^Y0ggkUZq72=S%$Y9I6ji&DM8pT;Hcay4FkAvM@C|)+fHYWBMvdb5<83=tkHG4BpT%i0)I><{YIDm3FbiCw} z$5|*2PRunGul&Z!PLX3({8?hCx-B%Nc(uw(++XdlQbfA9E0Irxmb7@+R03 zG_bb>CM#dzsdq)z&uGC}wL6^0$3iUbP$vsWJsmF$c`Xq?z9vdNw@c{jk{(ABL97sJ zSvVk5Fsz(T3P3UbK*Bb!eSi+OU-K3fly+r7Uq`IdYx^$(oq1yW6{Ti=PAxn{P>H9@ z^(1_-OT4AN`pE+y&9QDf$k@!q<0R!bedx1VThR09ZVKIL5I4+tw_m%|3PH0`RUo67 z)gK{nDaxM$wgE377ora$coc%KGMFnZa8#(zuMsj?a4ra)ObaNzxpcfa*_{HGp0kNN zZI!8^+SlCiF55J++`oOy8yl|g4%^x=J-E>0dgE!GU)Uw>bj-cdD+}0*$I6%gKlB`pn6udpxmZN4a=!KL!C~C-a3oBb$J&%l_fH`@?b)=j%Iy3DaZGfI5ZAtkSxDSa;ad- zu?;5`b7U&5NIPNMiDTYjG^|M9^A(#O>0ucS^afvByUTT1py{dF>@d>D?VW3-{O&TW zFmKFuaZ!d_M6&Pi-GXuP83o=wq78p8?HP3IRw-`W&(^)x?X{wI73h$SxA%3?CH4=x zh=v>nHi2d3*=)YovfZyOX$tnO?=EXyNu_vtl4p@Psvu`SqLWzpJhEqAn>3A;MY_#% zvc#v4U}>JPDg*Cwfq%^QXj_PHmf|Gr;(YE@Tj$FBZA@nkt10gE?379~nVnKp-KW?7 z+M%OYt5LH-&FisvjkArty;MiZcUR5N@MhraJ!!}fJ^}KuCvLtv3~mkH+auuio&hPh z2GgoNTU`#~^h4T!j8rGb(!S+r?8;k3^V6=JnQUD1g2o}nf;xWwabNs)imv|ZRhHGy z^o)UdJ!TyauMf5Aqx238zhZjk_bPWJc@6BxwE$|kZiKzMrK~t*z<45(q*$tw=Bdz` zQ+$H^LkL#Q&Tf{+V+b}6%f6U%;)q(kD%!=LJD(fXU$2gMcR0qoEq=J1dS@h?t4lO7 zn)2Ri_{I|6TSDBg-$$x9_$XWDJFO(|C+kQJ8?^9YF!PYjw=Pz!LLQ^*kRZ->_O8y~kP(JrmbG*Ex_1hY ziNm}HP{l%j`u@+-{+|3F)c)~jU;Pp^P5iA1yHtdC3wx;BIUIoohmBxPlo!x3B6TLz z9xEl6CIq0gj1Ig>nJ)#B+YE6cIitd)U#4XI%D<9r%MqWq+<Ro08XS~Xhbl{6MdISV~NEADsip0Ki z_`x!JNkF}jM7#WUwNIw8Sia}2_?_;>RkJ;p)uw(rXZo%Ttv?<9x~}lrAs(NY%q?5l zT^Md8RGV0ZDS)JxFKGG}zEv5o)-6Vjr=gree5dH4(YN25`z60sM%hzM>Zk60JUkMU z91{Y#vaWmaR1gF`$+nsXTRT=VTLo(=?s@IgX;9tHC%K)B$L^8 zh3Z3r{$Z6-+}Uzp6WfdR>p{IJ6>Ndq6$PPs$SOEYoorZdK!DD?80HgO1+w@2(U#L@->O{SLaa26xOa=g z(VM+{1lra`ad`fIY(WP)_Pp=jk3;-2zc+{N7I`iQBhvi!U=`L$0yO32wR+N6_{CI(OE^o^{o?aN`#s(x=*q%4dNE`jS6vW(My?0{sB+H zS)n;mZ&?>e;jQVpFk@0duExnCa-Poe|Il>S&?Y z!_FDKXbGLWgHd0}6>Ilaz+-|}RD1_beR=K|-tMsR)55?vW<3q|7HBVOYR{3DBc)~c zL($%*c*wb%;$hpk&?=4NFz65ccW<0;`&DmJop8u9z)&3$+a;Q4KOa-(AaEi4j@e4} zM!W&RLJoW2CVxpW!=D#Ap|g?6Q`M!C8eJ+kZ}wDTuug!T5geSVvP8 z>J%m_K$y5mea-n(SleCU> z%LBxpt%~Xjih};OxPOP&o5Rs}r_^AN)`Ol>7%bJ4F0?d(=ZZf&!nL$=i5GNr=WVLg z>`k_{SIXv<)t3!t^v_GN8VX`PdDY|n01b@M!X$=PTIH5oiz>bG+F(Xg=V#|w&ey84|(Z{aTrDuRe8h$x7NJSje+2q+>C0zROq$ywFj!~aH_oJ}{~ zbls$P(rf7VoHS{AHl5WwlgZ?h&rIeu^B@2I+du!?Z~yYA$v-q21og*1PQ1T~zq}{q z-@XL(i)DYk-g)TVeo5ZjAn2>=OAtRevtSL}Elsy~;p?J#^G9T^G-+Rm)Y%|gZf3!n zNtLF$7w}t%*ITG6zB+soYTVm7b(W-D}--cd}jhG%kNg0Qh z>+jlaWmZ^^;|8kwEP}g^46Pw0i30>)8%1d$Hi9@dn}VR~kigK_qnzv z2i>-P()#=NaSb$Y7%CK#(!i@VjnWaW$AkzSmBDuK5sf(`4_D7pI>G$e_-^B)aCLG| z*cU{$pl>#i`T4e-S=Z44F{j}Ynb*V3G9exSIm4whLSEe?I@t|~MtWqj1Jf|sLk%f2 zs7gHB87Bk>9!5vIMJu!ifX-|m^aOAxdhQbsx27i55~LC3-fC1VJw${3slc4A^2fs+ z`udwaCmHXg=+xA+*rf^jxigP!oF97Yu;4WH-J>rwr{{i24ybGFruNS}>W{egIqe_O z9lxFL(80ky!fMy0{Wdx#n;LrE!XDUN zeoG~G%`YUZD?L6C)x@PzC{(aFk)QYLo{cUx6|gsckqltCJ~6}~_?!ZbM*peggE9d6 z2OrpsE2=ay#!xOzxsUBR?YuHCl_n)h9H zH1y6!n2xAMZ$;4ivI&91E`};}i0J)$1n!Sj176|Ob&Xca+d?s_RQ$KPt)v*VF%`?j z^)=no+m+I*(QOwyH+5m%_A@qk)bUZ_jPhrnDgg4PvK*pA{akWHUc4t=ZYCD}E$}s` z0^270&PRJuZ{Sj^t)rUBR*u#p3(jJU$+3@k1yFi8183Wn^-D*O)P zDjRL?gLv&Z{Nult^KXkL|8AzXZaJFwAsJVTNu+Kdi{3Z#*@no=z(w=2NwvUZ^;((+ zwrS(5?WZLbZ11 z)BEAuT356@o0+WvL&(uEx<4Boa@!K_v(|YSN4;0G=Uo=A+ZA#axV@jnZ1jQ6>J}l; z)^n5{$mjcY5s`Wxzcp=mJTN;i7*NFRLRXrF;-CsIPMf1V_DL5l>gIVv&Wwl1qyaU> zIBHt_>V{icOh-81ShOjz0P~Y3aWZ2ksR&BUycI``IO(u}5cF!!d!{|zkDkn9Trm2p zSa{_eJRgY{Yws+f%OETO9NT4g1P$urNxu$hCke!@qJp4`b92;rnm((YoyZm7CB}bv zm@9uLj_vBb<+37~E)|nVecl|3!}>Jbr-YU7=Z3-?UbbcfbU&@wWJBy9CKG%Z)MwS9 z7=1>8u~t6JajZ>bx00toaITJBhOvr~Tj~CgRemR{A`gJ9E}KU*E}R2uDx9@JpzJeudC zKB2$Z_2iIvH-bAtQc&XcDl{uEO6pO`6U<@O?=)hoDNs%annqD4BjYLrCjFX(psn|P z8?XsN0SQV^;LBpF;{|-Sj=SE2q*%#A6)I8%$_P5pJR3K zly3;*Lhw_+uMZV*0Y4Sgm}B3`w|)=6{T@6_qhvD z|N7>V(end{sq0?lP~NQQkEKz9^P#(TAZ)OCuoPO_LtTBlZ;sSu63y+cSLTe}3ST^bXo_vxR)1Iwcgh`Z zeg~sZxpf{$={c+*m6DA}O{pT$Cj;hyXGpH?Y}MGp)4E&LgPO{4w+JHdzA6rPGb zkCoos9)j7pxbTNNxqJ%DS|8|ZYH-%d%{AG>9p2XD7bIVUdbbxULk@DymmLLqthFuZ z+qqbUVSRV<2T_W0&vE^*FX%bgKPM$=%$+Ulhc;(I!TL$ledF!c1ap!4j|4eQrPjdy;rcYltE!3q6C2)x|5>1aRQAWe zn%tLa(*K76uan6YFX=2tG%XIyrj1(p6j z63VZe$#_T&cqwO1F2OQFn1NUnf*+OC7s(fX)}@1$$QpDn-6s|)+r zPj#Jbd(xY~R*R(wp57arH%#$?n-@CuZOL97E+ww|S>r7=zgegL>D|#R?9db7^G6p$ zt^d+h@08XL$CU(BH5KINbH-3o~+GXS+xX7A57 zmcA_1pAvs^E~9HQW8m~UZR4|nV5^xfS&vC8DgY5*e31_(NRY0|??Hb&CSB`G$M~SB z$Ss4NAjx0=_*}zY4n!(>2+fUi-Jhs#8K^_VB3P!8`^iSPP#7flDKH-%luPwe2gFS< zaYnlK*b>2Wt9EYX7FNh=)e}m$A(|a4b9K@*mG14S_2$?VC=ZDt=U=3xJdWaTZ_s-R z1sADvP(!Z3ahJ`q*KHb|J^YvfmyL6|di+Yq{(2_7bHtgJdv?3Dazb)=h|}p+r2JZh zk$CW4BH+)eF@19M;Ovtxv3fE$U;}Lv`gpZ44hQy=4qAkKywo4BIX=JjrLt4`KJnss zA~KoB(z}n{D>gfWl&&`e^?JoU07}r2DcELxAs#1rmaocqta$vT{j8M|f3%uS+y<*t z?Y(_79s9Z!Sf(vMr2aVtN!W$X9GU+wq% z@4x;gn$1qL`M Date: Thu, 16 Apr 2026 13:31:18 +0200 Subject: [PATCH 10/33] trying to build docker image --- scripts/installation/install_deps.sh | 41 ++++++++++++++++++++++++++-- 1 file changed, 38 insertions(+), 3 deletions(-) diff --git a/scripts/installation/install_deps.sh b/scripts/installation/install_deps.sh index 5d643fc80..32787af0c 100755 --- a/scripts/installation/install_deps.sh +++ b/scripts/installation/install_deps.sh @@ -23,6 +23,37 @@ if [ -z "${ROS_DISTRO:-}" ]; then echo "ROS_DISTRO is not set. Source /opt/ros//setup.bash before running this script." exit 1 fi +apt_update_retry() { + local attempts=5 + local delay=5 + local i=1 + while [ "$i" -le "$attempts" ]; do + if sudo apt-get update; then + return 0 + fi + echo "apt update failed (attempt ${i}/${attempts}); cleaning apt cache and retrying..." + sudo apt-get clean + sudo rm -rf /var/lib/apt/lists/* + sleep "$delay" + i=$((i + 1)) + done + return 1 +} + +apt_install_retry() { + local attempts=3 + local delay=5 + local i=1 + while [ "$i" -le "$attempts" ]; do + if sudo apt-get install -y "$@"; then + return 0 + fi + echo "apt install failed (attempt ${i}/${attempts}); retrying..." + sleep "$delay" + i=$((i + 1)) + done + return 1 +} PIP_INSTALL_CMD=(python3 -m pip install) if python3 -m pip help install 2>/dev/null | grep -q -- "--break-system-packages"; then @@ -32,7 +63,9 @@ fi # Update and install required dependencies specified in dependencies.txt and requirements.txt file apt_deps=$(cat ${ATOS_REPO_PATH}/scripts/installation/dependencies.txt | tr '\n' ' ') echo "Installing dependencies... $apt_deps" -sudo apt update && sudo apt install -y ${apt_deps} +apt_update_retry +apt_install_retry ${apt_deps} +apt_install_retry python3-pip "${PIP_INSTALL_CMD[@]}" -r ${ATOS_REPO_PATH}/scripts/installation/requirements.txt # Check if apt failed to install dependencies @@ -46,7 +79,8 @@ if ! (apt list | grep -q "ros-$ROS_DISTRO-desktop"); then echo "Adding the ROS2 $ROS_DISTRO apt repository..." # Install ROS2 prerequisites - sudo apt update && sudo apt install -y lsb-release ros-dev-tools + apt_update_retry + apt_install_retry lsb-release ros-dev-tools # Authorize the ROS2 gpg key with apt sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \ @@ -60,7 +94,8 @@ fi # Install ROS2 packages echo "Installing ROS2 packages..." -sudo apt install -y \ +apt_update_retry +apt_install_retry \ ros-${ROS_DISTRO}-desktop \ python3-rosdep \ ros-${ROS_DISTRO}-launch-pytest From 4bfc28ace134d4e94aca5ca6d831a7cd9bc2a16e Mon Sep 17 00:00:00 2001 From: Sebastian Lindholm Date: Thu, 16 Apr 2026 14:20:06 +0200 Subject: [PATCH 11/33] handling COT->path_name and send back request/info to pf on ctp --- atos_gui/atos_gui/main.py | 100 ++++++++++++++++------ atos_gui/atos_gui/static/fleet_map.js | 115 +++++++++++++++++++++----- 2 files changed, 169 insertions(+), 46 deletions(-) diff --git a/atos_gui/atos_gui/main.py b/atos_gui/atos_gui/main.py index b25814483..fc40bf69e 100644 --- a/atos_gui/atos_gui/main.py +++ b/atos_gui/atos_gui/main.py @@ -26,6 +26,7 @@ FLEET_STATIC_DIR = Path(__file__).parent / "static" FLEET_STATE_LOCK = threading.Lock() FLEET_TRUCK_STATES: dict[str, dict] = {} +FLEET_GEOJSON_CACHE: dict[str, dict] = {} @@ -35,18 +36,18 @@ def main() -> None: pass -def _candidate_geojson_paths() -> list[Path]: +def _candidate_conf_dirs() -> list[Path]: candidates = [ - Path.home() / ".astazero/ATOS/conf" / GEOJSON_NAME, - Path.home() / "atos_ws/src/atos/conf/conf" / GEOJSON_NAME, - Path.home() / "Documents/repos/ATOS/conf/conf" / GEOJSON_NAME, + Path.home() / ".astazero/ATOS/conf", + Path.home() / "atos_ws/src/atos/conf/conf", + Path.home() / "Documents/repos/ATOS/conf/conf", ] try: from ament_index_python.packages import get_package_prefix atos_prefix = Path(get_package_prefix("atos")) - candidates.append(atos_prefix / "etc/conf" / GEOJSON_NAME) + candidates.append(atos_prefix / "etc/conf") except Exception: pass @@ -62,20 +63,65 @@ def _candidate_geojson_paths() -> list[Path]: return unique_candidates -def _load_fleet_geojson() -> tuple[dict | None, Path | None]: - for path in _candidate_geojson_paths(): - if path.exists(): - try: - return json.loads(path.read_text()), path - except Exception: - continue - return None, None +def _resolve_geojson_path(path_name: str) -> Path | None: + sanitized = Path(path_name).name + if not sanitized: + return None + direct = Path(path_name) + if direct.is_absolute() and direct.exists(): + return direct -def _fleet_snapshot_payload() -> str: + for conf_dir in _candidate_conf_dirs(): + candidate = conf_dir / sanitized + if candidate.exists(): + return candidate + + return None + + +def _load_geojson_by_name(path_name: str) -> tuple[dict | None, Path | None]: + sanitized = Path(path_name).name + if not sanitized: + return None, None + + if sanitized in FLEET_GEOJSON_CACHE: + return FLEET_GEOJSON_CACHE[sanitized], _resolve_geojson_path(sanitized) + + path = _resolve_geojson_path(sanitized) + if not path: + return None, None + + try: + payload = json.loads(path.read_text()) + FLEET_GEOJSON_CACHE[sanitized] = payload + return payload, path + except Exception: + return None, None + + +def _fleet_map_payload_json() -> str: with FLEET_STATE_LOCK: trucks = list(FLEET_TRUCK_STATES.values()) - return json.dumps(trucks).replace(" None: if not uid: return + if payload.get("path_name"): + payload["path_name"] = Path(str(payload["path_name"])).name + with FLEET_STATE_LOCK: FLEET_TRUCK_STATES[str(uid)] = payload @@ -136,25 +185,24 @@ def render_home() -> None: ) with ui.tab_panel(road_tab): - geojson, source_path = _load_fleet_geojson() - ui.label("RuralRoad centerline visualization").classes("text-h5") + default_geojson, source_path = _load_geojson_by_name(GEOJSON_NAME) + ui.label("Path visualization by COT path_name").classes("text-h5") - if not geojson: - ui.label("Could not load geojson file.").classes("text-red-600") - ui.markdown("Searched:\n" + "\n".join([f"- `{p}`" for p in _candidate_geojson_paths()])) + if not default_geojson: + ui.label("Could not load default geojson file.").classes("text-red-600") + ui.markdown("Searched conf dirs:\n" + "\n".join([f"- `{p}`" for p in _candidate_conf_dirs()])) return - ui.label(f"Source: {source_path}").classes("text-sm text-gray-600") - map_id = "rural-road-map" + ui.label(f"Default source: {source_path}").classes("text-sm text-gray-600") + map_id = "fleet-road-map" ui.html(f'
') - geojson_payload = json.dumps(geojson).replace(" {{ - const payload = {geojson_payload}; + const payload = {_fleet_map_payload_json()}; const containerId = "{map_id}"; let attempts = 0; const maxAttempts = 60; @@ -164,7 +212,7 @@ def render_home() -> None: const hasEl = !!document.getElementById(containerId); if (hasFn && hasEl) {{ clearInterval(timer); - window.renderFleetRoadMap(containerId, payload, {_fleet_snapshot_payload()}); + window.renderFleetRoadMap(containerId, payload); return; }} if (attempts >= maxAttempts) {{ @@ -183,7 +231,7 @@ def render_home() -> None: ui.timer( 0.5, lambda: ui.run_javascript( - f"window.updateFleetTruckStates('{map_id}', {_fleet_snapshot_payload()});" + f"window.updateFleetRoadMap('{map_id}', {_fleet_map_payload_json()});" ), ) diff --git a/atos_gui/atos_gui/static/fleet_map.js b/atos_gui/atos_gui/static/fleet_map.js index 2db4e9642..d6feac4b1 100644 --- a/atos_gui/atos_gui/static/fleet_map.js +++ b/atos_gui/atos_gui/static/fleet_map.js @@ -132,14 +132,52 @@ return item && Number.isFinite(Number(item.lat)) && Number.isFinite(Number(item.lon)); + }).map(function(item) { + const normalized = Object.assign({}, item); + normalized.uid = String(item.uid || "truck"); + normalized.path_name = item.path_name ? String(item.path_name) : ""; + normalized.path_index = Number.isFinite(Number(item.path_index)) ? Number(item.path_index) : -1; + return normalized; }); } - function computeAheadDistanceMap(trucks, pathLengthMeters) { + function selectPathName(state) { + const trucks = state.trucks || []; + const counts = {}; + trucks.forEach(function(item) { + const pathName = item.path_name || state.defaultPathName; + if (!pathName) { + return; + } + counts[pathName] = (counts[pathName] || 0) + 1; + }); + + let bestName = state.defaultPathName; + let bestCount = -1; + Object.keys(counts).forEach(function(name) { + if (counts[name] > bestCount) { + bestCount = counts[name]; + bestName = name; + } + }); + + if (!bestName || !state.pathsByName[bestName]) { + const available = Object.keys(state.pathsByName); + if (available.length > 0) { + bestName = available[0]; + } + } + return bestName; + } + + function computeAheadDistanceMap(trucks, pathLengthMeters, selectedPathName) { const result = {}; const candidates = trucks .filter(function(item) { - return item.tcp_connected && Number.isFinite(Number(item.distance_m)); + const itemPath = item.path_name || selectedPathName; + return itemPath === selectedPathName && + item.tcp_connected && + Number.isFinite(Number(item.distance_m)); }) .map(function(item) { return { @@ -167,7 +205,7 @@ return result; } - function renderSvg(container, coords, trucks) { + function renderSvg(container, coords, trucks, selectedPathName, pathsByName) { const width = Math.max(container.clientWidth, 700); const height = Math.max(container.clientHeight, 420); const padding = 30; @@ -203,15 +241,21 @@ const totalMeters = computeTotalLengthMeters(coords); const totalKm = totalMeters / 1000.0; - const aheadDistanceMap = computeAheadDistanceMap(trucks, totalMeters); + const filteredTrucks = trucks.filter(function(item) { + const itemPath = item.path_name || selectedPathName; + return itemPath === selectedPathName; + }); + + const aheadDistanceMap = computeAheadDistanceMap(filteredTrucks, totalMeters, selectedPathName); - const truckCircles = trucks.map(function(item) { + const truckCircles = filteredTrucks.map(function(item) { const p = toSvgXY(Number(item.lon), Number(item.lat)); const uid = String(item.uid || "truck"); const speedKmh = Number.isFinite(Number(item.speed_mps)) ? Number(item.speed_mps) * 3.6 : Number(item.speed_kmh || 0); const courseDeg = Number(item.course_deg || 0); + const pathIndex = Number(item.path_index || -1); const color = item.tcp_connected ? "#dc2626" : "#6b7280"; const ahead = aheadDistanceMap[uid]; const aheadText = @@ -223,18 +267,19 @@ "" + "" + - uid + " " + speedKmh.toFixed(1) + " km/h @" + courseDeg.toFixed(0) + "°" + aheadText + + uid + " [idx " + pathIndex + "] " + speedKmh.toFixed(1) + " km/h @" + courseDeg.toFixed(0) + "°" + aheadText + "" + "" ); }).join(""); - const truckRows = trucks.map(function(item) { + const truckRows = filteredTrucks.map(function(item) { const uid = String(item.uid || "truck"); const speedKmh = Number.isFinite(Number(item.speed_mps)) ? Number(item.speed_mps) * 3.6 : Number(item.speed_kmh || 0); const courseDeg = Number(item.course_deg || 0); + const pathIndex = Number(item.path_index || -1); const ahead = aheadDistanceMap[uid]; const aheadCell = speedKmh > 0.1 && Number.isFinite(ahead) @@ -243,6 +288,7 @@ return ( "" + "" + uid + "" + + "" + pathIndex + "" + "" + speedKmh.toFixed(1) + " km/h" + "" + courseDeg.toFixed(0) + "°" + "" + aheadCell + "" + @@ -250,6 +296,15 @@ ); }).join(""); + const allPathNames = Object.keys(pathsByName).sort(); + const pathSummary = allPathNames.map(function(pathName) { + const count = trucks.filter(function(t) { + const tPath = t.path_name || selectedPathName; + return tPath === pathName; + }).length; + return "
  • " + pathName + ": " + count + " truck(s)
  • "; + }).join(""); + container.innerHTML = "" + @@ -257,13 +312,17 @@ truckCircles + "" + "
    " + + "
    Selected path: " + selectedPathName + "
    " + "
    Path points: " + coords.length + "
    " + "
    Total length (Vincenty): " + totalMeters.toFixed(2) + " m
    " + "
    Total length: " + totalKm.toFixed(3) + " km
    " + - "
    Live trucks: " + trucks.length + "
    " + + "
    Live trucks on selected path: " + filteredTrucks.length + "
    " + + "
    Available paths in payload
    " + + "
      " + pathSummary + "
    " + "
    Distance To Next Truck Ahead
    " + "" + "" + + "" + "" + "" + "" + @@ -281,29 +340,45 @@ return; } - const coords = findPathCoordinates(state.geojson); - if (coords.length === 0) { - container.innerHTML = "
    No LineString found in geojson.
    "; + const selectedPathName = selectPathName(state); + const geojson = state.pathsByName[selectedPathName]; + const coords = findPathCoordinates(geojson); + + if (!selectedPathName || coords.length === 0) { + container.innerHTML = "
    No valid path found in payload.
    "; return; } - renderSvg(container, coords, normalizeTruckStates(state.trucks)); + + renderSvg(container, coords, state.trucks, selectedPathName, state.pathsByName); + } + + function normalizePayload(payload) { + const normalized = payload || {}; + normalized.default_path_name = String(normalized.default_path_name || ""); + normalized.paths = normalized.paths && typeof normalized.paths === "object" ? normalized.paths : {}; + normalized.trucks = normalizeTruckStates(normalized.trucks || []); + return normalized; } window.__fleetRoadMapState = window.__fleetRoadMapState || {}; - window.renderFleetRoadMap = function(containerId, geojson, trucks) { + window.renderFleetRoadMap = function(containerId, payload) { + const p = normalizePayload(payload); window.__fleetRoadMapState[containerId] = { - geojson: geojson, - trucks: normalizeTruckStates(trucks), + defaultPathName: p.default_path_name, + pathsByName: p.paths, + trucks: p.trucks, }; renderInternal(containerId); }; - window.updateFleetTruckStates = function(containerId, trucks) { - if (!window.__fleetRoadMapState[containerId]) { - return; - } - window.__fleetRoadMapState[containerId].trucks = normalizeTruckStates(trucks); + window.updateFleetRoadMap = function(containerId, payload) { + const p = normalizePayload(payload); + window.__fleetRoadMapState[containerId] = { + defaultPathName: p.default_path_name, + pathsByName: p.paths, + trucks: p.trucks, + }; renderInternal(containerId); }; })(); From 9160c24f8223cfc77d7f49fccd93b8f784771c84 Mon Sep 17 00:00:00 2001 From: Sebastian Lindholm Date: Fri, 17 Apr 2026 14:33:22 +0200 Subject: [PATCH 12/33] tcp data to pf updated --- ROS2_ATOSFleetManagement_cheatsheet.pdf | Bin 24908 -> 20494 bytes atos_gui/atos_gui/static/fleet_map.js | 4 ++-- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ROS2_ATOSFleetManagement_cheatsheet.pdf b/ROS2_ATOSFleetManagement_cheatsheet.pdf index 85192d242a5538c141d191ef58c66fb9d6d4176f..1abc53a8565bede23feec5b7ad9ac38620ee8181 100644 GIT binary patch literal 20494 zcma&NbBr%Q8>QK{ZQJIrt$W+HZQHi3+qP}nwr$(J-#0ti*<>c!{qIdxQmN!Uc}~?y z6}h~qI6X512MqaO(O}nL%U~`H3lS5MouMTR0Kh11Vr%AXPQ?135k*FE3u|W+M@DgL z17{Oa6C*og6MlXeCuc_!0~;9k>}mD!nnMnxu1{L$8KFg3H6kc|jRhZn$Ro%GAa1ZR z?T63u5^KJ!1$kdtCRC^vs*J1RiuG-_=dEv_kIKSW&%`D06Ml&sL){xaU%u}zk=Iie z(ABhExxvlxFn$*W;oQzvy$(56H|gz7I{}oW_(W1U;KDxUzZ2OrR=^A$m znQQx`Y)N&e{>+={6Kf|wPcNeup1eLg>;c-0xTyMI60#J*C}tDhp+wB=fIDys^5_~% ztpkzN4H1G#a9WR(IqmGW*X4uv9RgxW5HknPDErgC4175V)SMF#6$}gyNrNJsXvuux zvV5zF?6-mk5GvC$11fwy`3uw@HzG@h1^>Fu{FuOb3J6d`v4h8YBgQwn^N${x$~K>@ zT*vfgx8;LIjSjI^>d>N4ATn~CYg`8GH(IDf2NeWK24!Hhs=8?>m2ZBOQBC9IXLo+k znt7kpYZg%IwUG zf49%eSPz=*on-!rtP*syb=T2fZW0&YUJjR^ZJtLh1kihFj}wR>!(1Yk*d`z(*m6pX z(h25j~{FjH=akwq4k zckFkZdqe$T;dNiYK^#E*xJWadGJkmyppJPqQ~d@1Q1zFa>jP(c#AznJ8aMbqLfI+R z%iC0^_bFhPZx{jMD_C4fM|AnvjM)$hqd6`!=aI_&rxHzz{su`$VrN>9JapEppG+|! ze@TkI$k`8`oNQi|juyzJ2Fg`=>8~&sq*e97*B^M%TPhU%S9Hd2 z%xp-KtPwrgGW3`4h-&uV2PT@?6c4L(mm%+8FsOqPJ zhTstkepbbVuv--zrFzFVC?fM~I0}#Im$X@79;Ig60ymfFYF4JXNO%54u*>WYHU9Mk zPdeg=nb@dh*jdXrDi`j;6Q_{ZL?CO0*;Yp43?mEg1+^HLiyCu;8#lhVJ^ACCtz_C1 zWpZvp+_@C8uv_!(xVY9wXlF5GhoH){t(nNjQ%o6t?gxUN%b@0)&d z>hVPX0zs6`S8$Lg@DI?ZI>Fb4)mwSTY*9yJzfbx|pnCkxBl|xVWAd}xy`_FU7iE;t z+kIOSv;X4W->%~p?F3x(9#P-CKQEoz|832M%}maX-n@9dK2G5)xqUuj5Ux3XMjLUS-`BP1@lur95#qTd{%=qVrS(>~!D^pqfFu^CYTGvSEV-bSBmHl$ur9={jUT z69j_Qs8Wd}M{3#+CXf)(_XRH(4FDbSy3x8LEVv@Kxv^_GPoHt+D;pI7#D-;AYe=T0 zyx&@ksrk4sHCSD2m>Bjx!Qq(sL4GCE1Bi=GLac*lGf@D;joIh~f*V6&f#Kf3xcZgn zYafXr+Jk)vj{r734p3FO1oGGn93tofaNr^$P^u6%Gd5V@MpGwS984|62d3CPHe-5F zEH#)R|6)Q2>P6r`7YETB{WL=##&r1!2%r}2P6?jBzswJs zi#8P5aV8A}(p?Ox2OqrqZ2fR{lW*(nQ&Szw`qTD~!7-S)C=-~y5JYz}lyAPd(eRw* zed(pzsuryS{1gl}RJqk~m!6Lb2F@j8SnaF9wVG(ii{a@TC!&)7_)eWH>Nw z(ieYzL9v>cS3XvyPoo6Ck)Vi`!D^7I=nSi3N1B~UM+`IRPyMJt8^Bi#cg?Ik@qsI6q$RMFW%#2)>Tm(5tEBZ zZJ0_nV7p9YiuE3`Hcf8f)F?mLXOKprYvf|A6oW^usDDbAy0G{vJJSzW^smtaqxy(J zTZE;1io_6CWAGN8(3!4`pSIk#4@$ykzo@@Qkc)TJn8@G`%!OivN~Sekj8rptQcB*^ z)XSAP2k?u<)6ktlI}Gt@h}_U$BLmVEtr{*BHymQ|g}v4aGb5K*T(6C@`YI~tyidRF zMFAg|?9?>ZTycTC$@bJFAu9aB6yPE6>!J-lxlF5;skvXn`aG8AKk0u zs)JUU8C6wajE@Tq*&9KBz3Ttek%KeQ**$#g*lfx+_238#AnJG4?*bo`DaHZtKjWy7A>MoNIY%AXAfUIeful))xcD*qH1y1zj-Ag7wI8@MZN|;) zXndXZx+Z&n{D4%fNCJOsY3Gnl*Ob_oYo`E6fHom$}rvBOfdVl$_ayoHt z?eY|O!8yY+|Nee#jxq{t{Y=o>`+Of7eSO_+clUzd+}yvsUtW)9*c#jB_Wl|doLg}G zGhofii6CIx9z}tEg>ny>LE#WYl^Cu5d-o;!*}-m zSD+Zks+_C?ua3_~_gEy$`hNn0R9c7pnHgexPaNq$NN(<7g#Ip?ODJK|c*-gqz`F(h z(J4Mk`}#ND`FDSIbf_#qANTDV5jY$+NBXYMa8&oP#)B>z`cEY$zt{Ilpyf+ve^>8kv&1Ecbk}<|` zI)Hc4Lr)2zSIRvh*m?O7$`ZpylV}q;NDQ>4PVi@XaaCT4&r#d+(1`Z)HK>^=p*T`1 z1fhagrg`m+1NC$Uz9l*&burooeT+B6?Azptc@U_ zOSl=!sMezPm6sy2VL3>bB1vm5&RK8m8aB^7Ic9K8EG9epEB7Zso4 zF9{=;R@qvDBSY#r`r*C7+ES84ZrkVAz|hY`vRO?wNlKjK#HN#b(c7f9lTV z_E;o4yOFhohVKo%WL3I3llq*DpUw0 z>z1!6tN^OT2Al}_U<+|7U|QnyHUbb&J&fh@WMCR=OLjSDA)P1C7YH;ZybTAbUm{!wpV%mlBFm~bD~u0J-t?Aq9&=*L zw^aQ*U>3vRPPxZ6u`^}Z11{XZ5`D)S7uy|Z)0Lv9UYMp=ho(^r$^QE2EtX9&X4boN zE3Dxyv4j`U!(beFsMJ&MZZbJUsxItE$i^PbvbqvEumd?YQ@hbb?nS8nW*V?{Zx9r=t@aRMkTW9iNa~A)U2)wB_c|SIEu62 zRx&!?U12>vOH@~~#_OP6*eA6pr0sTIR!+SFh?^@ZykMG%!}nX6Ig>X~hcjd-M`#wn zbB`Q920oL~)sD?1c<4YgdbH~~TIuTK=t#!0;-s{z@xViBjiFsx;~2I%)9d0syWcPc zPL$9Mo-f@T8M$E80V!oAl!larHDQxcbFcIAgq%~!0IdcGXLFvc}mLi``>?7u$sKkSTyo%#Pc!!tAgZ+^!7-)Q3h zD~M=LnYP?!LJE2Hg{kh8KaKvOqXPfb+<=7h3HTAj3@}|>olTH|QqbXiEfLb6Y$(cb zIzdH&ee`-W845?omjSbuWum^l4YM0eLp<@#?SHv5N?#=w<;^UbH*NG~C!~cNs@fGW zu#l-h!z}jI^+Pbhzd^txU3+t^R~BT+Mk~KiK)*tlx$*+rGO>!?ov*|F zOdqqGxy^c+eP1Qo+f--VNxVv#Jqu4#Yc7&wX9cYHF67!XCBpk%_jc*^N2gABM$MHe zB@7^j_TO+d=Y-N{sM-z7mst1mTdDc2^ScX!g3gAyYQYqPN8L@hlRYoJI0b;~Lk!h` zh97+-agiSn|w`@7)$$=fi4y+%fn4v4`M zLN9lEe~F)N_};xB7q!FEk5L_H4$~7<@%)TV1`Cbpr7RcA=B>ER^OdffPH%PC=S5S? zVzG&EW%sjLqadvu(P&TIED3fMulu z`Nwb#lsFNX7`q`}e=K+kD3A(RjHn7k5}l?k&z}et3_8-^Tnkga()?4EwrvFi>_QdY z@sa;@ii~XOWAyd&$Ahx{HRs!7rcvN@CRh_=iGES=SpLb?&tY-v0Q0wyO-LLQVA_?g9!ey5z zCZ1Yz@&=7bv|x?u|w=Bt*@j2khs&)B{w-2pjbmvatH_|+0Y zKc@|&`u~cXn$T5#?9SW~Ve&fXlz?9O)o;n!U|S6^RPvEDV|jpFsaI#ZY2qX>?}?vi zRP#F>a2}cV?wCHpe`#O){pj1UuZ7$OzNxlalORsCnli8hGSj!Mku^bis|-g(9xH*o zNIYR?1seqiae!dKia+?RitCK#X`DHeF`jPr)wJy-GHY&u582I0^%wk43bD;4n?8|1 zH3L@~8P=wq&Y*0xHu==nCOKF~u}w}%&xGExfc)6YAQ&Zh=Ly<%cLG`tNwkAG#syrz zQR(-i(xLxsx@7!rKi|ru;>4L28a_S$mYAaTvX0sF)n96z;4C<~7Nzb&)`$sQ!5Lpk zgIIJ_URG$6r}bljnw`2mhP|oH8!A)Non9oQctuHWeLk?P9ZC>kGJ{w^gS`R0(dPFk>I z!+ZV{uU4c1%Q3=^UKnPop@C@}FDPHTrG9vmVp$j_G4nAH_e>~lqoMm4W&*UK$^C`4 zd3YN%Ou>>32T8Z1SrhK4@UzTFf3#B9)A8tt_|#8Z_g6$uaK(xhrF zC$QX7GZ=2FR%~GfVE^Hz8K@#WX{i^pv8fy=X}SZFGjQ)qWX!I6+_3K`t)t2oSHQo80UZ4dR`vwuYEix)g!ZAvH*3?Q@uxS&;KIdGtNj;0NL1Rt`tw->KP$ z`Faw4_L34IxJPP1vh-yykTLj-1=Aw*@3@3fP6r^OVPlL(zB)LK(lI4v^lv@&+hA}} zBIm!FqDUpG!NtN*$x{i9kPacLZ}F zYw|?QMWN?t#?2}*h88ctG;j(NNFGx$Q%x6kWp0$NM5zl>9EIN8z!00ZanaXsIt;eX zd&)H*mE16IZyRPUu+~(?(A`8ZgabV?8;FXkcMPLR*Z?b-Bd+PrL(#~fcU0>nhO?|9 zl1I6ZEH!yd(1gy<>&x9lXbJ0Q%yR*3FUq2wnQRI-_}rh5T%l_jvUDA$pqO_r%F)BH zrlMC`8?aG@E2H*AyftU;f>@V_fgD335w2zF6V407{r*NSuhe^gD1EKG|>u**5eO8 zsqXn$V4*^%E9^t0pc(CkTe13za2R1q z`OluROoesrW|IzX{VBIY*AwIzH7BNH08S0emo4oYaUt3{07h-ws5J64%#Ce~mx%fF zq~0p?4jo@}%f%&5e*AB{sylX-Jzm;Onw{I^$h)oit3U6{;_U>#vDz{2?nXsqtDF5yt>XQ-*w(;Ty4FBsn5Ja7(I{gM9d6%b4_M*(jZ$`U9KGD^W1z*+P(7YBP{uICn z&;BAjIJD20Z)9G=fTHkw*PO{Q>4lLxX2TRlV&YIEl@$KJROnbjMCu-mrYJ*Q8p*1C zPFLZHp$F#201{p@WG^SkQ}_vEMb`B$p`h+|*Dw-PFVQUa1P3xl?e4Gb*@yW0(St!P z*Bb(EHc19!oC61%k|>iR52e1Jh+t~;BwWVqO$(l|Bs=apd5e>294kUkXP zv10W?gpME<0qrwme6*4VjNgztQ@88yLDG@O?^7AQ=HeIi{mHyj`GFvIoSJ-Cz(NsK$L)opO~A-WPh7yAHq!eB z`W<%@rR1}4(tu1A0ZvFg<5#`hiJM&{C)ZxtccR+qs@(D&L7Op{y=N?2lCi9yv8;@V zELL$7s^{|Ps_LcD4L_M3EIAa^1eVmqLDObXy>{l+p7#%de|mm?@5k(M*1Js>i!<=j z1>R!(ai0c!u6tPTdJk%T+^?&zyPszwYrm~^Qn-CB`M`-Pj8HHk(>HTkbW>=#!)44+ z#;4#uP4I$B=5^0P)aAOJgS>(FCp=tQPSb~e06fe`Yd`W_Ef+r%ANJNZh_azCm4d4u2mbZWpc7b@p0=_uoB29yjx-mVGyb zJ+EVQ?|DW>2MNX5asDqK(NiMO6HRJe>O_qI=20d|bYu|2ASS_EVo|-9&2)k6_^39a ztsSeywr*b6_-5Hb%|QctbR0$#gSrn%as6c+I4kTsGVKTrP=-F>DN7Byb%!$xF$~ffeI7b-arBx#P` z`;mQ3FzQH4u9hjL;>T$>7rj#`$Xmn+d^ZW^N$^3wo!Eejdl#pcQT|ll`*Ep zT1tVsAT!B>dp}+GO-^E?^KUNjta^yfquHZJ*{_{`347@L--w#ZtJyE*_{)Apyjn$O z)yn=GCJQMaicXXdDIb8ThBeVxhObOB{T@Ph@&;HZF-Lz=a-tbez1^7(7hPKFC!xFJ z?V=n2_#@irxR%2?lp;n>GpXH_wvm-YYO-918l+Q^Wc?b7I|U|6p>*+*UY3I+bkQ6x zHMT}6QPqy7o+`x^v@$M|GsQ+Y9hdFG)uu!UTwT77fP;J)gk`UJ4IdmU-uShl>d4b_ zdhsM;tHv&Ny}@uX&Szq!3PDTy(!^qHQff6BdNOmDc0NE4lSPo<>SVCfEy3=NHLO8MN2S3^>T zQ+&GXmXqT_#G&ppDM-a@hVy#fb9V$CQQ<4`1us*3j2sbfr*27WbHwv-yMYnJqI*JA zVRI6%=(uZ5!wF%n_Z-ygyv3!Rgkr2z#+e-q*Y+~kJ=&>gm1e8AHH~4)v*F@qV$N>= zGUbdrweDbMYZ?rE>sqFReP0eFT-nb((WpT7Q@>G5o5fIsa}so6(taUDT;%B3!FrC!@fI+iIjB+>Fx&gjc9Jabox z`|!LGbi-3i`jDheGvh(_n+Vx1)xU0e+F!cHV4X?obW+bci6r#_5ODxQu;yCeQcE)N zLZnQrX0sV@2#a65zJ`uDotJSEdc3T^8E~rOe6-n40^Yls*Zr^dae&aol6T5ObyRLOj59Xpbl>t5Mg1gGfWaR5gO2iKK z?I`eqXQC>-nAir%~iH zn=|tacKeP%9-vM=gcn8RGAO(Q;fS)Ib4^mmq+SSub3Lujj6 z3JMl8xH2%$1YD@tkXW=S57&|@H`ovghLzNIaO;o<%Bk;!*%moayw#i-Ssf)Pj32kg zhsN(z|JUkVlwsm@6UI(nVZO%yI4=dNq@ov4*)Zb`P%Vw?EQ6|WzRQyjBU-E zb|MuI50pAg(`b3^O0`)Am>I0pACL^eBo;n z1D^n@qHtt7_;k9*?R`+9Ak<(eDGKrWB-B-Qy`HNp3R+hHsUCisb#s=gS@tlv;}+RDn_AA?458F1}zOVdxl2b>Gx0qjsc#iM7t#S}J|7 z-IhL2zSh&m0bvJH5>$ZkLVxuKHhitm)A)M(W0Okns{wOE7)gATil78L4%x7qJPP& z_4>q4nIAPVK&2Q*Lf|OO(vB8QM7Bs3_nt_xEV&TW{oXY2;YI;IRn?TGX9xv$W0EXw z?nt6-ijQ3!C%NwD&jD{1@91CKm$i0lbpbM4cRS@u&UWsst1}44h@=GE&lV8~t}7tpks4T{FtWtNIZ^fFb)u z{KiN}Uw)!sZUwar=Z85>%&QV#{G~fdgZOfb+>yXVby{!k3|0DR@E{)NA#c~kaObU; z`SbUtD!pmq5#v!UPs3$%(5vI_SWkjm*N(@@04|}=!zpKB!znDKZ$+$w~i*Sl^oS`{@RJU_~6LMMvUl|;E=gB=3g$MmLG|kzkK}n(j;7;OTHOx zYBL#NWu5jUgU_F*n|8m$-HVR1996kGw7*RTU+tU&x5NDT?N;-MCT?_|j)!R3uV?+o zRywV6o~oH>td7=s){~30X&O=^wz#T>ZqK=GL=ABmYhj4y(*^E-VIv`rPTe^m3d<70 zN9GZb8ZkI-=bqiFzC!ed{s5l305Q13DZ&^h#NLu*rhU*TD~#(Hklqaevx+0kwsmDw zcK74()(x-0(8zJioMO1-yU#}4v-rpehGPtlt-Ft-i;N8IcAKDxfF?)iq@{P$um@)B zV1r%#8LeyUGO)qlce7gWr*TL>@1%)+6~PzMYzQysKbEQCd;^lMuF2C>2r6K|7*AO-1D0JjCWj-iv!j zazw&U9Mz?NZctpV1hW-lOT-6d24gnoPGrhi!wC=+cNq{*c z>*1M#kEP9;GK!oRz3Jv2F$Rh}B2K?x!1wXg$k!5|9X=o1;y+7`=Lwkk{3Xlv4W^&T zEGyn$@Rl=&_fq6eh(o@IrqGe0^OV+6rXwE)so4(ALNlF+GUbcfh{dF#6%@?Tb>uMZ zf)Ny}i0UT|I{oUYCuiE4h0zQzmM`ET5L`QhR>4_Lu!ql`?uZC_g9JGxmu3Ou^x%P-v6;4(Rg@F z>NqYK()`-!=*@EtAYk{)Q}+{-5q0knVFAABIQSGJi}_;~#zq2(&+3(s$xs) zb8S&di^aD|W}=Ke65MnDr92KDaLVm1N99f#qz?}G%AS2W&x2;IR(qN+>&&t%3um7j zL+tW<;rc#gn%wG(P{DKhKCFD|eIGZs3UM%q+4Mrdr=iqo7VB%smMap&m(E{ch2YTE zEdY^V#2gihULC(e(f_rMKqre~NhkzyZR~pg84SCZxSce*+kER>ck~x<+MYc-s+zIM zb3(oEyy})_wdWFFAV~%N>ouPi!d&u!qJk1{07w`gxxLvCGj>N?OJaY*g?(4d)+jc6 zvMhu)#7vs5Q7d;Z-0*iNGSv=#-suUs|e*5trh8yUDE_(!f=F^ zJU!ZpVu+4+;0xLbONeojwC_QF6eHAkp0AV24tkKmzSkxJv`9`ybbhY}8IIiR2|22E z_>IDPZC|c&ORKu<<}t33Fa2b5JKxqaZ6q8fqs(LpIlmom4;-9Auf}S7ZEgz4fxiP5 z8TlHi@Mbd*fM;#P((i9>(iN)b1qVXVU{F{cdofru@pI4;*@Rk#m`Z#~WwSj`%GKnu zH%8Ns?T!Yd((a^)QYpNjF33ID=F`IQA$}6wpvn`)^gWnHXvk-6a{F;RWE^wDNewgB z!(Dp*T(er`sb-$(L8;j;#_X|ZxAJHk#mJ7yBW9UlI z7x%;!U-`Vq&b*5a;gkzau{$+CXuPv}fD#0b?#-9-_cXjkIN7lpyoYz;!|f@&GpYW3 zZn&w-xE$j8a}(%4jFF!QgaAC8{E1RT6x0>uk5rJxm3z3yuMVPWWH#pu)x@ZPRsLWz z%SH{GHq)RZ?NG}XOyZzEX;eEnjN~Cs^-iC_eOQB(La@-Zr{Bz#j~*=|bl4)0MxBT*mO5^@yX43WaSDZ0bS#(2XIIC?3|l4FitSb-hG-jsRseP2IEgXw-84+2{jT6SX6hJU ziNezc2uoox?&P(Farwb*WDNr7FJ)sVV?Q~GaubZXBfQ6Zx%NB+Z=rZYa{Gsl#7Q$} z$Jp*}r?3BR6Vi>S?;KHl^p1hdSX3s0*S7IIqL6a+?G1w#kByyW$B_;r4XmxD;VKoe z1NrZ=TJqws9(4_&UPxZ9V%QQkZohQxaJP^qUOXRx&SPV;o*vzHef+RpzQ3mDpXC7G zd#O6StrO^&8I;GTN7rpQa>K0m&hLNUEwY2x8OZ#|{1|?tpX0v2Sl7%|eZ#qhjWiTk z8lYIiiF;f|;bPZjoA@Z#5Bd>F-G+kdaTZ+>k zuhe@zKU=1?IQGf$342}-9!NPDZrV@QJ88V*mS_BCX4W_E5d+@oWdg_@FLfqYTdFj7 zSA1;6+zCPJOLmaWj@T`;aOgCwvl|L8E6@sAQ@HR`_)C)%X!=7f*(75F%974i@2hU$ z3!C^Kpm?P*NEq41_tDnDBF-QXi8EL@_b2^+Nc09Is|FT>W>b4QUmdh{+qji}pmO!z zp%4!0-67m}Nn5`TJHFy(mG19s`V?! zBDjNEfUBj>u=63O;~ixpBK{M9IrxV^OSK4BDO(JWSvIj)u2_PjFN&}g4^yaCg3CDV zBGu!zv#7LYSonx6mYFjjsW=8dM8}9rz>r9K;-wTP-p=SG)<4iNP;KVEevy7UB7n>J z_&g#dq9`#!#u=oDhcd~Rp(Et$IXhB@`w zI`&y)L?bDyS7*x(uj5;$OzcPRH$*2zr--kZFRiOosjSL{xk$=ug(N1X>`9^0;{ZL) zy_Aqi#V9?emuR?-LRsH$OviG;aj$e5Q5KOO(Rsdpr=4QU7;Yp6i6uD#o*9j3G32%% zOSDHYb^P9YdYkY^7~Ljvy`xJT3rSmAv>@2dRR0$B3q%h6z?3YgRrGbD8Qf99s4&I} z7c6IK*f}bx36nPP^7?_v74VOP@eqPhK2Wfx_mK6UQJ(O4#rnaXD>NrpubwYOXEI}9 zH{-CF&{rNL!#2@2M=Q!(a1XOgi;oGh^Ym!U!_>1>o}KYLgB-2GfOA26Vo02WwfLHH z;_yX8aC>?m?cu|~%i<>jg3Y?8WUHp)dwE*U_VJ^1Q+sFnaIP(H%DG?T-iyIlb;^!P znGv@lAmssspqa*VuQBI|@PJrhQbDMK6`~MD;QP=%E`{C3Nh(3(2drs~f<B{eC_p9U zW6hjqk&~a){%wp}UYE=@oS>yX1al|jNqJ<@t+izweyVL%FV640TYa&k=o7QvFFd=0 znM&s;aTC5SET7rDkZliyXRcJA-Sk`}cQC)f zpn2MQu8l1v)88~Bbd`v8r2!kKQ6s7Ub}#IZcph|D$kemy82`Yee=o>siu4=AD2m{DQ9(LqehV=)p7}eub2C>ePc7wPp9c~12pbn z=rHd4QP;!uz8c&~q9s}W;Kp47S%0HW|!je7F~5OK){HV;Ww)5 z$n2foL&RaY>EgWfzH5>Tx%cUo^daHU>9qRm#`m3&@=l&cg!qYS?bJtqRAPV(EYf*nqSD-Fso&9+0i4a*j)C zj)C9|{$xzC8J35lcHlJdi<;4*`$xvGV(yOh>QKIVyfV^Bpth&`Jnc-hee)_WYn;WO z*G{&r$pqGtECGhD@l=_v&eb9HoYYckj4#pNw^@B$-T!bq#2OgimSPO0SVs`EK(Z~4^Sgw}zYZw5?a}K> zj-zo8BC`$GuU#uk)Q@6>*{Qs}>(Vnueqf^fMW6P4JJ?YA=dIdfoUk37i0-VPg;a~( zYU7CYCui$IEohVRtVHi4a2xO2Jl$ zq63C=jYMN>Pd85X5TqE5c!+<7pZFk8>oFgqFK_V-A3G2Sm^VP0q#?JI^MvYQFle!X zVJ&0&9IeaaSoOg@IAg3i&1BxU33ju>##;^RHd{j0H`}A}cRhF?XK*-7t}`$Fw4@FZ zC{7ZV7qj$g+2B5b+evzxJ$#-L%{u(z=3U^M-e&#z-iZ-VZHA8ANdCn=ef)*I`GiFb zZg12FK!}_}D~D0!%ap-a6YbgyAI@9ct-KqSNAhCDQk&kG$<|qNWZ4+|jp`mZw4-mi z_)_)i$-}q|Fznavb&t}#&@xaNr39r^rlcubF4HXwtAt(XxO8;Ld`FCwk1uZ(zpG{y z;aA!$=FCGQ6*ONUocAohuCAWzE&3UhF?X52tzlVd8+;8JvoB{6aQtI!&4|ll)DTyH zQJ2-~X(>@Bw`z0=dIl>#BV)sO2pfxtM>fglC7R|L$rMcdV@G0pk^gzk2k09EV#oVe zEZ98H5@o1h)HKzf4q0GAaPS&dU;=DdNm*b5t{FHm-+-cV-)F87^5ibsR2BMV=-pH$ zbU!toqc>;5LMS%LlRR2cV8Rz^B7tMZa>M6`k~gy(#t0l?R)S7PY2lnKg$?~IgAQ$7Wn16320_Mwpf$aip+l=C=w4_JXD4h!n&;SHs3aKrb@+rYAM zutgkam}D513n*`*p?u4*A1viWBwbwKR9IOUx!6dJAqIiwdkQ+;1g3`S%ZRWCU`$>0 z_h6Dw2sgu84MYFPmvLa3;~XP`(Dhk6>kHN8mviW7EAPtdFfu$q4rgxY4`x%G<#elG z@k#~j3S5eI1T0XQd2=#Y2jQHNQLKn`h{Ql=-QMO=Wpz; zwPCe!)MA&+Kyc7oLM@}d(Zl3=fOsM&LV(7=S_L^f zE7{n{#Mc!;v}`~n0ofZ7*;B47>g0jXRY5PCJq>g+%%KIA1JCOzfqFp(f>&TpkxqeV zhIJ%7s9?#C6|!`ISPmOpvTHQ&CwP7%<+L{wUc8U_fY0-m}r2$ zW0>?r3-h-?8ppQfR^js~k|UOyPFvL`Ll%;$Fvy)P2hb-zZm$USZY%37{NK3nw9yLiOrJ+qS*Kw7)61PGytcxO3A_=G9gH?4D9V^AY9J|9o5HWoK?T^!QpN(!Xi|y~ zO53^ctT@zLP`b~`vR}!Q%?dSPR4?1gl@T|IeYPd^*hmr#c}!X79bf}&b!dlvF2hjI zo>J8N(xk(jYgX43kv8x)uut-t(U#G|kymXAnnk=`ZORhljKb469hxJ*d2aZ{GE^|^ zg3J{Gy->TPC>Z|juvD0&ndk|ZbAAdla_6|=Ggc+3)M&#d1k1^uG-1=`R70s6{`l{c zMGI?MeielqN!Y{Om%opIKsDB4eLH%K^L`V8JuZGr$V|^CJ0V7yn-fuN^}_olM>dJ$ zQZ7mE`x5lJ#J_Z;w0*@v304>u)=+JEI?TQTmD@a`o&R37$m-> zOMXIzI}*#l@Ek3wXIB~6X<)HU2yZCuLMG^AL;f<58=P7ZJFMn@glL;uKR7qumYAmE z{*(w-irZea$ly5Xx5v`4C4t32NF7kScppfiL4d*?9(pyVpieMcDW1y#q2y<05B<&U zhugj2>hb|#b!3v4k+5qDqQ<&wAiFEjO{c$x_CFm}Q+NK`F;vEg;nx0hFBW-O$r#$% z^F=am@&-cb{i9Jod54|KPutST-z|T|*#T5LZXC@!i2H1idb4EpWVJ3+TR?`N2>|L z1Ww=x58Z5Z)s8Cz)Y7^qasV-U)G`k2xqSx_SE!}3b<>oZa!jFzHHXqG_ko82hmdi( z0yBiX@dqfp4ZM=PnY^03?hi4uZwQnAhKCAt1oV6xo^0O9R(vS@|5r6v8r9U9hD$YN z2r7&N2u0wIj*24QB{v&OXb1!mTqdaO#TYJ-0tsYMKv@(N9Z*|Q>#2gMMXMr$0xoq} zm8EPJ2jVhS1erQeMPs#u&3-pX3E-Um=nv1_b8haJ_q^Npz019Sp0~<82aX&0?q!sI zyQg??MbP<2D}@W^a!iw$jbT`-7;`F)Iy`@-w=gofGAZoGOa1>U+x!bUUwJ1* zR%HEpTZbgg>&D+K&OE*u=ALzWTP)#P>BrY&nhP^rq0c>~P141U~?4 z+}ew%3cgqpl(utQ<_~6xW?l2nB=6}xer4I0!rsdBi!NVmZBIM8QGCvMmLz~zB=l-O zwB5d;H0W-X<)=3SOez9ALm3xV$2$A9cRQ3DgiEUmR{9?~TycsxVqX0HmB0VHuJyuo ziK1pshUG;jlGW~8V3Z$zNhPdyP+iMc<`*|dEN0}S;UQ^umdi?eP1@(mcHOL9&Rs$e z$WRq5k3DbU*jSR#7Na^ln10pT{C@YntFtZp=Jd6c9cwBxf&;hZnJ$TrHSYh*2ai76 zZfEA$AiCZx+wrCSoRmxJZuuIhiV6(&+c2pZYYkw$>-0S`xCaiIQ7g07M;Ha!o*zv0q@1Sk zj&CTYgneRvpfu&d?d_G%c84xCF;3amow~tWD6qBPRXMYATG*d+5zBPrY_qgL=5jyr zdSqr3wY6vI;ScSm&%g3>2gA<162CL^i=?A}8V*_gs;c%~kcu&AJ&Ms$_Bg0w43P?k zhOCW`Az;2#qHrVRBAGZwAq7K2z~5EEXaaWSl9!#gcq1VbD#TLBN~uIjC5MWH%ON^g z07h3=XFnPbcoKY|Y44FwjMMs@ zD78&Oinmc?L8!m_USxgxknIDf4|W!%noO(e@3?H#*5>*3+$hsEKWz1BRFpdZG%JxA z{_xasb`9HkbE4zT`!UD9I-Jv)*PB(97p-dCl=r5o#5ybLj59|fzivJ7vM^x5-Cw%z zM(0@n_x35PRWE$@CuZMcDTfy%4iq^rQ`HW#M*g9?(<08vSBWiKypFZJ>dz~y@jjPh z^i+N2h)s)Ey-RhiDD%wi-tD$cw`%VDF=NjTdU>ypFI(k1IP9=Haq!`#=T-Y}U)A`p z&M5KAkAD{W&j^ofUPezVcS~E6QLwsfz^?5?PVTj{xgp=Qy*cafO?Wkj^0Vb&+!Oo0 z$ftWt0-tQ|nVrzWc|M0d>=>c;ed7Olc|!WaWYgUGkTb`ItcNI?b8{lp8L$6ZzGVj^ z2{Czmq4>~{UL!?g_sY@*TbgMT31`}r>>Zk=VX5`|v%8IwJVtWA8p<1}w<%oak+g55 zS?$4TRwt(}EU&(2rJ2UE(zwUh^?jZ?zdS0p9uDwI;wBdjt;>4YwZ%X~@!$Q_rmZx& zV*Vi)^RywoKGOyp_O9*=&h>Cl(Qs;sp(guv@o;MXjs6$GX{3$5?`s1`{7x<^ujq)2n`L)N^_0#U3Y(*HiJ(z^7`%_Rji{&+IkTe4~rm zesjf*M_o?Ni+*;yIMcw&+rb90xW*aD4(>uutVk}`b1|MrysMIQ@Y=tX4D6%as37ZS ztxQR1sjfmfLFVrNtQ@gSuHZ!oWn>|E3*WApEGq0P4pl_R0~w%%{=iNB1G1#u|7-c+ z(Y`U>$RJcZNQ4Tnm9CSB$r(tHws}l!Cu4X<&rAnEIkV9G<0(d16Di)Ic zlr+fzfH)=_3x+*pQe_PA&Ija~P$G{Zb%^3&o)2iHo5UhQ;Nc2;LJE+B91hF_9s(0U zCAklswy6W8doTWIQF}W6=ydV-;*U1YI{u(KVSXgh*_z0MfolA@bM-kan{Il7lBzf`r6%4g%AG@@fx)Cj_r@ zX*ft;VE%3HQsLDxgoG<1KMDdz^}~Mt0gyx)6*c;U0Oj67Nw_T`v0vx2ShqAVouOqs zFp&&=b2#zc`FsSSGZBP|AqayFo;Y~Y$^RT}t%WK@#26pW8|7gDUM)Z1!+|^l^f2uW z|gp>y?2o31P+`*FubZJcjzumxSG+Q?Z48a%(jxtej(`a@Gwg^EME!H`ZI2jQ} zMIeku1)APo5Q9#q(V;MCl!mfc3~==11xeo0z?$q76KN<$cAyC~6!;2aGl7Pn7!4%o zL>h*(KtG#6L)j=km5+{0wt?UnHrW@Hjj=%>Cg=lv^eOwmS?`d=r}~GprpTg9oIb^8 zCeEDV3zNZ^LIY+e?+s-!nUj46XODp9GGQMmn@*p61}GcEY@AOa6M_#+8ChID;sgSO z7ZymR3P_v&0xIk&36nzFOeO!gL4lAHpM^0QIEJD;I)mlTpy6CTolWE7JO+)yq+=kr zexHS`IX9_@7eR<3<;rLXrlEW{ki>M1&SJT_vcW}1+!;6*qXCCecP?2ta)nT)(54~E PX3-Gpf(7oY`P5$lAueL` literal 24908 zcmdRVQ;=v)(rw$eZQHhO+qR9_C{7vJUmeJVvGb#f0IfC^kPiZ3jevuNk&c6dlY>B)fL_7S=^xsR|Kg)$>TK`oWNhk8@IO5N zY4|_H{~A+pvNu*Xb@^+H{+|w`S26YYOG?t#(A-q$Unu-9lqAso+e89-C3}~@9sIXA zy^52e-T#>UM-D~sZ!!vh3;zchmcOe1?zjIX^N;lZLo%%YPcr{V|34(d_8(-#ENxs& zo&K2zVm5z=SH$%1TsHk@D#@7InY&mJuro6MGp5ciPNs&oP#)Q?OS0Bjh8UrHT|Gvr zw7Tgh+<=IQ=>lk$(zILHzOuBg$#XRKHy3vyXj0tyn*)28* z_L{uieY^Q4*W-p-(%uUBd9L5LKIUE*YOMxJ{l9;(AJV?Q%{Q9AS9POH7wndn%ksQ* zF8S%2tFV6_R`b%jyRWs&PP_{|+O(Sn{OW853jOVh#)gI(vlhQx7T=fjXGG{dk1e}C zyD_%6yEt?gl%cg}$JH9^V6SY%!MSJ0Hl()txjkSRn={V?qxmi z8|S0VR-9{`USK2$2%;c#I0%YJgMi2tk!e8%J4eMi$HluRG>QyROSa4f4Nf^uHL>-4 zO107$O|69?$4r}Xq6^zaDZ`JSs-~Acqvgqm?w^~t+>--`X-)5&Ccg{!o-0@g;t-x$ zE0Nj7&;Hhs)Ld}5{*1Ui-mb=qVl`%AYf*@}fEM|)M6it%w-h`=I`r*3()3xp&dqa#od5-E_(B8FZI&QXBD3fED<4HfNC0Fo9WQb5Ltj9H*a3I4WV!3rKObfkbS zmlemf;29Uuq`;OHKvsmy3?VDhVF5iV@L@sSih@!gq!L<~bzuQUEy}P!yY++1j5;p( zzyQTPW%0fl(BA8a&ROfN0&3r^fpY<>cDB(2=Zj(A8(`Ru^4fK#eTTCiyWqeS%c*Tn zhYxM2vLM%9lj0MPIYfP=;|k>qnmgeL?bZ6EgBnT$STFt7l_1B-37A1GPC_|A&ve zA&Cdw0M-{^2bkxxqfrNYkOOMOwE)+PqRaQf3zU;MMcxBKhgHxAfI`p;aDze*5vSze z{Rq0H)e)qSxJ2ku*%JUg+R*!i!ka!Sa5wydQV}_h{}>bZz{Q`YCskLVE+6q}gvddq z`=wW|iW;jm#I!j$V_t)r%x6c*kAEYq4sZ-~4~!EIWX6p-1ku%t3gIb)H>(47;#NPS z_NL5I&@w8!`=KV+kNjqn2&S%f+jgqh2(BguWR6flC|T4H;Q8wJkWT=^LjZzD!0HmXhxaQvzx!R1A!AY* z+6GrGE44CU7Arrp92(j{JYJtAM_$TU^_(s|gxl*>hJUY`mYsL+qkzXbFq!%}L%e6( z;&2+Yd{9hZ*G}^8JaQ~a z4n}g1a3`+|+wvZHe6%{JUHV=y9bVidvx7kOaRG9Sok2KYfcE=IBvsrR=&8N5h!d?c1_FxO(FFu4-=LVi}JcaD2cw^yVY{7xsVFoz%sSzs#t2g;m+yCLZ88j_Jtdh_9wO3gaV(`N+HgY%LML6Pq`5F~=o(SNFe+S;% za7us&5xPa@}Hzp`N02Bi#@(|B>S#W48j32pWwJC;o4-4}POfyo;9(VryY8|m1 z!M2fa38Z~v!M2@BSGE8|NmCDWoHT!H@>H%s#rutfEE=SJU_fN)S3(3NpPHDShXzRO zpxVcb{xKKTeJM@#9wS>0&E9vE556}3~^aIt==%j$BgTY=-@BTi5GgGEG zdM^8vysdl*81dY$>$TaZ5v$x5wy*W_I1L|isGC#!RRn5#H-o@KGRj+##7-p0rA@pR$$H}$mZV|wk{GtNi6arSxD+5@EcB5Dt z*U{nCE>klmQ~2J80ZJwG_OgD2t-kXCt&`|FY$F>1-crPb*&SxMlXlDMJK$iJ4yIfZ zIS`>@{AVu#39fy&YMx3YJ}xPXR|BnedH0A&X*oJkYlXH@BMT zj@4S_h6-wBz;2SdiDL>uX^=XzT9J9pZWRcP5EE*a0WOf*r&LB{7CR!cThfNG@cKw& zYDtoYU9DHDH@3cS2d)Z_>RI0N@+5!%bVka5ej#0-gS9#X*cD?}NA{~4=3u?uqv7^m zoynbsXPp|~J@2dR5z|T%bMS=sZNkoqO1H2a&y&xmU(?@O=jjdC%uQO;vq!{%Z7~+{ zh)0hJhkJC1fwEML9Rz9qw4!0J(^z$-T8J?Bg$VnJ>6P6*Aka>#`oq{$JHsUtKFXl; z0*>6R0#{!4;uAVy$e~R}m~rFNUiF$%&Ifv(2|&>$xfSpJe)6F5KYNV;Eh~X-J$np7 z9+8$mAhR5>OhrX2`gkMr!(MeS9)RX8=<5h2R}qYmQeXj}2{h@F^(+hfkYk(weX{za5jA0U(D%fdwa~OTVS5pjuCb zNGSTyXm|B;nFcn*BP}T>UP%=qPtJGME68DMR!HF<$TKd ziSvTv8S5|SA?G9al>N>*<-C64B0i_+lgup@doLbZw6D0N_@p?cc%`WOJVRJyBYJ&s zgM5vAqr6qtEicUe%_$&!yx*4Y3A+aD9p-43M9K(Q$c{e7H?W5Jd6t||RC-(%o!M#U z?EG*{J2j=<-RAd|WnZm(P)#PElgUFxpSD5SMVphjoxDAK=RqPjeAiK8I*)b6KF`#N z_=8o`kDYO8Cx9~!wq~gqG7#1Hpwm&S7`o1S?!cHHVB$hBSdB;a8e+{N83ZCJQBSU9 zIs}$Y#T!-D9U=bQSr=zNd6n++(9hRK<%O~v7VHB&*A+{FVu3Eww=$Eb0eo3VVUj*6 z@7d_xbXT4uOhy~t)VIuljEQ7&9@rI&3p`V4<8c*z=?~g+&_p%~(=f0tsQ_2!Aa|fP z&yk(ps|;_`+nV6rTy_WXEbC-uug&`rvmRQL<`d$|oKr-1xzTb%5{ui?$?JcV8hvLT zMy~Vr@M#Qt_EY?72jnXQ<`Z5B8Bk;1(bBBhuu`N8W7&KKYXfhO+~UoTd~^`;D^1@a zgfcxO)WcG!5I|qx)u$zcGr>+lz9OIH4S-zefC65rSTYR9xX4k-zxGNvv%KXhxx2ml zwky=MhIZBacDIPzjpb&u-OtS2b0N=}Z?K;X3F+;f*#C{nX!Lh6qHUIcgYV6K#XG8S zI9~G_pO@oMk`qp7drS==r_-(}GY;Wuum^z<4->-N?{!caHmh2_sGy}zi$!?GbT6;+ z0b{}&CS1*Aj*&bdvb;h=*n0lVE~fB7-NeG%j{^Pf@Au!^utEwm>W5XigXASStDedC z)uvLjpR~5c}Bp8Bv1+c=U@di|fbVIrH)Srp% z0KoF+0F>(SE$CQ-9OE?&n@!u*rtX5Q3>uYtTB@atE-uq}Cap;+F3h;>l+SE^wx*)4y1h1(iJ?O!aJZCk=Yiy-0{ zQZRn;3STa6J?dPicERSqWJHue-KB#%#S%&ctF*$(Acm5?8lY{Nij0*s(kjPxq4m_f z*XZkaP(1mB57ey`hnGZId7APG{ zv0r^2ZI9W&OW4+Fc^q$M zvtwihkGuiA(ciqmD;gx+&(ik}!B2HO5zph^5e#*@>q=(I9w`8C_-@X)kR40L6~P9b z!eKHjYTaxL>}dBIBK2W{lk(j29rkup95t`b3GZxcr}%6KOroY7j6Gyb=8T;4K0OekU=dgmMm2GX}Ww2V^T3k57qSYB&`ADzTHJ`T9-}u zx*o-ae$f!og907&{?Ds8>hY`sUm}$*&_qT%q3XQDHV5Q`EbmNWkapUu!hMB{4A&jf zb_4oNffMEM!y8$>c!A=~4O-il1g$~FV0B3Mde%B$XVjYl)bTQ@Tum(J31Vc$;&YY0JCa<{UeNsO!Da zdE2Z{%Z=LWrRpW7dVAX6J`COLqIKf^h_>Ik??&L7^yCeR(|Ro?#9qdK23c)@>VewN z(Lr<$%Qaeg7^9DWD-aTETryx_fVZuB^Oy(!aeLTt18OZk`P3Htu;V-ORF zXGA2RaBGEHQodxY>cpzr^tq{e)O8F+9A-!O%L3p{ca-55f9L$G7J2`NC1L9o9va#H zaQ+X&t94s>N%ms=5}Rw4n<3OJ{Pn>U$pb6(?VHVy^{6briz8>Zm$L?z*g$tRM7cf}%SwcDUK(1kme0hEx{-5|tIY6WTqsAkh?agaY!w@C z{FZh3nAJgz6pAy0^Gi$Q@qGFcJLF@D+eCKK_PFki=9E*;^)F9;6^E+`{I5mOCPJ2P>%cE+a@KkAIW)8^-w~_I0b&+qv5YD z;BblR4gvbLcrlMM2uVSdW$hFmq89U>ZClUKiOk{Tq4&x+yj>#gG_Ho=aG5Rs=UpGy z+@ANR)(5(Z_`A5&Wb%Ditzl3MmWOMUob#FpJCtWa5)|9*X?qf;90_;G=cumSFXVW( zi;)5~3_8H3=ADzWXj~&YVS=6H$y6*J{pPQw3p$Hu3tcx0dY5w3p3IrvUF@RAN0b57 zOsxo_O)^qG+=KfjNPmRJuz0WhM-41B6K6+cD3*og6YAzKI7&`HS&J~jRcn-tUE=N8D>j<2V>W29RBPc#1E+J`9}rc3=<|K=#-z?C+5yS5 zpw*`M^tQcv8dI}wuU{UxN8EP2Z-~nzkGt`r9%tx{74Yrl7_ruI3#>ZCU`5l$;f_+Z zb|D1mEkxJ9P#yV;WJXNL#jyYG_@COSS>?A>tEFxc|C~b*3h^dR9G5yWFAiamx9C_X z)y!gWNC>L}A#tMSpimC}nyY|tr5Ynd9)zq|GYM0mL8~+?M^8F3$ry)%g9`~nW{pp; zNM}g(+wu;{@{_&a5F}Na=3W2T$eq%8N$SFl&V32Y^;))naXrm&Ixt|ui9wL1ag6Gq zkzV0`{94#-!z*(LRL#SX4R@8Tm5W>EG+K=3s)X2UOdr6C9l+E=6_3YSEdWk~`_`F_ zB*HI4RqPD6`Ls)Qd2;;;QFbyttd51lYQmUug+W}@ib4HAs$5q`4lI%g;gVuTDvM7z zSHucd1f{19Ox$Kh8K;~T$lt)>!!_aoKkK;rYO@wP|B9J$h-lM-z|9)J~FrlG}{Vi=P0pepK!SfN;f znVqCvY;~Ag+1O1)Kx1}*;|QkrYeE#uk4KmgDSeN)6O;IJAYwy8DGz~eQ|fT19doM3 zl^W5C+!imiL@_UT^jIk@ceDL! zt?x*fd(X{P*PXBY&C4cgcJNn;zYxex{CC8cLu{jtR*ZiP`#!8kQ)A|v!?`?m2K9OK|=g4<9KusYlL1sdc5e^I+}-oMQ~wO92k}3MH+3~%_hy!KDwQe zT)XeZ1bb-fb_0Bkc8l#z8NTkP_|1Zo?Hq;ghy_~%)Jj;8teLR@_njHt9FEO1>sid` z3J}b#E4LUGDzi0ZRyziSr3^6A9N1wsH}=r` z#mQCb=en#9!kYHoM{w}6KNP-CEZe@oQSy)7zK$2E&){w9T~^OhN3CZOZCzO*(Ei>h zd&HZQ+m$_@q;P;)Op>(ND9^U^%=TBpOwFDq7wW5)NT%=s5kxHz!@W_qOZ@ zCv(a^9bHqMz81A==t%iOh=7z~dwLbgwXX$@V%KNgGWPMc^-c6ct=7(Y?R>GBF>0YI z238_NhM5^pu6M%M^D7YpjmNk^oOEq$Cr6%D-j#TokJF52;kl6yQ5QQ@JKZ&o)<+vP z26560+@ZYoF?bEv-}CnlJ9{^s9(y+fVO@DiZI+@}NLl(iC)e!X+(;I2z?|2BoD~ZB z0|eu-^r%E$6#_Rf^=3Ny5I|ugcTe-R3rZ{^xmoBSZE11MfIq|axiZNERwQVH#wsFPc;$EaNcpwSM$oKjPiu2_o>8(lPT`Q7t*k*tW6@<}}c(=Q}cE z)2l7zMmw*6RP1Ox8^!fmSL`3t8H>J-RT8mzkMg=Q*|=C;A6t`{=n#zF#5L)OR}e!j z>4DcoN2@tWq0QIa!H{mDL6~gH+1fY=b5k?a+F&^Zju!Ph7A`Xk-7={UM5=Li+~qQT zbTfVI@(^HSlaF*I>Ogtl2NNJ26^fQ_wP5GY8+YYgxoV+NQT*NX6e?A-a~!;iN8E~M zW`)XCkP!=mwdA2i-ppF$LidxiN_dUm_xlY(cC$jkQDe}o&i#E^RT7Rv;}Q5oq&o)Y zwARz{N^*v#kX+PsLEPc%9$p}Xm*%iK=!EhHOA<-=R57aLmZE247SRicc%rDum|xHO zZ5*2`1bBs&|72C_m(#7VCy@FinMKgyBrjxR1f0=BuI$|=Refx4?ReeK&n?*Ff$ro; z_IvRibK8q0mGCZB)AXjvr4n}@r~jz~=3nfE#QkzQfmSfw(6kNV4D>^k3wPBF2h-S|1mxNvmJvdf`1TzwZ8dAoIaGV2ZE2E&4x7!WS31xDGFk#qJqH)TXbSz(NN3aR2F*-HP*>WbXw3y?OfOE+imZavO z`vw+~?y^<2!5+LPWgD1_+E+X^Qg2r~2v_Vo{uRj`q6>l)7*iUXiT4lT!a$~6!S>$q z-)npyH2$xv7Nxq=rFUcbi@(9tAT`y|;`R%V3f6`^bvn1^3>M!Jd&?W`a;oixEc{j) zXbU?AmGga67@Zp6sEhC_G?_QTLFuk#CsL39vr6^wsEa(Y>`itu?Uk!BBz!B-Nzjzb zKddQWI)DeobXRZ49w)h33LbaR9^hhUXiABgJ^Brd@)tk(T!+Fix+3^7BD%X+8#LJF z!OT`}?pf9@XJ)rE|A#hmX+7SJK$w+y|7SkSj z-45K@FAXx=!HHx%7ml8w<$&^aM|cr5|E%qma-nb^Roc{9vyHj@RKlFpt8Tp2v=?Rz zQQy)S^sbRD<90D&^&lUA7C5AcTx+SHrR>5+{h)o}ecQW&JHw7SyUwhQs;#PxgNBXv z4fmEEJO2Tl&6Oxgxx%Bg4qRp|^16W1$K2i%=N#j_*RA_;@w_4W9-jBNC4Ua*3;msg zr;TIWEM2$a*0;k|2<$Wu|EkL8tpUZhF6(Em8*kT{gag5gEV0LMhnqVvs;Tkqg_8a;N#H)V`*DP3=EO?<^yR7HA>FNv0Ki|)5@?vpC zKAM0Y8UUn8ljwk+Z!H*|T8kU7Ij@XHM+K3Y2F<5rO5F+x@W;@9kHwdVGIQp;!nh`C z;^LQXCuh|9-Dm5BnC#WsE1R8mTVhVJ*gwWE$NiWdEYHJc**wDYT`zkhC-gp#1=x8; zyHV`BNk_i|wD7I7ShGa)gf(pLdTZXBJlwo&;E8ndB@c)Tvjw6g`>Le<7DMXKnN-CV znkxTXf^R6k9$X#cKq!z_V(M=`oXwd>#i^=hr_*jF@(niI?|gIz@3cE^`6090r@qc( z7b)oh9hVr${~3WhvnPMyaW)`N!G9f<*v%rx0@PtP?0S#?r9Ep5cGsHvph6f!;DzC; z*O&&Apy~Nd70A|(Oq1-qUF(f--O(V&o#1oWHc#mh(^w(g9#E9$srR_S1(x=S}n#U!F&xMN@#> zXV8IimO-a~ofxy^68J2tbiHUVPhZU+jxzy^P|Zr2Fu7i9Hjug;Q_1F{^Tp$CzZbN1 zfWLKcW3d_ZlpXbe1}^50{TyS6~hQkN4Sxrh%H?VZ*XEE&29Vo6_eOYDV6cz>eVYjvSZL zk{YLZz0h=VozS&`>TgJL4N(aq&TS~n>P;`sSUhDfdMujA3<_y@oEtfZGU>D%iL~9c z8|hSw5cN5pdV#}5EBtBoTUByL;*f-Mo9bxm!&Hb}qa`c;Tf*+3ywjR{Rd~{beuX6q zCagl2NGeUsifWOCO&@3*|nq1V^ zY?ey)tO*U`62PmDK({IkAfC8@aeTxp!Y@fDiGX6%9&o~)*z` zko+sFr~$ZX3!xhTEQPs^0aj*5An zR4gpDs-tvr(p}}+7dw)*A6&m8XS~!Mt-GsAc0NDfjOXh=E>S%k%ewG-I=b|_E4#dY zu)U8?_@^dQo(BoxWK8^GpPUELpsnT>bVQ69&5)vAIP{MnKR!5tuOH!jNJ6RYJ#YI` zr{MWo-^O3qyE~l@z}g=7ywUmIPy4;`uambEz5%(O=B_xACJ;3B|bl+*e99~?Ab>>2g0Vh z%0z6f_h8Og&!i=g<)(U9QWgcD-?EB<_(Jr@ozQ!K)4QLs*uit`x%2!or9&nN$6UcL z0pDj~pgQP-+ro);X8)tzV!6=nip}w3xS?BC?Q=&~Oi$l4DXNS^K=s0>z=2FIT>RYk zn;A;mzq9XtMl>JZI#Av$8Gc^z41y`iO}Om1Rf;-N0=By<%C!VA;=#PUeT4gx%%q>J z0d}o+3MX#gv5<26QUH!;`;b9iRQXP2L8ZZzE>$_x&$sU0TAY4n%^$+U-Np7eF`=09 zIkzYG%r`CXXKr5AP}t^U#D_?+XrS+%Nv(v|7#0q48UpIsGzZguF*@!}Z)$tQ zM|;vIs%zoz0V94ha||8k`nD+TVR|9MxGbv(eJ|g*!AHjn=2c;_dz7{EsfY@9XiOE}wDey1ExMxm*;x+nqM2 zn}plj>Ehz~jLLB5uz1_bTo1bBxpp*<8ZQ>19BlWCuq{T1vPeX7F0_^Je0b^8t{Bz1QVkjdBls>gKv@j1cB1GXKnt83)R)L#Nj~O)9sV zoU0$^qnkbnwN-2jWLeoo9(o72Sm(40j!jP#^S#R=rL7f&m8j8g?T#&8>|W8hsaP7^ z5QQo+Y3>&;+WZp0Y!PbPb&aFa7*)G<4R99Q*dLvR43VReWo7P>7YyS7@HFKXEwyL82ik-5_NJcL5nIWgzCeh#~A1~G0$Zsm?& zl?dZa)UB$;5&A>)=zb--B~PH;ZQeZT&mM3&t6#6V$l=9V;JN%B02r;w6Q@@sS$0>) z<6iEgPE(+I)xdh^v&^v(@d@Q3qv9T@Uq~L4hDJvL!jEfKRjNfpkJ4qJ*H(XeH(;tF zM>?@grYVW(#A)aMggz4{e{BDFCW#MmV8OJ-Rlum1r|y!al=lZjEo*0u$IVXfmsu|f z6oo;XH*uFk@XJ^9;i4DAogG293RiFj7depd_q;@H!+lmH56d3QHy|2aMDL+5uh{-iGFslS*5~!RDIZazTG`wZdf(46%Nh$+pvd~f?pA0+5{rkC2 zu$vF9m2@oOL=xKeJ+tp*zY0JR zwsFQ;L5Z)4;?Qpx4LOb^%98kt5jhIjC|`r=0o9l341%M)-ZqbZNS$pDm3Kl`di#-1 zI=-3{LnX=uuS~vye)@pV_EkiQa^CVPoGE7bf`j;|-;n!`a7Yb}i!EfIIx`k1SD2ig z;)#mqG)$7b!6=owCJF5cH0svijR}0D177M**50uW*53Gg=sa5wjvIVN<{mo9aEaFO z;P8T`u_XLOEpm<4pR(J6TzzO#@z?QSxcxPjdM|v3Z-u%wb5O!Qp9=A6w}+kaQ}%zB zDONvl%QoeezeD;KiC4N105{=J7NGBdo{Eg?J_w~SVsNTd+%E+~39x*}SV!V!XzVcA z?H&rv{aj31WU_R1nXR>?PlDm~KTEp`nyRlhqBgY5ZQl0oX#D`S-M(kJa(Kz@6M|ah z7$rRAzJ~y3#$K`&bCY)H!M*|($Mrv-7P?0haE#z@=dhOmx$OCg=~p4{xlsb(LVM6| z+wbDvM7{I#dRh|rt`2W-?e-GY*!%U;6#12?jj#f z*J7d<-VIGoEtoH<%;a`+I&a3w({Eoo*yw#e#vO1)vH#RMO9$+vj`*+YY_?zRx7fN? z!Cvj?_-K@tVYolYio^jC)U+7j+KYhswcu+c(IH?G0n1(Dxh53Sj?El=;N2GqY~=)pA07IFC|bAhr-l|A)k!JTI7UlY?#U!%&i2PRcnX;2hu|?IH4jgzXpnwdy89!Ghj`{I?`CUTu`;#Y}qsGd4|(Y+TL?!cW!=`x9q=%cw9GlU_3xGPy;gp z%R3xHRz*3}IM+Dyn0W@>ux!I@p3dDHWu~P?ho_kZ{)AK<6vTR*nri*Dy<*+AxmNfzUq|fJ`R`UJ*@c2=qV0 zn9vjnzQr&r6*17u|5`|)Rv5<1Mcni=wGlWXJhI@a5Ns-kTq=NSL~tnpT$@!;i7edZ z1{4&FLo#dxFk4jso3s>ox~&$bQvt$41vYmU(3Vo9iJn&^?X)S+hgaj}cIs<^UZx3@ zC!bs_l)<0{f>;pDvaM~$8D6R#9#TtYcx4Mvg3W7UFbC|d3h-pp&!v<<)2c!0_x^<(6k0{gWYAsmg;X6 z_A}NBce>IZy4ifJMwDYE*{>h*pK>zmoAw5p7$gkX!MC#JQqh5Lbo%y>AJ~sB?v@YB zYK2w{;}z+2I2Fcfx-e%}f;azg=n@;MVD+K=r;8?-j%;0LN*7v_4i)Aq7bXtV%Zf^S zcFq3!<{^eoD~zxu8{aA;?t3^^fF{8N2hatmj8vu21-o=un5aHCMW6<$jIc$j*)4=q z8%%f2B%R4e6IeLxd{RRhCzq-u-xcOgDMQ%^p}arACHX0;FjBxB`^WN_(Nw6sc%CNt(~RVkce)fOfB z&hr|rvL6i+r7Mo_jyZ#!JC^K|ZXr>bdUInAqe7Mbp5%&3RwV_f3-b)~!-RPh{la$x$Y-$2gmfx8=)PxjTxGI?aUec6IF6;ZM&UjI2(0uu9f${=2bTMXbB10*k18l;laItw8 zbxrh8iI|^U>uML74Wmp|a}Kzv0r5b=dNkhF4#?LSVj1_R7s-!W} z9NmJIfppgC0LIYtxKc(9rf8to;#%{5OsI|F*t0p4^gQs|({&n^$>&iZi%=Ugz+hg` z>EnL)RHnFU40!X>K1N)50T%3(m!hAMieGDRy5f!rjI==yUmbA6RN+^r!!}Sjr~sc4 z6pT>y z#yu3i9LD?etqdPP-VFU+=N{$=Rm^rC-d=U4xdV!`;3}%o77q-MFD@g!X(Pje`Fs7r0mf=Qp)t?z9c0u|9mIZ~xf(Sh)p^?hb{GuQJS zGiZ%accvJQd(mHrTmxLknzJa4Z9{jMQF5hBd$6ZNtqDj@-5WoIhler4AHPr=Zv-2#zC)5`jt_n%VXyo+HuM$KQi~gH}rkFZX|eb`|op` zXk2%JOEV;$DOU#>8sf8m^Xja*n+obsn?A`2=k4I(w1Jz&eQBQcelnW{)ZnH=j_P>; zWof&PbbdF~`==VK`A;!3gm-8kI;oF2$?fEQwKu@8xlc6B^=Y#PHjBX?ckQTUr+;x&a8HQe$>? z*N6i~3jtV(s>;SiNhVc|C3%H~4S-1%bzIEFFK1E;U_%4`fs&K4nF_rqbF8JC$E+8R z(PgGJlS{h``VgRj8bCGlle@We6mV5*{1hzI)uyQAh*gzeS71nCSwkHbyM0y2Jf^Cf zY*=ll)gYm=F)%m5QwNet5|XOwx0{***ST`jI#H`ieZE;llp=vOqHw-EWQ8@B0L;_9 zCRNJ1A8dTh+WCZZKshK~sC57sh36Onmvx08{ke@Qt7~;fqOG+>g=Z$*y)f?}R{+ODVF=k%AR)Buj0lKXaze zXZg8IN#nz|M0B&ww0~yM22QuHp_>`3+6|*^337CX%+OkUGbt9u;(ex5c zHg9HAa*6^pQsmL93LKfBDj6u(Lk-J^3_M0>Z)O)}Fg9Oi7xF+cV+WLOl$N5S97X4) zX6YGZaa9>u`5B`1j)K&Ba(b?T)=DoY(+~gw#LVPzH4vEh4;Er*sDO|g+lYdmWoRnT zLz9gtz&r#V0%2z%L2CfPXfM08w>lVk*;UgLmzCF+6Hxm?3Ly?kHV_^;fL-)_A$?OKEk%+$Q- z0BA}qeoW|%)Yh86E1Lt)n~gT>y-liBp7R)GtWC)QsCWvnVE#txu>2VVO`7{ryy~4+ z3yE08O?p@S{4InSla~vfeIR-bd0og=aadO}Y3lQg5@C!#6^i?MzLKOiV3qG?fju<8 zLrkB1E#TNF+y(wrRR)ksUr=H7W~@qK?fu%rV`q;hg; z^3$b0iCOP$?(*%>?+C|eYt^+9RReAA)vvDKuX7BMQ_pbL=56?kHYAr(fPHmNRt;~1 zcE+RXA)G?xyD!~}wCLNu#f;`8ckH|&paHeJw_1h=p)06dQ9I@keT<1PA znBRyzob=ua`^-WXT`zHz@RiFYsS~1BFMjkv&S_Ti+k^m|=8!XjhnmxC44<@KpUA&9 z-WTcbFWtenGC-c|0=etfXt`KY&zWO4c{pUBex#mG7;iZu&k~1_5trLXZyCP9ZEWitySBHSiYa|IV zNNWTMPZyK@UJ-wolYSCMwIV);vk5;-Lg(u&|wmiV<60_Kw~W?&Oohj=Xw7|8~E1MQYP0YC`(`n-0ZlUBY!qp`blp1(q}g zNi$mjO|qCMFnNarIxe0mqLpMnS>i0npg@HPI$lx(K}L#%NuTIN(qx+^hhawGm}@MX zebj~{{YupA48p>t4n2b~;S4DQZI@g|nm`nZGFc)}QK@I3Xe=CLE;*b z$&(@@`G8tRij4DsRw$M}BmIb4CV3QuBXgHz%^}%0#N;heh9e-+lk3uoi^A`kh~je31Uf`Lpzk;i~zs7^?rN^jYw8 z&%E-Zzj^-a;b*?`5cur7KJbAs@!`b#{wMOvkHYUQ9lsLpeK+*Aj|BeK$b3qlnS)=D z|Kc1p{?YgTy$FB#+rekZ*5MmB;J02jLGZW6-+1?P-+Kx8$XEaJSDyQ;Pk;Z52jB;(N1NY$_-7K8 zmG;5dqvzi>`uy|XDZc0Z^5Nlo8{QY6diT#g_pAVZBK_s_KdnCe!ykX+egFH{pZV&K z|10vIAO3da3t#%u6Pw++dz;;b^YRuM!qUkE%lJ4ERMfq)Wv@5)(jC z4i~XL-_Va4CWXgb5`?C?1f3+g8-!%31daRK>nrai``2zOUpvbK8f+VN2%b@$-3k74`pJLju9~lBd2_2ZpY~DHUzNZKzwlQH zFSp>BdyJRU)w-+I9UR5g4)z^Y=hV|9egD_Tkr&V7FYWnr)o))5mU*n|== z(~^FcQf^&<5C7U)%gasldN_O5(q7q?=@;3xFTO8}s?82@BXpacl4Dtyx_(SuiFHL21fxPjY~nvk3XxA~dZ-BsA=x>elE;g!LwUYl!iKu7Nbrm&)9F)!&( zI}n+a`lRlz77GBThg`a))t4KkSshsmVV#1&ULq*=u061guG>nrDegUI1dk5OP&(_pC5>*ZCP)%BZtixiSQI^>D4-##UCg7fWc`j=-FSYKVJ5 zO~GUPGDz;kOM%sxZ33bIi-uLX>zoHtxtm;$`x`pnJoa~+cphpmy)DXf(s`MHGX#&I zEZ$+N1khO8yWnU`A1K|`_zQR~7;~0ls!3yxt}gTKsO`>D*+i?&XUx3A6eBbagyw;D ztmEFDT$ zpz|Dtis#t6Bg{t#+zknX_9oaYaNH;#Un6csI<@oVT0*I`N=tH7k55BmC>QEXwIN9x zm{p#ZhGB7c3LULs$Q>~57$01Br!88`ii=G_D@8NWSinIBai+Pm0v=Fh>rV5qRifNK%4sui#ZP~$OA`hl=$XF=5u7-q0t&4o62ufD*xSBMF*@<~7`CFxq zS{zwqh58`dGDUkjdN@U&bUAT%!ZtdE&0`{^R zoso|ne8NRSz(hLMhx7VqCz<(NS}=nLcQJ()o3@zl#T6;4_7~N?SLoPC{K$>M>}s?x z7iNdB!`3d-Why_r3TP3W7v&zbJ!ainavG8cN=n!_X|rJf;Q$cN)H9)6?>wxTx%~+{ zC*}5Wr|i0wHB??i@vsROt+E<%JE2fOfVd97G8XGy8%Olc})$!Q&uo%w}jot`7 zo*L4TmE73@Zef-aaz@KB3T{sAk#kTAvc+_{`4SR9I~L0Tge?$Pskz)IhlOAcRr%v( zerAeJ6ip=;l~K&z6ci?J9h6`LbM_^2NVds@(36!$GEil8U61*Vsa^u0UTNjG{Q@Z$ zku+tuCSa4PUK*JJg={NFa6|Z>2`ehh#dgGMfUF}JqSTMYc|fV8P(bj~!R=zA8@L5S zOK;Ha0^l{Ds4se^7K)tBJaiTVYDnC!@_lpb87^PhFgu6er4}`JGgoWzVb@foNQan& z3d3kW-B<=Pqe4s)&-2_hQ8~ie+x)Y-8v<6CHmc@O`$Y+b1fR|_CvN3O``8X~`(krL~Oyir4 z7$TWrB?~Xk)+AQZtNVFC%*6-vuF0y!-0+OYx_v%(o;y{~c53Jpie*!D1T&{cc7$f6 zbIClS%^ob|CNAr0s9`#(MG3r)XEpG&))oPwGcl;Dv9x9yIfi0x6G;42m z4ZsUv3vkoe5uY3b6S2}0Ge9$d^>%ZpJ>WC|+*E6!@P%_)i%mTrfF0XyYn@DlY)B

    @=P4!7ea!Ro)wk}HAa*76aY9%l|(puE14a?mHO!Ny!d59FI`c{#f z__#EzXNz@!NCc;4+G+G>t|4HBrJ=@0P-T|_Ogyu$VtPAb9L9V@S=j&)Ifo13K(%1h zqh`F?tcsbryn*$$T0KN#(|Fe2!JReTh{PskDwUXNqgf59Ps_dy$-+}-?;0`(5 z3~-Qocf4Kkohwlf6(FR&4qF*jX2<>Uyti3K+$w7%`V(qpz!MC$ee;d(H}lMSmL$QX89K)B1GK^RI)m!<_#BtF!=8HGlcm{sjS7txDr8cPQKxT1ZciG; zOeN{BHKK;fC~DQxMhQDKrGDiq>S8BuUQVov*hTe0z#cd(j-)cJGH5~8>_?@ z6(PDrIBOLK%;Auf4I}CfJ9*sUV_K4%MH{*X2NAH(l*?e6?=I_0EVYN4a15^1mTC&? zZzfP%WiAykUW@rke=Ey?##Z(9O^!HN}JZz62RV zb>+u{^$`vhr$kIh>0`_67<}a5WemT+!;h1>UO`HhH?~_|M!?xMpW4mhDS2hIHjJx9 zYVIN3SY%^zyB6q#AZH6yhc#4}n0`VC&Z(0kZl>r`EKikE{jxZ0y?wmto+K072v)1H z#@im8o2%q2by<$qmTb0~3DidXvKUH*#^t1FEIoEpGA!nhkCo|@rf9KV zgD|`LI96_?X+KPA8hM?o!WNJ1R?=3C!vnM)7a_6;jVG&)>NYS6TulchhuE%tmqPBPcgz)>WT_V4UDAe_ zRE*-@m^Lr$p^wLMWu9-?*Lu1f%GY}(?XYg3+n&FwA(tWSAQhe-DoOIX>=sfBkswmQ zHkQm-;;!!aQH3&>hqKCfiV-+aEsS=^MrNaD21#$HT~o3K*K@2?O!aX(K2X+u1FWsb z0ua5Rmz|3sQ*jl;#*)OIHGHvmH6>Wp7#+CKIcb)P&_S^22BAtZ*v@tg)xai(i0m6g! zehk`Z#{*Q*Wg4j}s`iOq1KHw2Pj06rE{l$)(7NoN5@*)o18%6NhF9Hk zY@c&+a0}+;X}<{DEdRQfdq@#g?a+kbWC!bI@o4$I2j|3)6V+ zbnGyzG&hZ23^tDJ@k=OOgH~2(Um$oFJ#A@|cAM1{iHt-?5lFAPuXnNxoS(opgaRFj z*k6;oi5;k;od~__C3IpTi{y;y@agi5Ci`lAkVD}$GZZ%?F}P$DJCnH>bro!TF@52d&F=w^up(poS@ST|3ep5{_mQq5zJnq0=~J ztLa^Mtl?*Hr}c7VI9Y7#iL=9c@i9B?(ta;y+(`47dBzN@5}PQ^K^1S*9E2O`H8Olb z6O}Ga+NZq#d3gNvbSAMwx$7MjxVD@KBEt*Cb;OOXBSlEhR_j<}D^op8UJU~RolO@U z@GPadzULF-i6IAHr;_FvvA%aenj_OLqv!n&PsA7IalQ{yxKr4atSi-WqqVk}nx>j* z5ciz7a5c}JtknbT24@~%Ke=ugnkMO+)y+@RSbwn6yeW> zyh)BBpOoYLnV>hx;SBNSGH`!-$aju!$PZ_etUq@6#c83^14<4YE!^*wE@1Xt 0.1 && Number.isFinite(ahead) + Number.isFinite(ahead) ? (" next: " + ahead.toFixed(1) + " m") : " next: -"; return ( @@ -282,7 +282,7 @@ const pathIndex = Number(item.path_index || -1); const ahead = aheadDistanceMap[uid]; const aheadCell = - speedKmh > 0.1 && Number.isFinite(ahead) + Number.isFinite(ahead) ? ahead.toFixed(1) + " m" : "-"; return ( From 50f3c7718629bd7992f82d61230197a14b34c87b Mon Sep 17 00:00:00 2001 From: Sebastian Lindholm Date: Fri, 17 Apr 2026 22:51:00 +0200 Subject: [PATCH 13/33] calc distance when standing still --- ROS2_ATOSFleetManagement_cheatsheet.pdf | Bin 20494 -> 21159 bytes atos_gui/atos_gui/static/fleet_map.js | 11 ++++++----- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/ROS2_ATOSFleetManagement_cheatsheet.pdf b/ROS2_ATOSFleetManagement_cheatsheet.pdf index 1abc53a8565bede23feec5b7ad9ac38620ee8181..86e56f45f5f5ef47173dbb4989aac8c643f1fa43 100644 GIT binary patch delta 16474 zcmajGbBy3k&;~d*cWm3XZQHiZ-(bf!cZ?m|vt!$~cWmw8-tYdoyYDW!N;;`Ld8(_@ z>2!B>cf}WgmY0IogTeu&I^*$|oCw{sT8}&t@zOHskPx8VV=vGMi9};yM<~QM$-7EP z_l#{@DSerQN}I`JstwXEQz?Ys{vV&r$PesWg1ALi2#+u<-(O#wqfA2lUlUGy?_Z-* z-*5XlR{vnO_zw>sR!0-i1jhIUf$wqg#RYTRK}%K!G{mKSK@~tM&IYlGB$UboJEb%l|M~jo>*#6 zur$?i_Q#V4i3Ip!_d)C9n{*mU$e=mW6#9%*ghz55miLVF{7sydb_}p>z#p!ynv=j1 zHQbCRKw3c{@Db2WTQSTN~n1* zVQh+~bU6-ub_~=bB0J+ldy2b+9m`yJvAnE?9J=0wwM_zIo^XXxBlh(&wh-BqkKupKFx{ZS#ylZf66!8h09{wiDKhZdX0 zfsOP>Ugl&ku1reiKf5h(16U7a+_Bx19iZ2k4R;zE&ebDN72HF!G4_))kA8CWDNIQr zbnXO;L>Ak!-YWcCR^R&tY}j$?tyn*UQEQXJ%>$LzCK)o=cx=XQ-8a|>GhP$t@kOlo zg)Mh~W}KY9*}4QCc(AV57}QZkGT*wl8~9NE4sgXM68(xA?xmiBin!=2`ppeK$(t60 zm)1|#PT8=}65vFv0CB^kJBj#(E$?)-&B{1Jn&vbqKe^M-B%`{Q>R_%jhehJT=d+0! zR12h>R-jJFl?!v7pbf!?^Ui z>Rj_)UsA&&^}&L%!??fAaR{UmehJw>sc~)q(w8a?8%+SGDvz_Ke`-!9~qJ zErrLr{|_w^XSmx)ppm-$W^So(*)bBf{qI%lz1+R{JLl>X87=`mCsmKXd$YpZPMr>y zJVvf$r7)T|7J3TZB9m@^>rF1Wyfg$n%(y#CmgDfvS z0*!Bc^sAjkn{nK$B!XcjX*6y`+G(6Sr+R+YL~uK{E@e~osF2(Iy6qw!W~-A8^_kHj z+==ne1(+@LX(v-=ZHIlAqXM!-m$B1($fv5B`K5vtbOG%Y`mplfdE0sfP9}i1n)uS- z4cSl2qo@$K6q8DM^V^Avk=o-10ijb4U-bE_>UC79~uv4R3asXA8-9 zPR@%bqoTt$%UaAV15V@6)q>r$0 zC9qV(f8@wPkoz$im!Pn0?*~-Na-LFSX*JEKK6^Q=dF#`#u)M&8%xnFcSt+w^s zl+*ruC90>K^I)f2RVgC`20!smS65a^Y7u(YU?q)$Bf@lEaILyB&Tbk5&caS!ZrAM8 zEDhyqmxd9GH0{ngT3v0Ssla{VaN$q~+4-M!X3P*iY{}C%YRr2teDt^T<&b0RDRKSk zK}By1R#)xw)g6;iUPLOKg`>HfyQ_tX!~bYbrnYc!JjBez|D&*QF>|COp@;y(T9b~O z%qZRe=-jVGwxMaFg;j)S{XoD!g9U>K;n*|20!yUG2aKmVnu<*uhwaI96zPXtwsKMO>`BDHA-*drhhvI3KWZ2WtCqfkG( zrH3|_+hv7oi~9G~Y2a0s;7^r!@~k94Io1qP8~2yd*;KvRk-dVpOMC$yPK?F_JgLbC z?&~TLKgUnPx?GG??WDd+PbyC>vme4%a+$9Rbgn6&ea%)19u;J^`HSaU(Dq^N{RN<$ zB$VJcwG#byns==k%c@{$KSb87|2EWQnN2uIMuVa#3w!YG9`=k*wIXHYNep_M%2Z=? z3!72?BlJjr5~#CJCs_sT`?#|Kv;->5AI?K&RF{sdpciP!%V7FzPFKsy<8ivIXH}@C zlrkWOo{X`NCB04K^lO@y z*7{ZbqUl{`N;UVj0?fj%-EV2THKjcou%a>=g!5(-+J*j(jB@Jy{GtChbQbj zJLkC@CT6<=w=q0Y%0G;-yGI3O#LTbS9CE=mg|6=Ew_|!ec<&b!6 zsAHe<6rw}5%j1IZwEYMVsWH8$qq+{WwA*LC_Z%+{nCBAB!3^ViwS3?+-evOZ-u_oR zeqVB-yg!@ z-0a*b^k}32M2IkB;!u4AMKUM}PGh3+*w7S62sNk(aW$AM2JP>HAY$+k2npfl8u;HD z%>h*y`gZVO9#wE1KKX8zKYrML9{vmX?oQeAoBiTHUC(#35@mR>O~Py;#gtig`<#|B zgN}kJY=SXyd-_P!ULEo&^hYH*>F7LJcO&BSJbi%yT<}m;@w2wu>ltq+&5(&p7({=6 z-@oeuao>kUUogkLNOni7VuP*m0bruwh0*UzvxwVGtTRR7zC^!oY!TG|F?~h>aOdV9 zwH;r6*{d#c+CQN!|Cy`u+VqZ#EK-Csc^=+55addv7%#;Lp-n zNb7s^v=2o_ryHbvYN2}3y`_UgGpTl5@eP;)g|Ync1sU^#&1ip+N-^eZHO>N?@qS%% ze4zV+3&gAxp;_bUMnHYcS|kke#@N`vSAOiy-4UV-zZ4jNynt%oP_U!5nP6%ZqUc6% zfO_J1MfzzF#xU*=o@rEzz8tWhp!V&fKB9hWU59?_I-_lbUWfc+X?G$+oNBX1V?*Eo zSi06tn{hq0hhxJ|v_PM|o8gtD*hB_#LEs@u-wo|b>x<`Q-I-EPo-X$@wz+?@|Jnr~ zshWrCD*@e(D=U_nzaB)ehSeSIH0GKuEN*sCSk*US+MLQYP9;bvir90)|5z@-?MA;6 z_B-||v0 z5ek8Ic%z<$mu!|RJ!qsVW%1jXuK&C{9EryfsC;}*J}62e3zrKU3`gp4aPBVvg5P|2 z)^)OQ{syS2CI9nI-eN3YHfKNcz-^JOK=F>ZV3x%iY_H^=qKql~(ObTD$TTgD74z#! z0_X2=UUFsq8P?$Ko+^K7Y4i9Ya*T#4HzeT~;k1SctmMizt7>#?YNwQGF*gJv2{FIv ztA3J{{pcd4rm&Ae(kNAUZ{e{E&^%Q-7mY;g$Td}jT8kaikpKmtue$OB?19LzOG?8hds#M>AV$3JUeq$8Iu!M_a8xvW8y7moC zk%2Y20V{nh0^M1%DYyth4JvYJzN6lzqj*#0LNiAm?)NVYlNDsQ&9#G8pf$}zO}9P2 zoO;V?9!qh}$ANoWO&wp6tSUZh3DIgwA(4BE3Qxdv6B(xU@a@F)1`}wOkmmdfl4hBT zML3kfaJ-MA1J!M7N%I~`Zmr}5{H!kM6RaPb2^MTOvtxp{epiE~Ti^J*DCoDF*D%>v zSB{1T%-3Ydt)R~)5G{bwmDwe%PDXf(2gRX+lN!CYM!$;y^{ zY=fjHK-SqIH%IH3sV`D4ULICMHu$@emzOSAL;gvY-m!`{xHF{3L9O5d_g4rFgeCaXG9GZO9u*qxmsQ`8bcocA~aai8bz*P4~GTf+C-x9 z2O}9uZtIVJ0v7S=K9}Ei>;-)vg3M3D>f0q>|2$t7`ZSHiKdAT0v{F$bHiXB^ zsfI0tLtv~VT<%^PFf4-(8B*lN0 zij;BN!bge%etpl;lFBxfa}E*YQPggq#*L9y>IykluEQw2xR>JrE-^6lY`CGL@;FIJ zNM#N~B*=ywcZol>6!J7(wqRIxD`_wyaHe9bargO_pxBMwp$SNXBL^ZP^ zFenI!y!1tXr#-ti2d`9ZvJ^Q?)N;mFZe~g6uz*nmVPMH9-~#$3Qs=nSA~~!JE3%ts zrzoW4DNEfYsp`q;^DHhA%r?9T3{0Z2l>=u!3@iKXV$60w92`e$BGgs&&Bf_r6G^da zX?tNi{dI>QbRibc2teY9iBPbt1eo!*WE-@_AS@c{dTJ)5QW4jKIc^gM*Vqu&j&PtN zVK(Z44K+T}>bqFttFV$k%nOW(o{*8$t!9}!aNSJsW~88tbXx_fserX-I5rTCK^b7>pDP+%jeYx^g2=hc_uICx22fW>eO zoJi*JtY>;`ZdH$pGAiX^F}4j)CQrg&Il})tlAanj(q!Kz5#leknZV1B|KQSm&8EFI zz)--r@sb|)o{{!!|FA9APXMH9CHb3KRFg08j!W6z$$0j(qDWtQA!sOW=dZ$iTIN*8q#^_yO=QZV{)*q3N zi+=*$vwHxm*Y#+>sexd&@sx4wQxCSs<5xxpZy@dhNKXJomC76`=sE#yK&cWi+>kCy zWG514GbV#_p8msLJdfO*mDGvRG|q9}oZZiDTL0Haetuv&(62uV)ZZh%DZD+65(U`8 zH)#n{JN0{b)8<819c|wWj;y%Id};Q?{$d9eMgxiP?IR(GOMi#+PZl5{R2}l>o3${f zv@u)5ZkWhOL>X+Tl+7nkg^dM>QL4Lj*kDX|=w<2lcs#{r#9W$P36lsiBmZ=~`GB)# zEy;5_3HNZVUMriMCRjI7vc`+YS7r46(;?)3_xAUGh~FMOt)})heJc6d4G8=)eTO|A z`FF(hP{u@W*z~)R8l$bbH|`=iV@7XFRh4DT>|Cg*+vYJiqxI+ZbPExHfRjR&EubSt z$&vJF4}pUX_Ag3GtmLQmPxp84FbyXK>7~E&lpb?ds(ae#c#kAhiI|xev0F_==+6tl z+;F#v_a1XD7x8ckIkjp;zan+{llFeA!<5hIZG4ME4a0@|)e4=JVEcVnjnj=Y+s$mw zzn3RDMR~+6x83cs0Du=DKnnDkt#v5oRN-v{H8dmUawe@qw;RwJ#W0LOTVoi#Ew};$qYSL`C*3r((;*464w7YLk!^cWeAV13{J@M3@Jz8u_@ zaz|n{u1z~gBhA9Vc0(vuMBz+EyZR%Tw%HZHr*nZ&&4Iq6 zTG*h92~|~R4xw4oU!M-D)R&gYu_ub(SDikbTw_Et(k&WH8vvJ{08&)Oh-hyhr`v{! z7HM1$s}A59PF$Y1KcM?DwP|q*?cDJgAvpU~gSLAm>to?0!BRsR(q^?$l$Vs1{7_f$ zjMz>(m!3A03L&rV^$Yq1IsOX5=K$FhSLblD>b8V$QP;(GiK%hSbs>hDR|A5> z0Ls%)q|GiO3r$ywhR%W+BRV=V%%_qy`&S(=ju@ztEY4|;y|5Pj$sc#@w z3)1eWx5An@F8rQs$$~;R73ud~OI3#-3j$V6z0ggCleZ@ulkHZ|%cl=Je(mD7SqzsE zeW%vw@D!a~x%#jNk~~1aed2AtnXV^!d2i@`)EOB@-!@}-cfxsR|7=u_W9J@3L9^2= zD~mR*4xXN2dHHq5meX<9#`Ql^M$EWyN?|Q@1pO>YPTo$aJ^>cN2hy4&}MVgukJc~&i)(1 z{4v)!>UAJwn1_~aR414eh1K&V`5oV@;gIF-`gLGxBs_0#Z*9nJ^-bmtHM{*R>?|7w z`{MIAF;|CErTw8nMlgQ7Hjlj`{1!O&*^@)KT#srbkC1_Vzf$KhGgZ)}%k-^Mu~QBn z10jNQiejdI>$)-$x}Gh^ zlIMO|$cm>gc!BB&kvke8=9=>>N*!siH&RO$D|OA%U*~awvUAOvHQ_j4><3=X9HdoF z&^qkn_qkyh;oXYk|NL8t)61|M64pB3Q1Hpi$%Z)b5&jZpDsaG{SK$(YLL8jB6tFtn zRPfq;LrZzs5&-z0H`crI$Bomrng)Dc$dIcIj~pH|m>81iHqu98vm%}N*!++zNrvJF zv!Q?kD2J~eJmBL)?)jJ27Z7+puo7yKHcIMhg!otk`d(IKN;9694Hp$UnPv?lN>%y( z{NRj9>{h*P3@Y12S^}l(Mk%<6XV61-ANfkWF|F+yE{FCEhYT;T z`uQ4Nfm!BM;2*f1m%C3k!ZIu1dGptRKnW#~e!MnEMT5<1<4efP97uW=@Z()U?Y@CB z$V9%2B3PCUqbx?nFGRCxL84l~dx{k~R%PKebgFsGlJ`Y|T@N9zhdHSIC92&{t&ax5 zKnFvM0Z&o{xvd`BB|{xvz)W_)IkjCI$_#B!c!)`2?H+^qBbb84`?hI+!vK6)L?`U^65Z{yQXROw7RTU8~W8JXZt@%l!zxC@j@)+x0U~b9~IWd5erOi_} zUI`+CY39sDBQTm!xfTxWXGv8UKUf`0w6*+7Ap!}JfW$L)1kH5o>-jrROe0c`QIBh< z6V8H-AvyxJk?~&T391I2ln>2OH3%-^vDBV*yQTBO6mVp{#Ejr)yASMdZ|%j)Y+{9b zvG(IZleQN6j1)Fa<&$-a9rTG5@HPB4lwBeTBi61S!^PUQ?;kU5R@cd9s7YVM6d6|C zB^jYe*QHl1N6oGKrIGQbA59UB2&YLR0$txH?XR~Hn`433mm>5|Iy*=2tjqCtQ%m0AC_F;JgeO zgYnC16HKJ)j%q0b%~Q7z0GFmZZkf%Q%`NP)1MjPV^id8LM*x!aaTy}l)cq<6d^CT+eO*7TuGSgNQq}@D zk2alL-BEl)NGA!zggg|8&>bOQ4R+t9Rn&ag`2r zW5AFe6Cf0)!aNHr#qId+<$WCUB6xdSd90_!Xr6q;bX2#|bkXwW*WqNod&_5Z!|it$ zmmv7|kfXBh2jzST-a`W!m+?*qkwL*J2|r7o2-N&)p-O#Iws)g1HZe21k|cE` zHf3*(AB&MOmyV2**kj#Nfk2GxMBigO9uO$eY_j(x0v3@B=0jHA6l*ux={Kl zvDZ}TsUS4+YNbX7q*rtB{L)ynEn}6e-GdaoHS-%JG*ZGUuV_xup3_m+JU&|dp+tit z$F7?MD?|Oqz7=R9fA3*Z%8J8j&*MW@n86-@U`FrC{x`(n_nTFn|Km6`;GZ(F&msIw zmJ8+QE^V70DKM~EMwYtF5bh`pWz_X^@d!DHGEQEoHd+Dl4UasyxPdS6E=VbuT*{v& zS$#vbB#9mu5$Lm;6aE4ThB}k2Ui4;UVBl)xS<|Qu6-NYjSZXa@$Yfw5g1*qS&)cv9 zbS4vxeeT4H(=}re5RvOEvJ5YNA-^ceBut-Ww3xsh%bh;W2yW%`Wq{9c$_(E}4xUjE zQBs>tpg@+W@W1SjYG|MeZW!xbEfFfEu(Gh?Jy!SGO;v|$aGRw@V@v4MSFPHiv#_A( z;7w)wWMkk;4u%dFwnA3NjLYTekfU4yCYJcyl}a^;IkE>fb;I_PMAjsrPeCRK=xatZ zj?~iI4=W9WY+DuSX?65y1hO^hHHe43-5q6zM#eFDi2!aZ8_`WtIVbynHz(a>;UqV1 zI=(B*xB?fl;ufk#3b}dYt?BLDGx6L< zcKawi{h($PqU`0;L*Bx&xST!`@By77jk?ElyORNTp8gJp2UhXpTCI+MZfO+ry}A9t ztB|ipViLNMZ2Imbx=g1c?oPcFGOCVB8D7R@;441bJOU*SVff4;LQ5ec5`!LnLyd1A z8`d7BY0}@=Wy$d1!wWVQB~l9jD|$Fg4%~oFncQ?-j~H&IOCFOu{$kXKT!Hs1|BOD5TiQf8 z8KoS0p%Bx4)a#ZS>nK*Jk|37PpJRvN($z13kYdIi6^mc)zrZla*hgUiXd5b9W+^JE9N=JU*!$1okc`5LNv(tzmm&sHQ^pIPMmcJ zUX0}JO{Z7Cz5kLgQzc)-1;4fmWiufvU72yh|Amc;g6p3zN9J*ml~dx#nJO!o1zE}A z$kFi<4cR2Xd+fldraenR$HPGAFbKUB?H`#-{kqOcf>dc_gq}+Q=2Yh6q#0j>1uMX> z9DSc77o7>mMZYe{XVW7)ShxiL)uZtkUWOu8wJPjh*X4MOhRe_qYS-)dExqTnoiYjZ z_+GGV-5;#8QOWmi^*T>o25-WjJLNYSs|W*oQ*yKPFr#i|gMCeW#FpJ{be8IN?1|1K zVIf1YTXn;+mJw7y7`Z|s152#U?%$O1{Pm~v@x_1GO&@Xk?r*GtutN(!6AZfO6Ug6x zGz|6`-6N4`G{Vp$k7$=3bvP{gh-JuJW4MUZv*9(&Wc&`WL|l?Z`-I)~gISYdg*X0=&Ev@(+`RZ1o9Uec*R}pfidQ$&@7rB;huZG$r5$CU zdZ6X92(M}dXd>eqasNX{%uSsXl^b@>gK^fdK5+57bm^rrRbX~!oToQZn~Wo}!?IRW zkP4uz8wJ0k=D&X&)xr|=`EsD7CrWDz2IZ$D>Yo6NzLBab9R0zWN5SL?JIDp<6RDh# zP)b~>;ppI^yWvQ)Cb-HofBD}%Rcv-WRbo>Vz|oKaBQ_0d)taSX1ALIO?mA{6f>j)h zGuV&8Z*A+jBKZn?OTv;ASc@3E9y$KFMK8QJ@sj8(k&6W0i-ENKnD+n8+(v7wCoq9FvN2vSZeIk5Phdi!UZb0g1 zi0qvLJjkHQrpwdzgK+lQvxHDA@A&_jznEH;9%#qjGuLVzMEpD$c_I3h{j!EOsdxa4 z3;BD&nt5Kwfm?Z9^3JBCVE@c;0!2WiO;ha?t_rTiHLd2z)r^pct4PJZc#DR{K+wL- zq3VW&WP_Q@3&zMCJiGWsNck@r|6!>@Z7NX-u%J+z+UnI(JdEL^&7$Hp?7AZYBPyam zQhS9|?G9owkHuH5Xy#R{P}Y1$M^PX~bk4eZn<&)NDuxqN1SzH&$hj1LRC65cLn~k3v+;NSWuM5R0V?Z)jj)sg|{XdGB-D2oW@$@{1%{i&=By zS`@eKh4<}ta2BK9Je&qE;p221nB5HkUOq9F-X6F{R|MW3#d5r^*GRSWP4klTQ`+`z z`5|Zf7LUr0%3Xd~@+EyFeRO=Vtq6TR%o-Q#yqTO($5}{>kC9Fhhrg~|E%KNOE&AGY z3?m-Cn5KO|$@`X=A+7TXWQ6}26*)%F< zYLATZEuSBKjemyr9GS0}yC!id**^nGBnRW&2GCsFxIQsLa>44B#)RY6k>9dID$t&6 zYgh~dhhNG0-*-BvhcBFUaNTcx=S^`A)03?MjjrQ=sx~prW|{)FD;@N>9CjkHT7cJ` z7`cRv_V&|ktE=i;0YkqRH_sWs6XqMGy{nX_UfS`G)!*#&w2O_-nHrcIGvbvczJu6q zNe8u1ikLXHRxiU8Q};5}^5TV4cUD=7HRU+TaTXL0Rs-6+G^8n?rqk5i_5i|Qd-euh{3^)A2szGI0T*5icRFpPu)V!}9}JoH6?_l$_^ z2ypjbFdDMp16@Vo9Z)zXgyw0HrIoBY>;M=1db57s`s57D);`&@>;Su2k9}d*xJT~> z&~})V*3nyNK5hIB$V;iYr32V;8S28C0fvznW=D`4s(NJVJAR5o= z)lOC(y%{$ST>QUtABJhEES;ogahgh#Cf)B$!Xg?5&IVlu>+FAJk=s*g6`j-47X;ao z5-}uo$z-Wk_Uyc>C@Kqwzv(Y( zXRej2)RC+ZGAwe9tJxbRi)U(0YdQsatJOj_Xj4mQ4l1oU}{hZO*hUW50057+Uy|6N=L(W~4h z>W6E@$M~o>E+4C@U6;9VsAGya?nkloWt}l?fCXH)8Y)ymk@C|mHM#Dev?od~4+?hD zuBt<+Tk>-?`GP|dXHqBDw?_(CNg)=lwTBNq>4eHs`#*qUx#d!9|Jd*4L>fCCjPl%a zgF=hc5ZOtZbZbswq{{ib>1fT!G#v@Q)4y26y_(d0GILGbvQ+g?Y@Kswit3eMiCk7Z#qz7U3VucaRD8Mz@V09#AXRS}t9dOhiv|G?u(7f0c>}yWwAoy#Jt(vrWS58YF0m8kY-unZ+|GC)T*bL^(B| z!T^j;Yu$?7^_>OZfUpUy+#@|U8sGNwoFoMXH%KJB|D~p!wY~-)obp%CiJP-J@w_qY z3Pw%7CJ?d;{?1x&Q)+uK{oUy_^LpM(uzo*$P`8`=rhj$S-^;ib@Yz4=d=NGI?bFL& zb<{e*GpHvy%VgY>B9!X&VVI@uvU)L^-v>l>Q7LWGKHOkv?0pHDu`zzfbe~zhG57?? z3@@GASlw>XaijJ=-W1=@qq&{bzB+b%B&2vVrxl|3B3wQQ)>{>5;hRE`Q>S9P;16QK zftp11!6mP2($+(b$q^4n%0AA~*R^Ry2a~Lnb#HaDc zl8bDy{4|XHCm{c-8f^FjrcASxoLcP;lK z;VdqUx?)2D%G?tyJ_$^L59AO1l>h@WzW&+qWzhQ_C9O2jjk5i>^&O|#wGM|h9FE$!shoB^d?$5K?RYy*hldUy zWq@QhjHzWEK5i{DR-9##zh>m0y_7T4;^0ZmKLL>du2QD|B0+=RRJw6#54>KtUIb_DqIj4+5?@3Cl)&RmNR(3i7LB$0LfM>a5% zLuwYOu~a*}TVc;T7|r;5>-n#2;HG=*9&3=q6)I#-myJyEzFo!<20L+lE~!4i`~x#I z4^m{L%;Q%m8+JqHv9~a7*$WSM!kt2oK+8$_<73$3*NmlFvMt$dhKx-ZDl)% zbhAnSTu{2PE*tYMfWwaV?EZC!U%m$;bh~wWYA2rJaxI=O(@V37#!hqhbrV;+7rDAO z<6bs-t2#&>KLQ8QTa#jzCN)>JT!L0}uRfVACAA@O2rE8>kUbLDBofD_=qQR@L7srn zOOY@_NRX;++-_z+;n?MLjTFj@{+il6+_QV4dw~8Xp2$)KcvUO)H!Kyc)~ykyhbT$J z=zOKmW9h3&tUJoMg?@wY9vHsozs;(81m&&FIR*RElj!}>0{uB*P44jXLdMfh%E;1B z3K72J%+qJXtkH`vdc;V6s`$T5t$36cQ7llncFZZ>kf`Glf z!_d(SD?!3D@M;4u`9~;|4_cC)NLHzcmmSnW+`$^>tqcn{W=43c5$u<^oC=*Up@Z=9 znWH+Y~{21dOq%())TRz5LlR>VRf1w_*-3ns$F9+3+M02ku_M8Y5R5B?0Ydeox**M8;? zOCGmmjiz;bvVVNfxEa$7hmN zD$1$~VCOONZ+FME|A7BG>r~^Y>&Sfz$1tr@~(I$~kCL<@&$oX`RFYw}5Yb7l8|=kaeEahBS#m)&HaPOSzM~vVzym zez1CDOxs1jfQGrk^SN6Rn7lRia8Oflh{+_`VcknVCum_1QJtBd8lUP~_nI6FFF3;z zWBAJ6==e(S8WJ{e!XHZCSV!zvc7SIGIN76)vr^cGS%KfJH019&wnH@AXjDtf9O|nJ zBj+3Gi9}G)qEA65xjqbt{n_ERU<{e-aRJD(@<_`wbr_>>Vuc&V1vWp0VCq4dl7fBu z!^<|@T|@%JJpQ!z5uw}Jh9WlQ6!@)(HoVZI`hM2ul$G`QYr6}|+f~%qRUobaT}6V5h>6E$(RkY$FJ76p)5qr?2=xiyrKmh(@BFI!@~x!OWr>2a_nl$=im zod5o>fK9gqT`p~o?%>0mNl4=Y2<$U;hzs)yEiD`(1If5+wq){PeR08k2Ig`e!I;8L zl@cP?>w#yW_W9YbI}>2?H@rsCz1e+W)|L5HF8)sw3P*e zc0KVT(XkRoJr$bf%zI7G+`!2OwVdu4tsCBG2m(9m*L1NwWlko)Z8bMgh;ya}HWoDx z`qwnW{lxr4Tm%FMFI%jC%2>?ozopZ2-Q9B{;lOp?6@Qg^u2o=yXr=M@6xUk2VnG)B5osJ+Jh4Lk30=<89 z8B-}G8mLQ~77N+oPP7IAI5U$S`m?K!+Q^Z4tDu=2#K3Y?U%ff!TaM+q1qiz&9MXxP zi#Dy-s~tXTrTQ)bEar(|x*M(2&*yB!`gmy?{2~E}(GK*Rt^x*-ynk(H!~^ZR6D`D# zOAES)25Hp>NMw|YDi-dPg(8unc16WyA8hMLynS3eDPHYVMzd40b%SlohV!52*2z%r8U)~Hpm*v z+EepoIeSeZ9m<<+^h_S)52`OpdKTVQqGJWLKCS@hXdmrHJFE{dJd-YcJkvUvF~A=Y6J^Si_3ir%JO`m5%Fi5X0A4;p@qOEOlJJb0rUTKh|Pyt4|ejXl z{j!3BVY?leg2W${yb@Mx` zjYCvMR`Cx>P0w;iZYl)8gt6rgt>2Tdf+)H;{sqt8qVwb2x48VqSBe(A=~GL@=y2xY zH@M9W>Q0(+ISX3{Akb zxby)FMT23!I@8tViK^f#`c8TS%cTriUW4yf(&49NEHMM4z@#%)|6nx1@9H6oR|L8I zv^9f#Lwc8Aov$;)@K`#3i@17^OrsV(!1U&s?V;K+W%yDVdYK<@j`I;d+wl>(%=nDO z8S(6Ih__hmC&MT9Oi|wU(MKq4SU3N^^L+vB`XB9=hbsw#L~h{;cU_!yRrkw%RdR+F zO27K+QcK&jl}zu?!G^Bi_UOd3E z2?>+X+>;Q7$XJoc`voMWrPo=g=}^btcem?b=02~zaECVXKVfIC0SeRPSnPV0#{I@C zjm@04rUb#{gLea&Xga^D<}^_JJYa~>NYq(HpolAtzriBX?$jBBQBq8J#4$lo&beW! zZRE*WzkIU(crRT|PuoULR57Y~m=J*Gswo_*=dp9!KcUE}b38$0GG%}>t|4d5St)HN z%-;IjS&wSvG*la>gO}AeLV<_V+}|#;)wfUw;^aJ%5qZ(H%7(V0Q43#f*0gFmsJpsR z-BR7WVok%67vgU8lMauu^?`i_-!qb8R7QlBO;>J(Ue2Y4P++LV_E>p!;usLqM~=UC zztRqln;bq~pIpC$zD>5!)-78|hX@t|MX9B1H*IL*AGx5{@?LiZ<<^7z?-7yUraKZN z!hnXNnYe@gj)JW`t@E0G+(jYcl-*m`b^4g!aPZE$sd4forL`nI=;8TuZgdH=QPToF}n%X(B;gZV^p8ypY z3pOOX-$_s#O7SW>zWk%C1%j`>W-HH*j>|->gJo5DCaBD0uI{xRH)B%S`Zqg0i!+p1 zx=9F$@tSeF@fkZcT;Ww^Fd{MY5ASkcNIDXP7yv_wBH(jDoB^wf$&c`WT*k!xHdI02>2v)f961m zvMx#}UI_FQsjvwr5(y12156uZV*SFDF>W?a$#NDmq$Z4{^WPB%OXd)pdU(=LZq)bQ z9f-%06asNVa84fX6hZM#Fiy^tD+r<#UI}DyHi5nrc?lr^?R2r#&a!La`$RWErge#s zsEx-E`O`HU(J}_)|dCIP|a?u=j zS~`2``=M%NR;Do<;pwwAop4%ul)rz-GO~2hLMF@Be8;(>kbD01BSj;xZgDxVSJ8C$ z>*bQdYWy#-^-*0ue2!_C=cmK8p>{e~)OkB6;;|-%)m`_c+VZGkPLE+1LpK&vH#6)L(pK(qFfb^09Z(s_6 z^MXe&b#o)gP*Pv2cH>6D>q))py?10{Rv+43Y=OX%@4esA;kZ%%4FhoNcdu1BZuC3< z17qcQoM>)W?`v#S@5^cK=taAW3V>8p3REHdkG?m~glBypvyyByXG9MyA0fXQiUx{k zn*Nh&xgxraJVu5h0m5FQj}!2R?pO1Uu%6bw`ZvNp4E;8~{$qW-GKzkQejXv8q0s|C zHA|w=9&9V{DEJ2M4fY1fAN&#G1$j7{2RoAB0eP==-u7`92lfVkW%KcFcn`nwW6a2! z;f?N?1OWXSxMz9{+LRel8Kr+`6J0nFRG5673odm7M=QQK+|A-dBCC9S!2@EbkpDjl z^;o$7r&2E^NJlGWW@lq&|KAt~+kaO7Pg_=I z7UuumA2SCVE6e}lv2e36bN-(g@PBj6&BFR$>EZv#{}l{38_WM2l;DxRaTfwS}3Tn}-82lcX3oE3-I^nVp9Nj)Fo$NfPe=0IJB7qW}N^ delta 15744 zcmajGV~n6p*fct}c6MxY$F^s7Y}>YZk9KTphdZ`y+qP}{Jm2^0ZFrSy0{QDCKt2@6b`7=7_ZuAN7&r19%+kI`A0Sm76In=>l(U65XeQ&jRgBE3v`Dj zxaoPc$Qe-;q@dx}_3oIuH1-~0VL z9_ng!H}$~fZ~7tj*9ci6XHCC7AD52k#P0F5fWX}GaH{m5k?r<+Vb;+_uhK{zO`(=F zuvmz{lPzbK8$!~^l><^5HWU?4Lx$x2Mz3tU&mhsz71^14-fi#o9x*6ZwSNWrskHa` zGt$L&9@x`BP}~8JP(puKtp(Ij89WtLcF^s7|EOeNb*U`yN#W-MCn?}LD6K6EjR+OD3C0ykjvwF^^vK zA+A!-Fo4xzw5TISw+0Xbwln4m#KgA@p+j2PAuMUqU%z_$U$` zB1eh7mK0!&Kf{}|{6c(&+JT!!w3n|=-BcOXiBd7>CuDi5_s%FtS9{<~f@5L_gI&(Pj+(f#eCqUb2AJ4Ku8*Xb2)@)91y&JG8j4lZSpYzP)MoFT(GW6qA!riEr0j|Dxm%YY016yrtX+61MwJ|Pi+nh=CCH1h|q-!ZCZeMGf z))*U~(^GOzw#@qO$O(}Hie#-7h1UD*V;u7$a zy135Bx;Nz_OPlYA%bfP&F`uU{hCaifIyl?JEPe=5ZiaXs>3wtwv zM4uyrB8(7X)|Ti()<8wVzHUskK~`Hz%ElZ68uK$=UW|V)0Cqoo){;W2PS_OkV*RL&zDRvWYl_aDX!oxFo)&S;U?==9!2`H zO`DOODJm#`w$4)$!(kkFs@76(uQNJEsLgFj$i?h&&)Xo5$45nsONq{TxepassxYdy z*Oj(WC#^RcGl*K_Sw$1podb8fBFb6SX0yq%%Nmj*^iBBY5m9=l->!#FqD8dJ&dvBa zq)zQwMPpUnZtu#^XC2AONk56KyCQL#%C#yhLWqbG!w+IDxs;6$x0hIsPZBhgZSdM? z=XOag3urrC7FAL%c*QN06`wH8#Swe0&0WarsKe;fRl>FM5xIsA?}5NavR~CBQ}Ld< zuneB9`cBq*y4ku?F)TRAt?Jx}(Apzt7dALXEiQC=xQ`xJjDcfCzxq!Xt`3Y{F={}R zGvkZ@l!P{5lTmZ6@^ELTE2O)#+i)k5ThJQ1kKmd{Qd4THA$_hI{HdsNynI;yX{YY#_fdyx0V@e!G5ds>MC#^OaQT{yp!Bw^^97lcqqJn(a zT7yRL4fqno3^1Eto{pD=QPkylE)p^rultwoeDo6){@(k=bRY~JUlzhfj*D z75T_7r}yc`IBl6&lqcigtXaJu8zC*iK*hFzp`~mY8fKxN9)M(ue}#ley7J;wt0Ksp zg$9(KDWYGX%U*axcEhWP%NWGWuuto=?gp-@IhpL)Oz-SJO^l=^`F=$zaT;!ywm?ee zm#O_FC*fiTUf1Eop`K&$ItAJOMxfd^{uHBZIEk7k!Ye>4f+|L39sf1O1#!HJOfSfq z0~w?FuJqfaWdm#`CqCW&YBv23cZpcp_7?E5znkG}el@dRE4%BbOnaT;f;)~^F1ust zMQX!Ia_FLn_1f{H`b3%VcH5&>X7%2=9g#t6X+jwTgueAVOx-2E_z|Xh&FU$}qx4#O zcH{Ks%rL*bZl+Q&+3;R(9pPxlTR&Ej*X=Hvs!tQ3iy)4aZO+G1x+3iY8~pc1<_VOUg%_Mr2Z`07OL-U)J5-!Fa})`ev^ zJNhY}m)=fqsX4ij>1x%u5xai6)N$4BqXGXoZ)Q~}Hg>W0q=~^n8uxmM#>THxDSJjV~GCIiCXl`y=ei_-}`+5Fo%Bw*`St1 z`k$tJ!4`l(ukJU`y!NiB&is%nkCJOE1eN#aCR%pkPtMZm`|ZV!?DkMGixGSU%*>B_ zt8UJB5VX)=V!!KaxKI#e#oWhXqTjL~IGPvO^|#(c(1i;sVQvBGT*Q9DSBpHVIPQL@ zj$DPZ1)NGbd&Z0jjPl7VhoOO?K}xbVQTK#meJ3faZ&2|0ITq&nI%lWOpgSn!R&wB^ zvOgr2Do#8GM^{6|M|(7_c7I4dE$nOi{l@%-<%FOMW>o>b^ZC_5Bp#hkxa1tQ)K&Ma zEGp^H@~+`qpcYUNCkUM#w;){mqZc|iX0dGL!mt)C_lWI>+8K~7c0S|SfL|#Q^mSY} zZ1A_Rp#fd>%l^az87{YdMj7OpU*npb6~5UJLp2XYD~21yje2>ilO|RI^OpF5MlG-1 z5$B$9=Z5hu?3?zv7ocmwz7%rr`=r`vPJ}wrZb-)t$N-0ss(7IVh9QbO_wU2P-oZbGVY(rOx30xqUT*!b~ zww0fk>5!-PVu70v&xGObP zrbv~`;57VbsWq0eqot?8f@`LB*VH90KPyggLeAnK> z6AYkprf$d&idaWvp;Cj;uBFoHsOhRKfpZwR#Cf!sKGAX&=eAO}&MgU0YUwU+#eXIY zunaiWXKnZ^B!v$O&qH^&94t+#j)p$ey4e-=z+~829Osrd>oQuy$X$^u9f1hp+Ms45 zFR<8D)gNZ2UTA5}%l5@X(^o-w)Kn{GYYUY36*b&|$Qyd}Brs&v+^;#b71vN@iL2pr z6hcq6;1W5e$+LS6G!dfM4Bt-{cF}!VM8?^CfW{l8VdD=e(`+34R7YsAv!;Ct8?;rm z{fentS|yl0v9w;cTF;2|-zaGSOF235a z9P|=s=m7$TeWkP37g&lO09R5Xcob-=3%8%l6_;@{HYLH@lZU(ZaSJ`+G)}R%KOw{5!H{O+U7wnljKpiSp9E=`y1Kp^^z0Q zceAx525dz|LJ0TNf@Eon-e4n$>A;-XKlIPo_+bu55TZd7jC;Ns1kK_RWfk;KeU0m2 zNKqmeNi9*7BDLT`VXEI>RKpQ#_4NiJ-Ede-3cu5_r|wuoE3*~(Hpnf|*^*ResAnw= z&rcZ3%3`MA%+VD)Q6tyN;7F*&`YE2MI|Ym8uO4H}++mfk8B4LgOt6>E)~Vt=4A>5jd|#(ozr-j?RhX8VkkR z2R@L$)_20GbX`v+KVa!1`3zWrmG*HLNbha%yB{RRPYE_-K_aju1(%R->KYW~ioo7+ zwFR^P(BclCiA2xVik((w2q~O{tK$$RkUFGdqM9t|$XF|0iqsIKI0(7Af+IF-;iRkL zaO`iM^^$KqD7s?W+%(FZW2vf${&f{j9|m&IWGE`C(Kd)CVGF8gfeftZ&BD;gqPJD* zCWJ9BBa=sZ3@tQxj?sk7&Kk&Hg=-5Nq|b8l+MSg|xiH!mtnqm~9=O3)(`V{APQWm2 zpOvDAVogL<;T&3DL;Io+i#p&mBokNui&qg&2C+bU^>5)k z4_Mt%ix|Kmp_2A}v{mKLw;4gI2mtVgvVZhYOv4C@&V(#xuK|jN(dKBD3`CcZ@ooVP zw1T3w`27!RJHD1!KVj1pccGHejJLzAS^Pve55WuO%s!aGF494k)%rd6*?iuNxb;-es7K{V`tcm5K zMd%mkCsip1)&b7)v44wY6HuKY%G$rw+G!PgVUdsuu%<@u1638;S2du|Rd~dj6Xwqv zpnYLAFk_=A_J#DLCL3g$6`PfZ<>pk+(j+N(M^!A_jErOxXiG)S=h8hw|ES(mjcf0) z9S6b6W^w^X|Aopl6V|hzPTafpr`!x#jhAQ88k>yf1)S@c&YM~_WB=%6^D^kzMW&Lc zVywCVa;Sj{hR@Z*2lSKP3v?(ooN&}>~NMci!6Ui^Mt6mKQ?j?szs z@G$;Iw#+eBy|)`IE)Nn-0E%-;lu?~@7bjQTr$tw;p01gq?1#$lr$VEjv;pxRDX<+48VsT(H1NcTsIH zhl86y!tu{zPV?2=@_8luy2#n?e(>%%rF_+WrE#ot+BZ zgz9tNM(^nQ;qnjE=UrN@AXf~mjz{H9Dks924oK!-3WS@BOC(w#C0_~Sl^ih<-O?Gr z*~}lccsj~x<$e_q`26)9*gExXaEN=Cd3PSmcXyy>R_&v(6>)yU%8RW&+J5L8Re7KP zQRxHAV+rO@0gCkKFT#yO`-u5M<}J+oFYMMWdwfJ@Zm5RID4BtnIK)^rnO~9$9ZQHv z1MsXjLmlYQOj7G{z6eVQ*|Rw0CE+3a;q454j5ubZ#Io8U6x7-37E1EdTQrj`-jU2n zr}JZT`Yx_^c&}gE?TUcQ-E?=~x(MOp>fRZ1E|!O5lN3njFm{|>BTxQQ+ETc0tCQN{ z(>s^47op8t6i_ghIYi)LX}z+k^WIzI2yo@ECbHQ`{al@WD9at`QfGN-txj**u+!%)XuNy<=}C^`{wvTE*si&FY3R{?p5VBApD%T|0mj?2 z*Z9p?3Y~Fx*pQsSDet6e-J&2vW(Nn#WDEI6-ng=9A6GrWoALs|>^Zdfut0^vD-K%= zL>oYVBtCG0w%f|=8XC0Sj1`klBgpV(s0wgE>zh35=Zsx#qd2>D%e@j+PgdlVZVB3s z!0kL@*^!K7290E=u?h>d$!Ql!Sh>MsWRBrsb~JH-W$U z0PuRq7HhNJU^zd9D3k9a#vl7`$mh0$^{RiT4sbm$KX1REh^%}!*GS{`H06ONs4~F7 z{h7R)(f&1omNQtw1Y>dx>Dz!PsBBU5C`4VV*WS+)czeXnsqH+u5AfmvnD;gSd5*R# zVCswiUURsfY7*n=`LVueJ2{EyNs-}|fa?x*5dii}IGt6YzeXzxLa+27PPo@8OQ^ah z3hGy&T0wf%z9YSt*i^!&3ccdM_`w-n{mVv=hLTP%G|HD`Gmnm|@uWQ)+omi2r~Cy!fPqg=mMzabqu4uh#-&6||C!6FWVHTDgePPisG zeGlk_mFBNis3e04TC|e6VuM#0BCE5xo~6crj}}Y;Fi@F-jk|}ygz{haER*Y_r?I19 zZh_-S5c*g)kY}WXa}rW!(7_&Q^82QPq=h7vz_QV+8XhJ5QqK#ExM0M4=vB?QDCC;? z6oKKFH4Hy+b2>R6f83_s44!^FMOD9gVzk{=skm9AxSzBUVu}A;YLvKzei+wMx!CA* z{7in!l53oWsk34Q%2t)OJ$EB|8sIcg7Tm0o&BPB=uguOlsx5SB#d@z7*}KZ_>WJW|}qt(MYq{Id`r7i7w}_)RcEZ52x#Sd0x=BDC5Iwc9#%}XnC!~Rx{dq782?4QeA4Wb|sS4 zOBk+XxJbp~`ExoscJ`2Y3xt%ID&+(k;24 z{$hlO)ffM3nSnvfV-1d17m-(1ShGYNC&Mr-oQ+mlS&V6^$Lu)mUH&P3a?L|ThUmy= zA~~%JORs-uU|@J%+V@tx5}GQE;@v~9lpGH#_UA6Zs3;w$6~^Or%heWiK!vZ&7raRA zIdnk0nX)0H!yd=Y15TfjPbT&6qvv+`NUErDfRnsb`hSB25YJ&dWVG%(S|wT(p4F z#fuF1ga`2PD5e}RQVVter7@SO7`1{_wr8_Kq(Q;n5$Tov0YwhPZc0A}7C)dC*t68K zA@&yXAj7{i_A}e`cOzdI$5eHFk33C1+N_R)zdn3On(K3Krb9k#QK>tGZ*U$!dC8~D zai(0_OF5OO&?nOJP)-@h(m!%li2HKC5Oe~F)Kb1Asgq22(7mQYb_=!78(t3QZqZmL zQo8Nb)6OD^J-o;`yaVtS+K|!jQER@f=S0is%hJ5_kr9UQ1!slj4W-=z(w6&YYz{Embw9Q(pABrg z)im5$h+dTkbg>C;tEiBXuPQ4OJ36$YA_|^}s(ynKOo5riG~)gev{VmX5VW=@v$zzq z`^j{2^pRP5RpVJY?k-C3gHW^`TYBoZ<(%Fwae!o5jt0|Y-ve?IL&bU`i_-$cQLDr? z|D($BelX#9-MN|C;>6#s@9Dpq|9pYz?5RhH>(^X(cw+wA0>6V^ad7m~`QT~V)s zY^D+&8u8)t%4GgT$!TI@?KlVwp^Zjq7$AsZK)2yn2{zp7rCQF3nVATd)QeIC&?%y zNz|CDS#pQhv-L9sMdaPvhgGxdrAMs->w)!2V8l%h!B$t%*Y+bww928BH(D12@LtFx z8_iWLCzeql`o~f71Aa|$W_;qEA+DATOlBUenrK4nrL(6e^Ior$V*R=ox(gFmptTcU|i z08>^lG#Pw6+2#J)FOeT&IFJ|#AYUGZxXG>7a&|<*>ItCKB2KcbPcs5BCrIJo_Mn^L zVvkDsNYoI$Mq*1XFJ|!K7zKr6b2gTZS#>ZpDQ`$j^tA&olxh;_QTp9VH-*!On}K;Z{-^{VzuwX=xXbIgKGD)AyZu_NnE6=padHZ*`TlrB_j>n zcnNl;_#oC8qz()dD9J5r_`a7UsVahkd)RsnJPGNA>DHTI_5;OI29&2#?IR-5-=yVQ z17hclx2kB6VvGYJNK__3#>ujQ$PT6K)(a(uIR}cm*M|l%%s9ZeqLQ-s1S#KsM2fk^ z14Xn&>Ar*gD97XUG2q4W75!uLyxM-HCO~%MW~)@$#omKuc?#(enUsKw91*%T5Ulpx zESIjtTNWkcfR|Be0`_%-kpFnCTdKtfQYQzxD+Wx**5Fl0d(RW-)HA0{xTqZx&Z(H%qgRm<#@B8iGEh*i#zuqsS{spl0OhQt4!_AnW9QN4(`X}*yrgu8*IP! zws`zpSEDmaIAA!a=B_($40?9j9_fmA@7VG@>cb`Uy*uV8sC!1(SwZ$uhRdKrRDq$F zwu!<>>Lqa~{nQ1tc&z2A9`ja?G{pOdhSs7*Hv|VPY%nD`g_^!3rX>0JZ)He0-xvJS zThyo0AxhdENctb&kJs(L2RrATrr9fUbZNgE3_n^q`mP80^IENEkxkuyc{%N)Wj&wt z9$M=*%X_J1ps_gF`()CEi{4Kdl( zluX#)jw0IBJqN@7h+X6m!zJH-H0GMd|AAyQLhsbPeLp;$3o zJHzG=ko_s5CqSwqWFcSM#lR$t-HG-=bHF5HUQo$O8_0i?YO>;d`A_Ete1L)>&LnFT z-`Z$t*_k-klS?eRcN=GYIOtXbp3&cV#qO?dQ9A`%soYBUX|{= ztc>J*?8bsiL#0U!y31{Ej=)tLp+l1-O^O7bIKwNW5Vmp%6;bqFE3!aUS;WmO)cRTO zlSf-pc>HRvIi#Mk8Cyjkc7e(O)t-@Nr(Qu=c)-Y-!P2jpiUmd-f?r6DbOyRIdUq2f z0TuuQ%l?U?ua)hJ3aY#}o!R;hF$SsvGET2xz~|w_(8mIwJw6}n{9h~0$1%9syalV( zHO8;;OlzKR$fgs=*Ww@TQ2Ts$4Ix7Vr^(I3j0e8-($j5P1?IZpB`Rmt;q!?DOQ@K` zt3N_@@`q5Z!z&*&e;HIxJh;$S%?+n}Gk*XUA;EV59UciKr$L2i*b!Pd7P6FNKSi}g zo;=5IwQ`?V?^c1WfNDFfw+Y49yC&OXfx**6|EiRwS!n)XRRC**nK)UcUg}Y>=I=pd zrW5?Q)fJdQ=orRH%@#WYM0<$ZxFZC#6YTpLJ1a9&R!*{ygUcHpA`F7?z6AS3UjO5j zt(Veh;SC)>>v$G6`f1(uG#p7;!XK|=k2lZ^J}Yc#VkJvX)ob zstG%!FI3G|#p*=cPt=X8mE7q{1684%e=GkCMlNZBG2z%liXQIm0Z|Om(KdWRdtnJN z4wBX#=(j?o+V;~`Qn`LlGWgf(L|$!@qaoezi+=h8k6J?ZiY9AT|R4VIPK8>piesWMw%#v z!sp?P+>>=SH4Go>E5RM6G(pV3lW~ZKeA+gr7q?B;DJP87D19}|wd?mKi*>GA#)&?R zy2Jd6v`goDp*B~A0b_xR;{dZWs2yo@BG1Te?JvsoOL7B20QsfPk-DmHGL80+HZ9V= zJCl7F47n*X;;UB6bmyVmV3Jrb~&2)Z!gCXb;P; zqgn!CYC6mZYR{mh!pS+P20T$69HAb$1Wk~4^QWm@ye)o8lYxPCm6gdWzKzwsjnCr! zI||Ln?qNt2zzn^@I8{f#GXQ^+tLj}Bv{xux=HQpgbg5wMB!->@U13*j;f3#$+|;Ys z08XjU1euXMdt(5k(C}g#%TTQI#&P(TlyvD%tf}pg>)W3RLwsHj`ZBpjjggI?5Kc zLjE|;&j-y)N5`RDO% zAW(u(-x2Iv`fIm66082cijcGZzSP->LE2?Yj)U=KP7-N#p<{d?NS5adYo9OgDWbG=cPw?!x zZ#@4dz9H7yWBilDFbIz&GBiS*ELFar1ZP?(4>}7m2a>wFRgLH6!dAw30Ku3+(lLq@ zuixKa)g2HzT74_VQ{~%i3|$;KKwA&8#9IT2lMo%(Ny9kY>js%^u7UB9AUtV^v=AB% zc#u~Y#O4LJkktvGKb4FejeKP%$d57P4DlT9yoBHl$nWku5hqTa9AbO8A3y)S zj!!eDzHvhJ)jtF=XI32xUfIO+j6}&Xa4-s*KQwWX8%5cVFto9eK`2+k4&=YdY|4$r zy4N#;c_Mkbh-QsnyZ+F#$K5~~d-A#mKu%+#Gav5Vw|xO@*Uyj1*++Ta&z%%qp5`%h z%yi1bdQ{Y%{ zkc24;!kKF5Q<(1L8o50U?s{?=xYF6ubq*$fW`63Mgn2OAKljSUif~*6$E(C1CJc&tPw{*W813pWSE1Q$$;PzY1M>59R_Ys#l6*y}pDn$4R6f$u-Gso^Y@I|8ECsolmA2glP)&A_Lqu0Wv`~|?|=)b}s?bUif zd2Ew5f9$t?#7@aWMYOnJ?%)WAwpwl&6BVmT4qCYKy7vqV5Kd-$Zxt#09LL)rQQzUy zz9A@*%8q!pA9_D5%`9LyC5o=Rr@#Imw2H|M%kM=pXhd-6UM0VmUZs)xd1EWCe*nFTz?lNTF5^DeJtAQj6Ql ztlFGz=_@i{V!?Ev>J)qz6)i3SMMxtA_}xDh=@wu{zi+1VEtt7>hdTGzIrUhk zN1>=_RA$KyuHsuIkL^b7)oC5tavEUcc6B%44>7op<@iJu*B6jBHEBb!o~TLw>lKI?_<}2%Q!ndjM>D*kf>ULP70zGG z)Ug2Ta~7_ax%}jW`w{G584i*Y)(b=86r#z`j01-+ zJe0}o_QZJ=@st4!N>Z+9MIrg;>fyd1$#AL>^9u_X#0d^+6n6hzDlHM@*okZz;;ri7Rs(S0{^@bFi@=3i*QqRFmIU zENJFA_(>gJMyM6^$lSsRnri=GZlyn{4E4J=15Kld6D`a7vA}7k#(dkqcg$KKEUS%) zO7|;a9kC`fkIABdbrAa)i(vQ;JN=IrU(V=JI29Z6VKjcKLIw5LOtAr**{MiQe_owo zd z5Mtu1FOyb4sU3q`y%V}mJPW=pWad?Ih`(pryA$L*LHY@197*svuPBo}yMY{%{#wG0 zn>Rgixz%Y$N=mlL=Xgc(&EjmAX?Q%S;QE~vz6kJiJVqAkcXOS$uB`_Gx()AZ;IVrH z`>~(*dY*2#m5|O7O;NgO(>{~{)%6F^$y~cPwH(VA^V2<5cAMh<8eMHCK){%V{yVbc z!2FfYQ^aww;q0{dwqu<0NB6@u>0SK3^Ks?JmES8N<&6T32=N2e%CWD(v|u&o0L*U% zDyBWo5C#;mKIQ;)vieF<1H`yg!H6HpC%c+@_Bm+aA{Fv1QN^pL6P4z-KV!#$%tL6x zR#@?_gR>Cg8-#uwBK|%|D*u%D95)cW1ir}$NJb73tc2n|Pl!Gj|C`r>*c}i%FO_W9 zlx#!6Dg5#1LUSxnC7r-Y&?j}{d5^dBL8Y86o8^H#jW`vQqd*-mk6GHOD2K*n9+p_k z-_Px=8{_dTMTfCRjBsDM4#CF29f9`S5$sXWqBfyM$3YjJb!%JdqcuKKjp}CtqR~t-8>iZGg)Y;2SWd zNYwmK9R`6m?s|uqj4=%8Ql7fR{|W#n#%#9}`G&8v{q%>wfH2cMb~4j+-_nru4gAyL zIYUxFL>1R(b{HMUbr+~)S@J-wnfIJZ`_?r?tcmeyB}QM2bpSODBG=?JyG@w%?TDJw z8nvqIG#q;;GF^B1+_AJkeJ@6smBQ1#Dl=saK#21Hdw1yB!iE71PMdW`IN;kjkUd!5 z3#jHh)kl$Qk4{#Fn$X7Mnql!69KbGe)|o^`?Q;Tt=s?w!bmkj4Bhy*3`aI@$BtQ+s zD+gO6i}o4K)Dw-YJX|?DK#^k9<01bZeBgsVuEo5II={v+w2{5d8 zSl5ca;R^Usb?Ym@x%ScTR_}BU(>&4AQyC`*C6_0ss#q=lS`=0dJ=1k0VVcK7G|4@bA(-&np2Y4f@BNaG zH*uC14aheo*n1AmHw85+r_48n)C%mItwYtk?Xl4O^WY)cP!aNB8DuHsrg}(4qk3P#R9*&1e$MSocTx|Ro&VZ@JI9^m~cnWdxo|@=* zoQ5;ce=EObpNFL+mk4wVXsP9SeCae)n04({1LEU2E|GWr7MhV)3a168_!;FHxOC^U zM2oF`AzX-gA>@s0M$rNXnC0M;k=i&%3!wvFi{Jwr7g;v9O@an;#+97hGjn(rx1l&- zvgv2D9LVG7#XYFxUkYuUMWx)%KK&PKBN~pvxzvn>Zc~Bc!1#!PtF5xEZDbnp7VAzC zaCX^9C!{nrIWaoXKI=X|5|Vp@DMI&^vF7=e!ZRqStB*I3vc`qjt6&Sy%FY^om~NVG zR4Smdj)wXv%eJ?W9iDi0hEry3Y3yn%J%Strp64a#d=;1yVjwHR7JxBv(c6VdJ|^4< zZ#@Y6rBK3-Wr1^u4EC$X#>GIWCa;to(A81dR?uajzk?pkSTpF)qBzO!)VSc04%QR6 zC@Q6{LBY_4nb)#9*Rl$FM3i+N$nB=4g0j}eIm?$0%ku)lE`0TDOA7PWwpUuPTG*?x zi>9F1>8xNDf4Boia=l+ULqkg|`&a@--B`!aEfKhE zFaS@LI7%@#6opC22JRaH%EiP@qZDTQk&@eHYp~6dmo)1-X zfM}41+q1Qhk3-gX7}`cCwTKePUNE==J5u6hfvyNx;Db!-Bt<*zpw0rej5Hvh(Tw_{ z1$i4_^&^||%ZRxYN#P3($Ia^Ff98^?FvwlpxRZu@KCCnhc_%UvJ3E)rXCdAYW;~sH zwpoSk;c;O3i*U3u4Ct6eM6n${pWwPTy?HKSpBj$A#JL;}KhHY0_3A;Feo+4rX$aln zjNS94Y=G=B{>H!;#Zm{PTQP#OwY19f$`cyL4Mnl>Ri(^Pae}EDD7dZ8ss*zkP|gkf z*bWVzN37b|$e2OLQczh2WZD4Z;+j=5-^{w1wl_GHXz%7eA7qe{@X;ZK2?6BVI<|G;@|&-}{UI zD5)=4YY00t^PT=0t$>lG>~oIi33qB^uq8oN!Aw>MORiWKes&4U$4-(q5okn{R=QK( z%t2(qq27SedsG4BK9a^86{|w2o;Fp=!>L6}D` zY3f}W(m{?D>r09VTSQy<2Zf9%t0>`!i~nTDiY)j!jm{{T0`Ht?)ZhWRB&v9 zOl1MxFx#Z482+vBRG6e0=<(+>fZ~+=DQ?)5bx{g6+8|&`u$bgU6FO-@HISm|kN-OU zZ*D~!C@WY?#2(~2|Gwu9RA(tPu&1*;?KLIX;pDf1&hUD$7h;gTIuga!D7am4VwE^7 z=9J>PEkX~w+OXkqF1fsDj|^jF7vvMYg;mlro8y}Z_>u%Me6qC?8?K*k1?Qp?*5+_oJj;u<*hZgL5VDX0bzU19>pr0M2heG39 zx#q>Ed!R7)4bIU1^y(;qI1Vhd{lgPNJC^}|UzfKC;*OyHj}2aHH(azuy%&-TZ$nH= zX?H>dE7^S~N~C|3^wV?U(2Bsa?@tX#t9TDsfguo}c!P&ti7DtC%vy}+x<@GW-rhxb zb^YdkE4aM4$Gbc<&ci_1F#%O&)6tjJ5$LYlTSfa{SJgFKB-;i`7%<#hzi!1M&dV7> z+PgkTW=&tfD1E*(YsYV}Q}}6{+W9*bF1S5+|H2_M{?`V-- zIsi@o{qpKg1(-kfoml6hF8zPB+SGIgISkj(;nMtx6gmhM#>)K1+%*2^{^8jA;&?h- zNf;_{gh06OZmXw$SRSCB+BufZ8?8?*>&TYVvlo8xvskWXl2S{aF=W4LUuNkxa6e!l zIyOgOijXI650$5mM~WwdM~lbfE_(U}X&mr3+E@HVK*zV{#p;t}&4$3Y5Yc==C!@u76qRa(`wbbzSjy zswH>|*s%+i9w5VJGAJC=<~c5`c%X7-b%{my1Av`EF;TuJ|sIWky=CPrIbLr z@g|+sK3u&4EwmmjNfY*Jc}G5tr9s4peVV7&G;58gal9goO6UI^Xd9A&|&tvxRmJvMULW{|lb$}`3rmS&O zp}Ayi#B$3Xs=6g~OrqI-ZBHzdzXq&MbFXU+Rt`g#(nJG>F?3V%y0h&eg$)(Q5zZL) z3@gGLI-&Z$6E++|a)>F^6kssIEW-?efQxvHs0$?q+Mvr+0}3Gyc3o#o;Jzb!sXRV6 zH{?pyRulzaE@d8kd*WDJ$4=)!std%?|q%FVX4N5I0-j*_A3pH z7J{_pVTYX-tCw{JG2Sl==2Kt(v2Qpr4-GA`T^tUajfpF9M0_2LjVtjIf)G_VX)-2L z5GDAU7jk&g5K;ztB5_GV07&dBv|XNXEBu~pBuKO=<>R+=?jU%(uhAl3eLh^Z%_caV zFT$6*RZj5zyT=Gi5u{{G1w?)$!7f76#R5465&%11V8RCr#Q zbXrLK6^T5X6Y^g4L)N|PdZS&;&(3Wt%hfMg{4V1zG4JPxe87G8IH9|?zKm(qK5;)o z^_u3t!Q}envA9z|(TwZ%3Z($hb4T?{b3O<@AYaQdE$$+ph`YW2u=_Eh9aJ}@e1J|s zzS2J83NZhQBD7|R0tg#pK9F8H@1S4)9wPfmeC1|*6uylT*3e7%#ePlQN$^kI0YMbq z{cHHI;GFocVxA+{aY3ZrHo2n=TGD<(K<$TW;`Y8b>?@>Z_$A?D6VOANB7hbA0eX!- zPRJE@Sj{)ea8~;n)Cm9j_o?ACsAAydRyX+ncwEm8Yk%o$6i{~e?Av~y_}+Qnc-+Bz z@V-J`-wnm`?MD-y-yI;IHGkYj^S(-7T7JCg-N7&Ijp|#`z4nVE*ZtMNvz*8hC?K)D zns11L#h2kGWW;Xc|N5)D&zLU-bb|_@keL48(|{nQ1IEEwZzUxLTHh=!00G6s%EZCK zlK3Id0l~@1&X~xdzy;38$eQT<-^j$unOLnr0WfnBG5((-vLk}~zY``dW-jLcfibc& zvoZcJjPw6P%gVy}zj^;l%f!LT#{PeK!2ipbgO%fdh2dal|6kaDzW>vNiIaole}-n_ z{F?LoV nW|seanZ$(P`1#?ST@0OEJe-hC&ES~0I9V9s$jQaz#o_)RVqQr- diff --git a/atos_gui/atos_gui/static/fleet_map.js b/atos_gui/atos_gui/static/fleet_map.js index b2d81815b..6ba181ddf 100644 --- a/atos_gui/atos_gui/static/fleet_map.js +++ b/atos_gui/atos_gui/static/fleet_map.js @@ -175,9 +175,7 @@ const candidates = trucks .filter(function(item) { const itemPath = item.path_name || selectedPathName; - return itemPath === selectedPathName && - item.tcp_connected && - Number.isFinite(Number(item.distance_m)); + return itemPath === selectedPathName && Number.isFinite(Number(item.distance_m)); }) .map(function(item) { return { @@ -280,6 +278,7 @@ : Number(item.speed_kmh || 0); const courseDeg = Number(item.course_deg || 0); const pathIndex = Number(item.path_index || -1); + const lastTcpCommand = String(item.last_tcp_command || "-"); const ahead = aheadDistanceMap[uid]; const aheadCell = Number.isFinite(ahead) @@ -291,7 +290,8 @@ "

    " + "" + "" + - "" + + "" + + "" + "" ); }).join(""); @@ -325,7 +325,8 @@ "" + "" + "" + - "" + + "" + + "" + "" + truckRows + "
    TruckPath idxSpeedCourseNext ahead
    " + pathIndex + "" + speedKmh.toFixed(1) + " km/h" + courseDeg.toFixed(0) + "°" + aheadCell + "" + aheadCell + "" + lastTcpCommand + "
    Path idxSpeedCourseNext ahead
    Next aheadLast TCP command
    " + "
    "; } From 65b07f2592acbd6cd88ca78b2be996b47d70ebb1 Mon Sep 17 00:00:00 2001 From: Jonathan Ahlstedt Date: Mon, 20 Apr 2026 15:41:28 +0200 Subject: [PATCH 14/33] TLS support --- atos/launch/launch_atosfleetmanagement.py | 18 ++++++++++++++ docker-compose-fleetmanagement.yml | 4 ++++ scripts/atosfleetmanagement.env.example | 4 ++++ scripts/installation/install_atos.sh | 8 +++++-- scripts/run_atosfleetmanagement.sh | 29 +++++++++++++++++++---- 5 files changed, 57 insertions(+), 6 deletions(-) diff --git a/atos/launch/launch_atosfleetmanagement.py b/atos/launch/launch_atosfleetmanagement.py index 2d7f6c3f2..72bf26486 100644 --- a/atos/launch/launch_atosfleetmanagement.py +++ b/atos/launch/launch_atosfleetmanagement.py @@ -16,10 +16,18 @@ def generate_launch_description(): insecure_websockets = LaunchConfiguration('insecure') foxbridge = LaunchConfiguration('foxbridge') with_simulator = LaunchConfiguration('with_truck_simulator') + cot_tls_require_client_cert = LaunchConfiguration('cot_tls_require_client_cert') + cot_tls_cert_path = LaunchConfiguration('cot_tls_cert_path') + cot_tls_key_path = LaunchConfiguration('cot_tls_key_path') + cot_tls_ca_path = LaunchConfiguration('cot_tls_ca_path') insecure_launch_arg = DeclareLaunchArgument('insecure', default_value='False') foxbridge_launch_arg = DeclareLaunchArgument('foxbridge', default_value='True') simulator_launch_arg = DeclareLaunchArgument('with_truck_simulator', default_value='False') + cot_tls_require_client_cert_launch_arg = DeclareLaunchArgument('cot_tls_require_client_cert', default_value='False') + cot_tls_cert_path_launch_arg = DeclareLaunchArgument('cot_tls_cert_path', default_value='') + cot_tls_key_path_launch_arg = DeclareLaunchArgument('cot_tls_key_path', default_value='') + cot_tls_ca_path_launch_arg = DeclareLaunchArgument('cot_tls_ca_path', default_value='') fox_tls_bridge_params = [ {'port': 8765}, @@ -50,6 +58,10 @@ def generate_launch_description(): foxbridge_launch_arg, insecure_launch_arg, simulator_launch_arg, + cot_tls_require_client_cert_launch_arg, + cot_tls_cert_path_launch_arg, + cot_tls_key_path_launch_arg, + cot_tls_ca_path_launch_arg, Node( condition=IfCondition(PythonExpression(['not ', LaunchConfiguration('insecure')])), package='atos_gui', @@ -74,6 +86,12 @@ def generate_launch_description(): executable='truck_object_control', name='truck_object_control', output='screen', + parameters=[ + {'cot_tls_require_client_cert': cot_tls_require_client_cert}, + {'cot_tls_cert_path': cot_tls_cert_path}, + {'cot_tls_key_path': cot_tls_key_path}, + {'cot_tls_ca_path': cot_tls_ca_path}, + ], ), Node( condition=IfCondition(with_simulator), diff --git a/docker-compose-fleetmanagement.yml b/docker-compose-fleetmanagement.yml index 49d35cb09..04d819e43 100644 --- a/docker-compose-fleetmanagement.yml +++ b/docker-compose-fleetmanagement.yml @@ -16,6 +16,10 @@ services: - INSECURE=${INSECURE:-True} - WITH_TRUCK_SIMULATOR=${WITH_TRUCK_SIMULATOR:-False} - FOXBRIDGE=${FOXBRIDGE:-True} + - COT_TLS_REQUIRE_CLIENT_CERT=${COT_TLS_REQUIRE_CLIENT_CERT:-False} + - COT_TLS_CERT_PATH=${COT_TLS_CERT_PATH:-} + - COT_TLS_KEY_PATH=${COT_TLS_KEY_PATH:-} + - COT_TLS_CA_PATH=${COT_TLS_CA_PATH:-} volumes: - ~/.astazero/ATOS/:/root/.astazero/ATOS/ network_mode: "host" diff --git a/scripts/atosfleetmanagement.env.example b/scripts/atosfleetmanagement.env.example index 07b91f8b6..4c3526f03 100644 --- a/scripts/atosfleetmanagement.env.example +++ b/scripts/atosfleetmanagement.env.example @@ -3,3 +3,7 @@ INSECURE=True WITH_TRUCK_SIMULATOR=False FOXBRIDGE=True ROS_DOMAIN_ID=42 +COT_TLS_REQUIRE_CLIENT_CERT=False +COT_TLS_CERT_PATH= +COT_TLS_KEY_PATH= +COT_TLS_CA_PATH= diff --git a/scripts/installation/install_atos.sh b/scripts/installation/install_atos.sh index a37f96441..6f48442e6 100755 --- a/scripts/installation/install_atos.sh +++ b/scripts/installation/install_atos.sh @@ -32,7 +32,11 @@ echo "Dependecy installation done and ATOS workspace created." # First make sure the submodules are up to date echo "Updating submodules to make sure they are up to date..." cd $HOME/atos_ws/src/atos -git submodule update --init --recursive +if command -v git >/dev/null 2>&1 && git rev-parse --is-inside-work-tree >/dev/null 2>&1; then + git submodule update --init --recursive +else + echo "Skipping submodule update: source tree is not a git repository in this environment." +fi cd - # temporarily cd into the workspace and build with colcon @@ -63,4 +67,4 @@ case "$SHELL" in echo "Unsupported shell detected! Please use either bash or zsh shells to run ATOS" exit 1 ;; -esac \ No newline at end of file +esac diff --git a/scripts/run_atosfleetmanagement.sh b/scripts/run_atosfleetmanagement.sh index e73f3071a..f3534925a 100644 --- a/scripts/run_atosfleetmanagement.sh +++ b/scripts/run_atosfleetmanagement.sh @@ -5,15 +5,36 @@ set -euo pipefail INSECURE="${INSECURE:-True}" WITH_TRUCK_SIMULATOR="${WITH_TRUCK_SIMULATOR:-False}" FOXBRIDGE="${FOXBRIDGE:-True}" +COT_TLS_REQUIRE_CLIENT_CERT="${COT_TLS_REQUIRE_CLIENT_CERT:-False}" +COT_TLS_CERT_PATH="${COT_TLS_CERT_PATH:-}" +COT_TLS_KEY_PATH="${COT_TLS_KEY_PATH:-}" +COT_TLS_CA_PATH="${COT_TLS_CA_PATH:-}" +# setup.sh from colcon references vars that may be unset; avoid nounset during sourcing. +set +u source /root/atos_ws/install/setup.sh +set -u echo "Starting ATOSFleetManagement with:" echo " INSECURE=${INSECURE}" echo " WITH_TRUCK_SIMULATOR=${WITH_TRUCK_SIMULATOR}" echo " FOXBRIDGE=${FOXBRIDGE}" +echo " COT_TLS_REQUIRE_CLIENT_CERT=${COT_TLS_REQUIRE_CLIENT_CERT}" +echo " COT_TLS_CERT_PATH=${COT_TLS_CERT_PATH}" +echo " COT_TLS_KEY_PATH=${COT_TLS_KEY_PATH}" +echo " COT_TLS_CA_PATH=${COT_TLS_CA_PATH}" -exec ros2 launch atos launch_atosfleetmanagement.py \ - insecure:="${INSECURE}" \ - with_truck_simulator:="${WITH_TRUCK_SIMULATOR}" \ - foxbridge:="${FOXBRIDGE}" +launch_args=( + "insecure:=${INSECURE}" + "with_truck_simulator:=${WITH_TRUCK_SIMULATOR}" + "foxbridge:=${FOXBRIDGE}" + "cot_tls_require_client_cert:=${COT_TLS_REQUIRE_CLIENT_CERT}" + "cot_tls_cert_path:=${COT_TLS_CERT_PATH}" + "cot_tls_key_path:=${COT_TLS_KEY_PATH}" +) + +if [[ -n "${COT_TLS_CA_PATH}" ]]; then + launch_args+=("cot_tls_ca_path:=${COT_TLS_CA_PATH}") +fi + +exec ros2 launch atos launch_atosfleetmanagement.py "${launch_args[@]}" From 0c49fd37e296998b6702ea89379b32f35c7a483e Mon Sep 17 00:00:00 2001 From: Jonathan Ahlstedt Date: Wed, 22 Apr 2026 08:51:29 +0200 Subject: [PATCH 15/33] Path index calculated instead of sent in --- atos_gui/atos_gui/static/fleet_map.js | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/atos_gui/atos_gui/static/fleet_map.js b/atos_gui/atos_gui/static/fleet_map.js index 6ba181ddf..70d7d07da 100644 --- a/atos_gui/atos_gui/static/fleet_map.js +++ b/atos_gui/atos_gui/static/fleet_map.js @@ -191,11 +191,13 @@ for (let i = 0; i < candidates.length; i += 1) { const current = candidates[i]; - const next = candidates[(i + 1) % candidates.length]; - let gap = next.distance_m - current.distance_m; - if (gap <= 0 && pathLengthMeters > 0) { - gap += pathLengthMeters; + const hasAhead = (i + 1) < candidates.length; + if (!hasAhead) { + continue; } + const next = candidates[i + 1]; + const gap = next.distance_m - current.distance_m; + // Match TruckObjectControl backend logic: no circular wrap-around. if (gap > 0) { result[current.uid] = gap; } From c43d8e5324b2846874bf7a3384533d4d24158e5e Mon Sep 17 00:00:00 2001 From: Jonathan Ahlstedt Date: Wed, 22 Apr 2026 15:43:02 +0200 Subject: [PATCH 16/33] Distances and wraparound works, but still some TCP message issue --- atos_gui/atos_gui/main.py | 3 ++- atos_gui/atos_gui/static/fleet_map.js | 12 +++++------- 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/atos_gui/atos_gui/main.py b/atos_gui/atos_gui/main.py index fc40bf69e..d20385d9f 100644 --- a/atos_gui/atos_gui/main.py +++ b/atos_gui/atos_gui/main.py @@ -24,6 +24,7 @@ GEOJSON_NAME = "RuralRoad_center_of_driving_lane_ccw.geojson" FLEET_STATIC_ROUTE = "/atos_gui_static" FLEET_STATIC_DIR = Path(__file__).parent / "static" +FLEET_MAP_JS_VERSION = "20260422-1" FLEET_STATE_LOCK = threading.Lock() FLEET_TRUCK_STATES: dict[str, dict] = {} FLEET_GEOJSON_CACHE: dict[str, dict] = {} @@ -153,7 +154,7 @@ def render_atosfleetmanagement_pages() -> None: @ui.page(path="/", title="TruckObjectGUI") def render_home() -> None: - ui.add_head_html(f'') + ui.add_head_html(f'') ui.label("TruckObjectGUI (ATOSFleetManagement mode)").classes("text-h4") with ui.tabs().classes("w-full") as tabs: diff --git a/atos_gui/atos_gui/static/fleet_map.js b/atos_gui/atos_gui/static/fleet_map.js index 70d7d07da..a2042e0f4 100644 --- a/atos_gui/atos_gui/static/fleet_map.js +++ b/atos_gui/atos_gui/static/fleet_map.js @@ -191,14 +191,12 @@ for (let i = 0; i < candidates.length; i += 1) { const current = candidates[i]; - const hasAhead = (i + 1) < candidates.length; - if (!hasAhead) { - continue; + const next = candidates[(i + 1) % candidates.length]; + let gap = next.distance_m - current.distance_m; + if (gap <= 0 && pathLengthMeters > 0) { + gap += pathLengthMeters; } - const next = candidates[i + 1]; - const gap = next.distance_m - current.distance_m; - // Match TruckObjectControl backend logic: no circular wrap-around. - if (gap > 0) { + if (gap > 0.01) { result[current.uid] = gap; } } From 1068bf809b8e9565ba0ea742bc8b800cfea5f0e2 Mon Sep 17 00:00:00 2001 From: Sebastian Lindholm Date: Thu, 23 Apr 2026 16:06:53 +0200 Subject: [PATCH 17/33] Tcp client can reconnect. Report tcp error in UI. COT and TCP displayed in UI. --- ROS2_ATOSFleetManagement_cheatsheet.pdf | Bin 21159 -> 3943 bytes atos_gui/atos_gui/main.py | 2 +- atos_gui/atos_gui/static/fleet_map.js | 36 ++++++++++++++++++++---- 3 files changed, 31 insertions(+), 7 deletions(-) diff --git a/ROS2_ATOSFleetManagement_cheatsheet.pdf b/ROS2_ATOSFleetManagement_cheatsheet.pdf index 86e56f45f5f5ef47173dbb4989aac8c643f1fa43..2060d7f31daa32264a5319b40a5781426c34acf0 100644 GIT binary patch literal 3943 zcmdT{*}9_I5x(~+P*G7-=E-?h1VFs z@LocYPoF*&{dZnlJr_04iD3W=c;{QJQi&lw48Wu996)+FKzv;h0Qp+QYPFc*>Mzz8 z&o_~l8W=q;Yt|`mellUnJ&65S>vo0^mK5uY|l3 z8|j+*o+cS#qzZWf0$CWpM9JbE^=h=fKHFcPJg|>K4tY%Dy9&Yh{K@n60DvOC=$wC~Dnf$()FK!|->bz75~6kXRsS4LRYmM5tY0^gD$#JSp$T9V!@X1m zVHA8P)<4JJf_hTomzlXy!4)(B=T$=!bJJn~URx6CrVSH=^NS%UOc_NMv}t}ab+1j) zoU=Rnu;>LNw#DP!WO}$v7M1?B#_cNsZoqjFDh3HW;me9}>s|$3!L|;4U2n`$t}`lC z=tqMZgynv&BLvw+YTu?NXk*;EE|}gBOsTLX2OC1Udo2?uQJp{T|=KCRBHo9l7J5FcpmY=lb;LZ;>BmB|a4~vv@M=%{~=gFK~ zgD*DUqh&XJErSaXKl;7)@!>NaVve*bkDBV#Hf>RUtV$bpcu`@oZ|)fFl-TmKs<&6u z4jov6$z#LIrK#{OS4ur_NFG-vP_I!bEbO4$DqL67#%-d`$vGvwWydrBc-XY*$s~lb zf(^I(2ZsT(k8DF4j?zg%-|I7Lm$`dudfk)Wbnmf&Z+7`%gLG@}WaUDFYao@rK~=VU zJjZpcTL)&<@o`v!2GappD=|HK)z-A)CE3w4m4Hg0OT*v^b7IpzSF(_d(w6Z|J!O;F zrFD9IB%nDxy;=j}=C;-)OdECu(;@dx3ElYSW*xoq|G{l@h&X0*q@J zr+-(v#k}JRZ!@uw9PrT6lIV2vwte%&LW4DQQmW^%{(iJVJ#u}1jAybw$k(AGSD)ST z)seQmm(K!I%1nIez{~x~I;xm!-wZqCm`Qb-b%v|&GN&FGP)Qc0u+C0ZD1J|-y!*0% zEtxG*nPCGs-{j(>X0#cB0Li)O<|rn~p0sod68Ho1804^$B^e zuO*@0sP}g)oE?lFRsqL?PQUAUb#a`-2h~n-J(w%@Ibq8UyP##1Mhb)z*63bmyn=$| zZnMYjansuwc=}$b zdJSd6Rk>|3^iNSKI3LGJ{X{_>lCH|7s^{Lj2hrDpDcvJfqIRyb`NMuNlF3HvQoi_F zPhQQ>Gq8-R%VlFY$j8$+#N2}JkY&oyv=8@Ce6KCy55cUh-4>nkMn-8WJRdVC<*&h; z(VA-AYL%w?SciapaUk5@vk#8_?X}`6f`C4Xs8M*h& z9va|oW!co8J>f9gO@xFsFT+*P>2h8^pA(r{rZ{Ky8+Ge?UDO&PM+MS&cjxbK&Hbdy zq379#7*RJ^PgF<0T5eny8Hu^1T|MD+ap4jZAu>za1z%f%)RDUnut&4ZRX{ zvm?t@-m+B^v9_UBbokW6^o9H~u4CBcM7Fjqenn=R1)>7)$ZZPC=}8bvtq&{#txt<| zg8N%7YJ9)GG`_aL*l*hA|LrwIlK98BkQ%yi&YaHM>9Vk$X6zuiHK?0HIMs|5j5Y`r zw4TXzMxQS09(SqhBSy#uj{cn4B0YADI=oGExOqkG7Q1U)IOdH&IC^u3(Wn)PG|}!O zvBdg{F|b+>bSI3S`AY#Mz!cJZl;%CsUGbiMX~$i%)+y!T6(|rTxg2iFYvoul^S65Y zKzB*Seqws3ed{cWXXPNy5eGbW-Czrv>0(=b;P=$wLf;z6Wk}^wiG|=YyRsX1v+86* zJD!bKG9mId8JIBNJ-|bay52KKzImCsJaRA%W_57`abj0a>QU*Zw<}?I7*yg!qA?65 zUuLspC2sN2C}Ciz=y%-<9CWU(sLbz8OFSK&XKpC3CibMAf1dMrST5;Dc)09lb^{k2 zSJ$i~Winy;Aqy9p<$|NkJ}eI80@j+8=Z5W2p~w?e+KP+Cl-) zt*9rDy3_wO;$~wpgm*6<@aoIwv1q*ULg3ZsM{CLYtMN+MdsnFX;d=@RlA&X%RCm&g F{Tpb9YZU+h literal 21159 zcma&NV~j3P7o}M?Z`rnO`z_nHZQC|(*|u%lcGWH0nD6W9Oi$9C%>3Kg=if=zv-Vy& zhfG0KoQ{c}9foYEc&K}*btn&pnUImt-pC4umzP1t)Xv<+f{^7u9VG^FOB)wcCkAmF zLl;v~Q)7D*Q+|FJXBQ_^Lt7Y+oEeRY+9P(P?oS$*S)nCZbwVfu%|+h;$YaPxATF?R zorlkgQX9VPMFl@OMii)4%FL^h%8eb?=k0IbkE)_LucT%0Q+|mXBfT4aKfdoTk=HY3 z(6#hE`Jt_eaDG=s;k>Rk{Z4rncbT0ndx1a62}vaKz(tag%s1xPlU=Z}tOr?{8Jc&B zS?dQRtjYCf0ZdyNlk2BHPcLJZUOc|LY=JrqI4A~S5_04rf6OO+!U&nzc<;c;$zp1) zv=2qnHbwBKz-c^B=XG-0Usn#_ckzh;Am)x7(GF+*nRxOLD7mK~s_5vR0K;PJ7(jt= zd4ct0&RgLF2&GxMAtj!^!UamNJE0Z*VnF>?L2S?hIRvPY*x_S?F~b|}zmHzos&?P( zJg1Bn_m#sY%}%j4s<7fPAW|~yYaDv4HyWrUM^yv>y$UdDb^VO9>No$NF)fpnXAgeR z+6AcekTb|b?a~h!Uz?GYzE~oHM~KBC%!ju)Zzj?q*UN0aj^iYAV<o5s8emCSA=qk4R-CaefWG3=LF3rH0K(@AE|K@?Fs{Cw?5L*u5t1421UT3(edHA>+h;Q6q4l|jb@ovYjb}~NyeC!q zz}{ePMz6x`ZVH2O^@C`D&P>x~T1k~~HJ)8XlbMJVSxN|a`Dmp@rPrE8n&tWyinNx* z*-ZfH+C1$~Im0JDyY`fd+iPg}VqG2A)b6DMW?NXxo#kiWH1@P;4`x|OIb55nnn4<9 zC@zuUXLW2Sn|0xFnomNb5;DK0lkk{9X}dMXaaxWYa7(G4R#m#IOqUdbeO6DH$*&i9 z@-chtW~nIJtrr0%;q}jtUY-IB7&5sO5xw^tcn;gvrJ2sia?yvRQMq z=|5AVuI13hz1nZ5rS*OSd&^;a1U2p*tt39~5<0o1%QC>e8V1|n`fv!b)y}D4yNTcz z4>HADoJji_g(Wfswr&E#lwPMaYr4Z~ZKQa2%WO5dEtH+5X6D(Of2LXtWY=@Q97Y@V zv#xU}Q}owE8#={XA_qHxMRFr)7x=Rq7%_*w)C{)Y8m9$ODG=Y>1Wt=Wf)RTHtNdt4 zIwY^Mk+xaljqd{c37ONbXq^;h6*b@(G_P8-8XHJ4jlNE6nZmmHBgDlv6!r@S+lf7hVh-)Wouod+iUk(fm0aIjW?pTt=a}=1 z9|TsbMk$gKrR6Y`NK8oAAF@(B2z1QjPUC^F=!V?x&Zg}$bIw_yVqDBCHX_GTOFSd( z^VVuY#m9B2$>M6uNWcFH4#&g~@(aiaBq}})wF#Na`olZYl!JyZxH%ji6yXDmV^H;P z{Ua$yv%;(anc58yHzEzwdDg@1axS zE>pQ@QmF~nA!Cuud7)?l+&pyY@tT;JY6)2K;xJ}&fO`1Dgtj0N0o1a?InnF)m+3)k z$(B4P-n0>4rkg(P@Pp@obpXzO>TQEc^VQS+w9c5)_dNSS?aDtxrBzz7 zWq5{{NGwqeI#2N_t=a0tS?g`b5CA^sMdLk^OuVz!R2FA&J`6n!kluVTT0`$e0l1}Z zkS}!!g+H;5mI8NIJR1$Z2w+L5fK94aNfsJ;yg@c1XPmmt5%K!yJaA>%PMF(M z`?=_MPxbxy1F2dQ2mRR5%p;qvE3>WCP4go0whM_nBP4QF`R16Rw2PGmP5>@273|bF zfM#dH*S#?cPRMsh$w}2^#UU}-R~#J8Gf;-i06bTj;bdvjlW$SK3xsD7R;pV~OrvbN z(OaY55&rZNQMNB-Av#q@6%3wpy>T(G7Fjn;@#!oZ^lO2wk);KsAFlWS?`x$$Q#~!D zZa)?kpI^3NeJ;Pi9>!sp{|D&(muCMr^f>+(^f>=F=yCo3fL@8_MBLSXpf{)f#1$SV zDU}8e2HZXV3JI4$Fb;BzM0lIDC!hF0-?p9HpMfvGl{Bu@An7=rjQ{KN_05R*$hyss zU1$#X1jY3G^RqR^Ai(=GX}|yRGbZu-evobc3AN38bo9737LUR=&dcxrJ0ZHXXo5Xt z#!QC-zkI;2C=qK3-$ax|!P+ozE1Ae`w{eht{KUsH!Sa;_OKD}zK$KrhKoPh3b_TP zy}5f)(d{iM6QG|%j@FVni)Wl%ZjU!J(sb~_kpYU}i8%nSk89FwBqD)iOO@@{R}>u0 zwq4mb$n`OFkl)qCv;=*;F>g);Nl|s*9IzfGv@Y;^Gu^0D|P|5Rh7&-`Sa%!duhH#!cZ7-GmF>wg2UpR>?xI zgDg6)&}(S5yf3$h-?74h6H}fXxP$eN z&4c`Y&z0eg{m~m5hb&M*=x$kIOfFLD`DB8BN3~yh5EX@OWgx7;*M2;GK64RcSo%n| z6{L#}KTkr3Nv~LZyXa zx)df3i-B|ZEhgNo`{YGjAv114%YDzRz5Ops7vCcn#?3mN3bIhfd-qNQ57NIuj<^JZ zzmX$-l+zI5m;Ht1oS;+OsR1~t10?O_4F^oV_Jp!vw_I9N@IRO`_SZYi^rOV7_ER!b zy8{eTO8Y6cCYtjYM2zpXjceAE}$FiCtdq3zNi>~MLeO7His_##i8R>#I>koL*DT-9t!>Cr!%tekg-zilU+G%G+Ty;@2@j6X`1Xq*fL6GhQ z+g^%vE{$?Yl)r6)~rKJ_%} zh{C@Gt9m$g2I^|UE8TZQZ*|wg0_+lW3Wcp-dkT6=*IPKa0ZF*$FuOR>t0}i(ZJfHS z9ZPDYP`Ngoxe=~y1h08%cdqn`4y#NH5#w|i)gvcUqPwWwk!o8Qs{*S+S1#*0F+ubU zB6n7&>jr=Qi)Y!mC{QCjGiBT-wrn`*KU1;svMV;;)d2Q0N(}X;g_I``6)Wl!=Tg$S z?LCdS^{keXCDKUZ>XCI=4r@8Z<}*4~ibWmC6-da(%I84h)*aVjwTIM1Cr--Km+f>V zjQ7KqZ0FY~V>{!d+%$H4bb6UjJ@J15vwIACFr6IPhnt1-+Rmp@ z{|c-VVO{v7@HI-rO`X%3WxC8baHe|uT@v&PT4xbho(ZjgV#m~RU(I0)V zQNIpX!*;D_g!OBO6@4ujU9~UQ_Y4BL;VJ(I6a25c{SOo5U}XD$CVwWT|IG!N{u^lg zf5jIg>Qi=GjQ=CPxDnccq>2($6rA%00{sr;55R|EP5<#Tm*R1w40Vj*Clquvp?7z4 zGZRyunyDL_3&XaO`Y8R+weOP;f4$yM9V~U7O6Ub%-ftQ5;`^+<$R^ar&fGgc`kjPT zaQS@`CWe@6Q8^t8c{xsRFAUNOSB%pJp%Zq_*r=O zdB!0AbW09zuCxOLYYPVsRH$GTmtoK3xpK|LfZ5h{QyLFeP+62bSP|U=c8a~Y*wGpf zaU`Z5Id7_fyzRd6>vGV}G!pwO-6-7DjsFOmOJ}^v(l{jp_cxo%x>k@_md| zBvp;pEoe;siSL^B%vWcVMzm(r&za?q5>J8t$6?r*;>wO0_!0$i1w?1v{(41WB36s} zyb9TfTnb3fjXvh7xUXrVUSxW5N=N-5lIFcc5yB?0Gsz6*EbB$d%UH@YwWE{I`Vw%q zVM;e-p;I+Liuo`XoF|_cx{TtfN-l2)5R)kPEm*mN6riOr#iXjzNcUb|(d@2qW+yGP zSzdNsXP3o%J@jyBv^W<08&S=yG$!Pz`Qj{ELrG8pABwN%yD?5of+6*C%9q~&<4I{l z8sqzNako)5W)h*QL6jC-i14qLoMqkbXx(|q%@6pndrWUA^vIuW(DMtPCFLz1}3qCVA@y-=8WG z8f(o9cjPiq(0{W;H@grK7n^>`f0hwb^Gc09yQaCkQs~gR)L5<^XDQjCHQ9HDi*a8{~7d0{S+W&1VQ~{q-?=}9h*5`8fhVaZK+3|>Q ze7NI){0yu^smt{e|E&EO2ca>ox1+iay`IC8i<#wr z37_?U7yf@^fd7;5|7WhrM99R+$;$cvPBB-yJbhGFuo!)2cg*(Is+}oB{t$YHfdvWD zCk)qzlO+KYVKpWgj1Nx(1yKSE5mti$(5TDv0|-Hbz{CWbYhcS&n|-U$bgW@PT&rL@ zJoB8*{`|4}KKk_i?M~kIp8MfF+sJb^7p8l(NZfq_>T04xiI;v?fCJ|T62`v2!UV| z*0p(d0|Q5jDBIZfL-PBof`mpZK;g_( z>9TuU6N_p}`K034->4vlH!eSYfxj802cZOQp;q4ciy07bsu!Y9WdRj#fCt z*PLnm5O=htC2Zx_-uyj2s^DwBF3>Bm#w{5uN}D0NY5|f~)F!YSmV1P^DtnU>o0rC^_ulh~!ua*PKX7EkWCsVsU3H)@M1qurs8&lVYQ8Tui#z;))sXFkB zM>DLv1dGrRHV`aW$%meGNqx}*z=a{@^!e%_eTOrI_3s|&Xw?Ele=+b*Y-y3y!p#tz z1+>;!rvb-YK~b}$xj-3!nnYBhHi&fHBlw$vI6PUHYf5Z2gE~MiL|X@3R=zzmeRe%KCGRp}T!0-jb5$ zi6g{lRU=Mt{2%-oRYPd;)g5M~sF;*a38NxTFnA(DUZXdiLV9N`#c~NR@zqL}#RZ}kh$w8z&`+-{#Dm@; zMmk2{Xh;I11SN_7LAmH}wu8npsxXm$^gG{9PIEvX*ENv)Oa z{@&)7wDA^C&3KDeTUpV8+vU|DX%@FWjyo&C(X=DuXpBG{mmRGf_M-#nr6(85P8Q zWnEr2C>gRb}2@sgNE#W=>+-8PfQwM^q3|b@$=g=7BCVL^1c6Lmvc3+qvvbQncd_ z3k}2uY?Y-#aF0}iq!}yDAmi|vi)O`W-|>lK9F9PQBPQsuy!CLJW$Vg1Xy1AIw;|x7 zgf5m^qDZAGAtl6=l0TGVQHZAI191LmjMW7Z%q%%4mI&H>B%K}7^VD`3IznZlWuY}B zL*KW zI`HSZe>ITL5TyC{>*tgeFG-9bQ7_z?;$Xt#Cnr;)n3DO7$>pjrQN>6Xh0>0aqU|+D zm60(tA;1``Wz?e3WNd->#gG0~=~CiEabC{Rm83R&Wxtfc6*;17c^6LGk>t3ojiky$$OMdI}S zUG1tWX>*CcxIo`Z$z@938b8cE4}xMOKob`YDG@5=w1SNg_U?a)5?8RSoVN`pjihw` zHfW46SCP#&cN#(B#=e^Hb&Q6hWx);}lfg<%L@2c#CPLI>yH5yFm(5jk+=gP>tE55; z$C{3*#y;R#hWwz8i{59opb^oMBB_Wd7FNpwLnFf@aMuwoPknJ}4qUC;Vk)$qtYwd> z+{%>9W&)uM1xZ2z<(N37h*s5u7)3{i);JoV~ftT=Ek4gOk{ovS%%{e8)20KPm- zTrlB1k8a3JJ7+(4J70(H1mLj@givHq%c{#(OF5CHUdV8x+}77y;hTEG_MCz+7Fe>} z>02#z>idqsw{P; zGiJk&r2v9Cr|}pVXV@hfGnM`6${emEyT&m)^vnQ+M02@T9B&!?WS%v(`r{jaB2E4h(Gy3swZRlRtzv&&^ ze%OmZy}o2siu1(48+g>dCGsN;X#fIip-{_lDWr?EKQ^Mdq$bS7_Vh-vb_*t~-p(^R zf4}nb{L=gfbjJMaAK>3*-=D__e63)c)cGmx2V6aylV+fI|yER#$jCN@O zH2Yj`qEaG`%})4~6oF5)O z9*=Q5Lub{LUPjNwKYPA@KSm$WXQQ9T43DJ@w0ceDjg)9@&3&<#N$IoN+e%7IP zh254yPLRIZ$C71unkh{*C zEA4Bb;ye*iB%o(p#%wngqP{Hp&yRE)dh9dia1f3plTs>$56DrLJ!>4a+D?0(-Nm)o z*3ezLT(8oY^S3{A)!5%Uu-wjNf4)9T%gMlRJMZn3`ucO@A^3UD)!G)ZD{{91>lqVr zI1tyNT90WMVCu?LZ>pRISLYpdFz5Z{Fuwt{OweCH|86Kc_$X`~LRC^M`!4}aWQpS!Py_9R>omP*1YY6j}lfaUvBGuZZpar>*&Mv`jusYbhn z1F3ys(&B*%OX=ZlbfvZ0&`}}`>Y-I|^d_$^+8f&7}a_V+d7#)xxT~UD*}I%H1xN;o1U@%X<2~InY3d! zU__$^qHCy_?lJQUaL({}pJCb%NNE1Sw$d8F^Q2^jas{yoCa(m@{de3u@VZ~Gqjuwe zSJ5Lc@J@Qp5|`SiwTGO=$ZqL+dO?MHbJX#754X*9q?F({rACvh@bucrlhu#t5%T>TXY1W)BhlSsQ|qfvUoYye8O@^; z#v?1FQ6ZLS3$%Y9ge;B;t471Lj)?y%U%$oRJO_rq8% zLcm3D7Ruj?8u&LRY67MqX5! zCpZljJDuvT^OvlDL5yGXjbrYI0(!Y9S^9PSiIEt+KjOb}eX6#ZE>1s(M*4yaHZ~S| zoaVnI9uRXoFM6pIRku{c3^t+Ct>)}{whB!aI1_aJF zD-T6m+^Y@A`ZsfB7&4r%i`L7TiLDI*ggdFqUg(;&AHu}m*A|+0lIvZS$5Y|qa zbB1geO9QXxwvvjcC>=I&2b@s!u+BwsJ)f(w+UeHAg6bEWvYxrwSzxE0f(l3A2fBY^6SA$Jc$B3Qu;h$=NKS~RYs3vl= zU?PL3QZ0alDJnl+9v#pLovU{YfB{{^#SmKFBtq7RPT3Jc92naG*YFu>-CmN1h&RH` z84af}X_OZjL|7T6@3*K5^iunLAHSX4oCA_kra3;h+kd)v@<@KP6Sdijsx0Q4KLYM1 zi7$NK+^fjlw-CA+i1(3rD}Ye)B4peG6w4L_ibb5~7@-p-CT>0Znx{+|F9hg~AkuoM z!`i>X8ts%iC}4CnP}FF!M1|lxD#2Y+lwtXdB!}$NJGH@#kT&>7=tLGS(dd5y$(TIu znhrK~!Fll05T^vD^qJ*&JtuOH_vZZEI6`97PpYa2u<7eYobOCd6MQV*b`Zyz_x*Cl zS=W7NCev_nL8-#Ej91pL)LLYLnZU{dcvB!4McurRGBvpBCMto1(2X59sQAX>E7!yP z4>Bbx3?9u-#M)Z^CKCV!iGkx7*a2ra_xJu=Afys1L#xNO)(m69L=&Ee)YG90kRRha z;$naYFv$W68*;Wt9df(3`9UKKzWMmmX!X4uVAbF}us;cmct*opX{&nqUx0eUlm{ z^{3kOe>nWGlr-u5-uKpy8&vG#L4S3(4N#X%G3u8pqGDM#YzXVwyJkzl(m$%)fR!N= z^B~!(hCqc}m)kS%wl!ZFe2*=b8R5LG4i0v<_v56tFv8rK2XG*XTMIl#3!0|$NIFFh z`-Sp(8p`zm%S54s8r9?27<)DY<3`OYnpt!;X-nurBT9Qjqhx7Xv~p$0Idy+k)8BQX z$fDq3)QE&2>-!~rv^Qh2%`!z%-1nXwm!}ea$o~AegD32V6E%uXF+P67rXR`UXrBr6 zdOqIbtYynxpm(?4h06_-3KM3!I7It-4OtWTI@!(Y*sk3AYQ$U>U>+Nh%`3)2NH!p? zK*s=Pn61RC6q&-DfH8z*0wj5558LqE zl40raLJ{j!c;B z3|lP|wZysxijW2Jt14&NJuRwW-GBh^67ALkNG-7RyCe0z#{v|mp#?SL0N6zuJ z%9Gh9IgplHSDmbS)Hp)_16-3`NBDPHOgDwZrTr@aHsdM8Y0bk zT^}g1)^V5dP|HGM@o_PDI4x7BZAgpTnx#02BGa;a>MEy5!-qqEsPz`50UkM4^|^f>qQq~-{s38tMEc}tO;4nQHTmT#m(cryXbD~U1Q zHc-smJ50t|H@QVXA;zzAk75_@JsWe%<)OwMPS8EJ>$;6M*VB3GUxg&_@faZ{uQ-}< zJ2_&88tV0#)xNeV2OIi*H?Q+~nt=58NuE4l6MP}bf$(;bv`UNM8{8@-Nm-!_vlE2S z?+RHuMhqa2l@X|ok_CUqAq^~Q;7Pa-kPjr4@S#dl*;Fb{q{W8!?>CmZeUH(OU zm6wX2IZtmfggKEud!FUr&f`f3on@CAd59P~CnF%IG@e8PFIMDzJs49}Mdsf$(7s;A zmrrJ9V#ax@?zf(<4pZecPKm-4)1j?evqoiNLej*U&hpGc!xkS39w}%Aua2IO&ebGE zx;8Y!-KmtXfzJjUTGox&Oc7WR0Y3*A!lAC~&)QK+?mVtG46$riq@~u;qTm5)(rVz3 z`no$xkMs?qa})frTUiKh6U*3H{}Y*XlY|l7I%|5Zu3+cxX-M0Y%ozb&o0gVoJEro_ZvQ=yU?EkxvpzuL8UQvX6J{-w9QF_dV6%IQ!TNRz zHENyE>`nRJyZP829h%2YsJGhn+)>Hpd2srGRw3SuM#py}SoYtGbs0^E-=De5rdJ)8 z(7ldJ!B%{=x%!D6LGhS?1($$@Cj>ls1smKwHLO2LQl-7K0!VOR!}7P}#8UFjX<^XW zuzfqFa?-F}qd6IpPQ(s<-OOf_VfT0=)#-ew!wuu0KDi2xmcSA1w7MP+QeS!36rdeT zlPCI&SJl3jU8i<^L@41o{2o?6^}kPA+JxBYC2ad3;8Xw9Yn2#i%2g;4!Iv#uV1{DT z)Gq=NqsJZ>iC!JNLeWXtM52+#GA9;+xFvU6z9*8r6}_G)zS(~5&~o+X^&FVHIH`@P z&3D|t>Av`tVsYYjvoQv}kxAH&?>c%_fEp$+2qO6)^ zpZt_HICT~h%nDE;KKxB8vr>b3Oh0wd#Cz43u`!xi`}O!ox*P$DHCp<7Z zpYn5)od~YjNDn!mJg>MAD@p$r$X@|^ZRhn8vE+b1A^dYmI+qsF!NkG;S&zb{cNGj@ z)hfGpQJ7A8f5uTih*z5J2Ka>l^d>vhSr{cyO!LLt+)-RCfU6}Sa^VV~DzpeX3S zKP^2+3pM6kI@I69Lul6BMq{R8&6?mq6dE)fvt2h5V-`*kN-CQ`#}s3^_b<6DZ{zu5 zV(AmJ=_^*p<(=6NdU!D;Ubl-jp7i5S!%)Be0|Jq1BNQ#-s7A?ghwYN5NV?PwnxiN! z3r@pqdbzI|{4xOL8}CmapVEQLBw!;HS)cHhkbl8Zm>gtVy#7495Z zpPhb4wH?dZ)WLeh@QeNw9=V|Ss(?s;PFVyNS+QOC>Wt&h`rJkoowiA{sr2ygOsSVD zTthZOs$_+h9tDs`*a+{c2xZAh5I>pyd}p)fWb5?MvnxyR)+Vli|{U`_5Eu*(qQ#q%T*yx)oK$7&!|fe4Iw9GVq{L}1sB?R!^Yrc zdCBr?V+!Bg?gUp~ga!#)M2A_e8b5_UdEFT3JtgnM)0jF2zvquFIW0kITOcqmIl;iB zzy2>mRfU}oDB~E241NbGUws0FJpxj(6D14{OjI`vapojPS;k+V`{#ekU<`BfQzPa5PuaL{Ve8F;CtIfj!>TL{<5Gr8O9PCw`;Z!cHt}cZJapj zYQz$r$C4j4FZ#n_Scu|w#(SRq=TO@rEO`3bwWrri`6ujF`qZA4tO@?{^h`ry2DTr)k*QaOUa(Yd2upM*X zM7?zg{`+wBmEdpI>pIGm+@b%3fR8)0vD-~7sJZ(U_goqhW=Og{FdRH}s#3pTRbVBy zQ8inRTDVwjMGEHSdlV!ZoW@l)MK?G&3)FmWAX>)I`Q=}H^3NpP$K?v;=>&OGGUe&* zK6SaHXddcJ3U0lwdjb%GLNWy9H*lry049?dT&0R;Zn+8twGUJzSweV+%OyXdn_^EiK!Z3 z3iZ)%zZt}DH0>Qhv>v_g%&{bD)d%aw#xVZ zB$DlZvreq8W0aeem)v$>#S1>yzjR!7T;}-4j3@Cc@vGyDWmVwkan7Jf^WE^2GS*aV zVw`xIFzjvhdWp+OV9Cp}V+8)_)hP7~LdL7u7-55lFFmYhOz1Qz`cx8JPCJZS6~kV7 z@gFmoHuZcz4^Uui9fOYV=qD-f_g?et=#{+=ru)7BqAAvPW~$Y<(P^TmY75KCcKtAwgv(yqt+UshV` z4b-hM;p#HaVNAEUt#U9~bgXi#yI!)9OQ}*>(c+m4Gk|PeAy#~X3CWdNmpXT) z#V}=68s9K{Lijk8oGXVon-E#|M=*)X`Kb5njN!qwdYGD*?#*VS%R8_CL@b;6B>pZG zE&h-YKejCRm5=+JfaBQT#b;4}*pvr&4T*bD_5>f2s|7$UUUk#~%K!6j@v{BR9-5_b zx^LD2a=Q`p%BcE)+U<|B%OJmj+Cueh>7z?pLdhxV%Zg1`7uxJkHyX`o2XsqOk4SmX zOIE9FL`GbvJgzwuBkXkPIx2Y$J+s`+8G5{Iqc|o%>YK+X{^(~n`y<8C8jz~r=ZK{( zCO3`MjAo$=n1kZ8=uEK&$e=c3$AXIfXU0P_Dgn@ls}-dxHmT8+XW$o7(XlsZ(OG1D zmPYJMr&e^%NM7P)iHk)O)g_UnnA@;&DxN?nPou<#oPH{dV&AJY=_V+~9T?OyADl6tGua^5uZK zC4WBqs>h?s_@6sZt(c% z5(flTaFnR zFJDQZverZ^%PG?>Fii;pOi`s-unQtoF5J&VsYRq}ig};?!yxQaqwJTOZ{h?{)IYOy z&Y#Q4OfMXfK;9}?I9&7}2l8H;x*3>;eXZPs$J^-NHGa85tXil$c9}5{JWErVaV!3l zFT(GJeJ%3fM_`@Ezr|T1!|}ik6%RlXtr3DL0Xc*ZbS&I8Pge{WKW56J6D?yoUPck{ z**Y-daA0s#anT7pUy2{&2XhiYJUl0kD2h#(IF`OAuR<+J@+g{;75*R|FGabK;&ZNCkKOcCFn6G^ zQNL-%ZlRL}kQHna9kNcqvfhA;-Kd^0aE5{5C3u5Tko!gS;o2eMb_(D(LypaXzRKhp zk``&)WgwqkNTwT`QNI(p?>`T`1!Cb_eL#3>G`Jh!I!*KoY!Hk8_(w@TXK@2MH0`63 z9XoG+>UOKw6^NX4gC}4f_?x-WCg1jGRNiSn`*zWXxA8D?Shtt+u5*1o&_}=S`#mt` za2PrE>)FR!b=*40HKZ**$6(N#ERf>~*B{wMA-_fac#EdG|07_`LjN1x zeQy3v=jkgovV37_ez#4-iQM;eTlBDi;(S{BX4ml*pX|YyT7cvQclF3$Z=SD?YXnA0 znS$wvJA?rPY#7-OleD2mT@NuXO*j$(c$%lJYg3O3Bw8)Q8eO_#J#BSof> zf2oUob1;szv^eI33GT1JImzT1&*1k+`Z!QYhe-dQ{e#@ge31K)rC z&tcjaKk(4!{w{66`U`w=1RZhTk%TG=C{e*HK?w(GUOEKSTqN5d1@qSo>@#06;9TdAB zb~9nYBgg4L84csgnMY6CEA$oTnWS&&*_!P4H+m?>o?5w;8LuechXTR2SDY{4+!BG? zNqR(#)q=gj;R*eOA97r6=5LS)Av#x^&%O+kt*+nnjYk<|c+#0aDRWvpD`+*7Z@}7 zscV5rM1d*%&fAt!giMB?{h00h8FDB5PZGihqMdNimsvp|>An-;Ud{Oy&;INU0-rLSC^@jVIV+a70(*=w#yaSLYorri6bAn;MD^%@gU z8Vc56X~t7NIlwgIU6v+YY}VB04{y7?GQDWQJFPR*yK!V!>v238?rKd`)@pliTi6Hrnoa4dKaHL^LXlpLipF>1B_`XrX*l!k<1jJP0t)(C9F2rSFO<496j z89aP)C_&6UVc4VhDHI8%mQfx9-XALE76m0yD)olke=QIkH;3s?@0?%+mK5sH){!f zGj9oa*p72I&rRb-cb=$GeVOT^|Eyz0W7O~>euA~*_HlXy9p{;~dnGP(O`mno{4w^K4H(ntH(^+{~l!g zG2?PhQf=C>A^GHa!A_r{JF>eAe*X?qDo1gH@eh?WYc}dTEM!*8tkWs!u2!ltt30jR zRK2ZSTCu&5XO)w!J;k4C+a$fVu+4s{mJ4ubQFYgNt?8}nmHW;5ffb285E~jB)i`24 zq9CuT*nNul*WEGWGw8FyJl#0vH2ToOHbSkLU2|DOQPXG(R&piEiv3VA4xNc?TEIhe z%{!YV*d%FBYF zWN@MJkEY|Vg=XlJhemUCn2)thbG6X@bj6K6su?qZ*c5llSY@FpUzBM$@)Z}_@=Fui z0<(A|E(RTo8k2{5 zY_qlS;og<+;q9v&oBJ0*12^LuPUiVVT#Nf~Y!KPZi^MjBDKv_KKP5j3ot%{w+|D*b z)tloQjym~Nj1_J#-QxaJcLuJu%Cff68AQ9x`)L<=Ep$Rkvoq5Z(_I_xQ{!Rz=NKY% zKUtd{KWSaVg1Yv&!)cov@B<3Au&nH?ktdnSEF;YR<*ihtAK6yJR68gX%PVXuYm1{7 zn`sFI5Rk&p0jD{hbnpXNVOAh?85^;_hyWRcmDxJ9v3HSzO@n;P5CN!qpr*t?&w;Sg zO&3QYUlG@y_I?6XYpY=RrtEz0RpEwLT4b;9`s~uuejg1Nei`eE8tV%9RSwn}^x&V; ze9UTF%kpS?qOgl^qQ=*bmV&e}D}Uo7;r-2RvkY$7_2zsc75V)(R-n=&YmBR@t3e1T z6-XB}oKyBTuv2AGmY@~7mKIr{IsK!&mHBm~pq7hox*sb-ocY=UZpjIdWu)wHS*-uO zN&%T^3AkF`8r#K%Iv10~hGU(rgI`>bZE0Z>8cf1ow<3`N>5mQU*ENxL4MZ1gB3H>< z0KM%LRE8>r5cQXZzruq;FVE#jXN0xDyAYn$-}w{Jdn)b^rIv{^U5yx;1!-vi0eUG#Um--M7g+vQs0>T!#-am@ z7ZhI+sfzdzwifU>Ewo7iO^>h(R|Gz{4|ylXhCV^h5WDf~v;$%*kSQ+Z>+8{?Pazj; zpe$)xDqw{zwW4w7+o+An9YXwD?|3uo_DxoTbW-3vrfb!o(#BbQ-8bO z<*|^j@8ZK?oD8J7RX_WF$%3zslcd5eA$(e1)PmPd zt=2^#AzxB7bs;YhiV(IgEGqqi8HNUGKsZ%bMsSO*F11FHim1j7aPm#bat>IeLk-5} zuCulm3i#S<{&mgFkZH)4H;gN~F2*nRCCSH+xuFqeX&mJR{E}NxJZS)}BdtF-UzM@e z6wn~OTSd+0g8!lTA*W^HUL!b>MLF62qAYzZnukigJm-@LHS#M&>znuuncCjr9Is-CSu@ z6K5Az5R{-`RovJ{ECL10%w(6C0)Z%^ECC9F0YV%gf=S3?LDT@Yu2n=?6fGVsh^?|% zKoD#}mQX}0qO95~0-`K}6h9ULznQ2Nu;1yA{^&X1oHLns?z!*1cjkTWGw(S$_a5Cr zy31MAT$^XC`lLEyNg#ofWPKW!7|@%YwLI0=+06JHKW#)k^2%>^S2I(0MhiBbay{m{ zF;1VtJ}UL3Bz0fvu)nmG=~**kIF$8Wo`!v@x+#k^u&gc8u1p2|(x5ZIF4st`KZd_G zYPMJPZba>U`0ERWx6_l^&Yd|c7941XHFH+W&$zFQ>~@y9N5bDxgC^uD8wPec0%X6h(_lO zoQt|~;%+>%qc?qmdDn$LCxz)h?jQcUx@_=vxvEw{QQg31`m7ag0nZXN*8932k=(4m zR;FLB!>^5wXsO!~Wp%*3K3Oeuz|QF5<;OAGv|bolelL2;{pQw#FhyHxN8BX6TT0TV zv>ACtZ7cVWj>{>X7k>Ouqh=~&b28`nx&duuFXvI>kZhtjt)$1>Atj_dLElr$m#gYY z*@ifADy^~uni_6hj=pSBWWfo%pR{Qm=go*w<*SBeOpf2axM%R5=*5gTS1OQ^`Htrk znmOlvEyBJn8u_(SbJ^DC7bqni0f+C&RDAA&q3VT89jxLlOTeN2&hzsp3iTFHcvk*) zYjs2Wfrn6N&4F8w0~U_9Em5rD!w;U1Sxpvh8!zl_Z;UNBlpKhCSxnBpJlIfr+0hqq z{`O{ee0pKBbHK~(|Ea!b((?3FrpbzOjV6sXJ$7w}OAqfiS$pnrtl`G6!+kA%NMCJV zW*>c2J#W5`DBdaaKt?Fa$dr0y%c$cfuF6&b^QfO0{tp-D5OiIvzEoQRtRDl zj-spg3ly(J+cIi$Iv*#x`FQaiZJiG1RT|8tWTsZ+&_X0_PmRxJ3!W~(2Uz0#Ge3pT zHl`OEBtLv!l=DfDup;Z&R^2nD3S-?Bc{dAf9+uW;&k~9hNr!rJn+S^=J+d^f@N%01 z%DtOfujF6JYYaAnQevcjUzzK!z4iyOQE%5F<0=Q6&j{X*jTVk!nFi}C@^^H)G~McC zZqq*gLfYpQeXORkv!$k`ckaMR#LN3)ed3(x&u1^pT2mNsHJ7gFvG5FU5znUjsGi6F`q;rsCW}F9Hw|Hqx$Ve!9VNo5nv+*@4RNe`^KzHRBJoCu=eP)+HRcB;CS zcGvZ^UE$QR#5`Vp{CrKP&yGqs-!{|Y;?_J}@LAlNHN%z>8T0*?AFWFKv``TrUv#TA z>>N=uR(;<$DixdBiuAMg8;{slH!QlCKgTL3^I=MeIfcu-4mG*_8g-FdmpxLrGO5NT z_!7|q5~(v2xiPE!oBa#@pAk)QseM00F95(r;TigW6zU71 zb(~T*zo@U^bGaVN6P(ZbhRth#^|MY-W9lawyt3=3%*#A9eummsnr=+1!>RGgR#~lV z(>xH98o`g#KE8CnsiozT7?aF8gF+YYrn#qZPMQa7Zw71Lj`B9$Y4B5q+J4UBYA;h@ zmTJx1o2SdtT)Y?V*NvHb+sK+r)>^gBa$dpO6FK#_{Z}ThNNYF)s(mq!C3>#d83@Mk zb2N=6sGp3L{61$W?XRjp-Zy!GU^0#Rp$_DIZ8X}x#yvuW;8{X})DDsG#C(xdh_gIgS}+u*dhKst&9<~>Ytr z$(U$1Na|_dt0%7v>FpvqH}>Qm>q@`VwR@cH$Ghlp=isi0hqbX?=^f|W+9AdLp8VCD z2zf8bja$xjPcAy;k&=C{VPA<;z@y3GUgMzZ=MTrPWK?!94M~aIif?u6s=H7$P-i1+ zW-hytJ;~0$l)>gC*5U7W%RM;X2bD9l#*Xb(8A=?^-&NIWIbG-R`R&< z?sbDn{dFC9^WedxZWH;elGXB2l`Z@3GCGE{>lE7R8z$Ky)qc?}aV730FIO{z#=VLO z`|j@^jO=Ky_Pg0{DIbU&^!%aP@6!2koyN{;!@5U4lfP8?ObY8dhP{eSsP^Y%~D*4yLM>=EmDh8WUI+nd^ zp1j%m^|)_j>m;f*KC;$!(srUl``Ka(`4#Ad-(!ty@*&2kYPia%8a;YE=1aBqsuT@H ztW0gtv%6sYuG;K#HZ2yI6)OrM6rNfn;_Irt4? zp-hBE=Z4~qE0BmV8@vcS(+y?TU3?zG-e8M&0Hi1u84NrVjSzK#n#7)C3X4uD1^idy zQPv$!iDwF_|10q**Lg}j(3EBVR{~O2X-{1-B}%Z8&i7w!3_=ndekN>iA8>=N`Jcc6 z(0p*@Z_P<~ed?6tE2<-@Wu4W>|r0+Kl8MZlQ)RJTkn`ehdeq2NW`zr!vL z?<_(DHau*P;Pci#-fOEjAdrQIzWo7FEUKJH8o%io9{d@(N=lr7KF~S zXR$z#L;*nx41#1DdL^P)5_Zo}=Gt}{4|%UgWWLoQqq<6cRF8|jz?`2Xk7u|1)vE*{& z??%w&#j}M%DWHTu8;5rg1PTEqQn4F5zz3kSm;@%sqL5*#J;~OdMkdj0$P|devLP{P z5EW!GaDT6Y<(!?6#|%bzArje60MBAlVbG4mBGAy&o<$>&SuBc;EtyQUAwmQS8A}|A Vlq;4hVF-gn8VP4?Z12p%{TGbYm2dz6 diff --git a/atos_gui/atos_gui/main.py b/atos_gui/atos_gui/main.py index d20385d9f..83f313436 100644 --- a/atos_gui/atos_gui/main.py +++ b/atos_gui/atos_gui/main.py @@ -24,7 +24,7 @@ GEOJSON_NAME = "RuralRoad_center_of_driving_lane_ccw.geojson" FLEET_STATIC_ROUTE = "/atos_gui_static" FLEET_STATIC_DIR = Path(__file__).parent / "static" -FLEET_MAP_JS_VERSION = "20260422-1" +FLEET_MAP_JS_VERSION = "20260423-7" FLEET_STATE_LOCK = threading.Lock() FLEET_TRUCK_STATES: dict[str, dict] = {} FLEET_GEOJSON_CACHE: dict[str, dict] = {} diff --git a/atos_gui/atos_gui/static/fleet_map.js b/atos_gui/atos_gui/static/fleet_map.js index a2042e0f4..0ef1a2ccd 100644 --- a/atos_gui/atos_gui/static/fleet_map.js +++ b/atos_gui/atos_gui/static/fleet_map.js @@ -3,6 +3,16 @@ return value * Math.PI / 180.0; } + function escapeHtml(value) { + return String(value || "") + .replace(/&/g, "&") + .replace(//g, ">") + .replace(/\"/g, """) + .replace(/'/g, '''); + } + + // Constants aligned with atos/common/util.c: // EARTH_EQUATOR_RADIUS_M = 6378137.0 and INVERSE_FLATTENING = 298.257223563 function vincentyDistanceMeters(lat1, lon1, lat2, lon2) { @@ -138,6 +148,8 @@ normalized.path_name = item.path_name ? String(item.path_name) : ""; normalized.path_index = Number.isFinite(Number(item.path_index)) ? Number(item.path_index) : -1; return normalized; + }).sort(function(a, b) { + return a.uid.localeCompare(b.uid, undefined, { sensitivity: "base", numeric: true }); }); } @@ -204,8 +216,8 @@ } function renderSvg(container, coords, trucks, selectedPathName, pathsByName) { - const width = Math.max(container.clientWidth, 700); - const height = Math.max(container.clientHeight, 420); + const width = 600; + const height = 650; const padding = 30; let minLon = Infinity; @@ -278,7 +290,9 @@ : Number(item.speed_kmh || 0); const courseDeg = Number(item.course_deg || 0); const pathIndex = Number(item.path_index || -1); + const lastCotMessage = String(item.last_cot_message || "-"); const lastTcpCommand = String(item.last_tcp_command || "-"); + const tcpWarning = String(item.last_tcp_warning || ""); const ahead = aheadDistanceMap[uid]; const aheadCell = Number.isFinite(ahead) @@ -291,7 +305,13 @@ "" + speedKmh.toFixed(1) + " km/h" + "" + courseDeg.toFixed(0) + "°" + "" + aheadCell + "" + - "" + lastTcpCommand + "" + + "" + + "
    Latest CoT
    " + + "
    " + escapeHtml(lastCotMessage) + "
    " + + "
    Last TCP
    " + + "
    " + escapeHtml(lastTcpCommand) + "
    " + + (tcpWarning ? "
    TCP warning: " + escapeHtml(tcpWarning) + "
    " : "") + + "" + "" ); }).join(""); @@ -306,12 +326,15 @@ }).join(""); container.innerHTML = - "" + + "
    " + + "" + "" + truckCircles + "" + - "
    " + + "
    " + + "
    " + "
    Selected path: " + selectedPathName + "
    " + "
    Path points: " + coords.length + "
    " + "
    Total length (Vincenty): " + totalMeters.toFixed(2) + " m
    " + @@ -326,8 +349,9 @@ "Speed" + "Course" + "Next ahead" + - "Last TCP command" + + "Latest CoT / Last TCP" + "" + truckRows + "" + + "
    " + "
    "; } From 75c90042d15ca5be90c38d00ab8f08d491db380c Mon Sep 17 00:00:00 2001 From: Sebastian Lindholm Date: Fri, 24 Apr 2026 08:58:39 +0200 Subject: [PATCH 18/33] UI update --- atos_gui/atos_gui/main.py | 2 +- atos_gui/atos_gui/static/fleet_map.js | 12 +++++++----- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/atos_gui/atos_gui/main.py b/atos_gui/atos_gui/main.py index 83f313436..eb1cd5e85 100644 --- a/atos_gui/atos_gui/main.py +++ b/atos_gui/atos_gui/main.py @@ -24,7 +24,7 @@ GEOJSON_NAME = "RuralRoad_center_of_driving_lane_ccw.geojson" FLEET_STATIC_ROUTE = "/atos_gui_static" FLEET_STATIC_DIR = Path(__file__).parent / "static" -FLEET_MAP_JS_VERSION = "20260423-7" +FLEET_MAP_JS_VERSION = "20260424-3" FLEET_STATE_LOCK = threading.Lock() FLEET_TRUCK_STATES: dict[str, dict] = {} FLEET_GEOJSON_CACHE: dict[str, dict] = {} diff --git a/atos_gui/atos_gui/static/fleet_map.js b/atos_gui/atos_gui/static/fleet_map.js index 0ef1a2ccd..4e124dfee 100644 --- a/atos_gui/atos_gui/static/fleet_map.js +++ b/atos_gui/atos_gui/static/fleet_map.js @@ -306,9 +306,9 @@ "" + courseDeg.toFixed(0) + "°" + "" + aheadCell + "" + "" + - "
    Latest CoT
    " + + "
    Latest CoT (Rx)
    " + "
    " + escapeHtml(lastCotMessage) + "
    " + - "
    Last TCP
    " + + "
    Latest TCP (Tx)
    " + "
    " + escapeHtml(lastTcpCommand) + "
    " + (tcpWarning ? "
    TCP warning: " + escapeHtml(tcpWarning) + "
    " : "") + "" + @@ -333,8 +333,7 @@ "" + truckCircles + "" + - "" + - "
    " + + "
    " + "
    Selected path: " + selectedPathName + "
    " + "
    Path points: " + coords.length + "
    " + "
    Total length (Vincenty): " + totalMeters.toFixed(2) + " m
    " + @@ -342,6 +341,9 @@ "
    Live trucks on selected path: " + filteredTrucks.length + "
    " + "
    Available paths in payload
    " + "
      " + pathSummary + "
    " + + "
    " + + "
    " + + "
    " + "
    Distance To Next Truck Ahead
    " + "" + "" + @@ -349,7 +351,7 @@ "" + "" + "" + - "" + + "" + "" + truckRows + "
    TruckSpeedCourseNext aheadLatest CoT / Last TCP
    Latest CoT (Rx) / Latest TCP (Tx)
    " + "
    " + ""; From 81e2a69279e79039448a3db4fb54c04914795bc5 Mon Sep 17 00:00:00 2001 From: Jonathan Ahlstedt Date: Fri, 24 Apr 2026 09:21:57 +0200 Subject: [PATCH 19/33] Add sync-to-server script --- scripts/sync_to_buildserver.sh | 101 +++++++++++++++++++++++++++++++++ 1 file changed, 101 insertions(+) create mode 100755 scripts/sync_to_buildserver.sh diff --git a/scripts/sync_to_buildserver.sh b/scripts/sync_to_buildserver.sh new file mode 100755 index 000000000..7ecc0ebdf --- /dev/null +++ b/scripts/sync_to_buildserver.sh @@ -0,0 +1,101 @@ +#!/usr/bin/env bash +set -euo pipefail + +# Sync current ATOS repository to build server. +# Default target: +# az-buildserver@srv-l039-p:~/ATOS +# +# Usage: +# scripts/sync_to_buildserver.sh +# scripts/sync_to_buildserver.sh --dry-run +# scripts/sync_to_buildserver.sh --delete +# scripts/sync_to_buildserver.sh --delete --remote-dir /home/az-buildserver/ATOS + +REMOTE_USER="${REMOTE_USER:-az-buildserver}" +REMOTE_HOST="${REMOTE_HOST:-srv-l039-p}" +REMOTE_DIR="${REMOTE_DIR:-/home/az-buildserver/ATOS}" + +DELETE_FLAG="" +DRY_RUN_FLAG="" + +while [[ $# -gt 0 ]]; do + case "$1" in + --delete) + DELETE_FLAG="--delete" + shift + ;; + --dry-run|-n) + DRY_RUN_FLAG="--dry-run" + shift + ;; + --remote-user) + REMOTE_USER="$2" + shift 2 + ;; + --remote-host) + REMOTE_HOST="$2" + shift 2 + ;; + --remote-dir) + REMOTE_DIR="$2" + shift 2 + ;; + -h|--help) + cat <<'EOF' +Sync ATOS repo to remote build server. + +Options: + --delete Remove files on remote that no longer exist locally. + --dry-run, -n Show what would change, without changing files. + --remote-user USER Override SSH user (default: az-buildserver). + --remote-host HOST Override SSH host (default: srv-l039-p). + --remote-dir DIR Override remote ATOS path (default: /home/az-buildserver/ATOS). +EOF + exit 0 + ;; + *) + echo "Unknown option: $1" + echo "Use --help for usage." + exit 1 + ;; + esac +done + +REPO_ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" +REMOTE="${REMOTE_USER}@${REMOTE_HOST}" + +echo "Sync source: ${REPO_ROOT}/" +echo "Sync target: ${REMOTE}:${REMOTE_DIR}/" +if [[ -n "${DELETE_FLAG}" ]]; then + echo "Mode: mirror update (with delete)" +else + echo "Mode: incremental update (no delete)" +fi +if [[ -n "${DRY_RUN_FLAG}" ]]; then + echo "Dry-run: enabled" +fi + +ssh "${REMOTE}" "mkdir -p '${REMOTE_DIR}'" + +rsync -azvi --human-readable \ + ${DRY_RUN_FLAG} \ + ${DELETE_FLAG} \ + --exclude ".git" \ + --exclude ".codex" \ + --exclude "build" \ + --exclude "install" \ + --exclude "log" \ + --exclude "__pycache__" \ + "${REPO_ROOT}/" "${REMOTE}:${REMOTE_DIR}/" + +echo +echo "Sync complete." +echo "Run these on server:" +echo " cd ${REMOTE_DIR}" +echo " docker compose -f docker-compose-fleetmanagement.yml down" +echo " COT_TLS_REQUIRE_CLIENT_CERT=False \\" +echo " COT_TLS_CERT_PATH=/root/.astazero/ATOS/certs/server.crt \\" +echo " COT_TLS_KEY_PATH=/root/.astazero/ATOS/certs/server.key \\" +echo " COT_TLS_CA_PATH=/root/.astazero/ATOS/certs/ca-trusted.pem \\" +echo " docker compose -f docker-compose-fleetmanagement.yml up -d --build" +echo " docker compose -f docker-compose-fleetmanagement.yml logs -f atos-fleetmanagement" From 1214a507cc653ba92f1aa4ac805cbab07fab9c7b Mon Sep 17 00:00:00 2001 From: Sebastian Lindholm Date: Mon, 27 Apr 2026 08:56:37 +0200 Subject: [PATCH 20/33] truck object control pdf --- TruckObjectControl_Illustration.pdf | Bin 0 -> 14713 bytes 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 TruckObjectControl_Illustration.pdf diff --git a/TruckObjectControl_Illustration.pdf b/TruckObjectControl_Illustration.pdf new file mode 100644 index 0000000000000000000000000000000000000000..1e3346f0feed10092104cd08c5c27b2bf17bdcd4 GIT binary patch literal 14713 zcmdVB+uEvHk~Vn0uHsX1gNg_$h$7+!5D`TXMFbTDm5uDwuBtxh8|Xgk^S;(*uFR}j znJc^h%ES7e`KXK;j2J--V)Mp`vNUHQq}Bbc{D1#n|L6bnBfCkQeO8wJ!?}^0Ss&N= z$tixE`1XCd(dXy+`6L4v{ew*MPR{syzi51L=fKQN2_ zbpHCQ^V9wH>o35M< zzfbgk%pX{?vR`^d{;DR~edmk3x$mFf75PgezcibxQp)0+;(qeE;9ole-nsvtYm0wb z4|n?4@7Z6!`5!ZX_CK&3fASra-wy9GEBbxPU*o_1{7p+ze)Au+gyp1vdusnB%r6mh z{{Pd2!T+f+zeM~WC`|vK3iC_E|AE2~|5TV?BK{8)2Kn`7|M{i)CFB1@Y5wR&epv@G zc5-X3zTI^kzt4m&?m# zI{o~`9RJ0R{lx(M<<0raj{AK(%D)`TUzGE^!v3h5KYB*i3qQHsxks5OGoJ7F=5Frr ze|hgPSIl28@~KvC^!Pk;x7KO@nkpB;wO+q*)@M|oEe{vAdR>eRqg|u{P9fR?!j?78<$Gjf9advA+G9j zxoWgtbjq}#OK;Jgk6h(7=}T9;=4_i@X^qV!4Ls7=XFpe)8qupfJd=)?MsYa46j&+T zs5Q8iJgrp&NcqkSVs{+3{C~@E8H_K|wjq#DLYM=M4ZXGYAvRqSp zks0pXeOJ%?#9_{fFI|1Z$2JGdjVhH^JG_unjRM8)3<4($w?$F@pm`jgc(=W)92hMy zJ_iNgSEi%GUg&QHPu*AM2f@BPFGzAVOt?d|i~e`Lb59HcpUK6$ba7|&$(23l`m#>kJL)!5; zsMdrv$G<7{2ECdtquQf>LaS(fJ>0rdaW>}DuT*Ij=Zx8V4QkVcI0=S!2Sdv{hkFj` zmE)ni$wR!=qC(HJy9lwDS$|l&uFPxm9lk2Jj_{a`Uc)orVsH8oz4(iyu$nfXmN8Or zuQXwA90-(3k1GjyWO<5j5L=9!LQg`swGhJO0T5ZV0J~suRee1&emPjxh6=7!JY)`e z!8umC8CJ!t@gQ8DDkDAn;A?NbVGieJC6VTH7wd+OaV|Z-@%z20!*e}6>R7(<;@Ft? zdPjFvon6Qj^Xuvm6q4=|-hqot!C)X7H0083K=;#%RkXdrr(NP(kD4X=<3`p)rtbC- zqeYM0wY1eqyju)--mOT!0qFhOMjwM|t2aA&XLGKwROxk4U@|!k)emdU|R-W`pKEy_5=y($lMxpy_?vGjoms zg&??hOW61`ua`QEz#8XWk?wvX18nVQVp|<-q;&m~P^oew+?%={W|`RBL!SSCPm7$TUzZGeaTucIA8%Sw9bS}y8w>% z-yeyuCc(|irsG@4c&~t&?KBtF=L^016d15^!2653P6*;MF|4suYPaUyaj`?jPYg5W zi=st>l*swvg={`mwD6VBCyvj&3O8xF@;`%VM7lfoMo`bY*_o()MTtf0I zug|^Hk$Kwf39o-qvyLB|uR1ycH3uwGpY{_Uj<1X2Z2I6f1RDA`gbyzIIY_TCqwCJ8 z@SQ)}@3f;<40!wC%Oj4m?+1mMy@}@aK8felp@$vY-BK`EYVfeTXF5DWZWy*5fgd+< z7B@hR-dFzcT_i*LIRYUdf(;8!+o75 zufdZhYLz*ewmZ?M$42Pes@|b4KU!{_cV*gX<8-ByNiI4S!&KXSALS7!3FocYzHjLo z{#;H`e!Rx!1nZ3Sp&y0Fb-4g#VQgGXdBpVU??Gg(&r7KK2^%QsG}Jp=E4TlWF==tN)pvK)yya$8TKTOZ^C}QP z@tKOv!O=&n+i2LTo%li^CI-lzPUY6xdh|{MV`gEgr;qjd&=NVLn|Qjx^ULEi7R*=5 zOF{FbmR1%{CQr&bjZl=K4rgq8FJtB7@+j?+#fKORUi+mTTZOiM3r-cdF68lq*W5&W z3Esf9A{~^iLz{)t8G(Y$tG)~A%6t3a>7sPI59gQaV?peirOBd9H#vemQg$FLaQmnF!x6(`fM@|1(5gvlzEuH6Tf zxD+2;W`rBkOx<0Am$yuazBQ;;zt`1k3$#=2+MQ6l#Ug2z%Zm^PTf2Sf_+C1}J}p#P zP%T6uLo6=iO;icB;$FU57I{G9YOIAQyd2nz($_4kFZl%T(VAS|d{9SM+e(y{AJtfd zLg3my!&|=Aiip`eDmu_->Ue0(NlIucS2XCB>n8j;rF3cmjO4RNE$D@99q6MqhfC` zxNZXi)!R^uFUcGwBJ~{tNMo1a%UgJpHRQsi)qb>qYO~;6vM@l4$JucL&Rskl9~na? z))pD#H99QTLSZ<8`;@Tv;3ac2Q<*Wv%G~LSLpD<%WnJ2}EjYCsr+%k=Tr@tZSYP<> zrePn(P-7;NXa`g4?YCa2*H^A^gNZ^G1gp*HRSp1bU0z1j{|7DHS9l$`A= zt@U?kru*)h9+h7;U{Jo9TkygV+`w`_jnl3|+1%q`9AxCk4H9q=k@ST~YwL(TZr1>< zIMC@eZ+F^*^o3MkC`!#3xIqP^D>SP^ z3Lo`m;f*VvcSjbGA8~CxlAypf+UWk^UY|8|@7B-C;Zu_9-LuhYq0cfoz0V?&f4=kp zq?ROfS~j{d#WmqKdpoqm!o2>aI>X^}etF*IL^7d}W?)Y)Sl36edyVF;PFX|1>3m)} zL)OL{-U9<%RsUH`@JFQYUyTV60{Od`;BCdfVuF5Uu)fS}d$pK_c58&F>olbx*#;r} zN_M{m?Xw056mTyMVK^G43Gnzd_K(-IG5MIc={UMRZ_`SRE59L4EU??-(Ou}nA@-cD zb`|HA^^74&RoUEr6tP>i_B|nqjq=w`V05lE;mWTXqjI~#tQz%QMj#uUD%UEC&>4$w z-U?`PkYzZ=+XnooHnZ7{##Ov2G<(R*RliqrcPk0ZX&8F5cnyU01zp|ug;`HsPuwTP>$8`7tQYxuI-$Rn{G6!!2Z~l(}g;3xWH1~l`F8nsMUbUA;+sX z#n#|nZtA`hjV}GgwfC;wqPIXVYS;%oS7~m8Y%{(~AWM)72iJmQJQz4U6>j!>E4U5g zIMS|zNsI6rQb~cOT66h5nT~V6OfyfI^3ToW`hfujXf#n58(#FBd zp@Vr%u+RZF9@QI0<34Uj%~kWuSFg|6z5XTIaPXdM09gHS(pFta``O$ykv@tew=Fd| zL5kEt-Nhu6mPgH0g&V@X@J8_?^vYGIahd_y)uP>a6%8iM;5y^3hh_kE9C4sy3DWN- zk1YdNB51MtV0EdhVTZ%6YM@1pjHl;yO#rOYqv6V{eD%Y~K|}9&68jUN)IJT=qiu6$ zZK_{Nw?bqi&j{Nj{j?CyW)d3JA7g^-BhXMBTf?R%bh|l_Q@l#V^?e!oakEJvov?Ei z%wuhow1hmmm(lY66&xoJ;pA3yae;)jj?(e9!G8@8C!ZV5t`t{Dsv6q`9miD6f!`UU z43DF#J8Fo%uoL$S4;9|2=Z# zDmZ0xYfpkoZ*q4`!DmEgHN-#eq1kJsqI>Mmx_tgli1LmIm1h+irV9Dgn&@k?tFT+Y zyqnxxa_&v0%krM@>z4|R$elj)i3$cBwLl^x^>;1ph5gxW5;7Wgma7DE7bZB&UxD{| zQ{adZb6ok>M%wO8;m!#MM#wZ_J%(B$*sMiH1Kg>WuDH*EBaXhemXpN;O9@BdxI|aXYwH`f z1@QaNOyD2u1OKWC#4zITOyFnK$bm3J{Rpqj#mE6;)8&7@vyn31Q^lo1~`YyEIA;RX2Ki;K9^)M7rfPvJ?Dt+8G zi}!=@mUgr$uRg*7Jr#LB_09%-YN)gBr%5I^YPFBl_U08BrE|8zXK*p{O&)^CnMR0f z*JPcNaD%`aoLFcOz$D_ktMUg(f{0cG=&8KS#vR&-al|G%!m=p}#+PO=AnCMYpgMr;g-gjQW<+J$01KQ^ zJ~o~fJZ;Q^?j+w6z=|}=LPprahFmJ@d+8$fN^cX3?Svj++Bsn`pf<43$z}pEH5iAk zD+r}L66tJITNxz0FM1uh3FsjW$9c?8=&da92X} z7!E<$merDbf|~8s*aV;0KD>CBnp6_{_4Vs}j6F=JW;WI(qZLGN*V{rJ3QMoi!~^NR zu+R{S<>FpJjE|dco|MXNAeZ9PNRnvmk`VGQl*c@E_of5hStR|=WI2PLgd+{F zA4x>Bt=6SJ%6KcwWzXcm1-J`4Ue>zdw`?NelIRwL@8@tl4;u8oMXjxv^seGLv=3Qt zY+jrL_qa#JCs@*)O0YdF`)?3gGPOaN+aP04t%^N3Cb+($AHFjXwdEEY-6eqPyzpKn zdJ0p)19tOM>Q~+kFo|u`5fgk56orJ6T6Gyf=>x&%p^m&gIg_O`9%*_p#(CGg_{egT z@%QhiL4J96blUWjg*vHPaX$6BtgyhdlGZ;Kp=WE99nD%^P&S3PSL_O_%d@l=hdN#r z$o9h&?S6f?ps$lp4pOQw%H6*bYZ%T-(*}k`|x3MYyh0r_R7{ItZgVj|ta`*aIM8&UAiuXZpn-Yl zxGEm*kGSc9LyZ^N!)F_mQLcP zx~Xfa6VMdBS2!-8~ZbvF%p8F z*-@dD+57N*S&Nw2ANXv$u-!|DI3IEcUJ56y17iZ)st=CWG#+uI$Ei6P;rF1DBj;gIyj#51JYmSaC2b<(>KhrzXm>30Nnb{8GNNq+bPtT@$Ruj|DBa|^)*|ioU zb375|=kf+#_VPs0FCKOSdUb!$wFt%>Q3Mww3Wdy94#iKQelI}#4u5;L3_J=#uBQ7w zczFti@3jRq&ZKOR&N5il2H+W=&D0f}8r%yie`W6-tztGT1uctkj;~p9gU#+d(}9HR zD$erN?wp53L*v_u~$E@_>Ts{N4)S>ayW! z-K=*&AeB8& z+G-sy@d+T6(CJ}9U*A^u?n-r#+Kv>y1?|n(mHklhd=+z-9AuhiS?jxhw}2s)y6WmV zS(W2%2li*Sb^?s@TIai3V~dv1Q>e4;@AD%*xAz5G+SZ0+Ol0Co=P%!+RNQd%3KT`yt@hvx!EkGLVU7mZ(^c#f9_`ixR2sBwcU2; za_2OC0`TqfID1*Tx%W!-gLf0y#kD9?ohT0^CqZk1_C83H(X9wLLU{p%si=|uxY!L> zT&z{(0oRRigWVMwZ{Ik7oFd(ApG|?iHN{-D6u}5^-0Qu?&K`s1ihVwj5i}oej3L%M z!nHc@Ua!i?zP)zW@@%yC!H4q%^%h@V&K5*5%T(n?P78JES-k@-yZ}VwV_vbMn9cJ* z_>lKGk5=74|1Nt){(Le=a{Y##iEMD)X%yZ#$a!wm3xw0U>leolyYVgdB5cB_-$yBG zz3-g6-nW==-*vZ-zGzol$R(>i?y<9-TDQfhB)>-LLGUZp$#tWR~zHw=H-)JrK%3OIVVC`0t&VIB*_+I#~dfwy|DL+E&# zEq!6~P|u3H2&rb-P|O%jpllG26_D@YgDcx=U4 zH5#c@%Ws^+$+@@a_rK*3hRge2Ho__d;SReMe|AjblzL>P`Q^~e4pXv{zhou$`b?g^ z@lI9uvvP-Zq&M$1Um+6+y6O~Sk>T?^wEy~;`=U{$+50?WwAF#?mE5s=C|~tp(iB+* zN)82idg(^}X|giUd9C&}4j@gkPMfT3ru^=%1 zbD(dxzV(o#m!%>QO;0)2F{rct`uh2ran#s?P!lnIE{TWd_K{WXH!n9wvcC4A9B+@Gt>fz>(o`oj0S2t)NiyfN`P;HfwN-F`zpCT;YO#{{ z+j(s9vR3D7&b=qScCU1mMr&MaD~=(Jdcu=k2D&Cww>ehWYK`xM5*| z)GkMxTM8$w{oXO>O1J0}*GrI>**o>w3}0xaieD-AuW7IWR^WQW-q;*cRZ1QgtC3f| zXzl|-%W+UPS~_*9)ZE0CAbv9Jf{HkNjC7$3t2&+6`{3G`*f$o%SM$nzmkaBiE<$3S zl*@*$+x@jFdXtQ~ComV@R@a3;YbFJw+sm;$ja}CE-HNBp0lak^SE|*4Xf@mW)vI#o z4bgVAE8W>@I^L|V_;_%fI%~aIiT(3rd@jJ}qFLY~W+~Ttnnu-6IxE-ES~92)I}pDu4_|;2+G%TCTGhzJoF5@yJwSF|C2EJq z@~RDglncXxJiZ7U3!yA-T^}tY6U+OO_T?f*l?=wI-Et?IpBIVFO69iX6W$|~A5iby zuG#VG3}F5Xn-ZIEHp3DhUUK#KikbAvO?IisAz_Ql`>j$w6UI5Io3lDhqfEi>)#2}= z(JD6Tgk4V4mp)=RfSvZE^>h(rQi%E39w_nG6!_Tq=-QoOZ7d5Npi7dcW7&MauEVPQ zP%B$A(Cb5PSFSFZ({v|EUc2(CfN+P-Q|poqme4**4o97LUV~V!kE*6vPP& z{nmAS!6I0|!q)w}OR=`ytq#fUD}vPGHc`IH0XHwhR?m5U@|x~#a#Hg{wda#Cvc`b@ zps$}3O1{kW#KPrml<{<@SuVbVv?7rmmDT6fsj+|5yeq%XT5sjD<~l1bqJ$O2y+d|C ze7C!}*RlHMy^YRiin*4jxh--JeWg#0^36u1>q~TJwqNd3DkG59zbV>e-JNrqQh&w0 zB35_$vN$*%qOs^7_p?Ht-crTXJwr8FP=tg%JWB6*YhGC$3-b!8_z9uc&xR_rU)$0u zV6Wgkgc@*()*A6;Im{+zIIdIzzvnB9L!r6_L|60Q?EF&-hg78~R9bc>EkvuRM= zQOfWdAM}cOSmgSsz0%u#j+Ayn=iN58YMBC&-9>NgXnOv@Yw?39Qb}HSn8}a>(v3RY zJ61Xeld3nk^tkT0T&abDAL&x6B6g9K-qx!eF9bXs>(POtb_@ zO~UGT(qJ3y7BA16r9NgutS06uS)eo_dOWIei>*DCO0sKVk0@;5LI6Am)s~Oe2TBcI zL~qf-i|8|jKINLZKMzjbp8F_?)XAG(|5-r($8(SWYCxV>Vg4R*ifZ%Rh2ASEZ{2j3 znD$TyquE$_P%!^Ig=9s3Y*-PzQ_l{Ttc57(4h8_NQiJQh=H@{F_S{wE#S;nD_{-_m zgZEjB5TRZObe!rlan+|86`G4{0Y*QrPQT$3bgcUFYuE1ByBz18;ysx+!qe*(DBUL! zCJ$cQ+f_=TMe)7GkU|HlxoU8J>=*JjQVkH2=|CK4eJwu+qjbpLqm^U*3;Hb2Qu6dT zDBa73Shpo30)f^g-62DVz;6OpYJ~A+YM6CK(l2YPy)keF{$Nw_&L@zaG><*l>FFv(0TCV~6A?TK1C# zCXz|R?}zgSk7_NcNq{*X!9{28<&-OW^ln_WX`8mho$MB9G85B9*QKs>>qUG^(DD}9 z-Gln{U)|5kZLEg!m~4B4@|q%ATz=Y#GL-XSzW|VGP@`v`%52ZaezW;_F%b2}o;`mWiHAEGhY|(Sj;sO9d|8U{=CTrXgX^=d=`^ zmWc==Vr>eoZgd%U!x|HdlM(cgo@)^)+%Jq28?zp%4ElPMb*2uuX5Gurw6@AoqccnDQxdf_1hd6gBSP` z3-IdYR+Km2x_A8zU}y_X;Qj0E?w+~JHnF1TC8eRaUAz;`aedC^CwkHF6EO}wF9DKT zR&p$7Fjva5ou5a#zT`k?lbaK3i>5o;4X46-`JE^Aq{n|AdGJlk-eO2}u1Ds6q`on{ z7d&Ct?1}KyfB@}5UQ-RmuBf!}g>rvs@AcA5HJQ2FoG(gTOGnCh=$qHJMUr|q)1Osx zaa#)-=LJj+^;S1=u4V8yfSQx9`K-$|dWrPg06pibYd?FGHc-Y{tNu+-iV$<|-%f?c zWank`(xz2yEnn@WS1x^czA^2CVCz-xV~I!0s5cQ|(fH!uV!w*;%;!`bUXuDvxqlM( zt6%T}*|C7C5rvf%2W*yB5d@!UBdI(O2esi$$-@Kw(uksUU!fyy$U_}gzlR&n-;ya( zp2wm5xp~!oPd>4>x!yA9N9m5$b_8*PB|vKZWJ4~x0u{Y{3K%$^D(`Va)0y1hYmdSDAn z)u}OZT9H{ZfG}kxspPi&@#uw38rjVw32nPsi2RdL%mPcGRoBTn+OGF>+E;42G^uS` zD%!<_y!&H*WWFP~T`3|Xh+o|~Bz20J0o=9qaQe0`TW60}$0S#gzvrOYf4=gM&jDWz zDo!^26(#&ld1ZgT#r0B9c-+BmQ&|-R++nrTQA9YTdmB`HY?V*@>?iPJ>>DKE5UZE- zgG|KgCE4qd*KwLMmn`2Mqyo!bcmqs3o8ds23&Ci<>UMx7h9~@Cw>iyD9%@`E^r+7L zhvKZ-TVXfd=;={qoI2$VS|DcExlVau^vY;5G={I~;@a%h*ShL2$6|2V+Rv(+cEoGD z4+Ik7+r5z#jFNP8fI_8BWN>#FMRA)(=IABND&OOG{(|_?IO1NQkd>}m(OBqGanPvJ zO}Bnv24;{SZXGdGSM9_Zb|esXbYJ4ds3O z0>0LHC!XUQox<39H`8WS;|Vp2ux!PbKK}G&=wx5mk%Rxl9$^cb8tyYUj@LIA)~L zz33`JSL?rP=T~_;yeQzek=M8zPbjGGUJ{|#7AN+Vg%PcewTZcJsO^&loo=%_#u5}r zNz3ATn+o8#N}s26QLN>~Jtw^wk3&s&^up#o)hdc@+5fd=b3+r!lmR{do+*@?Mj_zqf<+^InU;AM4@0Khfds&Y#xY z$BBP@eh7m1^5dd^n=fxw@Ip8Hy{C|$|NM3$9m2s~w0pB$q+{QRdz(OWbUt6iz1K(F zHctJoDa_x>e`s@j{PQ+P2>NlnywxOcuK54zcm#j@4d$bJ{PTOaiTqpM?H1?Fa=&-W i{RS)E|L*?UQopw=SlQ?IO@?&a{myUYGP7ZS`+orhv!7r9 literal 0 HcmV?d00001 From e2392c13ce491bcc56b1f1d3c526834292e22cd6 Mon Sep 17 00:00:00 2001 From: Sebastian Lindholm Date: Mon, 27 Apr 2026 11:07:15 +0200 Subject: [PATCH 21/33] DRIVE command replaces RESUME --- ROS2_ATOSFleetManagement_cheatsheet.pdf | Bin 3943 -> 5231 bytes TruckObjectControl_Illustration.pdf | Bin 14713 -> 14135 bytes 2 files changed, 0 insertions(+), 0 deletions(-) diff --git a/ROS2_ATOSFleetManagement_cheatsheet.pdf b/ROS2_ATOSFleetManagement_cheatsheet.pdf index 2060d7f31daa32264a5319b40a5781426c34acf0..317559e1166777cafb130c7a350f40bb2944f71c 100644 GIT binary patch literal 5231 zcmdT|*}AI8lD=P0A&w|22nZsMpdgMoPdG3MD1uC{*n3nfB)A%5)1w1`r%ks61?u{z8vaWBGyP~t{Odc zCrp@O_^QM3Pe1)s_CIxPSt+aT^P`eL60UOpXf!@xu986G?jiwmXaf7z!V>tm7M`fS zTOX~~hwfm!Bl?gv7t%= zUPemz%Pa1z;@`Oce#HO8SYJ%Nr}Mv{l8)e|BHr*FNWAS)GgAOy+9-|u?A0z^*m(pBqb>Dnj6NdM(0*Vlps z80*Co{WGYT3tFVcOa_0?SZ`8{Eyb7pUlOInA2L+_bJ^n@ao5%G?Zl6BSXrSR&;Lj7 zV&=SFaT2#2YFIcm;R_?bK3O=N5Vc zm5{JvHn*>kEqQy2272q))+|=1P+`+)rnl!{>@?3)0X)Jvp)6+5bg*NZ%@isOu~Qn! zc?D+@`PoKPu0#i&K@Z8C#wdfZa}PVr&9<`w1jD>`J0g9B9PA4tSUeu~@?IxTO^#U7 zRlvoQ%g6`)*2uIx2z}BEK$oDQsoBr*^N_ixI2G`2N{qh{+UaR|2p-PIu;pHpr>YJ=Cl}*h->#bQnadtQN^5H5x&AL=`dTGK&!oJ?~Gj*WaZc;!krMD2-umi;Vql> zrS0Utn{sCgu=U$eMut*;9d!E&Vj0_KF616#Z{nx2>&$TAt=WQ7kV0m#tAKiyFw8Eu zlO9K6g4;KF%-Z}V#GXy3lwPBhvEQGAWo9$Ir11%YKF5n~pm~wEkyD&s70bdrUUB#I zcqJji*?6z(O`t4qdxWK6D_}>*T$JTb?RFif?&HF|->w=sNC0tpNT`Xajmh@DKc~sfU5}u3B~E=<SEFrH@Igqyit*xj6~#n3wpf6YTI02&54yhd+q~M zzq2}@!+t)STot|lY{f^+Z#nYYVYBk}wE2I{x;32onswy}>GhC&g~|@-LjC}(M4exo zyUZP6t*d5FIxc-aTw9wKVdXf^JprjgVfXAMZLv^w+biUd0~SHof0q5h1px*^V|aLF zk`R{658+5w0=kg|vZQ+ll%khYj3RR7aph_r(*w@}>#a+sWp=pea(m3(SBQ}5xly&# z!`0kG)aB5eUs2eBPR~wthAV(*!L|-IhS$XqO9O|wdlnm9Pe`r{c4m0CNA=Au|9Ye- z8R>M~!MV0<_wLGsUOgqhyOHV@zBL<_YphJAHp6i#JN2hCMWSWo)oy!}s1lO$iN~e6 zPh@vp#9q2%c4YSRXHuO^ueG(SIO?OYx5v#MN4(NYRe_;2mP)+^va6c~*UingaZKyXz*R2s$+;X>51BT? z-?;Ocy=?_8;Gp@STR_WcmEl&oLaNapcaQTXwn`08AhfUdcZ=Pk-m&(>{l%5!>CTf1 z6b9Z6(&F-?DPu9I1`A{@*GQGcWR>6h$80KE_j~za7JfY%3x@BT#`(OQjC$Hmf9B51 z5a5OxNQ7NfI*c9Y#S zE>Gx@9xC~;cxz18On7}Ei`z__zPgGLbQ*I%w=*XLVtBu&JFAG&%6Ss+X_L*(aVE%W z=|1|U8+Ks!8)fZYa&UH|n_;(owlj+yJ-LtttYZD#yD|?tHiLKfKE-kj#NajjG zA9?1*Y@5xNOlG~d;0hzRs+Yh9CRu3EGm}JpC0H%>0CKtGsJjq-lYT;Ix5++Q{PC=? z@k_Fyz@lAa#%XmVxl?9p<^6+UK=s|2J}`svYrv6}UiljJ9-_WylH#aV z7|Ut|)lHVvYb!4tq{?FyOJQS{-=vDCUykPZQPq3!hz zpMmgY3cWfjX}GP;2G}(;hR{U=D_jZn#Lc;{UId&yQCJ~dbfmosnMA-_WQx9POk8IK=#*NqbQF8>&kS6$EmG~yOkzBvxo{&jAO>T zAl(}>mqDhz?sv6qF2~?Ov6~l^JcK%sPG8Xi)Gb{1s)0?xw*M^A+J42PkjD%pwe2-s zjqEOKbn1@8wtWonx87nk_SnS}VlK5{t8WRZ_1Zzn=QYjd@e`6+ zk%i-^90aQVEG(^Se7dLtPZ*uE5j7Eo^W@$t_C{Wj40l~?8VNyqvuq_h^;NY)*mO<~ zGBsWr4Vc^gdA@6Te>`R8DtQp{(X{;SQej%Udz z=jZp5>p6uJpIo~%7mmuuNol^O!tETME|whwcG^9PX!V+k1eyAJ?*el5h{MUjpq+%< z!Qp(GEO$+PuAN46cyNK|7mdo|)&ZV$D%E<1wdiS6N7UA%1tBza(A{FRCr=E1)yxO^ zDr#iDeuBhI21+0%S#4==A<#EH^y}uN`WHUjBvaZEoM&v&(&%7rYeHD+wIR~ZXZwwR@lyWT*KZ$4e5?6D ze)}YdY89&{Zi(MyWL&*@e-h4bvI-VgSbmV9cw930L55ZTI2I@4qreY(RWiP`{U9T2 zf7GLDf6%KSacSd+u@wx9uh9RDSHY3EJoC>ow1#|{U+Bx0t^1$FG@+OHI5tQ^t714- zm32x{RpdrtxT4||RUFs z@LocYPoF*&{dZnlJr_04iD3W=c;{QJQi&lw48Wu996)+FKzv;h0Qp+QYPFc*>Mzz8 z&o_~l8W=q;Yt|`mellUnJ&65S>vo0^mK5uY|l3 z8|j+*o+cS#qzZWf0$CWpM9JbE^=h=fKHFcPJg|>K4tY%Dy9&Yh{K@n60DvOC=$wC~Dnf$()FK!|->bz75~6kXRsS4LRYmM5tY0^gD$#JSp$T9V!@X1m zVHA8P)<4JJf_hTomzlXy!4)(B=T$=!bJJn~URx6CrVSH=^NS%UOc_NMv}t}ab+1j) zoU=Rnu;>LNw#DP!WO}$v7M1?B#_cNsZoqjFDh3HW;me9}>s|$3!L|;4U2n`$t}`lC z=tqMZgynv&BLvw+YTu?NXk*;EE|}gBOsTLX2OC1Udo2?uQJp{T|=KCRBHo9l7J5FcpmY=lb;LZ;>BmB|a4~vv@M=%{~=gFK~ zgD*DUqh&XJErSaXKl;7)@!>NaVve*bkDBV#Hf>RUtV$bpcu`@oZ|)fFl-TmKs<&6u z4jov6$z#LIrK#{OS4ur_NFG-vP_I!bEbO4$DqL67#%-d`$vGvwWydrBc-XY*$s~lb zf(^I(2ZsT(k8DF4j?zg%-|I7Lm$`dudfk)Wbnmf&Z+7`%gLG@}WaUDFYao@rK~=VU zJjZpcTL)&<@o`v!2GappD=|HK)z-A)CE3w4m4Hg0OT*v^b7IpzSF(_d(w6Z|J!O;F zrFD9IB%nDxy;=j}=C;-)OdECu(;@dx3ElYSW*xoq|G{l@h&X0*q@J zr+-(v#k}JRZ!@uw9PrT6lIV2vwte%&LW4DQQmW^%{(iJVJ#u}1jAybw$k(AGSD)ST z)seQmm(K!I%1nIez{~x~I;xm!-wZqCm`Qb-b%v|&GN&FGP)Qc0u+C0ZD1J|-y!*0% zEtxG*nPCGs-{j(>X0#cB0Li)O<|rn~p0sod68Ho1804^$B^e zuO*@0sP}g)oE?lFRsqL?PQUAUb#a`-2h~n-J(w%@Ibq8UyP##1Mhb)z*63bmyn=$| zZnMYjansuwc=}$b zdJSd6Rk>|3^iNSKI3LGJ{X{_>lCH|7s^{Lj2hrDpDcvJfqIRyb`NMuNlF3HvQoi_F zPhQQ>Gq8-R%VlFY$j8$+#N2}JkY&oyv=8@Ce6KCy55cUh-4>nkMn-8WJRdVC<*&h; z(VA-AYL%w?SciapaUk5@vk#8_?X}`6f`C4Xs8M*h& z9va|oW!co8J>f9gO@xFsFT+*P>2h8^pA(r{rZ{Ky8+Ge?UDO&PM+MS&cjxbK&Hbdy zq379#7*RJ^PgF<0T5eny8Hu^1T|MD+ap4jZAu>za1z%f%)RDUnut&4ZRX{ zvm?t@-m+B^v9_UBbokW6^o9H~u4CBcM7Fjqenn=R1)>7)$ZZPC=}8bvtq&{#txt<| zg8N%7YJ9)GG`_aL*l*hA|LrwIlK98BkQ%yi&YaHM>9Vk$X6zuiHK?0HIMs|5j5Y`r zw4TXzMxQS09(SqhBSy#uj{cn4B0YADI=oGExOqkG7Q1U)IOdH&IC^u3(Wn)PG|}!O zvBdg{F|b+>bSI3S`AY#Mz!cJZl;%CsUGbiMX~$i%)+y!T6(|rTxg2iFYvoul^S65Y zKzB*Seqws3ed{cWXXPNy5eGbW-Czrv>0(=b;P=$wLf;z6Wk}^wiG|=YyRsX1v+86* zJD!bKG9mId8JIBNJ-|bay52KKzImCsJaRA%W_57`abj0a>QU*Zw<}?I7*yg!qA?65 zUuLspC2sN2C}Ciz=y%-<9CWU(sLbz8OFSK&XKpC3CibMAf1dMrST5;Dc)09lb^{k2 zSJ$i~Winy;Aqy9p<$|NkJ}eI80@j+8=Z5W2p~w?e+KP+Cl-) zt*9rDy3_wO;$~wpgm*6<@aoIwv1q*ULg3ZsM{CLYtMN+MdsnFX;d=@RlA&X%RCm&g F{Tpb9YZU+h diff --git a/TruckObjectControl_Illustration.pdf b/TruckObjectControl_Illustration.pdf index 1e3346f0feed10092104cd08c5c27b2bf17bdcd4..7e388796d66639bac986a4d7ebc4cf9c197bfb4a 100644 GIT binary patch literal 14135 zcmdUWS-Y}Gw&wl!E4DZ*iVOlyI01qKC@P|Whzx=tC}P*CzUV*Deb@K>TbrGkb?VE? z>^@bO<+C4=*eeKDtcZ7wQC!n(18!nJivRn6{m=jNB{|PHNtfpJYvx2wVy2G$l~H`z zvGsX4QTpYj>9Y$#fBDN_n#nJ;p?Pgu@#6>l)%uFB_aFWK4~T`oknC3af>`7W{p}a} zg?<0}K~X=P$o?k%rukc!iTQTeI2!=sobbOnsc)HI5EEIk?MJsS=-@}==wZ7V&b`v4m_!o)6O_J=8U-}oc zHj{6;!oP`0a^BfXTF(3H7eRhY(|@#SYcsb9as@vk`cD**hubADZM zzbr@TTe18qoL>a?vuJ+SjI1A~8Q+;k8E>aNPqt|%Q~2N7JIn<0+fDYUr4!v!FH>u1 z>-$j|3y%5tF*DOaZ8|>~Z27_sG_3`U3MeGCxlM=!2w*mCYMI=HD|9*nwd;kwP`y`_ zW5e;=>rWaDU}x{32UR1jdfB>m%2+Mv0W~+k26h64+jTgiXAAvuE1|hn6wliTBO&GE zYHamC z0c*|Wa%>eWcd-r5Dn?(c-Pz~U(T_;CRWS-wXX-R)!Hag*fWBfmwkfUfqN)d@GO-ErND*l{IZQ&{WD{6!&N!IEgn+=xpWr0+GEa-c6#^m{-DU@SSXiWa__I^tpCg( z=R*CIRuA2}m&RB{j4D87d)ck=g8iEAKS>=gT#)*aEMasn?tnHAg+`r^oumYv>YOz$ ziu&l%zBXHvCa@0gx!dFg1b8NZL$j@GLio;Kl)-)%mDx7H#kJCHSLQ&`Yz^M)LSwyj zP&gd&%XVcml*$2q?3LLDbE1S1ys#sA2=)<8eZEn)8t8B>(uXB1Ar5HG?mIWhK;OAX zztSc~(%dMPSbtQspFt7Ej$(PM`JZS#>mLfazS$qX28Ik(3*dEm+P!4D1=ON$iWEB2 z@nrdWS5aNHQn!kpQ-0^LY3aGmSMAg$#mjcqUsoUR;TwTj;9|WLZQ67rx6QllBNoi| zjat*gzjBXphCR?J)C>_%BVhM<8f1N+v4fD3tzLX+QAEb+@GUCMeNH>W>tFLmFr$<7{rt)B4yAC zb9semO}Z9;0V6W^>2O704Q_4>Sjm}yP*ElMxNvqu<$a-jN#8UiY8cIk5a^pdwxu_> zS3S3RrtEMV(PxAcKz3WaUBp-%B~QlK4n?6>uzDyK*EW|PwWVH{a(_1+2i4b6b5~Gz zHRu;A7rEewhqJE3&mDw4heYo>tQ+}zEs_GEZTvQy)MjQMUKq=MpHZt7TTpX9`9lUpesG%K8< zV73XcXy3B;^2*=BZxb+*B23;l2r)S4@9F-u;>qg1%@uglW!?qLPbNbvWc)|KOm~Wd zoM`S;_7+UFE}gGj?F0p@N2L!8t+>{p#Zw*Z#~xnNXzrfRl&#sh#IjLmCpH6DeuUu-;UB_Zl)Zm+V0u#7_Vz?+xNY#D0ewnj&^oms~)eR zUX`v&ez|YueMEV=W07EZEyNDDpv$ar`7yQGIX&tW(~j8`>4CP^j&OUS9EQRZKg@*3 zLAeFqlUPW_2i)Rw^L+nCgTRdjZdE-nPUqltftb_DR&yWv@xfl`zNA5$8($pEs5Cil znd!R&Ufgj^9}|i=wy=C4mWoLklhN`F?u!iDimIvev^lG=kKPyl@Zp}lkvFI|I*ZX` z-PG;jW}po8R+Wh>cyDz49G~{=C{*@}Fk?qD#t(aPZPdR6-l$V4(fwBojoxiGLf&TC zZD8$OH5nwTAV^?KlKE??{+Ozl;qyi3(G7_YEpt14P4*!DBI5dS%GwHQtX(BWXH4#V zzrU>GTn;)A3T@|yYNLB^Z?@u0EW6R#7KME-oUj(i4(VhlNw0%<0t$0%`M4F=2J1qz zH42Z!ds-bI@=#v$Hc!Qe*P3?y*kCVzAt8Wd%ZnPbXtc0gN|dzwvnp6mrL5<&y*b71 zD}CPFN<{F6eEE}W;xAv5mq%nf$29EepERDIAH8_-dA#^_5F)lvB#8dgqfB*xAjjCr zRW@!BFGe69ysbS}2p2xS*&OteUbh0Z1l1ObTJu3M;`5LQRe(`YG!HGd@tSs!DtI~V z!cpnHN%;}5w%i_Vly6*T2dYb_%MH6m_Js>>gfNskkHFe;BV8B;2u_vTtgJL=)5@H$ zP3u7OGt`(ib;88E@JUpgv2B#ln(}6D5c5*KkW8Ei$I9#5&b~Tybx>4l_9oT&6@c8r z6AusfrGs<273K}c^ecDYS_8XrHnl^WL9ju4gmuBQ)5z}@ZlgwbyFh{3+S<@L>IY*x zjgyv$E^?ElPz;{E`gkA#oJUH9ab7I4I0h%Uv+ynJO2dywAokNsWRIy_>7xva;6BhC zq4-?QOTo)7^mZRgrx3YKnfVJ%Gas_I631TgUc_VwhGz3r%kQ1(ec#>0w58HL%nOjtGB4z`HD|a%Y(K&Sv}udY7SfHN6E%k$LjdA@p|NEQSC(c!T7Txo%yW=wZpI{$Un9%(jEY`@x?=TcCfA#x{4 zM}4BHFI!;eA;(Kx*`V%HL^U-b`DV21J^f>5jz|TZ@G`XkbMTUQT-aQzD&G2)sK48( zauEsR)%?0eB>UriXyF>=LQ3xlYqs3On5{02E8c?G3iN>SwaNsWZrlE9{AV`7&yl`= z(I#l)zc&fqb76SGly&(u|A0-l457-^%^<2Hn%XJpyPX{yNmo&EC&}E;z~;6w@K?}N zyTEim+j+?;-!8-hZqfw_u*J`YvjSRcgKK+xD+-V2g52ZSrQ^L1$b2I^7*fi-%MLsh z7rtx@`T$Mt9PQMld?xd&`N7F+4MhfeqS%rOgfaMio3S!35V zPacj&Y}+w(5+A=r5GqvfCkt$)wR@t7o`vHkW$T zhUb;hO=l8_epTejVbt<;*iK9I%t|v zm;eW^&W)>Du4v!vO@o}rnfA!SUDm~1bW`BPH7oT5bhUYN#m)Ql6F5cKs5^_nUSnwL zHc6c8K%;3?xuMnPAm{=u+vJu!5{)+sR%COmRAR<$7e&K58$inq280OCq+|o;BrIiT z`x^`0pC-7=Yq|ufbms7k5}`5ZR){49@^qOh=gSTG6Yer4#r0Z2WqL^l0gfD)!Wy`h zW(wc{E|GCq2yKon#_Xp5rW)PcczQAS?dKVJwbtj*RmjaHX(muE52i(SQ1D*<|oK?(s!Buetuy#P!DId&$`&6GILS_ zs%zqMeZ~@3CB%_9lNy)pp>tsBTmfjm(v;=KrP~^4CS0S%Torz?x`O9^Zpu~i%(QlG z$FVIjcG2nZnyB8DO8*_lA14vzb19k$7DXp zcP7W7Sl`76xzQ|8eyU-!XZxD7W&0UM4cXYKF;mr%^0xD5!4k1-4(n!bxA9W+ZE&5YjFDH+ed}F z6oq8gr_zbPYZPp=T1T(^DQ8{@czuAj@&%ppNV9M;wkH*#t7bu}OcHaglvoVyC#&MQ ztiXv)09XmE+hDhA&tQ66;N3?KzGN!oBpisSLEwW)X;>DX+HO3&b}B0{v31Fn4z6>< zOLLmM!rW-oaV9SM9Bq2Fx8;}mw_>?2uOdPeCMS*~MMtcK)_YVxPb)a$vP%1WaqvCq zZ@96O=cyQ|~wp<2?t z-ZT%zQP(`X`FiU_j-IpKwCCodZDD@Xv;e5V&kU7f8B}|d;`DLRSD`j>dW(%%r*3G& zhGn*Ln(Z@1J77waF?z{Olm&iE+*z-BWgNAZhgEr9sPX~Lq^QU-B z7wS?&^@sekKvXbq{86Z>reZ!xJM(5VVh+zCRscvLfw!-PH3$2^ESe6H@qX7pl=Ep5 zK3^NQPpw#_HrUExH>Vh9cf9LcV`M4TNYO|v9&3-c#+@M+Tp2xKMzWYZu$N=W7i#?| zP86wCpRD%w8`4&t9HB=Kymj4oq|+%}R`wb%w0$u+JH1^VV0dQIYPRv^{63xKQo6G+ zG0!j6OC>)}au@vJb#W3@mdiA^xEx~MGOg5MGM9OhJW8Hq+g#z_2##cHQO$EyiaMFEM!38)}fo4g%-t z=pO%&5*6$X0B@y?z3Qn*y+C9lqj4@_%F_&rA4g-jI>bX_X$)xqWS&)=rtn!hlG)5- zkm%ZIXb}wxBJW4=5HSdQ13d0^S|crE)^-b|*rxYtRq78aHW`}x>BwR7Su}NGNdtPE zH!bby?fvKoMpK)|FKr7$VFKwl8%N|^wRX$ym>&(&T-_!IY#kKw{@_rU)7)Tc5L>xR zzZ9pWJobrw+zt#j)lKEL7rh&B(LgJ9)Y&Ti6@&05A>&Zry>e3RZpGpgjP@5bMM^saJLwiHEuFhf$7dJi9{2U?#8<}o9^%ODQD0xx)m{>3Oz_J zxZ3J=^@p~R==Xd#ODELf;vEXynt<8_E}r{BVG$odl{7j7SSeNK!~O1L^c(cOS`N9f zW3mgZSz55iZ5ckeugL>!8=CQ|9yWUw_wNA*oGNv$#eaGMcdqsCE?NB0Dm2J@;N6&2 zSJz=|UeV2Al7N?ZwCsxPdSbBGbU3JZDul7MrEZ^Ee3u zbaePVX)tteBOPVC z%ZW#3u`Sllwo|z$XJrY<=(w4zhEgUZgR7sM`BKzrXcvg&cEm8JXueTilg{%x+E$zB z8NUK_dtqxJOwPzdVP)3?qglS~M@AQT!Du*zidGtrUb&=^%zUyy*QU=`v2+a}l8~j- zh5&(yJO_7ibxziyQU1we&oS1iZj~%hzr^mFv-j!oxnabd$}a$aZ)V~_#f4L-d^VT) zC`|}{xh@OMO>`$qh__xZqyE@x9$U}L@xc1*TqMOC9!p8}HYedYRS#`uetGX!1J(vU zllJcK??SAW3DToUti9ZWi_T3$L2z76*Ac7N)aowM+QVaJ%8hMV97(%%2Li&ic){PI7EujbiiB2)( zjzm&;7UO)csZ$nS9s|=-*rMW#c|G{9abJ`++64QYtDxByJpJy(;Czjsm-92*{t&8? z;lXS@*4GyUAiG2BB%Zxmu_uSh>FiGA(G6>et8;3YrBw@QU{G>XoQBew)vw?oZk)%) z)(zUFP%l?i{n8zEZx7hGc$h%C95p4!jNT=$?kPOh)J@_2%(ta#v%}e#U~5i!Oa+7c z`8BS6{QDyd@9ir0hMF=4+}{dLlY-tu)gX(6#WZ(D9>Z5}bnP4r9MZwd+ z2#^N1mwIA&^@|7v9;dxhfC8HvdsD>fOfHI}n)~eU;bC!%4&l!&5Qbc~%@ul}f6&GNG|#UsfBAZ;&&!tTiqJ5rlj?8})i`j8H5ovy_dMgtUT zU=}h@BZzT(0aZcq@)3Qybc9y2$8^*~*@NF~O*SSKfPXX>Ll{*b&OvX$yV9#65BPa| zGpgNbsCKms%weX_W&=`%yxc zX9B3@UyA{;I5MT;KCF3#{GJnx^J3+V)kbePAe4o6RSSYS^$l>y{9`JGNoM!hYNyt zKemd<$?0i>oA0z=vP`N?j_#MU(AaC-zciv6bR1;p*eGji_Xw!>0OemWSzX@h)_x8R z-u<*B%{~%6d0t!LqSf=C?zzB?vJJZ{Ot0Xy!-!cXZ`Fb+)lu(r;owDM|FUQf7AIDg z*3^7A5+;>SzZ5rPGZ>ODI8y|hkOfUOx2>b$>fxPgyI!*IT)BB&K;3ukl=ho}{V4!F ztt|DsSpM1BK37Qf?mK+9g61QlV@Qo*=RLjmDn_|gxQgln`V5t==+DO2*Ir)+bKKjb zj<9Lfxlv8D(fCD=J1r>p9G2^gInD)E`OL68xXO!sWv7|5lsyQLY#WZw?(X^hY09vN z%~sf2H)|nu9_!Ycz%<6cmTR@^t-I17-tpAn2_yH;+MVlEl3I8cRy967Lmzk=mM^W# zxGy&RET8borP@=%#4%a87krkqnps>v4fl_kJEI2s^RltJtj!V2SGwLlp6Y6@GR?xD z>5A4wZg5mP@}WG{1Grf&9aifLcpNT`ZAi?JG*&GPuASBW%=jR zgJG&JQ`*Wy6TWdaG8N>0aSj~{=*X*F2^r;!jdpHYzEAcmLZ9ylPCKclNWg=-{kSEy z)bLjpXF>UwXuGIaf=12yGRHPP`nDFa0~RUHYEyuSH+NytBa4%o8U*OjZZ@yu6m#G;<(wLCKUaNh;wh}PR5;y z_zCK2juXT3z_>q95xvi7zJHnJPqmstHn1c(0RJuL5ztwz`TT;Jpe_rOZ%9NySjHoCAOudg5b1Obk?oD=$614OSBmzP|0jhpNy=iK~EFJ_hT}0Uee{nVv1VTyAvaW&ulgXtC9iZ8%j6nv+k5TlrPCC>Cl~ zBX`{-mhE~Ks23+S1N%^++uJerz#Z~2#7f|2buMTXnGNa)72XJ1c<@i{pcoX?3->E2 z;tqQxa_{x|b^{@^(bUpUlO-Qi(qXM%4{L|%qu%a5nO^K&&S6viyg)YJMq}h%$U5*Zc;RupgMhh0mZnO!p@_XT*Z$Q z3H+PvN;j=|mKMkEb%AvfQbwJ@t1p56Du`=w;0 z!?NE`UO}l+e5Kay@TTC(s%g>=74OS6p{T@<%v@ZX((Y!Tqw`_&JerGOBi(d*N77nv zIUc;}pbirG@wNziy!ME@N&<{MK^dPu?M~1FeeT|>4a-T%8n@eaVRD~uQlJ_2dj|6>s={38xsoT+6v;~^OOHOIYLeLru&w*#E>T_`6 z7*l${6ld+K*OFe^Y52WRRG-}D-h}xPwYj5r;C+fFuj~E8miYTbUa@dvsn?UPyW5VX z+D^52y8umI(CL}q?=v5G_^L07!$h&DWCg8n4r|tMj;Lr`stbF|I^?wW^Wr+I7Pt)i zz^K|;#2v`#IOFbcIIa~#iVsfuprhUlYp}&Z#ttvan#@y9tQ*51Z%c>!{!^2dZx9+} zZr^8edva%Gp`!DEGSUdnqL->?@J%IHJ;yS{tm)@@C>R?YMBuv=Q392%!!#U1#7UE{ zB|_tnF18%AHcZX8V59=5VhebQ)k>DvQc%-O@^T1Pcjp;H4NAZq>3gtaw ztZ89Nl zd2IcHK{LDzo9!vny@S1L0N}-RyePeQy{tB>>~mf*2ANo$8mD|Rk;zS((<>a&W32d| z3XkhY+%V6f=XCnnt~p}(tJkGUd}OK|U92Oq9-f7af=WBvq45E(vhRyBR59Yf3jIg% zr$cJDKR=|_Xnfgzc2FQIjsJh0SF8T`0r8O=fBkY+K>CrLRf@BtPQRRW`T=eI|L%M* Zsb7lC%_RM0k-=F$w*8}6WJLDI{{_cN1Xln6 literal 14713 zcmdVB+uEvHk~Vn0uHsX1gNg_$h$7+!5D`TXMFbTDm5uDwuBtxh8|Xgk^S;(*uFR}j znJc^h%ES7e`KXK;j2J--V)Mp`vNUHQq}Bbc{D1#n|L6bnBfCkQeO8wJ!?}^0Ss&N= z$tixE`1XCd(dXy+`6L4v{ew*MPR{syzi51L=fKQN2_ zbpHCQ^V9wH>o35M< zzfbgk%pX{?vR`^d{;DR~edmk3x$mFf75PgezcibxQp)0+;(qeE;9ole-nsvtYm0wb z4|n?4@7Z6!`5!ZX_CK&3fASra-wy9GEBbxPU*o_1{7p+ze)Au+gyp1vdusnB%r6mh z{{Pd2!T+f+zeM~WC`|vK3iC_E|AE2~|5TV?BK{8)2Kn`7|M{i)CFB1@Y5wR&epv@G zc5-X3zTI^kzt4m&?m# zI{o~`9RJ0R{lx(M<<0raj{AK(%D)`TUzGE^!v3h5KYB*i3qQHsxks5OGoJ7F=5Frr ze|hgPSIl28@~KvC^!Pk;x7KO@nkpB;wO+q*)@M|oEe{vAdR>eRqg|u{P9fR?!j?78<$Gjf9advA+G9j zxoWgtbjq}#OK;Jgk6h(7=}T9;=4_i@X^qV!4Ls7=XFpe)8qupfJd=)?MsYa46j&+T zs5Q8iJgrp&NcqkSVs{+3{C~@E8H_K|wjq#DLYM=M4ZXGYAvRqSp zks0pXeOJ%?#9_{fFI|1Z$2JGdjVhH^JG_unjRM8)3<4($w?$F@pm`jgc(=W)92hMy zJ_iNgSEi%GUg&QHPu*AM2f@BPFGzAVOt?d|i~e`Lb59HcpUK6$ba7|&$(23l`m#>kJL)!5; zsMdrv$G<7{2ECdtquQf>LaS(fJ>0rdaW>}DuT*Ij=Zx8V4QkVcI0=S!2Sdv{hkFj` zmE)ni$wR!=qC(HJy9lwDS$|l&uFPxm9lk2Jj_{a`Uc)orVsH8oz4(iyu$nfXmN8Or zuQXwA90-(3k1GjyWO<5j5L=9!LQg`swGhJO0T5ZV0J~suRee1&emPjxh6=7!JY)`e z!8umC8CJ!t@gQ8DDkDAn;A?NbVGieJC6VTH7wd+OaV|Z-@%z20!*e}6>R7(<;@Ft? zdPjFvon6Qj^Xuvm6q4=|-hqot!C)X7H0083K=;#%RkXdrr(NP(kD4X=<3`p)rtbC- zqeYM0wY1eqyju)--mOT!0qFhOMjwM|t2aA&XLGKwROxk4U@|!k)emdU|R-W`pKEy_5=y($lMxpy_?vGjoms zg&??hOW61`ua`QEz#8XWk?wvX18nVQVp|<-q;&m~P^oew+?%={W|`RBL!SSCPm7$TUzZGeaTucIA8%Sw9bS}y8w>% z-yeyuCc(|irsG@4c&~t&?KBtF=L^016d15^!2653P6*;MF|4suYPaUyaj`?jPYg5W zi=st>l*swvg={`mwD6VBCyvj&3O8xF@;`%VM7lfoMo`bY*_o()MTtf0I zug|^Hk$Kwf39o-qvyLB|uR1ycH3uwGpY{_Uj<1X2Z2I6f1RDA`gbyzIIY_TCqwCJ8 z@SQ)}@3f;<40!wC%Oj4m?+1mMy@}@aK8felp@$vY-BK`EYVfeTXF5DWZWy*5fgd+< z7B@hR-dFzcT_i*LIRYUdf(;8!+o75 zufdZhYLz*ewmZ?M$42Pes@|b4KU!{_cV*gX<8-ByNiI4S!&KXSALS7!3FocYzHjLo z{#;H`e!Rx!1nZ3Sp&y0Fb-4g#VQgGXdBpVU??Gg(&r7KK2^%QsG}Jp=E4TlWF==tN)pvK)yya$8TKTOZ^C}QP z@tKOv!O=&n+i2LTo%li^CI-lzPUY6xdh|{MV`gEgr;qjd&=NVLn|Qjx^ULEi7R*=5 zOF{FbmR1%{CQr&bjZl=K4rgq8FJtB7@+j?+#fKORUi+mTTZOiM3r-cdF68lq*W5&W z3Esf9A{~^iLz{)t8G(Y$tG)~A%6t3a>7sPI59gQaV?peirOBd9H#vemQg$FLaQmnF!x6(`fM@|1(5gvlzEuH6Tf zxD+2;W`rBkOx<0Am$yuazBQ;;zt`1k3$#=2+MQ6l#Ug2z%Zm^PTf2Sf_+C1}J}p#P zP%T6uLo6=iO;icB;$FU57I{G9YOIAQyd2nz($_4kFZl%T(VAS|d{9SM+e(y{AJtfd zLg3my!&|=Aiip`eDmu_->Ue0(NlIucS2XCB>n8j;rF3cmjO4RNE$D@99q6MqhfC` zxNZXi)!R^uFUcGwBJ~{tNMo1a%UgJpHRQsi)qb>qYO~;6vM@l4$JucL&Rskl9~na? z))pD#H99QTLSZ<8`;@Tv;3ac2Q<*Wv%G~LSLpD<%WnJ2}EjYCsr+%k=Tr@tZSYP<> zrePn(P-7;NXa`g4?YCa2*H^A^gNZ^G1gp*HRSp1bU0z1j{|7DHS9l$`A= zt@U?kru*)h9+h7;U{Jo9TkygV+`w`_jnl3|+1%q`9AxCk4H9q=k@ST~YwL(TZr1>< zIMC@eZ+F^*^o3MkC`!#3xIqP^D>SP^ z3Lo`m;f*VvcSjbGA8~CxlAypf+UWk^UY|8|@7B-C;Zu_9-LuhYq0cfoz0V?&f4=kp zq?ROfS~j{d#WmqKdpoqm!o2>aI>X^}etF*IL^7d}W?)Y)Sl36edyVF;PFX|1>3m)} zL)OL{-U9<%RsUH`@JFQYUyTV60{Od`;BCdfVuF5Uu)fS}d$pK_c58&F>olbx*#;r} zN_M{m?Xw056mTyMVK^G43Gnzd_K(-IG5MIc={UMRZ_`SRE59L4EU??-(Ou}nA@-cD zb`|HA^^74&RoUEr6tP>i_B|nqjq=w`V05lE;mWTXqjI~#tQz%QMj#uUD%UEC&>4$w z-U?`PkYzZ=+XnooHnZ7{##Ov2G<(R*RliqrcPk0ZX&8F5cnyU01zp|ug;`HsPuwTP>$8`7tQYxuI-$Rn{G6!!2Z~l(}g;3xWH1~l`F8nsMUbUA;+sX z#n#|nZtA`hjV}GgwfC;wqPIXVYS;%oS7~m8Y%{(~AWM)72iJmQJQz4U6>j!>E4U5g zIMS|zNsI6rQb~cOT66h5nT~V6OfyfI^3ToW`hfujXf#n58(#FBd zp@Vr%u+RZF9@QI0<34Uj%~kWuSFg|6z5XTIaPXdM09gHS(pFta``O$ykv@tew=Fd| zL5kEt-Nhu6mPgH0g&V@X@J8_?^vYGIahd_y)uP>a6%8iM;5y^3hh_kE9C4sy3DWN- zk1YdNB51MtV0EdhVTZ%6YM@1pjHl;yO#rOYqv6V{eD%Y~K|}9&68jUN)IJT=qiu6$ zZK_{Nw?bqi&j{Nj{j?CyW)d3JA7g^-BhXMBTf?R%bh|l_Q@l#V^?e!oakEJvov?Ei z%wuhow1hmmm(lY66&xoJ;pA3yae;)jj?(e9!G8@8C!ZV5t`t{Dsv6q`9miD6f!`UU z43DF#J8Fo%uoL$S4;9|2=Z# zDmZ0xYfpkoZ*q4`!DmEgHN-#eq1kJsqI>Mmx_tgli1LmIm1h+irV9Dgn&@k?tFT+Y zyqnxxa_&v0%krM@>z4|R$elj)i3$cBwLl^x^>;1ph5gxW5;7Wgma7DE7bZB&UxD{| zQ{adZb6ok>M%wO8;m!#MM#wZ_J%(B$*sMiH1Kg>WuDH*EBaXhemXpN;O9@BdxI|aXYwH`f z1@QaNOyD2u1OKWC#4zITOyFnK$bm3J{Rpqj#mE6;)8&7@vyn31Q^lo1~`YyEIA;RX2Ki;K9^)M7rfPvJ?Dt+8G zi}!=@mUgr$uRg*7Jr#LB_09%-YN)gBr%5I^YPFBl_U08BrE|8zXK*p{O&)^CnMR0f z*JPcNaD%`aoLFcOz$D_ktMUg(f{0cG=&8KS#vR&-al|G%!m=p}#+PO=AnCMYpgMr;g-gjQW<+J$01KQ^ zJ~o~fJZ;Q^?j+w6z=|}=LPprahFmJ@d+8$fN^cX3?Svj++Bsn`pf<43$z}pEH5iAk zD+r}L66tJITNxz0FM1uh3FsjW$9c?8=&da92X} z7!E<$merDbf|~8s*aV;0KD>CBnp6_{_4Vs}j6F=JW;WI(qZLGN*V{rJ3QMoi!~^NR zu+R{S<>FpJjE|dco|MXNAeZ9PNRnvmk`VGQl*c@E_of5hStR|=WI2PLgd+{F zA4x>Bt=6SJ%6KcwWzXcm1-J`4Ue>zdw`?NelIRwL@8@tl4;u8oMXjxv^seGLv=3Qt zY+jrL_qa#JCs@*)O0YdF`)?3gGPOaN+aP04t%^N3Cb+($AHFjXwdEEY-6eqPyzpKn zdJ0p)19tOM>Q~+kFo|u`5fgk56orJ6T6Gyf=>x&%p^m&gIg_O`9%*_p#(CGg_{egT z@%QhiL4J96blUWjg*vHPaX$6BtgyhdlGZ;Kp=WE99nD%^P&S3PSL_O_%d@l=hdN#r z$o9h&?S6f?ps$lp4pOQw%H6*bYZ%T-(*}k`|x3MYyh0r_R7{ItZgVj|ta`*aIM8&UAiuXZpn-Yl zxGEm*kGSc9LyZ^N!)F_mQLcP zx~Xfa6VMdBS2!-8~ZbvF%p8F z*-@dD+57N*S&Nw2ANXv$u-!|DI3IEcUJ56y17iZ)st=CWG#+uI$Ei6P;rF1DBj;gIyj#51JYmSaC2b<(>KhrzXm>30Nnb{8GNNq+bPtT@$Ruj|DBa|^)*|ioU zb375|=kf+#_VPs0FCKOSdUb!$wFt%>Q3Mww3Wdy94#iKQelI}#4u5;L3_J=#uBQ7w zczFti@3jRq&ZKOR&N5il2H+W=&D0f}8r%yie`W6-tztGT1uctkj;~p9gU#+d(}9HR zD$erN?wp53L*v_u~$E@_>Ts{N4)S>ayW! z-K=*&AeB8& z+G-sy@d+T6(CJ}9U*A^u?n-r#+Kv>y1?|n(mHklhd=+z-9AuhiS?jxhw}2s)y6WmV zS(W2%2li*Sb^?s@TIai3V~dv1Q>e4;@AD%*xAz5G+SZ0+Ol0Co=P%!+RNQd%3KT`yt@hvx!EkGLVU7mZ(^c#f9_`ixR2sBwcU2; za_2OC0`TqfID1*Tx%W!-gLf0y#kD9?ohT0^CqZk1_C83H(X9wLLU{p%si=|uxY!L> zT&z{(0oRRigWVMwZ{Ik7oFd(ApG|?iHN{-D6u}5^-0Qu?&K`s1ihVwj5i}oej3L%M z!nHc@Ua!i?zP)zW@@%yC!H4q%^%h@V&K5*5%T(n?P78JES-k@-yZ}VwV_vbMn9cJ* z_>lKGk5=74|1Nt){(Le=a{Y##iEMD)X%yZ#$a!wm3xw0U>leolyYVgdB5cB_-$yBG zz3-g6-nW==-*vZ-zGzol$R(>i?y<9-TDQfhB)>-LLGUZp$#tWR~zHw=H-)JrK%3OIVVC`0t&VIB*_+I#~dfwy|DL+E&# zEq!6~P|u3H2&rb-P|O%jpllG26_D@YgDcx=U4 zH5#c@%Ws^+$+@@a_rK*3hRge2Ho__d;SReMe|AjblzL>P`Q^~e4pXv{zhou$`b?g^ z@lI9uvvP-Zq&M$1Um+6+y6O~Sk>T?^wEy~;`=U{$+50?WwAF#?mE5s=C|~tp(iB+* zN)82idg(^}X|giUd9C&}4j@gkPMfT3ru^=%1 zbD(dxzV(o#m!%>QO;0)2F{rct`uh2ran#s?P!lnIE{TWd_K{WXH!n9wvcC4A9B+@Gt>fz>(o`oj0S2t)NiyfN`P;HfwN-F`zpCT;YO#{{ z+j(s9vR3D7&b=qScCU1mMr&MaD~=(Jdcu=k2D&Cww>ehWYK`xM5*| z)GkMxTM8$w{oXO>O1J0}*GrI>**o>w3}0xaieD-AuW7IWR^WQW-q;*cRZ1QgtC3f| zXzl|-%W+UPS~_*9)ZE0CAbv9Jf{HkNjC7$3t2&+6`{3G`*f$o%SM$nzmkaBiE<$3S zl*@*$+x@jFdXtQ~ComV@R@a3;YbFJw+sm;$ja}CE-HNBp0lak^SE|*4Xf@mW)vI#o z4bgVAE8W>@I^L|V_;_%fI%~aIiT(3rd@jJ}qFLY~W+~Ttnnu-6IxE-ES~92)I}pDu4_|;2+G%TCTGhzJoF5@yJwSF|C2EJq z@~RDglncXxJiZ7U3!yA-T^}tY6U+OO_T?f*l?=wI-Et?IpBIVFO69iX6W$|~A5iby zuG#VG3}F5Xn-ZIEHp3DhUUK#KikbAvO?IisAz_Ql`>j$w6UI5Io3lDhqfEi>)#2}= z(JD6Tgk4V4mp)=RfSvZE^>h(rQi%E39w_nG6!_Tq=-QoOZ7d5Npi7dcW7&MauEVPQ zP%B$A(Cb5PSFSFZ({v|EUc2(CfN+P-Q|poqme4**4o97LUV~V!kE*6vPP& z{nmAS!6I0|!q)w}OR=`ytq#fUD}vPGHc`IH0XHwhR?m5U@|x~#a#Hg{wda#Cvc`b@ zps$}3O1{kW#KPrml<{<@SuVbVv?7rmmDT6fsj+|5yeq%XT5sjD<~l1bqJ$O2y+d|C ze7C!}*RlHMy^YRiin*4jxh--JeWg#0^36u1>q~TJwqNd3DkG59zbV>e-JNrqQh&w0 zB35_$vN$*%qOs^7_p?Ht-crTXJwr8FP=tg%JWB6*YhGC$3-b!8_z9uc&xR_rU)$0u zV6Wgkgc@*()*A6;Im{+zIIdIzzvnB9L!r6_L|60Q?EF&-hg78~R9bc>EkvuRM= zQOfWdAM}cOSmgSsz0%u#j+Ayn=iN58YMBC&-9>NgXnOv@Yw?39Qb}HSn8}a>(v3RY zJ61Xeld3nk^tkT0T&abDAL&x6B6g9K-qx!eF9bXs>(POtb_@ zO~UGT(qJ3y7BA16r9NgutS06uS)eo_dOWIei>*DCO0sKVk0@;5LI6Am)s~Oe2TBcI zL~qf-i|8|jKINLZKMzjbp8F_?)XAG(|5-r($8(SWYCxV>Vg4R*ifZ%Rh2ASEZ{2j3 znD$TyquE$_P%!^Ig=9s3Y*-PzQ_l{Ttc57(4h8_NQiJQh=H@{F_S{wE#S;nD_{-_m zgZEjB5TRZObe!rlan+|86`G4{0Y*QrPQT$3bgcUFYuE1ByBz18;ysx+!qe*(DBUL! zCJ$cQ+f_=TMe)7GkU|HlxoU8J>=*JjQVkH2=|CK4eJwu+qjbpLqm^U*3;Hb2Qu6dT zDBa73Shpo30)f^g-62DVz;6OpYJ~A+YM6CK(l2YPy)keF{$Nw_&L@zaG><*l>FFv(0TCV~6A?TK1C# zCXz|R?}zgSk7_NcNq{*X!9{28<&-OW^ln_WX`8mho$MB9G85B9*QKs>>qUG^(DD}9 z-Gln{U)|5kZLEg!m~4B4@|q%ATz=Y#GL-XSzW|VGP@`v`%52ZaezW;_F%b2}o;`mWiHAEGhY|(Sj;sO9d|8U{=CTrXgX^=d=`^ zmWc==Vr>eoZgd%U!x|HdlM(cgo@)^)+%Jq28?zp%4ElPMb*2uuX5Gurw6@AoqccnDQxdf_1hd6gBSP` z3-IdYR+Km2x_A8zU}y_X;Qj0E?w+~JHnF1TC8eRaUAz;`aedC^CwkHF6EO}wF9DKT zR&p$7Fjva5ou5a#zT`k?lbaK3i>5o;4X46-`JE^Aq{n|AdGJlk-eO2}u1Ds6q`on{ z7d&Ct?1}KyfB@}5UQ-RmuBf!}g>rvs@AcA5HJQ2FoG(gTOGnCh=$qHJMUr|q)1Osx zaa#)-=LJj+^;S1=u4V8yfSQx9`K-$|dWrPg06pibYd?FGHc-Y{tNu+-iV$<|-%f?c zWank`(xz2yEnn@WS1x^czA^2CVCz-xV~I!0s5cQ|(fH!uV!w*;%;!`bUXuDvxqlM( zt6%T}*|C7C5rvf%2W*yB5d@!UBdI(O2esi$$-@Kw(uksUU!fyy$U_}gzlR&n-;ya( zp2wm5xp~!oPd>4>x!yA9N9m5$b_8*PB|vKZWJ4~x0u{Y{3K%$^D(`Va)0y1hYmdSDAn z)u}OZT9H{ZfG}kxspPi&@#uw38rjVw32nPsi2RdL%mPcGRoBTn+OGF>+E;42G^uS` zD%!<_y!&H*WWFP~T`3|Xh+o|~Bz20J0o=9qaQe0`TW60}$0S#gzvrOYf4=gM&jDWz zDo!^26(#&ld1ZgT#r0B9c-+BmQ&|-R++nrTQA9YTdmB`HY?V*@>?iPJ>>DKE5UZE- zgG|KgCE4qd*KwLMmn`2Mqyo!bcmqs3o8ds23&Ci<>UMx7h9~@Cw>iyD9%@`E^r+7L zhvKZ-TVXfd=;={qoI2$VS|DcExlVau^vY;5G={I~;@a%h*ShL2$6|2V+Rv(+cEoGD z4+Ik7+r5z#jFNP8fI_8BWN>#FMRA)(=IABND&OOG{(|_?IO1NQkd>}m(OBqGanPvJ zO}Bnv24;{SZXGdGSM9_Zb|esXbYJ4ds3O z0>0LHC!XUQox<39H`8WS;|Vp2ux!PbKK}G&=wx5mk%Rxl9$^cb8tyYUj@LIA)~L zz33`JSL?rP=T~_;yeQzek=M8zPbjGGUJ{|#7AN+Vg%PcewTZcJsO^&loo=%_#u5}r zNz3ATn+o8#N}s26QLN>~Jtw^wk3&s&^up#o)hdc@+5fd=b3+r!lmR{do+*@?Mj_zqf<+^InU;AM4@0Khfds&Y#xY z$BBP@eh7m1^5dd^n=fxw@Ip8Hy{C|$|NM3$9m2s~w0pB$q+{QRdz(OWbUt6iz1K(F zHctJoDa_x>e`s@j{PQ+P2>NlnywxOcuK54zcm#j@4d$bJ{PTOaiTqpM?H1?Fa=&-W i{RS)E|L*?UQopw=SlQ?IO@?&a{myUYGP7ZS`+orhv!7r9 From 2c01b57f643c7598abd9e48a2449e063b2435b48 Mon Sep 17 00:00:00 2001 From: Sebastian Loh Lindholm Date: Mon, 11 May 2026 12:45:00 +0200 Subject: [PATCH 22/33] Build for 20, 22 and 24 --- README.md | 2 +- atos/CMakeLists.txt | 2 +- atos/common/module.hpp | 130 +-- atos/common/roschannels/roschannel.hpp | 33 +- atos/common/util.c | 20 +- .../DirectControl/src/directcontrol.cpp | 188 +++-- .../EsminiAdapter/src/esminiadapter.cpp | 360 ++++---- .../src/scenarioexecution.cpp | 78 +- atos/modules/MQTTBridge/src/mqttbridge.cpp | 789 +++++++++--------- .../ObjectControl/inc/relativeanchor.hpp | 6 +- .../ObjectControl/inc/relativetestobject.hpp | 22 +- atos/modules/ObjectControl/inc/testobject.hpp | 2 +- .../ObjectControl/src/objectcontrol.cpp | 85 +- .../ObjectControl/src/objectlistener.cpp | 18 +- .../inc/trajectoryletstreamer.hpp | 4 +- docs/Installation/installation.md | 2 +- scripts/installation/install_atos.sh | 25 + scripts/installation/install_functions.sh | 56 +- setup_atos.sh | 38 +- 19 files changed, 1025 insertions(+), 835 deletions(-) diff --git a/README.md b/README.md index b8c478f54..3c57d25c8 100644 --- a/README.md +++ b/README.md @@ -38,7 +38,7 @@ You might wish to mount the config directory at ~/.astazero/ATOS/ to a different ##
    Using the installation script -ATOS comes with an installation script that automates the installation process. It is intended for use on Ubuntu 22.04. The script will install ROS2 Humble, ATOS dependencies, and ATOS itself. It will also create a workspace (~/atos_ws) and build ATOS. The script can be executed using the following command: +ATOS comes with an installation script that automates the installation process. Native installation is supported on Ubuntu 20.04 with ROS 2 Foxy, Ubuntu 22.04 with ROS 2 Humble, and Ubuntu 24.04 with ROS 2 Jazzy. The script will install ROS 2, ATOS dependencies, and ATOS itself. It will also create a workspace (~/atos_ws) and build ATOS. The script can be executed using the following command: ```bash ./setup_atos.sh ``` diff --git a/atos/CMakeLists.txt b/atos/CMakeLists.txt index d9a311d25..017133a73 100644 --- a/atos/CMakeLists.txt +++ b/atos/CMakeLists.txt @@ -16,7 +16,7 @@ set(CMAKE_C_COMPILER "clang") #Set preprocessor macros depending on used ros version if ($ENV{ROS_DISTRO} STREQUAL "foxy") add_definitions(-DROS_FOXY) -elseif($ENV{ROS_DISTRO} STREQUAL "humble") +elseif($ENV{ROS_DISTRO} STREQUAL "humble" OR $ENV{ROS_DISTRO} STREQUAL "jazzy") add_definitions(-DROS_HUMBLE) endif() diff --git a/atos/common/module.hpp b/atos/common/module.hpp index 840d964f1..ff62b2e27 100644 --- a/atos/common/module.hpp +++ b/atos/common/module.hpp @@ -8,27 +8,27 @@ #include #include -#include "roschannels/commandchannels.hpp" #include +#include "roschannels/commandchannels.hpp" namespace ServiceNames { -const std::string initDataDict = "init_data_dictionary"; -const std::string getObjectIds = "get_object_ids"; -const std::string getObjectIp = "get_object_ip"; -const std::string getTestOrigin = "get_test_origin"; -const std::string getObjectTrajectory = "get_object_trajectory"; -const std::string getObjectTriggerStart = "get_object_trigger_start"; -const std::string getObjectControlState = "get_object_control_state"; +const std::string initDataDict = "init_data_dictionary"; +const std::string getObjectIds = "get_object_ids"; +const std::string getObjectIp = "get_object_ip"; +const std::string getTestOrigin = "get_test_origin"; +const std::string getObjectTrajectory = "get_object_trajectory"; +const std::string getObjectTriggerStart = "get_object_trigger_start"; +const std::string getObjectControlState = "get_object_control_state"; const std::string getObjectReturnTrajectory = "get_object_return_trajectory"; -const std::string getOpenScenarioFilePath = "get_open_scenario_file_path"; -} // namespace ServiceNames +const std::string getOpenScenarioFilePath = "get_open_scenario_file_path"; +} // TODO move somewhere else? also make generic to allow more args (variadic template)? /*! * \brief Facilitates one-line intialization * of a ros message with one argument. */ -template +template Msg_T msgCtr1(MsgData_T data) { Msg_T ret; ret.data = data; @@ -43,51 +43,51 @@ Msg_T msgCtr1(MsgData_T data) { * \param name name of the module. Is passed to the ROS node. */ class Module : public rclcpp::Node { -public: - Module(const std::string name) : - Node(name), - getStatusResponsePub(*this) {}; - Module() = default; + public: + Module(const std::string name) : Node(name), getStatusResponsePub(*this) {}; + Module() = delete; bool shouldExit(); -protected: - bool quit = false; + protected: + bool quit=false; ROSChannels::GetStatusResponse::Pub getStatusResponsePub; - virtual void onFailureMessage(const ROSChannels::Failure::message_type::SharedPtr) {}; - virtual void onGetStatusResponse(const ROSChannels::GetStatusResponse::message_type::SharedPtr) {}; + virtual void onFailureMessage(const ROSChannels::Failure::message_type::SharedPtr){}; + virtual void onGetStatusResponse(const ROSChannels::GetStatusResponse::message_type::SharedPtr){}; virtual void onGetStatusMessage(const ROSChannels::GetStatus::message_type::SharedPtr) { auto msg = std_msgs::msg::String(); msg.data = this->get_name(); getStatusResponsePub.publish(msg); }; - virtual void onInitMessage(const ROSChannels::Init::message_type::SharedPtr) {}; - virtual void onConnectMessage(const ROSChannels::Connect::message_type::SharedPtr) {}; - virtual void onDisconnectMessage(const ROSChannels::Disconnect::message_type::SharedPtr) {}; - virtual void onArmMessage(const ROSChannels::Arm::message_type::SharedPtr) {}; - virtual void onDisarmMessage(const ROSChannels::Disarm::message_type::SharedPtr) {}; - virtual void onObjectsConnectedMessage(const ROSChannels::ObjectsConnected::message_type::SharedPtr) {}; - virtual void onAllClearMessage(const ROSChannels::AllClear::message_type::SharedPtr) {}; - virtual void onStartMessage(const ROSChannels::Start::message_type::SharedPtr) {}; - virtual void onStartObjectMessage(const ROSChannels::StartObject::message_type::SharedPtr) {}; - virtual void onStopMessage(const ROSChannels::Stop::message_type::SharedPtr) {}; - virtual void onAbortMessage(const ROSChannels::Abort::message_type::SharedPtr) {}; - virtual void onReplayMessage(const ROSChannels::Replay::message_type::SharedPtr) {}; + virtual void onInitMessage(const ROSChannels::Init::message_type::SharedPtr){}; + virtual void onConnectMessage(const ROSChannels::Connect::message_type::SharedPtr){}; + virtual void onDisconnectMessage(const ROSChannels::Disconnect::message_type::SharedPtr){}; + virtual void onArmMessage(const ROSChannels::Arm::message_type::SharedPtr){}; + virtual void onDisarmMessage(const ROSChannels::Disarm::message_type::SharedPtr){}; + virtual void onObjectsConnectedMessage(const ROSChannels::ObjectsConnected::message_type::SharedPtr){}; + virtual void onAllClearMessage(const ROSChannels::AllClear::message_type::SharedPtr){}; + virtual void onStartMessage(const ROSChannels::Start::message_type::SharedPtr){}; + virtual void onStartObjectMessage(const ROSChannels::StartObject::message_type::SharedPtr){}; + virtual void onStopMessage(const ROSChannels::Stop::message_type::SharedPtr){}; + virtual void onAbortMessage(const ROSChannels::Abort::message_type::SharedPtr){}; + virtual void onReplayMessage(const ROSChannels::Replay::message_type::SharedPtr){}; virtual void onExitMessage(const ROSChannels::Exit::message_type::SharedPtr); - virtual void onResetTestObjectsMessage(const ROSChannels::ResetTestObjects::message_type::SharedPtr) {}; - virtual void onReloadObjectSettingsMessage(const ROSChannels::ReloadObjectSettings::message_type::SharedPtr) {}; + virtual void onResetTestObjectsMessage(const ROSChannels::ResetTestObjects::message_type::SharedPtr){}; + virtual void onReloadObjectSettingsMessage(const ROSChannels::ReloadObjectSettings::message_type::SharedPtr){}; + static void tryHandleMessage(std::function tryExecute, std::function executeIfFail, const std::string& topic, const rclcpp::Logger& logger); + /** * @brief This helper function performs a service call given a client and yields a response. This * function is used when you want to specify the request instead of sending an empty request. - * + * * @tparam Srv Srv The name of the service to request. * @param timeout The timeout for the service call. * @param client The client to use to request the service. @@ -95,11 +95,12 @@ class Module : public rclcpp::Node { * @param request The request of the service, with the data to be sent. Defaults to an empty request. * @return The response of the service. */ - template - bool callService(const std::chrono::duration& timeout, - std::shared_ptr>& client, - std::shared_ptr& response, - std::shared_ptr request = std::make_shared()) { + template + bool callService( const std::chrono::duration< double > &timeout, + std::shared_ptr> &client, + std::shared_ptr &response, + std::shared_ptr request = std::make_shared()) +{ auto promise = client->async_send_request(request); if (rclcpp::spin_until_future_complete(get_node_base_interface(), promise, timeout) == rclcpp::FutureReturnCode::SUCCESS) { @@ -112,16 +113,17 @@ class Module : public rclcpp::Node { } /*! \brief This helper function waits for a service to become available and returns a client. - * \tparam Srv The name of the service to request. - * \param n The number of times to retry. - * \param timeout The timeout to wait for the service. - * \param serviceName The name of the service to request. - * \return The response of the service. - */ - template + * \tparam Srv The name of the service to request. + * \param n The number of times to retry. + * \param timeout The timeout to wait for the service. + * \param serviceName The name of the service to request. + * \return The response of the service. + */ + template typename std::shared_ptr> nTimesWaitForService(int n, - const std::chrono::duration& timeout, - const std::string& serviceName) { + const std::chrono::duration< double > &timeout, + const std::string &serviceName) +{ int retries = 0; auto client = create_client(serviceName); @@ -138,23 +140,25 @@ class Module : public rclcpp::Node { throw std::runtime_error("Failed to initialize service " + serviceName); } + /*! \brief This helper function is used by rosnodes to request a service. - * \tparam Srv The name of the service to request. - * \param n Number of times to retry until accepting the service is not available. - * \param timeout The timeout to wait for the service. - * \param serviceName The name of the service to request. - * \param response Response of the service, to be returned. - * \return The response of the service. - */ - template - bool nShotServiceRequest(int n, - const std::chrono::duration& timeout, - const std::string& serviceName, - std::shared_ptr& response) { + * \tparam Srv The name of the service to request. + * \param n Number of times to retry until accepting the service is not available. + * \param timeout The timeout to wait for the service. + * \param serviceName The name of the service to request. + * \param response Response of the service, to be returned. + * \return The response of the service. + */ + template + bool nShotServiceRequest(int n, + const std::chrono::duration< double > &timeout, + const std::string &serviceName, + std::shared_ptr &response) { std::shared_ptr> client; - try { + try{ client = nTimesWaitForService(n, timeout, serviceName); - } catch (std::runtime_error& e) { + } + catch (std::runtime_error &e){ RCLCPP_ERROR(get_logger(), "Failed to initialize service %s", serviceName.c_str()); return false; } diff --git a/atos/common/roschannels/roschannel.hpp b/atos/common/roschannels/roschannel.hpp index 9e2b3f312..8e608f4aa 100644 --- a/atos/common/roschannels/roschannel.hpp +++ b/atos/common/roschannels/roschannel.hpp @@ -5,35 +5,36 @@ */ #pragma once -#include #include #include +#include namespace ROSChannels { template class BasePub { public: - BasePub(rclcpp::Node& node, const std::string& topicName, const rclcpp::QoS& qos = rclcpp::QoS(rclcpp::KeepAll())) : - pub(node.create_publisher(topicName, qos)) {} - BasePub() = delete; - typename rclcpp::Publisher::SharedPtr pub; - inline virtual void publish(const T& msg) { - assert(pub); - pub->publish(msg); - }; + BasePub(rclcpp::Node& node, + const std::string& topicName, + const rclcpp::QoS& qos = rclcpp::QoS(rclcpp::KeepAll())) + : pub(node.create_publisher(topicName, qos)) {} + BasePub() = delete; + virtual ~BasePub() = default; + typename rclcpp::Publisher::SharedPtr pub; + inline virtual void publish(const T& msg) { assert(pub); pub->publish(msg); }; }; template class BaseSub { public: - BaseSub(rclcpp::Node& node, - const std::string& topicName, - std::function callback, - const rclcpp::QoS& qos = rclcpp::QoS(rclcpp::KeepAll())) : - sub(node.create_subscription(topicName, qos, callback)) {} - BaseSub() = delete; - typename rclcpp::Subscription::SharedPtr sub; + BaseSub(rclcpp::Node& node, + const std::string& topicName, + std::function callback, + const rclcpp::QoS& qos = rclcpp::QoS(rclcpp::KeepAll())) + : sub(node.create_subscription(topicName, qos, callback)) {} + BaseSub() = delete; + virtual ~BaseSub() = default; + typename rclcpp::Subscription::SharedPtr sub; }; } // namespace ROSChannels diff --git a/atos/common/util.c b/atos/common/util.c index 761ce8f6e..8d1672350 100644 --- a/atos/common/util.c +++ b/atos/common/util.c @@ -251,8 +251,7 @@ static int recursiveMkdir(const char *dir, int mode) { *p = '/'; } res = mkdir(tmp, mode); - return res; -} + } /*! * \brief deleteFile Deletes the file given in the parameter ::path @@ -1070,7 +1069,7 @@ int UtilSetSyncPoint(ObjectPosition * OP, double x, double y, double z, double T } } - return OP->SyncIndex; + } float UtilCalculateTimeToSync(ObjectPosition * OP) { @@ -1222,7 +1221,6 @@ int UtilSortSpaceTimeAscending(ObjectPosition * OP) { } } } - return 0; } @@ -1237,7 +1235,7 @@ int UtilFindCurrentTrajectoryPosition(ObjectPosition * OP, int StartIndex, doubl //OP->BestFoundTrajectoryIndex = 0; if (debug) fprintf(stderr, "UtilFindCurrentTrajectoryPosition: StartIndex=%d, CurrentTime=%4.3f, MaxTrajDiff=%4.3f, MaxTimeDiff=%4.3f\n", - StartIndex, CurrentTime, MaxTrajDiff, MaxTimeDiff); + OP->OrigoDistance, OP->x, OP->y, OP->SyncIndex); Init = 1; while (i < (OP->TrajectoryPositionCount - 1) && i <= OP->SyncIndex) { @@ -1379,8 +1377,8 @@ int UtilFindCurrentTrajectoryPositionNew(ObjectPosition * OP, int StartIndex, do PositionFound = i; //SampledSpaceIndex[j] = i; //j++ ; - if (debug == 2) - fprintf(stderr, "Minimum: %d, %3.6f, %3.6f", i, AngleDiff, RDiff); + if (debug == 2) + fprintf(stderr, "Minimum: %d, %3.6f, %3.6f", i, AngleDiff, RDiff); PrevAngleDiff = AngleDiff; } @@ -1755,10 +1753,10 @@ int UtilGetRowInFile(const char *path, const size_t pathLength, fprintf(stderr, "Buffer to small for read row in file\n"); return -1; } - if(rowIndex == i){ - *(rowBuffer + length) = '\0'; - return i; - } + if(rowIndex == i){ + *(rowBuffer+length) = '\0'; + return i; + } } } fclose(fd); diff --git a/atos/modules/DirectControl/src/directcontrol.cpp b/atos/modules/DirectControl/src/directcontrol.cpp index 388955162..52f15125f 100644 --- a/atos/modules/DirectControl/src/directcontrol.cpp +++ b/atos/modules/DirectControl/src/directcontrol.cpp @@ -3,12 +3,12 @@ * License, v. 2.0. If a copy of the MPL was not distributed with this * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ +#include #include "directcontrol.hpp" +#include "util.h" +#include "tcphandler.hpp" #include "atosTime.h" #include "atos_interfaces/msg/control_signal_percentage.hpp" -#include "tcphandler.hpp" -#include "util.h" -#include using atos_interfaces::msg::ControlSignalPercentage; using namespace ROSChannels; @@ -19,41 +19,39 @@ using namespace ROSChannels; * @brief Driver Model message, used in esmini to send * control signals to a vehicle from a driver model. */ -class DmMsg { +class DmMsg{ public: - DmMsg() {}; + DmMsg(){}; - ControlSignalPercentage toATOSMsg() { + ControlSignalPercentage toATOSMsg(){ ControlSignalPercentage cspmsg = ControlSignalPercentage(); - cspmsg.atos_header.object_id = this->objectId; - // Convert throttle brake and steering angle to integers between 0,100 and -100,100 respectively. - cspmsg.throttle = round(this->throttle * 100); - cspmsg.brake = round(this->brake * 100); + cspmsg.atos_header.object_id = this->objectId; + // Convert throttle brake and steering angle to integers between 0,100 and -100,100 respectively. + cspmsg.throttle = round(this->throttle * 100); + cspmsg.brake = round(this->brake * 100); cspmsg.steering_angle = round(this->steeringAngle * 100); return cspmsg; } /*! - * \brief Decodes a DM message from esmini compatible sender. - * if the message is not of type inputMode=1 (DRIVER_INPUT) - * the function returns - * - * \param bytes vector of (char) bytes - * \param DMMsg the data structure to be populated with contents of the message - * \return number of bytes on successfully parsed message, -1 otherwise - */ - int parseFromBytes(const std::vector& bytes) { - int idx = 0; - decodeNextXBytes(4, this->version, idx, bytes); - decodeNextXBytes(4, this->inputMode, idx, bytes); - if (this->inputMode != 1) { - return 0; - } - decodeNextXBytes(4, this->objectId, idx, bytes); - decodeNextXBytes(4, this->frameNumber, idx, bytes); - decodeNextXBytes(8, this->throttle, idx, bytes); - decodeNextXBytes(8, this->brake, idx, bytes); - decodeNextXBytes(8, this->steeringAngle, idx, bytes); + * \brief Decodes a DM message from esmini compatible sender. + * if the message is not of type inputMode=1 (DRIVER_INPUT) + * the function returns + * + * \param bytes vector of (char) bytes + * \param DMMsg the data structure to be populated with contents of the message + * \return number of bytes on successfully parsed message, -1 otherwise + */ + int parseFromBytes(const std::vector& bytes){ + int idx=0; + decodeNextXBytes(4,this->version,idx,bytes); + decodeNextXBytes(4,this->inputMode,idx,bytes); + if (this->inputMode != 1) { return 0;} + decodeNextXBytes(4,this->objectId,idx,bytes); + decodeNextXBytes(4,this->frameNumber,idx,bytes); + decodeNextXBytes(8,this->throttle,idx,bytes); + decodeNextXBytes(8,this->brake,idx,bytes); + decodeNextXBytes(8,this->steeringAngle,idx,bytes); return idx; } @@ -62,31 +60,34 @@ class DmMsg { unsigned int inputMode; unsigned int objectId; unsigned int frameNumber; - double throttle; // range [0, 1] - double brake; // range [0, 1] - double steeringAngle; // range [-pi/2, pi/2] + double throttle; // range [0, 1] + double brake; // range [0, 1] + double steeringAngle; // range [-pi/2, pi/2] /*! - * \brief transfers x bytes, on the interval from idx to idx+x, - * from a vector of bytes into a variable - * - * \param x number of bytes to transfer - * \param field variable to receive bytes - * \param idx starting byte - * \param bytes vector of (char) bytes - */ + * \brief transfers x bytes, on the interval from idx to idx+x, + * from a vector of bytes into a variable + * + * \param x number of bytes to transfer + * \param field variable to receive bytes + * \param idx starting byte + * \param bytes vector of (char) bytes + */ template - void decodeNextXBytes(int x, T& field, int& idx, const std::vector& bytes) { + void decodeNextXBytes(int x, T& field, int& idx, const std::vector& bytes){ std::memcpy(&field, &(bytes[idx]), sizeof(T)); - if (sizeof(T) == 2) { + if (sizeof(T) == 2){ le16toh(field); - } else if (sizeof(T) == 4) { + } + else if (sizeof(T) == 4){ le32toh(field); - } else if (sizeof(T) == 8) { - le64toh(field); } - idx += x; + else if (sizeof(T) == 8){ + le64toh(field); + } + idx+=x; } + }; //! Message queue callbacks @@ -98,84 +99,86 @@ void DirectControl::onAllClearMessage(const AllClear::message_type::SharedPtr) { //! Class methods DirectControl::DirectControl() : - Module(moduleName), - controlSignalPub(*this), - tcpHandler(TCPPort, "", "off", 1, O_NONBLOCK), - udpServer("0.0.0.0", UDPPort) {} + Module(moduleName), + controlSignalPub(*this), + tcpHandler(TCPPort, "", "off", 1, O_NONBLOCK), + udpServer("0.0.0.0",UDPPort) {} void DirectControl::startThreads() { - receiveThread = std::make_unique(&DirectControl::readTCPSocketData, this); - receiveThreadUDP = std::make_unique(&DirectControl::readUDPSocketData, this); + receiveThread=std::make_unique(&DirectControl::readTCPSocketData, this); + receiveThreadUDP=std::make_unique(&DirectControl::readUDPSocketData, this); } -void DirectControl::joinThreads() { - // Tear down connections - if (this->tcpHandler.isConnected()) - this->tcpHandler.TCPHandlerclose(); +void DirectControl::joinThreads(){ + //Tear down connections + if(this->tcpHandler.isConnected()) this->tcpHandler.TCPHandlerclose(); this->udpServer.close(); - // Join threads + //Join threads receiveThread->join(); receiveThreadUDP->join(); } + /*! * \brief Listens for UDP data and sends a control signal on ros topic when recevied */ void DirectControl::readUDPSocketData() { - RCLCPP_INFO(get_logger(), "Listening on UDP port %d", UDPPort); - - while (!this->quit) { - try { + RCLCPP_INFO(get_logger(),"Listening on UDP port %d",UDPPort); + + while (!this->quit){ + try{ auto [data, remote] = udpServer.recvfrom(); DmMsg dmMsg; auto bytesParsed = dmMsg.parseFromBytes(data); - // TODO: add re-ordering with frame numbers? - // Have to think abt if/what kind of re-ordering makes sense for + // TODO: add re-ordering with frame numbers? + // Have to think abt if/what kind of re-ordering makes sense for // the application (driver model). - if (bytesParsed != -1) { + if (bytesParsed != -1){ ControlSignalPercentage cspmsg = dmMsg.toATOSMsg(); controlSignalPub.publish(cspmsg); } - } catch (const SocketErrors::DisconnectedError& error) { - // If we get a disconnected exception when we do not intend to exit, propagate said exception - if (!this->quit) { + } + catch(const SocketErrors::DisconnectedError& error){ + //If we get a disconnected exception when we do not intend to exit, propagate said exception + if (!this->quit){ throw error; } } } } + void DirectControl::readTCPSocketData() { std::vector data(TCP_BUFFER_SIZE); int recvData = 0; - RCLCPP_INFO(get_logger(), "Awaiting TCP connection..."); + RCLCPP_INFO(get_logger(),"Awaiting TCP connection..."); this->tcpHandler.CreateServer(); while (!this->quit) { // TODO set up TCP connection (wait until connected before continue) this->tcpHandler.TCPHandlerAccept(1000); - if (this->tcpHandler.isConnected()) { - RCLCPP_INFO(get_logger(), "Connected"); - + if (this->tcpHandler.isConnected()){ + RCLCPP_INFO(get_logger(),"Connected"); + while (!this->quit && tcpHandler.isConnected()) { data.resize(TCP_BUFFER_SIZE); std::fill(data.begin(), data.end(), 0); recvData = tcpHandler.receiveTCP(data, 0); if (recvData == TCPHandler::FAILURE) { this->tcpHandler.TCPHandlerclose(); - RCLCPP_INFO(get_logger(), "TCP connection closed unexpectedly..."); - RCLCPP_INFO(get_logger(), "Awaiting new TCP connection..."); - ; + RCLCPP_INFO(get_logger(),"TCP connection closed unexpectedly..."); + RCLCPP_INFO(get_logger(),"Awaiting new TCP connection...");; this->tcpHandler.CreateServer(); break; - } else if (recvData > 0) { + } + else if (recvData > 0) { try { this->handleISOMessage(data, static_cast(recvData)); } catch (std::invalid_argument& e) { - RCLCPP_ERROR(get_logger(), e.what()); + RCLCPP_ERROR(get_logger(), "%s", e.what()); std::fill(data.begin(), data.end(), 0); } } @@ -184,40 +187,45 @@ void DirectControl::readTCPSocketData() { } } -void DirectControl::handleISOMessage(std::vector& byteData, size_t receivedBytes) { +void DirectControl::handleISOMessage( + std::vector& byteData, + size_t receivedBytes) { while (receivedBytes > 0) { ISOMessageID recvMessage = getISOMessageType(byteData.data(), byteData.size(), false); // TODO check for RDCI (optional) // TODO if RDCI, respond with DCTI (optional) switch (recvMessage) { - case MESSAGE_ID_INVALID: - throw std::invalid_argument("Received invalid ISO message"); - case MESSAGE_ID_VENDOR_SPECIFIC_ASTAZERO_RDCA: - receivedBytes -= this->handleRDCAMessage(byteData); - break; - default: - throw std::invalid_argument("Received unhandled ISO message"); + case MESSAGE_ID_INVALID: + throw std::invalid_argument("Received invalid ISO message"); + case MESSAGE_ID_VENDOR_SPECIFIC_ASTAZERO_RDCA: + receivedBytes -= this->handleRDCAMessage(byteData); + break; + default: + throw std::invalid_argument("Received unhandled ISO message"); } } } -size_t DirectControl::handleRDCAMessage(std::vector& byteData) { +size_t DirectControl::handleRDCAMessage( + std::vector &byteData) { RequestControlActionType recvMessage; struct timeval currentTime; TimeSetToCurrentSystemTime(¤tTime); ssize_t bytesRead = decodeRDCAMessage(byteData.data(), &recvMessage, byteData.size(), currentTime, false); if (bytesRead >= 0) { byteData.erase(byteData.begin(), byteData.begin() + bytesRead); - // TODO: Implement equivalent of below line in using ROS2. - // DataDictionarySetRequestedControlAction(recvMessage.executingID, &recvMessage); + //TODO: Implement equivalent of below line in using ROS2. + //DataDictionarySetRequestedControlAction(recvMessage.executingID, &recvMessage); return static_cast(bytesRead); - } else { + } + else { // TODO respond with error (optional) throw std::invalid_argument("Failed to decode RDCA message"); } } -size_t DirectControl::handleUnknownMessage(std::vector& byteData) { +size_t DirectControl::handleUnknownMessage( + std::vector &byteData) { std::fill(byteData.begin(), byteData.end(), 0); return byteData.size(); } diff --git a/atos/modules/EsminiAdapter/src/esminiadapter.cpp b/atos/modules/EsminiAdapter/src/esminiadapter.cpp index a079d69ea..72691b879 100644 --- a/atos/modules/EsminiAdapter/src/esminiadapter.cpp +++ b/atos/modules/EsminiAdapter/src/esminiadapter.cpp @@ -4,84 +4,92 @@ * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ #include "esminiadapter.hpp" +#include +#include #include +#include #include #include -#include +#include #include -#include -#include #include "atos_interfaces/msg/cartesian_trajectory.hpp" #include "rclcpp/wait_for_message.hpp" -#include "string_utility.hpp" #include "trajectory.hpp" +#include "string_utility.hpp" + using namespace ROSChannels; -using TestOriginSrv = atos_interfaces::srv::GetTestOrigin; +using TestOriginSrv = atos_interfaces::srv::GetTestOrigin; using ObjectTrajectorySrv = atos_interfaces::srv::GetObjectTrajectory; -using ObjectTriggerSrv = atos_interfaces::srv::GetObjectTriggerStart; +using ObjectTriggerSrv = atos_interfaces::srv::GetObjectTriggerStart; using std::placeholders::_1; using std::placeholders::_2; using namespace std::chrono_literals; -std::shared_ptr EsminiAdapter::me = nullptr; + + + +std::shared_ptr EsminiAdapter::me = nullptr; std::unordered_map EsminiAdapter::atosIDToObjectName = std::unordered_map(); std::unordered_map EsminiAdapter::objectNameToAtosId = std::unordered_map(); -std::map EsminiAdapter::atosObjectIdToTraj = std::map(); -std::unordered_map EsminiAdapter::atosIdToEsminiId = std::unordered_map(); - -std::unordered_map> EsminiAdapter::monrSubscribers = - std::unordered_map>(); -std::shared_ptr> EsminiAdapter::objectTrajectoryService = - std::shared_ptr>(); -std::shared_ptr> EsminiAdapter::testOriginService = - std::shared_ptr>(); +std::map EsminiAdapter::atosObjectIdToTraj = std::map(); +std::unordered_map EsminiAdapter::atosIdToEsminiId = std::unordered_map(); + +std::unordered_map> EsminiAdapter::monrSubscribers = std::unordered_map>(); +std::shared_ptr> EsminiAdapter::objectTrajectoryService = std::shared_ptr>(); +std::shared_ptr> EsminiAdapter::testOriginService = std::shared_ptr>(); geographic_msgs::msg::GeoPose EsminiAdapter::testOrigin = geographic_msgs::msg::GeoPose(); -EsminiAdapter::EsminiAdapter() : - Module(moduleName), - storyBoardElementStateChangePub(*this), - connectedObjectIdsSub(*this, &EsminiAdapter::onConnectedObjectIdsMessage), - exitSub(*this, &EsminiAdapter::onStaticExitMessage), - stateChangeSub(*this, &EsminiAdapter::onStaticStateChangeMessage), - applyTrajTransform(false), - testOriginSet(false) { +EsminiAdapter::EsminiAdapter() : Module(moduleName), + storyBoardElementStateChangePub(*this), + connectedObjectIdsSub(*this, &EsminiAdapter::onConnectedObjectIdsMessage), + exitSub(*this, &EsminiAdapter::onStaticExitMessage), + stateChangeSub(*this, &EsminiAdapter::onStaticStateChangeMessage), + applyTrajTransform(false), + testOriginSet(false) + { oscFilePathClient_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); - objectIdsClient_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); - oscFilePathClient_ = create_client( - ServiceNames::getOpenScenarioFilePath, rmw_qos_profile_services_default, oscFilePathClient_cb_group_); + objectIdsClient_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + oscFilePathClient_ = create_client( + ServiceNames::getOpenScenarioFilePath, rclcpp::ServicesQoS(), oscFilePathClient_cb_group_); objectIdsClient_ = create_client( - ServiceNames::getObjectIds, rmw_qos_profile_services_default, objectIdsClient_cb_group_); + ServiceNames::getObjectIds, rclcpp::ServicesQoS(), objectIdsClient_cb_group_); declare_parameter("timestep", 0.1); + } /*! * \brief Fetches the open drive file path from the open scenario file parameter * \return Configured path - */ -std::filesystem::path EsminiAdapter::getOpenDriveFile() { +*/ +std::filesystem::path EsminiAdapter::getOpenDriveFile() +{ std::filesystem::path odrFilePath; if (SE_GetODRFilename() != nullptr) { odrFilePath = std::filesystem::path(SE_GetODRFilename()); RCLCPP_INFO(me->get_logger(), "Got ODR file %s from scenario", odrFilePath.string().c_str()); - } else { + } + else { RCLCPP_DEBUG(me->get_logger(), "No ODR file found"); } if (odrFilePath.is_absolute()) { return odrFilePath; - } else { + } + else { // Look for the file relative the scenario file (ie root openx dir) - auto odrCatalog = std::filesystem::path(me->oscFilePath).parent_path(); + auto odrCatalog = std::filesystem::path(me->oscFilePath).parent_path(); std::filesystem::path joinedPath = odrCatalog / odrFilePath; if (!std::filesystem::exists(joinedPath)) { throw std::runtime_error("ODR file " + joinedPath.string() + " does not exist"); - } else { + } + else { RCLCPP_INFO(me->get_logger(), "Found ODR file at %s", joinedPath.string().c_str()); } return joinedPath; } + } /*! @@ -92,24 +100,22 @@ std::shared_ptr EsminiAdapter::instance() { if (me == nullptr) { me = std::shared_ptr(new EsminiAdapter()); // Start listening to connected object ids - me->connectedObjectIdsSub = - ROSChannels::ConnectedObjectIds::Sub(*me, &EsminiAdapter::onConnectedObjectIdsMessage); - me->exitSub = ROSChannels::Exit::Sub(*me, &EsminiAdapter::onStaticExitMessage); - me->stateChangeSub = ROSChannels::StateChange::Sub(*me, &EsminiAdapter::onStaticStateChangeMessage); + me->connectedObjectIdsSub = ROSChannels::ConnectedObjectIds::Sub(*me,&EsminiAdapter::onConnectedObjectIdsMessage); + me->exitSub = ROSChannels::Exit::Sub(*me,&EsminiAdapter::onStaticExitMessage); + me->stateChangeSub = ROSChannels::StateChange::Sub(*me,&EsminiAdapter::onStaticStateChangeMessage); + } return me; } /*! - * \brief This callback is executed when objects are connected, and creates Monitor subscribers for all connected - * objects. + * \brief This callback is executed when objects are connected, and creates Monitor subscribers for all connected objects. * \param msg Array of object IDs */ void EsminiAdapter::onConnectedObjectIdsMessage(const ConnectedObjectIds::message_type::SharedPtr msg) { for (uint32_t id : msg->ids) { - if (me->monrSubscribers.find(id) == me->monrSubscribers.end()) { - me->monrSubscribers[id] = - std::make_shared(*me, id, std::bind(&EsminiAdapter::onMonitorMessage, me, _1, id)); + if (me->monrSubscribers.find(id) == me->monrSubscribers.end()){ + me->monrSubscribers[id] = std::make_shared(*me, id, std::bind(&EsminiAdapter::onMonitorMessage, me, _1, id)); } } } @@ -122,50 +128,56 @@ void EsminiAdapter::onConnectedObjectIdsMessage(const ConnectedObjectIds::messag * @param msg StateChange message */ void EsminiAdapter::onStaticStateChangeMessage(const ROSChannels::StateChange::message_type::SharedPtr msg) { - int prevState = msg->prev_state; + int prevState = msg->prev_state; int currentState = msg->current_state; if (prevState == OBCState_t::OBC_STATE_ARMED && currentState == OBCState_t::OBC_STATE_RUNNING) { me->handleStartCommand(); - } else if (currentState == OBCState_t::OBC_STATE_ABORTING) { + } + else if (currentState == OBCState_t::OBC_STATE_ABORTING) { me->handleAbortCommand(); } } + void EsminiAdapter::handleAbortCommand() { SE_Close(); RCLCPP_INFO(me->get_logger(), "Esmini ScenarioEngine stopped due to Abort"); } -void EsminiAdapter::fetchOSCFilePath() { +void EsminiAdapter::fetchOSCFilePath() +{ // Get the file path of xosc file std::promise done; auto callback = [&](rclcpp::Client::SharedFuture future) { - auto response = future.get(); + auto response = future.get(); me->oscFilePath = response->path; if (response->md5hash != me->scenarioFileMd5hash) { me->scenarioFileMd5hash = response->md5hash; - me->runSimulation = true; + me->runSimulation = true; } me->scenarioFileMd5hash = response->md5hash; done.set_value(true); }; auto request = std::make_shared(); - auto future = me->oscFilePathClient_->async_send_request(request, std::move(callback)); + auto future = me->oscFilePathClient_->async_send_request(request, std::move(callback)); if (done.get_future().wait_for(250ms) == std::future_status::timeout) { RCLCPP_ERROR(me->get_logger(), "Failed to fetch open scenario file path"); } + } -void EsminiAdapter::onStaticExitMessage(const ROSChannels::Exit::message_type::SharedPtr) { +void EsminiAdapter::onStaticExitMessage(const ROSChannels::Exit::message_type::SharedPtr) +{ SE_Close(); - RCLCPP_DEBUG(me->get_logger(), "Received exit command"); + RCLCPP_DEBUG(me->get_logger(),"Received exit command"); rclcpp::shutdown(); } -void EsminiAdapter::handleStartCommand() { - if (SE_Init(me->oscFilePath.c_str(), 0, 0, 0, 0) < 0) { +void EsminiAdapter::handleStartCommand() +{ + if (SE_Init(me->oscFilePath.c_str(),0,0,0,0) < 0) { throw std::runtime_error("Failed to initialize esmini with scenario file " + me->oscFilePath.string()); } // Handle triggers and story board element changes @@ -179,22 +191,21 @@ void EsminiAdapter::handleStartCommand() { * \brief Callback to be executed by esmini when story board state changes. * If story board element is an action, and the action is supported, the action is run. * \param name Name of the StoryBoardElement whose state has changed. - * \param type Possible values: STORY_BOARD = 1, STORY = 2, ACT = 3, MANEUVER_GROUP = 4, MANEUVER = 5, EVENT = 6, ACTION - * = 7, UNDEFINED_ELEMENT_TYPE = 0. + * \param type Possible values: STORY_BOARD = 1, STORY = 2, ACT = 3, MANEUVER_GROUP = 4, MANEUVER = 5, EVENT = 6, ACTION = 7, UNDEFINED_ELEMENT_TYPE = 0. * \param state new state, possible values: STANDBY = 1, RUNNING = 2, COMPLETE = 3, UNDEFINED_ELEMENT_STATE = 0. */ -void EsminiAdapter::handleStoryBoardElementChange(const char* name, int type, int state, const char* full_path) { - RCLCPP_INFO(me->get_logger(), - "Storyboard state changed! Name: %s, Type: %d, State: %d, Full path: %s", - name, - type, - state, - full_path); +void EsminiAdapter::handleStoryBoardElementChange( + const char *name, + int type, + int state, + const char *full_path) +{ + RCLCPP_INFO(me->get_logger(), "Storyboard state changed! Name: %s, Type: %d, State: %d, Full path: %s", name, type, state, full_path); atos_interfaces::msg::StoryBoardElementStateChange msg; - msg.name = name; - msg.type = type; - msg.state = state; + msg.name = name; + msg.type = type; + msg.state = state; msg.full_path = full_path; me->storyBoardElementStateChangePub.publish(msg); @@ -206,9 +217,9 @@ void EsminiAdapter::handleStoryBoardElementChange(const char* name, int type, in * \param monr ROS Monitor message of an object * \param id The object ID to which the monr belongs */ -void EsminiAdapter::reportObjectPosition(const Monitor::message_type::SharedPtr monr, uint32_t esminiObjectId) { +void EsminiAdapter::reportObjectPosition(const Monitor::message_type::SharedPtr monr, uint32_t esminiObjectId){ // Conversions from ROS to Esmini - auto ori = monr->pose.pose.orientation; + auto ori = monr->pose.pose.orientation; auto quat = tf2::Quaternion(ori.x, ori.y, ori.z, ori.w); tf2::Matrix3x3 m(quat); double roll, pitch, yaw; @@ -221,8 +232,7 @@ void EsminiAdapter::reportObjectPosition(const Monitor::message_type::SharedPtr auto speed = monr->velocity.twist.linear; // Reporting to Esmini - float timestamp = 0.0f; // Not really used according to esmini documentation - SE_ReportObjectPos(esminiObjectId, timestamp, pos.x, pos.y, pos.z, yaw, pitch, roll); + SE_ReportObjectPos(esminiObjectId, pos.x, pos.y, pos.z, yaw, pitch, roll); SE_ReportObjectSpeed(esminiObjectId, speed.x); } @@ -230,16 +240,14 @@ void EsminiAdapter::reportObjectPosition(const Monitor::message_type::SharedPtr * \brief Callback for MONR messages, reports the object position to esmini and advances the simulation time * \param monr ROS Monitor message of an object * \param id The object ID to which the monr belongs - */ +*/ void EsminiAdapter::onMonitorMessage(const Monitor::message_type::SharedPtr monr, uint32_t ATOSObjectId) { if (auto idMapping = atosIdToEsminiId.find(ATOSObjectId); idMapping != atosIdToEsminiId.end()) { reportObjectPosition(monr, idMapping->second); // Report object position to esmini - SE_Step(); // Advance the "simulation world"-time - } else { - RCLCPP_WARN( - me->get_logger(), - "Received MONR message for object with ATOS Object ID %d, but no such object exists in the scenario", - ATOSObjectId); + SE_Step(); // Advance the "simulation world"-time + } + else { + RCLCPP_WARN(me->get_logger(), "Received MONR message for object with ATOS Object ID %d, but no such object exists in the scenario", ATOSObjectId); } } @@ -251,7 +259,10 @@ void EsminiAdapter::onMonitorMessage(const Monitor::message_type::SharedPtr monr * \param states A vector of object states * \return A trajectory consisting of trajectory points, one for each state. */ -ATOS::Trajectory EsminiAdapter::getTrajectoryFromObjectState(uint32_t id, std::vector& states) { +ATOS::Trajectory EsminiAdapter::getTrajectoryFromObjectState( + uint32_t id, + std::vector& states) +{ ATOS::Trajectory trajectory(me->get_logger()); trajectory.name = "Esmini Trajectory for object " + std::to_string(id); if (states.empty()) { @@ -277,7 +288,8 @@ ATOS::Trajectory EsminiAdapter::getTrajectoryFromObjectState(uint32_t id, std::v if (state.timestamp != prevState.timestamp) { tp.setLongitudinalAcceleration((currLonVel - prevLonVel) / (state.timestamp - prevState.timestamp)); tp.setLateralAcceleration((currLatVel - prevLatVel) / (state.timestamp - prevState.timestamp)); - } else { + } + else { tp.setLongitudinalAcceleration(0); tp.setLateralAcceleration(0); } @@ -286,12 +298,12 @@ ATOS::Trajectory EsminiAdapter::getTrajectoryFromObjectState(uint32_t id, std::v }; if (states.size() > 1) { - for (auto it = states.begin() + 1; it != states.end(); ++it) { - if (it->x == (it - 1)->x && it->y == (it - 1)->y && // Nothing interesting happens within 1 timestep, skip - it->z == (it - 1)->z && it->h == (it - 1)->h) { + for (auto it = states.begin()+1; it != states.end(); ++it) { + if (it->x == (it-1)->x && it->y == (it-1)->y && // Nothing interesting happens within 1 timestep, skip + it->z == (it-1)->z && it->h == (it-1)->h) { continue; } - saveTp(*it, *(it - 1)); // Next timestep is different, save current one. + saveTp(*it,*(it-1)); // Next timestep is different, save current one. } } if (trajectory.points.size() == 0) { @@ -300,7 +312,7 @@ ATOS::Trajectory EsminiAdapter::getTrajectoryFromObjectState(uint32_t id, std::v auto startTime = trajectory.points.front().getTime(); // Subtract start time from all timesteps - for (auto& tp : trajectory.points) { + for (auto& tp : trajectory.points){ tp.setTime(tp.getTime() - startTime); } return trajectory; @@ -315,7 +327,8 @@ std::string EsminiAdapter::projStrFromGeoReference(RM_GeoReference& geoRef) { std::string projStringFrom = "+proj="; if (strlen(geoRef.proj_) != 0) { projStringFrom += std::string(geoRef.proj_) + " "; - } else { + } + else { throw std::runtime_error("No projection found in geo reference"); } if (!std::isnan(geoRef.lat_0_)) { @@ -385,11 +398,14 @@ std::string EsminiAdapter::projStrFromGeoReference(RM_GeoReference& geoRef) { * \param states The return map of ids mapping to the respective object states at different timesteps * \return A map of object states, where the key is the object ID and the value is a vector of states */ -void EsminiAdapter::getObjectStates(double timeStep, std::map>& states) { - double accumTime = 0.0; +void EsminiAdapter::getObjectStates( + double timeStep, + std::map>& states) +{ + double accumTime = 0.0; auto pushCurrentState = [&](auto& st, auto& index) { auto id = SE_GetId(index); - // SE_SetAlignModeZ(id, 0); // Disable Z-alignment, not implemented in esmini yet + //SE_SetAlignModeZ(id, 0); // Disable Z-alignment, not implemented in esmini yet SE_ScenarioObjectState s; SE_GetObjectState(id, &s); s.timestamp = accumTime; @@ -399,13 +415,13 @@ void EsminiAdapter::getObjectStates(double timeStep, std::map(); pushCurrentState(states, j); } constexpr double MIN_SCENARIO_TIME = 10.0; constexpr double MAX_SCENARIO_TIME = 3600.0; - bool stopSimulation = false; + bool stopSimulation = false; while (!stopSimulation) { if (SE_GetQuitFlag() != 0) { break; @@ -413,48 +429,51 @@ void EsminiAdapter::getObjectStates(double timeStep, std::map MIN_SCENARIO_TIME; + bool noMovement = std::all_of(states.begin(), states.end(), [&](auto& pair) { + return pair.second.back().speed < 0.1; + }); + bool atLeastMinTimePassed = accumTime > MIN_SCENARIO_TIME; bool moreThanMaxTimePassed = accumTime > MAX_SCENARIO_TIME; - stopSimulation = (noMovement && atLeastMinTimePassed) || moreThanMaxTimePassed; + stopSimulation = (noMovement && atLeastMinTimePassed) || moreThanMaxTimePassed; } if (accumTime > MAX_SCENARIO_TIME) { RCLCPP_WARN(me->get_logger(), "Scenario time limit reached, stopping simulation"); - } else if (accumTime < MIN_SCENARIO_TIME + timeStep) { - RCLCPP_WARN(me->get_logger(), - "Ran scenario for the minimum time %.2f, possibly no movement in scenario", - MIN_SCENARIO_TIME); + } + else if (accumTime < MIN_SCENARIO_TIME + timeStep) { + RCLCPP_WARN(me->get_logger(), "Ran scenario for the minimum time %.2f, possibly no movement in scenario", MIN_SCENARIO_TIME); } RCLCPP_INFO(me->get_logger(), "Finished %f s simulation", accumTime); } + /*! * \brief Runs the esmini simulator with the xosc file and returns the trajectories for each object * \param timeStep Time step to use for generating the trajectories * \return A map of ids mapping to the respective trajectories */ -std::map EsminiAdapter::extractTrajectories(double timeStep) { - std::map esminiObjectIdToTraj = std::map(); +std::map EsminiAdapter::extractTrajectories( + double timeStep) +{ + std::map esminiObjectIdToTraj = std::map(); // Get object states - std::map> esminiIdToStates; + std::map> esminiIdToStates; getObjectStates(timeStep, esminiIdToStates); // Extract trajectories for (auto& os : esminiIdToStates) { - auto id = os.first; + auto id = os.first; auto objectStates = os.second; - auto traj = getTrajectoryFromObjectState(id, objectStates); + auto traj = getTrajectoryFromObjectState(id, objectStates); // Apply CRS transform if OpenDrive CRS Transformation is defined - if (me->applyTrajTransform) { + if (me->applyTrajTransform){ RCLCPP_DEBUG(me->get_logger(), "Applying CRS transformation to trajectory for object %d", id); me->crsTransformation->apply(traj.points); } - esminiObjectIdToTraj.insert(std::pair(id, traj)); + esminiObjectIdToTraj.insert(std::pair(id, traj)); } return esminiObjectIdToTraj; } @@ -463,7 +482,8 @@ std::map EsminiAdapter::extractTrajectories(double t * \brief Initialize the esmini simulator and perform subsequent setup tasks. * Can be called many times, each time the test is initialized. */ -void EsminiAdapter::runEsminiSimulation() { +void EsminiAdapter::runEsminiSimulation() +{ me->atosObjectIdToTraj.clear(); me->atosIdToEsminiId.clear(); me->atosIDToObjectName.clear(); @@ -476,36 +496,37 @@ void EsminiAdapter::runEsminiSimulation() { RM_Close(); // Stop RoadManager in case it is running RCLCPP_INFO(me->get_logger(), "Initializing esmini with scenario file %s", me->oscFilePath.c_str()); - if (SE_Init(me->oscFilePath.c_str(), 1, 0, 0, 0) < 0) { // Disable controllers, let DefaultController be used - throw std::runtime_error("Failed to initialize esmini with scenario file " + me->oscFilePath.string() + - ". For more information, see " + logFilePath + "."); + if (SE_Init(me->oscFilePath.c_str(),1,0,0,0) < 0) { // Disable controllers, let DefaultController be used + throw std::runtime_error("Failed to initialize esmini with scenario file " + me->oscFilePath.string() + ". For more information, see " + logFilePath + "."); } auto odrFile = getOpenDriveFile(); if (RM_Init(odrFile.c_str()) < 0) { throw std::runtime_error(std::string("Failed to initialize with odr file ").append(odrFile)); } + // Call RM_GetOpenDriveGeoReference to get the RM_GeoReference struct RM_GeoReference geoRef; if (RM_GetOpenDriveGeoReference(&geoRef) == 0) { try { std::string projStringFrom = projStrFromGeoReference(geoRef); - std::string toDatum = "WGS84"; - auto llh_0 = CRSTransformation::projToLLH(projStringFrom, toDatum); + std::string toDatum = "WGS84"; + auto llh_0 = CRSTransformation::projToLLH(projStringFrom, toDatum); RCLCPP_INFO(me->get_logger(), "llh origin: %lf, %lf, %lf", llh_0[0], llh_0[1], llh_0[2]); - me->testOrigin.position.latitude = llh_0[0]; + me->testOrigin.position.latitude = llh_0[0]; me->testOrigin.position.longitude = llh_0[1]; - me->testOrigin.position.altitude = llh_0[2]; - me->testOriginSet = true; + me->testOrigin.position.altitude = llh_0[2]; + me->testOriginSet = true; std::string projStringTo = "+proj=tmerc +lat_0=" + std::to_string(llh_0[0]) + - " +lon_0=" + std::to_string(llh_0[1]) + " +datum=" + toDatum + - " +units=m +no_defs"; + " +lon_0=" + std::to_string(llh_0[1]) + + " +datum="+ toDatum + " +units=m +no_defs"; - me->crsTransformation = std::make_shared(projStringFrom, projStringTo); + me->crsTransformation = std::make_shared(projStringFrom, projStringTo); me->applyTrajTransform = true; - } catch (std::exception& e) { - RCLCPP_ERROR(me->get_logger(), e.what()); + } + catch (std::exception& e) { + RCLCPP_ERROR(me->get_logger(), "%s", e.what()); return; } } else { @@ -514,18 +535,17 @@ void EsminiAdapter::runEsminiSimulation() { RM_Close(); auto response = std::make_shared(); - auto request = std::make_shared(); + auto request = std::make_shared(); std::promise idsFetched; - auto objectNameAndAtosIDsCallback = - [&](rclcpp::Client::SharedFutureWithRequest future) { - auto response = future.get(); - for (int i = 0; i < response.second->ids.size(); i++) { - me->atosIDToObjectName[response.second->ids[i]] = response.second->names[i]; - me->objectNameToAtosId[response.second->names[i]] = response.second->ids[i]; - } - idsFetched.set_value(); - }; + auto objectNameAndAtosIDsCallback = [&](rclcpp::Client::SharedFutureWithRequest future) { + auto response = future.get(); + for (size_t i = 0; i < response.second->ids.size(); ++i) { + me->atosIDToObjectName[response.second->ids[i]] = response.second->names[i]; + me->objectNameToAtosId[response.second->names[i]] = response.second->ids[i]; + } + idsFetched.set_value(); + }; auto future = me->objectIdsClient_->async_send_request(request, std::move(objectNameAndAtosIDsCallback)); if (idsFetched.get_future().wait_for(250ms) == std::future_status::timeout) { @@ -534,46 +554,41 @@ void EsminiAdapter::runEsminiSimulation() { } RCLCPP_INFO(me->get_logger(), "Starting extracting trajectories"); - double timeStep = me->get_parameter("timestep").as_double(); - std::map esminiIdToTraj = me->extractTrajectories(timeStep); + double timeStep = me->get_parameter("timestep").as_double(); + std::map esminiIdToTraj = me->extractTrajectories(timeStep); // Fill atosIdToTraj with the extracted trajectories for (auto& it : esminiIdToTraj) { - auto esminiId = it.first; - auto traj = it.second; + auto esminiId = it.first; + auto traj = it.second; auto objectName = SE_GetObjectName(esminiId); - if (auto idMapping = me->objectNameToAtosId.find(objectName); idMapping != me->objectNameToAtosId.end()) { - auto atos_id = idMapping->second; - me->atosObjectIdToTraj.emplace(atos_id, traj); - me->atosIdToEsminiId.emplace(atos_id, esminiId); - RCLCPP_INFO( - me->get_logger(), "Extracted trajectory for object %s with size %d", objectName, traj.points.size()); - - me->pathPublishers.emplace(atos_id, ROSChannels::Path::Pub(*me, atos_id)); - me->pathPublishers.at(atos_id).publish(traj.toPath()); - std::array llh_0 = { - me->testOrigin.position.latitude, me->testOrigin.position.longitude, me->testOrigin.position.altitude}; - me->gnssPathPublishers.emplace(atos_id, ROSChannels::GNSSPath::Pub(*me, atos_id)); - me->gnssPathPublishers.at(atos_id).publish(traj.toGeoJSON(llh_0)); - } else { - RCLCPP_DEBUG(me->get_logger(), "Object %s is not an active object in the scenario", objectName); - } + if (auto idMapping = me->objectNameToAtosId.find(objectName); idMapping != me->objectNameToAtosId.end()) { + auto atos_id = idMapping->second; + me->atosObjectIdToTraj.emplace(atos_id, traj); + me->atosIdToEsminiId.emplace(atos_id, esminiId); + RCLCPP_INFO( + me->get_logger(), "Extracted trajectory for object %s with size %zu", objectName, traj.points.size()); + + me->pathPublishers.emplace(atos_id, ROSChannels::Path::Pub(*me, atos_id)); + me->pathPublishers.at(atos_id).publish(traj.toPath()); + std::array llh_0 = {me->testOrigin.position.latitude, me->testOrigin.position.longitude, me->testOrigin.position.altitude}; + me->gnssPathPublishers.emplace(atos_id, ROSChannels::GNSSPath::Pub(*me, atos_id)); + me->gnssPathPublishers.at(atos_id).publish(traj.toGeoJSON(llh_0)); + } + else { + RCLCPP_DEBUG(me->get_logger(), "Object %s is not an active object in the scenario", objectName); + } - for (auto& tp : traj.points) { - RCLCPP_DEBUG(me->get_logger(), - "Trajectory point: %lf, %lf, %lf, %lf, %ld", - tp.getXCoord(), - tp.getYCoord(), - tp.getZCoord(), - tp.getHeading(), - tp.getTime().count()); + for (auto& tp : traj.points){ + RCLCPP_DEBUG(me->get_logger(), "Trajectory point: %lf, %lf, %lf, %lf, %ld", tp.getXCoord(), tp.getYCoord(), tp.getZCoord(), tp.getHeading(), tp.getTime().count()); } } SE_Close(); // Stop ScenarioEngine } -void EsminiAdapter::onRequestObjectTrajectory(const std::shared_ptr req, - std::shared_ptr res) { - res->id; +void EsminiAdapter::onRequestObjectTrajectory( + const std::shared_ptr req, + std::shared_ptr res) +{ me->fetchOSCFilePath(); if (me->runSimulation || me->atosObjectIdToTraj.find(req->id) == me->atosObjectIdToTraj.end()) { me->runEsminiSimulation(); @@ -582,22 +597,25 @@ void EsminiAdapter::onRequestObjectTrajectory(const std::shared_ptrtrajectory = me->atosObjectIdToTraj.at(req->id).toCartesianTrajectory(); - res->success = true; - } catch (std::out_of_range& e) { + res->success = true; + } + catch (std::out_of_range& e){ RCLCPP_ERROR(me->get_logger(), "Esmini trajectory service called, no trajectory found for object %d", req->id); res->success = false; } } -void EsminiAdapter::onRequestTestOrigin(const std::shared_ptr, - std::shared_ptr res) { - while (me->testOriginSet == false) { +void EsminiAdapter::onRequestTestOrigin( + const std::shared_ptr, + std::shared_ptr res) +{ + while (me->testOriginSet == false){ RCLCPP_DEBUG(me->get_logger(), "Waiting for test origin to be available"); std::this_thread::sleep_for(std::chrono::milliseconds(20)); } - res->origin.position.latitude = me->testOrigin.position.latitude; + res->origin.position.latitude = me->testOrigin.position.latitude; res->origin.position.longitude = me->testOrigin.position.longitude; - res->origin.position.altitude = me->testOrigin.position.altitude; + res->origin.position.altitude = me->testOrigin.position.altitude; } /*! @@ -609,10 +627,10 @@ void EsminiAdapter::onRequestTestOrigin(const std::shared_ptrobjectTrajectoryService = me->create_service( - ServiceNames::getObjectTrajectory, std::bind(&EsminiAdapter::onRequestObjectTrajectory, _1, _2)); - me->testOriginService = me->create_service( - ServiceNames::getTestOrigin, std::bind(&EsminiAdapter::onRequestTestOrigin, _1, _2)); + me->objectTrajectoryService = me->create_service(ServiceNames::getObjectTrajectory, + std::bind(&EsminiAdapter::onRequestObjectTrajectory, _1, _2)); + me->testOriginService = me->create_service(ServiceNames::getTestOrigin, + std::bind(&EsminiAdapter::onRequestTestOrigin, _1, _2)); return retval; } diff --git a/atos/modules/IntegrationTesting/src/scenarioexecution.cpp b/atos/modules/IntegrationTesting/src/scenarioexecution.cpp index 61192f41a..7a62cd7a2 100644 --- a/atos/modules/IntegrationTesting/src/scenarioexecution.cpp +++ b/atos/modules/IntegrationTesting/src/scenarioexecution.cpp @@ -3,36 +3,36 @@ * License, v. 2.0. If a copy of the MPL was not distributed with this * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ +#include #include "scenarioexecution.hpp" #include "rclcpp/wait_for_message.hpp" -#include using namespace std::chrono_literals; /** * @brief An integration test for scenario execution. This test will run a scenario execution for one object, * and check that all states are correct and that it follows the trajectory. - * + * */ -ScenarioExecution::ScenarioExecution() : - IntegrationTesting(testName) { +ScenarioExecution::ScenarioExecution() : IntegrationTesting(testName) { RCLCPP_INFO(get_logger(), "Started integration test: %s", testName.c_str()); - getObjectTrajectoryClient = - this->create_client(atosNamespace + ServiceNames::getObjectTrajectory); + getObjectTrajectoryClient = this->create_client(atosNamespace + ServiceNames::getObjectTrajectory); runIntegrationTest(); } + /** * @brief Destructor. - * + * */ ScenarioExecution::~ScenarioExecution() {} + /** * @brief Run the integration test. This integration test will initialize a scenario, connect to object 1, * arm the object, and then start it. It will check that all states are correct, and that the object follows * the trajectory. - * + * */ void ScenarioExecution::runIntegrationTest() { RCLCPP_INFO(get_logger(), "Running scenario"); @@ -41,7 +41,7 @@ void ScenarioExecution::runIntegrationTest() { initPub->publish(msg); std::this_thread::sleep_for(1s); // sleep to allow the object to initialize checkState(initTopic); - + connectPub->publish(msg); std::this_thread::sleep_for(1s); // sleep to allow the object to connect checkState(connectTopic); @@ -56,16 +56,17 @@ void ScenarioExecution::runIntegrationTest() { checkObjectStoppedAtLastPoint(); printResult(); -} +} + /** * @brief Get the x- and y-coordinates of the trajectory points for object 1. - * + * * @return std::vector> Trajectory points. */ std::vector> ScenarioExecution::getTrajectoryPoints() { auto request = std::make_shared(); - request->id = 1; + request->id = 1; std::shared_ptr response; this->callService(1000ms, getObjectTrajectoryClient, response, request); @@ -76,47 +77,43 @@ std::vector> ScenarioExecution::getTrajectoryPoints() return trajectory; } + /** * @brief Check that the object ends up near the end point of the trajectory. - * + * */ void ScenarioExecution::checkObjectStoppedAtLastPoint() { auto trajectory = getTrajectoryPoints(); std::pair lastPoint; auto isObjectMoving = true; - while (isObjectMoving) { + while(isObjectMoving) { atos_interfaces::msg::Monitor monr; - monitorSub = this->create_subscription( - "/atos/object_1/object_monitor", - 10, - std::bind(&ScenarioExecution::placeholderCallback, this, std::placeholders::_1)); + monitorSub = this->create_subscription("/atos/object_1/object_monitor", 10, std::bind(&ScenarioExecution::placeholderCallback, this, std::placeholders::_1)); rclcpp::wait_for_message(monr, monitorSub, this->get_node_base_interface()->get_context(), 50ms); double speedThreshold = 0.1; if (abs(monr.velocity.twist.linear.x) < speedThreshold && abs(monr.velocity.twist.linear.y) < speedThreshold) { - lastPoint = std::make_pair(monr.pose.pose.position.x, monr.pose.pose.position.y); + lastPoint = std::make_pair(monr.pose.pose.position.x, monr.pose.pose.position.y); isObjectMoving = false; } } - auto distanceX = lastPoint.first - trajectory.back().first; - auto distanceY = lastPoint.second - trajectory.back().second; + auto distanceX = lastPoint.first - trajectory.back().first; + auto distanceY = lastPoint.second - trajectory.back().second; auto distanceThreshold = 1.0; if (abs(distanceX) < distanceThreshold && abs(distanceY) < distanceThreshold) { followedTrajectory = true; RCLCPP_INFO(get_logger(), "Object followed trajectory succesfully"); - } else { + } + else { followedTrajectory = false; - RCLCPP_ERROR( - get_logger(), - "Object did not follow trajectory succesfully. Distance from end point of trajectory is x: %f, y: %f", - distanceX, - distanceY); + RCLCPP_ERROR(get_logger(), "Object did not follow trajectory succesfully. Distance from end point of trajectory is x: %f, y: %f", distanceX, distanceY); } } + /** * @brief Print the result of the integration test. - * + * */ void ScenarioExecution::printResult() { std::stringstream ss; @@ -124,18 +121,21 @@ void ScenarioExecution::printResult() { ss << "\nIntegration test - Scenario Excecution results:\n"; ss << "State change results table:\n"; ss << std::setfill('-') << std::setw(width * 3) << "-" << std::endl; - ss << std::left << std::setfill(' ') << std::setw(width) << "State" << std::setw(width) << "Expected state" - << std::setw(width) << "Pass" << std::endl; + ss << std::left << std::setfill(' ') << std::setw(width) << "State" + << std::setw(width) << "Expected state" + << std::setw(width) << "Pass" << std::endl; ss << std::setfill('-') << std::setw(width * 3) << "-" << std::endl; bool allStatesCorrect = true; for (auto const& [state, expectedState] : stateResult) { - auto pass = (state == expectedState) ? "OK" : "NOT OK"; - if (pass == "NOT OK") { + const bool passed = (state == expectedState); + const char *pass = passed ? "OK" : "NOT OK"; + if (!passed) { allStatesCorrect = false; } - ss << std::left << std::setfill(' ') << std::setw(width) << state << std::setw(width) << expectedState - << std::setw(width) << pass << std::endl; + ss << std::left << std::setfill(' ') << std::setw(width) << state + << std::setw(width) << expectedState + << std::setw(width) << pass << std::endl; } ss << std::setfill('-') << std::setw(width * 3) << "-" << std::endl; ss << std::setfill(' '); @@ -144,12 +144,16 @@ void ScenarioExecution::printResult() { ss << ((allStatesCorrect) ? "OK" : "NOT OK"); ss << "\nTrajectory following result: "; ss << ((followedTrajectory) ? "OK" : "NOT OK"); - RCLCPP_INFO(get_logger(), ss.str().c_str()); + const auto result = ss.str(); + RCLCPP_INFO(get_logger(), "%s", result.c_str()); } + /** * @brief This is a placeholder callback function. It is needed in order to create a topic subscriber. - * + * * @param msg Message. */ -void ScenarioExecution::placeholderCallback(const atos_interfaces::msg::Monitor::SharedPtr msg) {} +void ScenarioExecution::placeholderCallback(const atos_interfaces::msg::Monitor::SharedPtr msg) { + (void)msg; +} diff --git a/atos/modules/MQTTBridge/src/mqttbridge.cpp b/atos/modules/MQTTBridge/src/mqttbridge.cpp index 09896c6c3..5f109f168 100644 --- a/atos/modules/MQTTBridge/src/mqttbridge.cpp +++ b/atos/modules/MQTTBridge/src/mqttbridge.cpp @@ -4,158 +4,148 @@ * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ #include "mqttbridge.hpp" - #include -#include -#include -#include - -#include "mqtt2ros_utils.hpp" - using namespace ROSChannels; using std::placeholders::_1; -MqttBridge::MqttBridge() : - Module(MqttBridge::moduleName), - customCommandActionMsgSub(*this, std::bind(&MqttBridge::onCustomCommandActionMsg, this, _1)), - obcStateChangeSub(*this, std::bind(&MqttBridge::onObcStateChangeMsg, this, _1)) { - this->loadParameters(); - this->initialize(); +MqttBridge::MqttBridge() + : Module(MqttBridge::moduleName), + customCommandActionMsgSub( + *this, std::bind(&MqttBridge::onCustomCommandActionMsg, this, _1)), + obcStateChangeSub(*this, + std::bind(&MqttBridge::onObcStateChangeMsg, this, _1)) { + this->loadParameters(); + this->initialize(); } void MqttBridge::loadParameters() { - declare_parameter("broker.host", ""); - declare_parameter("broker.port", 1883); - declare_parameter("client.username", ""); - declare_parameter("client.password", ""); - declare_parameter("client.id", ""); - declare_parameter("topic_prefix", "atos"); - - get_parameter("broker.host", brokerIP); - get_parameter("broker.port", port); - get_parameter("client.username", username); - get_parameter("client.password", password); - get_parameter("client.id", clientId); - get_parameter("topic_prefix", topic_prefix); - - rcl_interfaces::msg::ParameterDescriptor param_desc; - param_desc.description = "The list of topics to bridge from MQTT to ROS"; - const auto mqtt2ros_mqtt_topics = - declare_parameter>("mqtt2ros.mqtt_topics", std::vector(), param_desc); - for (const auto& mqtt_topic : mqtt2ros_mqtt_topics) { - param_desc.description = "ROS topic on which corresponding MQTT messages are published"; - declare_parameter( - fmt::format("mqtt2ros.{}.ros_topic", mqtt_topic), rclcpp::ParameterType::PARAMETER_STRING, param_desc); - param_desc.description = "ROS message type for this bridge"; - declare_parameter( - fmt::format("mqtt2ros.{}.msg_type", mqtt_topic), rclcpp::ParameterType::PARAMETER_STRING, param_desc); - param_desc.description = "MQTT QoS value"; - declare_parameter(fmt::format("mqtt2ros.{}.advanced.mqtt.qos", mqtt_topic), - rclcpp::ParameterType::PARAMETER_INTEGER, - param_desc); - param_desc.description = "ROS publisher queue size"; - declare_parameter(fmt::format("mqtt2ros.{}.advanced.ros.queue_size", mqtt_topic), - rclcpp::ParameterType::PARAMETER_INTEGER, - param_desc); - } - - param_desc.description = "The list of topics to bridge from ROS to MQTT"; - const auto ros2mqtt_ros_topics = - declare_parameter>("ros2mqtt.ros_topics", std::vector(), param_desc); - for (const auto& ros_topic : ros2mqtt_ros_topics) { - param_desc.description = "MQTT topic on which the corresponding ROS " - "messages are sent to the broker"; - declare_parameter( - fmt::format("ros2mqtt.{}.mqtt_topic", ros_topic), rclcpp::ParameterType::PARAMETER_STRING, param_desc); - param_desc.description = "ROS subscriber queue size"; - declare_parameter(fmt::format("ros2mqtt.{}.advanced.ros.queue_size", ros_topic), - rclcpp::ParameterType::PARAMETER_INTEGER, - param_desc); - param_desc.description = "MQTT QoS value"; - declare_parameter(fmt::format("ros2mqtt.{}.advanced.mqtt.qos", ros_topic), - rclcpp::ParameterType::PARAMETER_INTEGER, - param_desc); - } - - // mqtt2ros - for (const auto& mqtt_topic : mqtt2ros_mqtt_topics) { - - rclcpp::Parameter ros_topic_param; - if (get_parameter(fmt::format("mqtt2ros.{}.ros_topic", mqtt_topic), ros_topic_param)) { - - // mqtt2ros[k]/mqtt_topic and mqtt2ros[k]/ros_topic - const std::string ros_topic = ros_topic_param.as_string(); - Mqtt2RosInterface& mqtt2ros = mqtt2ros_[mqtt_topic]; - mqtt2ros.ros.topic = ros_topic; - - rclcpp::Parameter msg_type_param; - if (get_parameter(fmt::format("mqtt2ros.{}.msg_type", mqtt_topic), msg_type_param)) { - mqtt2ros.ros.msg_type = msg_type_param.as_string(); - } else { - mqtt2ros.ros.msg_type = "std_msgs/msg/Empty"; // default if no parameter is set - } - - // mqtt2ros[k]/advanced/mqtt/qos - rclcpp::Parameter qos_param; - if (get_parameter(fmt::format("mqtt2ros.{}.advanced.mqtt.qos", mqtt_topic), qos_param)) { - mqtt2ros.mqtt.qos = qos_param.as_int(); - } - - // mqtt2ros[k]/advanced/ros/queue_size - rclcpp::Parameter queue_size_param; - if (get_parameter(fmt::format("mqtt2ros.{}.advanced.ros.queue_size", mqtt_topic), queue_size_param)) { - mqtt2ros.ros.queue_size = queue_size_param.as_int(); - } - - RCLCPP_INFO(get_logger(), - "Bridging MQTT topic '%s' to ROS topic '%s' with message type '%s'", - mqtt_topic.c_str(), - mqtt2ros.ros.topic.c_str(), - mqtt2ros.ros.msg_type.c_str()); - } else { - RCLCPP_WARN(get_logger(), - fmt::format("Parameter 'ros2mqtt.{}' is missing subparameter " - "'ros_topic', will be ignored", - mqtt_topic) - .c_str()); - } - } - - // ros2mqtt - for (const auto& ros_topic : ros2mqtt_ros_topics) { - - rclcpp::Parameter mqtt_topic_param; - if (get_parameter(fmt::format("ros2mqtt.{}.mqtt_topic", ros_topic), mqtt_topic_param)) { - - // ros2mqtt[k]/ros_topic and ros2mqtt[k]/mqtt_topic - const std::string mqtt_topic = mqtt_topic_param.as_string(); - Ros2MqttInterface& ros2mqtt = ros2mqtt_[ros_topic]; - ros2mqtt.mqtt.topic = mqtt_topic; - - // ros2mqtt[k]/advanced/ros/queue_size - rclcpp::Parameter queue_size_param; - if (get_parameter(fmt::format("ros2mqtt.{}.advanced.ros.queue_size", ros_topic), queue_size_param)) - ros2mqtt.ros.queue_size = queue_size_param.as_int(); - - // ros2mqtt[k]/advanced/mqtt/qos - rclcpp::Parameter qos_param; - if (get_parameter(fmt::format("ros2mqtt.{}.advanced.mqtt.qos", ros_topic), qos_param)) - ros2mqtt.mqtt.qos = qos_param.as_int(); - - RCLCPP_INFO(get_logger(), - "Bridging ROS topic '%s' to MQTT topic '%s'", - ros_topic.c_str(), - ros2mqtt.mqtt.topic.c_str()); - } else { - RCLCPP_WARN(get_logger(), - fmt::format("Parameter 'ros2mqtt.{}' is missing subparameter " - "'mqtt_topic', will be ignored", - ros_topic) - .c_str()); - } - } + declare_parameter("broker.host", ""); + declare_parameter("broker.port", 1883); + declare_parameter("client.username", ""); + declare_parameter("client.password", ""); + declare_parameter("client.id", ""); + declare_parameter("topic_prefix", "atos"); + + get_parameter("broker.host", brokerIP); + get_parameter("broker.port", port); + get_parameter("client.username", username); + get_parameter("client.password", password); + get_parameter("client.id", clientId); + get_parameter("topic_prefix", topic_prefix); + + rcl_interfaces::msg::ParameterDescriptor param_desc; + param_desc.description = "The list of topics to bridge from MQTT to ROS"; + const auto mqtt2ros_mqtt_topics = declare_parameter>( + "mqtt2ros.mqtt_topics", std::vector(), param_desc); + for (const auto &mqtt_topic : mqtt2ros_mqtt_topics) { + param_desc.description = + "ROS topic on which corresponding MQTT messages are published"; + declare_parameter(fmt::format("mqtt2ros.{}.ros_topic", mqtt_topic), + rclcpp::ParameterType::PARAMETER_STRING, param_desc); + param_desc.description = "MQTT QoS value"; + declare_parameter(fmt::format("mqtt2ros.{}.advanced.mqtt.qos", mqtt_topic), + rclcpp::ParameterType::PARAMETER_INTEGER, param_desc); + param_desc.description = "ROS publisher queue size"; + declare_parameter( + fmt::format("mqtt2ros.{}.advanced.ros.queue_size", mqtt_topic), + rclcpp::ParameterType::PARAMETER_INTEGER, param_desc); + } + + param_desc.description = "The list of topics to bridge from ROS to MQTT"; + const auto ros2mqtt_ros_topics = declare_parameter>( + "ros2mqtt.ros_topics", std::vector(), param_desc); + for (const auto &ros_topic : ros2mqtt_ros_topics) { + param_desc.description = "MQTT topic on which the corresponding ROS " + "messages are sent to the broker"; + declare_parameter(fmt::format("ros2mqtt.{}.mqtt_topic", ros_topic), + rclcpp::ParameterType::PARAMETER_STRING, param_desc); + param_desc.description = "ROS subscriber queue size"; + declare_parameter( + fmt::format("ros2mqtt.{}.advanced.ros.queue_size", ros_topic), + rclcpp::ParameterType::PARAMETER_INTEGER, param_desc); + param_desc.description = "MQTT QoS value"; + declare_parameter(fmt::format("ros2mqtt.{}.advanced.mqtt.qos", ros_topic), + rclcpp::ParameterType::PARAMETER_INTEGER, param_desc); + } + + // mqtt2ros + for (const auto &mqtt_topic : mqtt2ros_mqtt_topics) { + + rclcpp::Parameter ros_topic_param; + if (get_parameter(fmt::format("mqtt2ros.{}.ros_topic", mqtt_topic), + ros_topic_param)) { + + // mqtt2ros[k]/mqtt_topic and mqtt2ros[k]/ros_topic + const std::string ros_topic = ros_topic_param.as_string(); + Mqtt2RosInterface &mqtt2ros = mqtt2ros_[mqtt_topic]; + mqtt2ros.ros.topic = ros_topic; + + // mqtt2ros[k]/advanced/mqtt/qos + rclcpp::Parameter qos_param; + if (get_parameter( + fmt::format("mqtt2ros.{}.advanced.mqtt.qos", mqtt_topic), + qos_param)) { + mqtt2ros.mqtt.qos = qos_param.as_int(); + } + + // mqtt2ros[k]/advanced/ros/queue_size + rclcpp::Parameter queue_size_param; + if (get_parameter( + fmt::format("mqtt2ros.{}.advanced.ros.queue_size", mqtt_topic), + queue_size_param)) { + mqtt2ros.ros.queue_size = queue_size_param.as_int(); + } + + RCLCPP_INFO(get_logger(), "Bridging MQTT topic '%s' to ROS topic '%s'", + mqtt_topic.c_str(), mqtt2ros.ros.topic.c_str()); + } else { + const auto warning = fmt::format( + "Parameter 'mqtt2ros.{}' is missing subparameter 'ros_topic', will " + "be ignored", + mqtt_topic); + RCLCPP_WARN(get_logger(), + "%s", warning.c_str()); + } + } + + // ros2mqtt + for (const auto &ros_topic : ros2mqtt_ros_topics) { + + rclcpp::Parameter mqtt_topic_param; + if (get_parameter(fmt::format("ros2mqtt.{}.mqtt_topic", ros_topic), + mqtt_topic_param)) { + + // ros2mqtt[k]/ros_topic and ros2mqtt[k]/mqtt_topic + const std::string mqtt_topic = mqtt_topic_param.as_string(); + Ros2MqttInterface &ros2mqtt = ros2mqtt_[ros_topic]; + ros2mqtt.mqtt.topic = mqtt_topic; + + // ros2mqtt[k]/advanced/ros/queue_size + rclcpp::Parameter queue_size_param; + if (get_parameter( + fmt::format("ros2mqtt.{}.advanced.ros.queue_size", ros_topic), + queue_size_param)) + ros2mqtt.ros.queue_size = queue_size_param.as_int(); + + // ros2mqtt[k]/advanced/mqtt/qos + rclcpp::Parameter qos_param; + if (get_parameter(fmt::format("ros2mqtt.{}.advanced.mqtt.qos", ros_topic), + qos_param)) + ros2mqtt.mqtt.qos = qos_param.as_int(); + + RCLCPP_INFO(get_logger(), "Bridging ROS topic '%s' to MQTT topic '%s'", + ros_topic.c_str(), ros2mqtt.mqtt.topic.c_str()); + } else { + const auto warning = fmt::format( + "Parameter 'ros2mqtt.{}' is missing subparameter 'mqtt_topic', will " + "be ignored", + ros_topic); + RCLCPP_WARN(get_logger(), + "%s", warning.c_str()); + } + } } /*! @@ -163,294 +153,323 @@ void MqttBridge::loadParameters() { * with ros parameter settings. */ void MqttBridge::initialize() { - if (this->brokerIP.empty()) { - RCLCPP_INFO(this->get_logger(), "No Broker IP provided in configuration. Shutting down..."); - rclcpp::shutdown(); - } else { - this->setupClient(); - this->connect(); - // Wait for all ros topics to be announced - rclcpp::sleep_for(std::chrono::seconds(1)); - this->setupMqtt2RosBridge(); - this->setupRos2MqttBridge(); - } + if (this->brokerIP.empty()) { + RCLCPP_INFO(this->get_logger(), + "No Broker IP provided in configuration. Shutting down..."); + rclcpp::shutdown(); + } else { + this->setupClient(); + this->connect(); + // Wait for all ros topics to be announced + rclcpp::sleep_for(std::chrono::seconds(1)); + this->setupMqtt2RosBridge(); + this->setupRos2MqttBridge(); + } } void MqttBridge::setupMqtt2RosBridge() { - new_mqtt2ros_bridge_service_ = create_service( - "~/new_mqtt2ros_bridge", - std::bind(&MqttBridge::newMqtt2RosBridge, this, std::placeholders::_1, std::placeholders::_2)); - - // Create a request and response for each mqtt2ros bridge and put into a - // map - auto serviceCallMap = std::map, - std::shared_ptr>(); - for (const auto& mqtt2ros : mqtt2ros_) { - atos_interfaces::srv::NewMqtt2RosBridge::Request::SharedPtr request = - std::make_shared(); - request->mqtt_topic = mqtt2ros.first; - request->ros_topic = mqtt2ros.second.ros.topic; - request->mqtt_qos = mqtt2ros.second.mqtt.qos; - request->ros_queue_size = mqtt2ros.second.ros.queue_size; - atos_interfaces::srv::NewMqtt2RosBridge::Response::SharedPtr response = - std::make_shared(); - serviceCallMap[response] = request; - } - - // Loop through the map until all response objects are true - while (std::any_of( - serviceCallMap.begin(), serviceCallMap.end(), [](const auto& pair) { return !pair.first->success; })) { - // Get all unsuccesful responses - auto failedResponses = std::vector>(); - for (const auto& pair : serviceCallMap) { - if (!pair.first->success) { - failedResponses.push_back(pair.first); - } - } - // Retry all failed responses - for (const auto& response : failedResponses) { - newMqtt2RosBridge(serviceCallMap[response], response); - } - // Wait for a second before retrying - rclcpp::sleep_for(std::chrono::seconds(1)); - } + new_mqtt2ros_bridge_service_ = + create_service( + "~/new_mqtt2ros_bridge", + std::bind(&MqttBridge::newMqtt2RosBridge, this, std::placeholders::_1, + std::placeholders::_2)); + + // Create a request and response for each mqtt2ros bridge and put into a + // map + auto serviceCallMap = std::map< + std::shared_ptr, + std::shared_ptr>(); + for (const auto &mqtt2ros : mqtt2ros_) { + atos_interfaces::srv::NewMqtt2RosBridge::Request::SharedPtr request = + std::make_shared(); + request->mqtt_topic = mqtt2ros.first; + request->ros_topic = mqtt2ros.second.ros.topic; + request->mqtt_qos = mqtt2ros.second.mqtt.qos; + request->ros_queue_size = mqtt2ros.second.ros.queue_size; + atos_interfaces::srv::NewMqtt2RosBridge::Response::SharedPtr response = + std::make_shared(); + serviceCallMap[response] = request; + } + + // Loop through the map until all response objects are true + while (std::any_of(serviceCallMap.begin(), serviceCallMap.end(), + [](const auto &pair) { return !pair.first->success; })) { + // Get all unsuccesful responses + auto failedResponses = std::vector< + std::shared_ptr>(); + for (const auto &pair : serviceCallMap) { + if (!pair.first->success) { + failedResponses.push_back(pair.first); + } + } + // Retry all failed responses + for (const auto &response : failedResponses) { + newMqtt2RosBridge(serviceCallMap[response], response); + } + // Wait for a second before retrying + rclcpp::sleep_for(std::chrono::seconds(1)); + } } void MqttBridge::setupRos2MqttBridge() { - new_ros2mqtt_bridge_service_ = create_service( - "~/new_ros2mqtt_bridge", - std::bind(&MqttBridge::newRos2MqttBridge, this, std::placeholders::_1, std::placeholders::_2)); - - // setup subscribers - this->setupSubscriptions(); + new_ros2mqtt_bridge_service_ = + create_service( + "~/new_ros2mqtt_bridge", + std::bind(&MqttBridge::newRos2MqttBridge, this, std::placeholders::_1, + std::placeholders::_2)); + + // setup subscribers + this->setupSubscriptions(); } void MqttBridge::setupSubscriptions() { - // get info of all topics - const auto all_topics_and_types = get_topic_names_and_types(); - // check for ros2mqtt topics - for (auto& [ros_topic, ros2mqtt] : ros2mqtt_) { - if (all_topics_and_types.count(ros_topic)) { - // check if message type has changed or if mapping is stale - const std::string& msg_type = all_topics_and_types.at(ros_topic)[0]; - if (msg_type == ros2mqtt.ros.msg_type && !ros2mqtt.ros.is_stale) - continue; - ros2mqtt.ros.is_stale = false; - ros2mqtt.ros.msg_type = msg_type; - - // create new generic subscription, if message type has changed - std::function msg)> bound_callback_func = - std::bind(&MqttBridge::ros2mqtt, this, std::placeholders::_1, ros_topic); - try { - ros2mqtt.ros.subscriber = - create_generic_subscription(ros_topic, msg_type, ros2mqtt.ros.queue_size, bound_callback_func); - } catch (rclcpp::exceptions::RCLError& e) { - RCLCPP_ERROR(get_logger(), "Failed to create generic subscriber: %s", e.what()); - return; - } - RCLCPP_INFO(get_logger(), "Subscribed to ROS topic '%s' of type '%s'", ros_topic.c_str(), msg_type.c_str()); - } - } + // get info of all topics + const auto all_topics_and_types = get_topic_names_and_types(); + // check for ros2mqtt topics + for (auto &[ros_topic, ros2mqtt] : ros2mqtt_) { + if (all_topics_and_types.count(ros_topic)) { + // check if message type has changed or if mapping is stale + const std::string &msg_type = all_topics_and_types.at(ros_topic)[0]; + if (msg_type == ros2mqtt.ros.msg_type && !ros2mqtt.ros.is_stale) + continue; + ros2mqtt.ros.is_stale = false; + ros2mqtt.ros.msg_type = msg_type; + + // create new generic subscription, if message type has changed + std::function msg)> + bound_callback_func = std::bind(&MqttBridge::ros2mqtt, this, + std::placeholders::_1, ros_topic); + try { + ros2mqtt.ros.subscriber = create_generic_subscription( + ros_topic, msg_type, ros2mqtt.ros.queue_size, bound_callback_func); + } catch (rclcpp::exceptions::RCLError &e) { + RCLCPP_ERROR(get_logger(), "Failed to create generic subscriber: %s", + e.what()); + return; + } + RCLCPP_INFO(get_logger(), "Subscribed to ROS topic '%s' of type '%s'", + ros_topic.c_str(), msg_type.c_str()); + } + } } -void MqttBridge::ros2mqtt(const std::shared_ptr& serialized_msg, - const std::string& ros_topic) { - - Ros2MqttInterface& ros2mqtt = ros2mqtt_[ros_topic]; - std::string mqtt_topic = ros2mqtt.mqtt.topic; - std::vector payload_buffer; - - RCLCPP_INFO(get_logger(), "Received ROS message on topic '%s'", ros_topic.c_str()); - - try { - mqtt::message_ptr mqtt_msg = - mqtt::make_message(mqtt_topic, payload_buffer.data(), payload_buffer.size(), ros2mqtt.mqtt.qos, true); - client_->publish(mqtt_msg); - RCLCPP_INFO(get_logger(), "Published ROS message to MQTT topic '%s'", mqtt_topic.c_str()); - } catch (const mqtt::exception& e) { - RCLCPP_WARN(get_logger(), - "Publishing ROS message type information to MQTT topic '%s' failed: %s", - mqtt_topic.c_str(), - e.what()); - } +void MqttBridge::ros2mqtt( + const std::shared_ptr &serialized_msg, + const std::string &ros_topic) { + (void)serialized_msg; + + Ros2MqttInterface &ros2mqtt = ros2mqtt_[ros_topic]; + std::string mqtt_topic = ros2mqtt.mqtt.topic; + std::vector payload_buffer; + + RCLCPP_INFO(get_logger(), "Received ROS message on topic '%s'", + ros_topic.c_str()); + + try { + mqtt::message_ptr mqtt_msg = + mqtt::make_message(mqtt_topic, payload_buffer.data(), + payload_buffer.size(), ros2mqtt.mqtt.qos, true); + client_->publish(mqtt_msg); + RCLCPP_INFO(get_logger(), "Published ROS message to MQTT topic '%s'", + mqtt_topic.c_str()); + } catch (const mqtt::exception &e) { + RCLCPP_WARN( + get_logger(), + "Publishing ROS message type information to MQTT topic '%s' failed: %s", + mqtt_topic.c_str(), e.what()); + } } void MqttBridge::setupClient() { - RCLCPP_INFO(this->get_logger(), - "Setting up connection with clientID: %s, and broker IP: %s", - clientId.c_str(), - brokerIP.c_str()); - - // basic client connection options - connect_options_.set_automatic_reconnect(true); - connect_options_.set_clean_session(true); - connect_options_.set_keep_alive_interval(5); - - // user authentication - if (!username.empty()) { - connect_options_.set_user_name(username); - connect_options_.set_password(password); - } - - auto timeNanoseconds = - std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); - auto id = clientId + "_" + std::to_string(timeNanoseconds); - - const std::string uri = fmt::format("{}://{}:{}", "tcp", brokerIP, port); - try { - client_ = std::shared_ptr(new mqtt::async_client(uri, id)); - } catch (const mqtt::exception& e) { - RCLCPP_ERROR(get_logger(), "Client could not be initialized: %s", e.what()); - exit(EXIT_FAILURE); - } - - // setup MQTT callbacks - client_->set_callback(*this); + RCLCPP_INFO(this->get_logger(), + "Setting up connection with clientID: %s, and broker IP: %s", + clientId.c_str(), brokerIP.c_str()); + + // basic client connection options + connect_options_.set_automatic_reconnect(true); + connect_options_.set_clean_session(true); + connect_options_.set_keep_alive_interval(5); + + // user authentication + if (!username.empty()) { + connect_options_.set_user_name(username); + connect_options_.set_password(password); + } + + auto timeNanoseconds = + std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()) + .count(); + auto id = clientId + "_" + std::to_string(timeNanoseconds); + + const std::string uri = fmt::format("{}://{}:{}", "tcp", brokerIP, port); + try { + client_ = + std::shared_ptr(new mqtt::async_client(uri, id)); + } catch (const mqtt::exception &e) { + RCLCPP_ERROR(get_logger(), "Client could not be initialized: %s", e.what()); + exit(EXIT_FAILURE); + } + + // setup MQTT callbacks + client_->set_callback(*this); } void MqttBridge::connect() { - std::string as_client = clientId.empty() ? "" : std::string(" as '") + clientId + std::string("'"); - RCLCPP_INFO( - get_logger(), "Connecting to broker at '%s'%s ...", client_->get_server_uri().c_str(), as_client.c_str()); - - try { - client_->connect(connect_options_, nullptr, *this); - } catch (const mqtt::exception& e) { - RCLCPP_ERROR(get_logger(), "Connection to broker failed: %s", e.what()); - exit(EXIT_FAILURE); - } + std::string as_client = + clientId.empty() ? "" + : std::string(" as '") + clientId + std::string("'"); + RCLCPP_INFO(get_logger(), "Connecting to broker at '%s'%s ...", + client_->get_server_uri().c_str(), as_client.c_str()); + + try { + client_->connect(connect_options_, nullptr, *this); + } catch (const mqtt::exception &e) { + RCLCPP_ERROR(get_logger(), "Connection to broker failed: %s", e.what()); + exit(EXIT_FAILURE); + } } -void MqttBridge::newMqtt2RosBridge(atos_interfaces::srv::NewMqtt2RosBridge::Request::SharedPtr request, - atos_interfaces::srv::NewMqtt2RosBridge::Response::SharedPtr response) { - - // add mapping definition to mqtt2ros_ - Mqtt2RosInterface& mqtt2ros = mqtt2ros_[request->mqtt_topic]; - mqtt2ros.ros.is_stale = true; - mqtt2ros.ros.topic = request->ros_topic; - mqtt2ros.mqtt.qos = request->mqtt_qos; - mqtt2ros.ros.publisher = - this->create_generic_publisher(mqtt2ros.ros.topic, mqtt2ros.ros.msg_type, mqtt2ros.ros.queue_size); - mqtt2ros.ros.queue_size = request->ros_queue_size; - - RCLCPP_DEBUG(get_logger(), - "Bridging MQTT topic '%s' to ROS topic '%s'", - request->mqtt_topic.c_str(), - mqtt2ros.ros.topic.c_str()); - - // subscribe to the MQTT topic - std::string mqtt_topic_to_subscribe = request->mqtt_topic; - try { - client_->subscribe(mqtt_topic_to_subscribe, mqtt2ros.mqtt.qos); - RCLCPP_INFO(get_logger(), "Subscribed to MQTT topic '%s'", mqtt_topic_to_subscribe.c_str()); - response->success = true; - } - // Catch exception if the topic is already subscribed - catch (const mqtt::exception& e) { - RCLCPP_WARN(get_logger(), "Failed to subscribe MQTT topic '%s': %s", mqtt_topic_to_subscribe.c_str(), e.what()); - response->success = false; - } +void MqttBridge::newMqtt2RosBridge( + atos_interfaces::srv::NewMqtt2RosBridge::Request::SharedPtr request, + atos_interfaces::srv::NewMqtt2RosBridge::Response::SharedPtr response) { + + // add mapping definition to mqtt2ros_ + Mqtt2RosInterface &mqtt2ros = mqtt2ros_[request->mqtt_topic]; + mqtt2ros.ros.is_stale = true; + mqtt2ros.ros.topic = request->ros_topic; + mqtt2ros.mqtt.qos = request->mqtt_qos; + mqtt2ros.ros.publisher = this->create_publisher( + mqtt2ros.ros.topic, request->ros_queue_size); + mqtt2ros.ros.queue_size = request->ros_queue_size; + + RCLCPP_DEBUG(get_logger(), "Bridging MQTT topic '%s' to ROS topic '%s'", + request->mqtt_topic.c_str(), mqtt2ros.ros.topic.c_str()); + + // subscribe to the MQTT topic + std::string mqtt_topic_to_subscribe = request->mqtt_topic; + try { + client_->subscribe(mqtt_topic_to_subscribe, mqtt2ros.mqtt.qos); + RCLCPP_INFO(get_logger(), "Subscribed to MQTT topic '%s'", + mqtt_topic_to_subscribe.c_str()); + response->success = true; + } + // Catch exception if the topic is already subscribed + catch (const mqtt::exception &e) { + RCLCPP_WARN(get_logger(), "Failed to subscribe MQTT topic '%s': %s", + mqtt_topic_to_subscribe.c_str(), e.what()); + response->success = false; + } } -void MqttBridge::newRos2MqttBridge(atos_interfaces::srv::NewRos2MqttBridge::Request::SharedPtr request, - atos_interfaces::srv::NewRos2MqttBridge::Response::SharedPtr response) { +void MqttBridge::newRos2MqttBridge( + atos_interfaces::srv::NewRos2MqttBridge::Request::SharedPtr request, + atos_interfaces::srv::NewRos2MqttBridge::Response::SharedPtr response) { - // add mapping definition to ros2mqtt_ - Ros2MqttInterface& ros2mqtt = ros2mqtt_[request->ros_topic]; - ros2mqtt.ros.is_stale = true; - ros2mqtt.ros.topic = request->ros_topic; - ros2mqtt.mqtt.topic = request->mqtt_topic; - ros2mqtt.mqtt.qos = request->mqtt_qos; + // add mapping definition to ros2mqtt_ + Ros2MqttInterface &ros2mqtt = ros2mqtt_[request->ros_topic]; + ros2mqtt.ros.is_stale = true; + ros2mqtt.ros.topic = request->ros_topic; + ros2mqtt.mqtt.topic = request->mqtt_topic; + ros2mqtt.mqtt.qos = request->mqtt_qos; - RCLCPP_DEBUG(get_logger(), - "Bridging ROS topic '%s' to MQTT topic '%s'", - ros2mqtt.ros.topic.c_str(), - ros2mqtt.mqtt.topic.c_str()); + RCLCPP_DEBUG(get_logger(), "Bridging ROS topic '%s' to MQTT topic '%s'", + ros2mqtt.ros.topic.c_str(), ros2mqtt.mqtt.topic.c_str()); - // setup ROS subscriptions - setupSubscriptions(); + // setup ROS subscriptions + setupSubscriptions(); - response->success = true; + response->success = true; } void MqttBridge::onCustomCommandActionMsg( - const CustomCommandAction::message_type::SharedPtr custom_command_action_msg) { - if (custom_command_action_msg->type == "v2x") { - RCLCPP_INFO(this->get_logger(), "Received V2X message on %s topic", CustomCommandAction::topicName.c_str()); - this->onMessage(custom_command_action_msg->content, "v2x", v2xToJson); - } + const CustomCommandAction::message_type::SharedPtr + custom_command_action_msg) { + if (custom_command_action_msg->type == "v2x") { + RCLCPP_INFO(this->get_logger(), "Received V2X message on %s topic", + CustomCommandAction::topicName.c_str()); + this->onMessage(custom_command_action_msg->content, "v2x", + v2xToJson); + } } -void MqttBridge::onObcStateChangeMsg(const StateChange::message_type::SharedPtr obc_msg) { - this->onMessage(obc_msg, "state", obcStateChangeToJson); +void MqttBridge::onObcStateChangeMsg( + const StateChange::message_type::SharedPtr obc_msg) { + this->onMessage(obc_msg, "state", + obcStateChangeToJson); } void MqttBridge::message_arrived(mqtt::const_message_ptr mqtt_msg) { - std::string mqtt_topic = mqtt_msg->get_topic(); - RCLCPP_DEBUG(get_logger(), "Received MQTT message on topic '%s'", mqtt_topic.c_str()); - Mqtt2RosInterface& mqtt2ros = mqtt2ros_[mqtt_topic]; - - const auto msg = Mqtt2RosUtils::get_serialized_msg(mqtt2ros.ros.msg_type, mqtt_msg->get_payload_str()); - mqtt2ros.ros.publisher->publish(msg); - RCLCPP_DEBUG(get_logger(), "Published MQTT message to ROS topic '%s'", mqtt2ros.ros.topic.c_str()); + std::string mqtt_topic = mqtt_msg->get_topic(); + RCLCPP_DEBUG(get_logger(), "Received MQTT message on topic '%s'", + mqtt_topic.c_str()); + Mqtt2RosInterface &mqtt2ros = mqtt2ros_[mqtt_topic]; + + // Publish empty message to ROS topic + mqtt2ros.ros.publisher->publish(std_msgs::msg::Empty()); + RCLCPP_DEBUG(get_logger(), "Published empty message to ROS topic '%s'", + mqtt2ros.ros.topic.c_str()); } -template -void MqttBridge::onMessage(T msg, std::string mqtt_topic, std::function convertFunc) { - json payload = convertFunc(msg); - try { - RCLCPP_DEBUG(this->get_logger(), "Publishing MQTT msg to broker %s", payload.dump().c_str()); - client_->publish(mqtt_topic, payload.dump().c_str(), payload.dump().size()); - } catch (std::runtime_error&) { - RCLCPP_ERROR(this->get_logger(), "Failed to publish MQTT message"); - } +template +void MqttBridge::onMessage(T msg, std::string mqtt_topic, + std::function convertFunc) { + json payload = convertFunc(msg); + try { + RCLCPP_DEBUG(this->get_logger(), "Publishing MQTT msg to broker %s", + payload.dump().c_str()); + client_->publish(mqtt_topic, payload.dump().c_str(), payload.dump().size()); + } catch (std::runtime_error &) { + RCLCPP_ERROR(this->get_logger(), "Failed to publish MQTT message"); + } } json MqttBridge::v2xToJson(std::string msg_content) { - std::replace(msg_content.begin(), - msg_content.end(), - '\'', - '"'); // Replace ROS single quotes string with double quotes to - // make it a valid JSON string - json j; - // Parse the string to a json object - try { - j = json::parse(msg_content); - } catch (json::parse_error& e) { - std::cerr << "Failed to parse JSON object: " << e.what() << std::endl; - } - - if (j.find("message_type") == j.end()) { - std::cerr << "No message_type field in JSON object" << std::endl; - return j; - } - if (j["message_type"] == "DENM") { - j["detection_time"] = std::chrono::system_clock::now().time_since_epoch().count(); - } - return j; + std::replace(msg_content.begin(), msg_content.end(), '\'', + '"'); // Replace ROS single quotes string with double quotes to + // make it a valid JSON string + json j; + // Parse the string to a json object + try { + j = json::parse(msg_content); + } catch (json::parse_error &e) { + std::cerr << "Failed to parse JSON object: " << e.what() << std::endl; + } + + if (j.find("message_type") == j.end()) { + std::cerr << "No message_type field in JSON object" << std::endl; + return j; + } + if (j["message_type"] == "DENM") { + j["detection_time"] = + std::chrono::system_clock::now().time_since_epoch().count(); + } + return j; } -json MqttBridge::obcStateChangeToJson(const StateChange::message_type::SharedPtr obc_msg) { - json j; - j["current_state"] = obc_msg->current_state; - j["prev_state"] = obc_msg->prev_state; - return j; +json MqttBridge::obcStateChangeToJson( + const StateChange::message_type::SharedPtr obc_msg) { + json j; + j["current_state"] = obc_msg->current_state; + j["prev_state"] = obc_msg->prev_state; + return j; } -void MqttBridge::on_success(const mqtt::token& token) { +void MqttBridge::on_success(const mqtt::token &token) { - (void)token; // Avoid compiler warning for unused parameter. - RCLCPP_INFO(get_logger(), "Connected to broker successfully"); - is_connected_ = true; + (void)token; // Avoid compiler warning for unused parameter. + RCLCPP_INFO(get_logger(), "Connected to broker successfully"); + is_connected_ = true; } -void MqttBridge::on_failure(const mqtt::token& token) { +void MqttBridge::on_failure(const mqtt::token &token) { - RCLCPP_ERROR(get_logger(), - "Connection to broker failed (return code %d), will automatically " - "retry...", - token.get_return_code()); - is_connected_ = false; + RCLCPP_ERROR( + get_logger(), + "Connection to broker failed (return code %d), will automatically " + "retry...", + token.get_return_code()); + is_connected_ = false; } diff --git a/atos/modules/ObjectControl/inc/relativeanchor.hpp b/atos/modules/ObjectControl/inc/relativeanchor.hpp index ac75d09fd..885e5abeb 100644 --- a/atos/modules/ObjectControl/inc/relativeanchor.hpp +++ b/atos/modules/ObjectControl/inc/relativeanchor.hpp @@ -15,10 +15,10 @@ class RelativeAnchor : public TestObject { RelativeAnchor(RelativeAnchor&&); RelativeAnchor& operator=(const RelativeAnchor&) = delete; - RelativeAnchor& operator=(RelativeAnchor&&) = default; + RelativeAnchor& operator=(RelativeAnchor&&) = delete; - virtual void publishMonr(const ROSChannels::Monitor::message_type) override; + virtual void publishMonr(const ROSChannels::Monitor::message_type) override; private: - ROSChannels::Monitor::AnchorPub anchorPub; + ROSChannels::Monitor::AnchorPub anchorPub; }; diff --git a/atos/modules/ObjectControl/inc/relativetestobject.hpp b/atos/modules/ObjectControl/inc/relativetestobject.hpp index 44d3bd1ff..72ada9bef 100644 --- a/atos/modules/ObjectControl/inc/relativetestobject.hpp +++ b/atos/modules/ObjectControl/inc/relativetestobject.hpp @@ -5,9 +5,9 @@ */ #pragma once -#include "positioning.h" -#include "roschannels/monitorchannel.hpp" #include "testobject.hpp" +#include "roschannels/monitorchannel.hpp" +#include "positioning.h" class RelativeTestObject : public TestObject { public: @@ -16,15 +16,15 @@ class RelativeTestObject : public TestObject { RelativeTestObject(RelativeTestObject&&); RelativeTestObject& operator=(const RelativeTestObject&) = delete; - RelativeTestObject& operator=(RelativeTestObject&&) = default; + RelativeTestObject& operator=(RelativeTestObject&&) = delete; private: - virtual ObjectMonitorType transformCoordinate(const ObjectMonitorType& point, - const ObjectMonitorType& anchor, - const bool debug); - virtual MonitorMessage readMonitorMessage() override; - - ROSChannels::Monitor::AnchorSub anchorSub; - ObjectMonitorType lastAnchorMonr; - void updateAnchor(const ROSChannels::Monitor::message_type::SharedPtr); + virtual ObjectMonitorType transformCoordinate(const ObjectMonitorType& point, + const ObjectMonitorType& anchor, + const bool debug); + virtual MonitorMessage readMonitorMessage() override; + + ROSChannels::Monitor::AnchorSub anchorSub; + ObjectMonitorType lastAnchorMonr; + void updateAnchor(const ROSChannels::Monitor::message_type::SharedPtr); }; diff --git a/atos/modules/ObjectControl/inc/testobject.hpp b/atos/modules/ObjectControl/inc/testobject.hpp index 16adeb8e5..903480f78 100644 --- a/atos/modules/ObjectControl/inc/testobject.hpp +++ b/atos/modules/ObjectControl/inc/testobject.hpp @@ -41,7 +41,7 @@ class TestObject : public rclcpp::Node { TestObject(TestObject&&); TestObject& operator=(const TestObject&) = delete; - TestObject& operator=(TestObject&&) = default; + TestObject& operator=(TestObject&&) = delete; virtual void parseConfigurationFile(const fs::path& file); diff --git a/atos/modules/ObjectControl/src/objectcontrol.cpp b/atos/modules/ObjectControl/src/objectcontrol.cpp index 27cd4a550..0d1b44b71 100644 --- a/atos/modules/ObjectControl/src/objectcontrol.cpp +++ b/atos/modules/ObjectControl/src/objectcontrol.cpp @@ -53,20 +53,16 @@ ObjectControl::ObjectControl(std::shared_ptrcreate_callback_group(rclcpp::CallbackGroupType::Reentrant); ip_client_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); origin_client_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); - this->declare_parameter("max_missing_heartbeats", 100); - this->declare_parameter("max_missing_monr", 100); - this->declare_parameter("monr_timeout_period_ms", 1000); - objectsConnectedTimer = create_wall_timer(1000ms, std::bind(&ObjectControl::publishObjectIds, this)); idClient = create_client( - ServiceNames::getObjectIds, rmw_qos_profile_services_default, id_client_cb_group_); + ServiceNames::getObjectIds, rclcpp::ServicesQoS(), id_client_cb_group_); originClient = create_client( - ServiceNames::getTestOrigin, rmw_qos_profile_services_default, origin_client_cb_group_); + ServiceNames::getTestOrigin, rclcpp::ServicesQoS(), origin_client_cb_group_); trajectoryClient = create_client( - ServiceNames::getObjectTrajectory, rmw_qos_profile_services_default, traj_client_cb_group_); + ServiceNames::getObjectTrajectory, rclcpp::ServicesQoS(), traj_client_cb_group_); ipClient = create_client( - ServiceNames::getObjectIp, rmw_qos_profile_services_default, ip_client_cb_group_); + ServiceNames::getObjectIp, rclcpp::ServicesQoS(), ip_client_cb_group_); returnTrajectoryClient = create_client(ServiceNames::getObjectReturnTrajectory); stateService = create_service( @@ -207,8 +203,6 @@ bool ObjectControl::loadScenario() { exec->add_node(object); objects.emplace(id, object); objects.at(id)->setTransmitterID(id); - objects.at(id)->setMaxAllowedMonitorPeriod( - std::chrono::milliseconds(this->get_parameter("monr_timeout_period_ms").as_int())); std::promise trajLoaded; auto trajectoryCallback = @@ -295,17 +289,17 @@ bool ObjectControl::loadScenario() { std::future trajLoadedFuture = trajLoaded.get_future(); std::future ipLoadedFuture = ipLoaded.get_future(); std::future originLoadedFuture = originLoaded.get_future(); - if (auto status = trajLoadedFuture.wait_for(20s); + if (auto status = trajLoadedFuture.wait_for(5s); status != std::future_status::ready || !trajLoadedFuture.get()) { RCLCPP_ERROR(get_logger(), "Trajectory loading failed for object ID %u", id); successful = false; } - if (auto status = ipLoadedFuture.wait_for(2s); + if (auto status = ipLoadedFuture.wait_for(250ms); status != std::future_status::ready || !ipLoadedFuture.get()) { RCLCPP_ERROR(get_logger(), "IP loading failed for object ID %u", id); successful = false; } - if (auto status = originLoadedFuture.wait_for(2s); + if (auto status = originLoadedFuture.wait_for(250ms); status != std::future_status::ready || !originLoadedFuture.get()) { RCLCPP_ERROR(get_logger(), "Origin loading failed for object ID %u", id); successful = false; @@ -326,7 +320,7 @@ bool ObjectControl::loadScenario() { auto future = idClient->async_send_request(request, idsCallback); // Wait for all objects to load std::future scenarioLoadedFuture = scenarioLoaded.get_future(); - if (auto status = scenarioLoadedFuture.wait_for(30s); status == std::future_status::ready) { + if (auto status = scenarioLoadedFuture.wait_for(10s); status == std::future_status::ready) { return scenarioLoadedFuture.get(); // Get the status value } else if (status == std::future_status::timeout) { RCLCPP_ERROR(get_logger(), "Scenario loading timed out"); @@ -341,6 +335,63 @@ bool ObjectControl::loadScenario() { return false; } +void ObjectControl::loadObjectFiles() { + char path[MAX_FILE_PATH]; + std::vector errors; + + UtilGetObjectDirectoryPath(path, sizeof(path)); + fs::path objectDir(path); + if (!fs::exists(objectDir)) { + throw std::ios_base::failure("Object directory does not exist"); + } + + for (const auto& entry : fs::directory_iterator(objectDir)) { + if (fs::is_regular_file(entry.status())) { + const auto inputFile = entry.path(); + ObjectConfig conf(get_logger()); + try { + conf.parseConfigurationFile(inputFile); + uint32_t id = conf.getTransmitterID(); + RCLCPP_INFO(get_logger(), "Loaded configuration: %s", conf.toString().c_str()); + // Check preexisting + auto foundObject = objects.find(id); + if (foundObject == objects.end()) { + std::shared_ptr object = std::make_shared(id); + object->parseConfigurationFile(inputFile); + objects.emplace(id, object); + } else { + auto badID = conf.getTransmitterID(); + std::string errMsg = "Duplicate object ID " + std::to_string(badID) + " detected in files " + + objects.at(badID)->getTrajectoryFileName() + " and " + + conf.getTrajectoryFileName(); + throw std::invalid_argument(errMsg); + } + } catch (std::invalid_argument& e) { + RCLCPP_ERROR(get_logger(), "%s", e.what()); + errors.push_back(e); + } + } + } + + // Fix injector ID maps - reverse their direction + for (const auto& id : getVehicleIDs()) { + auto injMap = objects.at(id)->getObjectConfig().getInjectionMap(); + for (const auto& sourceID : injMap.sourceIDs) { + auto conf = objects.at(sourceID)->getObjectConfig(); + conf.addInjectionTarget(id); + objects.at(sourceID)->setObjectConfig(conf); + } + } + + if (!errors.empty()) { + objects.clear(); + std::ostringstream ostr; + auto append = [&ostr](const std::invalid_argument& e) { ostr << e.what() << std::endl; }; + std::for_each(errors.begin(), errors.end(), append); + throw std::invalid_argument("Failed to parse object file(s):\n" + ostr.str()); + } +} + uint32_t ObjectControl::getAnchorObjectID() const { for (auto& object : objects) { if (object.second->isAnchor()) { @@ -468,7 +519,7 @@ void ObjectControl::heartbeat() { objects.at(id)->sendHeartbeat(controlCenterStatus()); } } catch (std::exception& e) { - RCLCPP_WARN(get_logger(), e.what()); + RCLCPP_WARN(get_logger(), "%s", e.what()); objects.at(id)->disconnect(); sm.process_event(state_machine::events::DisconnectedFromObject{id}); } @@ -536,8 +587,8 @@ void ObjectControl::notifyObjectsConnected() { } void ObjectControl::connectToObject(std::shared_ptr obj, std::shared_future& connStopReq) { - const int maxConnHeabs = this->get_parameter("max_missing_heartbeats").as_int(); - const int maxConnMonrs = this->get_parameter("max_missing_monr").as_int(); + const int maxConnHeabs = this->get_parameter("max_missing_heartbeats").as_int(); + constexpr int maxConnMonrs = 100; try { if (!obj->isConnected()) { try { diff --git a/atos/modules/ObjectControl/src/objectlistener.cpp b/atos/modules/ObjectControl/src/objectlistener.cpp index 69426a3b9..e67a05cee 100644 --- a/atos/modules/ObjectControl/src/objectlistener.cpp +++ b/atos/modules/ObjectControl/src/objectlistener.cpp @@ -6,10 +6,10 @@ #include "objectlistener.hpp" #include "objectcontrol.hpp" -ObjectListener::ObjectListener(ObjectControl* sh, std::shared_ptr ob, rclcpp::Logger log) : - Loggable(log), - obj(ob), - handler(sh) { +ObjectListener::ObjectListener(ObjectControl* sh, std::shared_ptr ob, rclcpp::Logger log) : + Loggable(log), + obj(ob), + handler(sh) { if (!obj->isConnected()) { throw std::invalid_argument("Attempted to start listener for disconnected object"); @@ -31,15 +31,15 @@ ObjectListener::~ObjectListener() { void ObjectListener::listen() { try { while (!this->quit) { - // handle incoming iso22133 messages + //handle incoming iso22133 messages obj->handleISOMessage(true); } } catch (std::invalid_argument& e) { - RCLCPP_ERROR(get_logger(), e.what()); // TODO: add comment explaining this case.. - } catch (std::range_error& e) { - RCLCPP_DEBUG(get_logger(), e.what()); // Socket was interrupted intentionally, exit gracefully + RCLCPP_ERROR(get_logger(), "%s", e.what()); // TODO: add comment explaining this case.. + } catch (std::range_error& e){ + RCLCPP_DEBUG(get_logger(), "%s", e.what()); // Socket was interrupted intentionally, exit gracefully } catch (std::runtime_error& e) { - RCLCPP_ERROR(get_logger(), e.what()); + RCLCPP_ERROR(get_logger(), "%s", e.what()); obj->disconnect(); handler->sm.process_event(state_machine::events::DisconnectedFromObject{obj->getTransmitterID()}); } diff --git a/atos/modules/TrajectoryletStreamer/inc/trajectoryletstreamer.hpp b/atos/modules/TrajectoryletStreamer/inc/trajectoryletstreamer.hpp index 3e2cfc281..98114f1a8 100644 --- a/atos/modules/TrajectoryletStreamer/inc/trajectoryletstreamer.hpp +++ b/atos/modules/TrajectoryletStreamer/inc/trajectoryletstreamer.hpp @@ -23,8 +23,8 @@ class TrajectoryletStreamer : public Module { private: static inline std::string const moduleName = "trajectorylet_streamer"; - void onInitMessage(const ROSChannels::Init::message_type::SharedPtr); - void onObjectsConnectedMessage(const ROSChannels::ObjectsConnected::message_type::SharedPtr); + void onInitMessage(const ROSChannels::Init::message_type::SharedPtr) override; + void onObjectsConnectedMessage(const ROSChannels::ObjectsConnected::message_type::SharedPtr) override; void onAbortMessage(const ROSChannels::Abort::message_type::SharedPtr) override; void onStopMessage(const ROSChannels::Stop::message_type::SharedPtr) override; diff --git a/docs/Installation/installation.md b/docs/Installation/installation.md index 196c3a0df..8ec4a020b 100644 --- a/docs/Installation/installation.md +++ b/docs/Installation/installation.md @@ -17,7 +17,7 @@ sudo chown -R $USER:$USER ~/.astazero/ATOS/ ``` ## Running natively -ATOS comes with an installation script that automates the installation process. It is intended for use on Ubuntu 22.04. The script will install ROS 2, ATOS dependencies, and ATOS itself. It will also create a workspace and build ATOS. The script can be executed using the following command: +ATOS comes with an installation script that automates the installation process. Native installation is supported on Ubuntu 20.04 with ROS 2 Foxy, Ubuntu 22.04 with ROS 2 Humble, and Ubuntu 24.04 with ROS 2 Jazzy. The script will install ROS 2, ATOS dependencies, and ATOS itself. It will also create a workspace and build ATOS. The script can be executed using the following command: ```bash ./setup_atos.sh ``` diff --git a/scripts/installation/install_atos.sh b/scripts/installation/install_atos.sh index 6f48442e6..2bf37acef 100755 --- a/scripts/installation/install_atos.sh +++ b/scripts/installation/install_atos.sh @@ -13,6 +13,21 @@ ATOS_REPO_PATH="$1" source "${ATOS_REPO_PATH}/scripts/installation/install_functions.sh" check_command_failed $? "Failed to source ${ATOS_REPO_PATH}/scripts/installation/install_functions.sh" +ROS_DISTRO="${ROS_DISTRO:-$(get_supported_ros_distro)}" +check_command_failed $? "Failed to determine ROS 2 distribution for this Ubuntu release." +ATOS_VENV_PATH="$(get_atos_venv_path)" + +if [ -z "${ROS_DISTRO}" ]; then + echo "Failed to determine a supported ROS 2 distribution for this Ubuntu release." + exit 1 +fi + +if [ ! -f "${ATOS_VENV_PATH}/bin/activate" ]; then + echo "ATOS Python virtual environment was not found at ${ATOS_VENV_PATH}." + echo "Run setup_atos.sh again to recreate Python dependencies." + exit 1 +fi + ################################################ ############## Install ATOS #################### ################################################ @@ -42,6 +57,8 @@ cd - # temporarily cd into the workspace and build with colcon echo "Building ATOS..." cd $HOME/atos_ws +# shellcheck disable=SC1090 +source "${ATOS_VENV_PATH}/bin/activate" source /opt/ros/$ROS_DISTRO/setup.bash MAKEFLAGS=-j4 colcon build --symlink-install --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON check_command_failed $? "Failed to build ATOS." @@ -51,15 +68,23 @@ cd - ###### Configure setup scripts ###### ##################################### +atos_venv_setup_script="source $ATOS_VENV_PATH/bin/activate" +atos_python_site_packages="$(python -c 'import site; print(site.getsitepackages()[0])')" +check_command_failed $? "Failed to determine ATOS Python site-packages path." +atos_pythonpath_script="export PYTHONPATH=$atos_python_site_packages:\$PYTHONPATH" atos_setup_script="source $HOME/atos_ws/install/setup." ros2_setup_script="source /opt/ros/$ROS_DISTRO/setup." case "$SHELL" in */bash) + add_source_line_if_needed $HOME/.bashrc "bash" "${atos_venv_setup_script}" + add_source_line_if_needed $HOME/.bashrc "bash" "${atos_pythonpath_script}" add_source_line_if_needed $HOME/.bashrc "bash" "${ros2_setup_script}" add_source_line_if_needed $HOME/.bashrc "bash" "${atos_setup_script}" ;; */zsh) + add_source_line_if_needed $HOME/.zshrc "zsh" "${atos_venv_setup_script}" + add_source_line_if_needed $HOME/.zshrc "zsh" "${atos_pythonpath_script}" add_source_line_if_needed $HOME/.zshrc "zsh" "${ros2_setup_script}" add_source_line_if_needed $HOME/.zshrc "zsh" "${atos_setup_script}" ;; diff --git a/scripts/installation/install_functions.sh b/scripts/installation/install_functions.sh index 4b85d6876..da9b9dae5 100755 --- a/scripts/installation/install_functions.sh +++ b/scripts/installation/install_functions.sh @@ -3,7 +3,59 @@ # Function to get Ubuntu distribution codename get_ubuntu_codename() { source /etc/os-release - echo $UBUNTU_CODENAME + + if [ -n "${UBUNTU_CODENAME:-}" ]; then + echo "$UBUNTU_CODENAME" + return 0 + fi + + if [ -n "${VERSION_CODENAME:-}" ]; then + echo "$VERSION_CODENAME" + return 0 + fi + + case "${VERSION_ID:-}" in + "20.04"|"20") + echo "focal" + ;; + "22.04"|"22") + echo "jammy" + ;; + "24.04"|"24") + echo "noble" + ;; + esac +} + +is_standard_ubuntu() { + source /etc/os-release + [ "${ID:-}" = "ubuntu" ] +} + +is_ubuntu_core() { + source /etc/os-release + [ "${ID:-}" = "ubuntu-core" ] +} + +get_supported_ros_distro() { + case "$(get_ubuntu_codename)" in + "focal") + echo "foxy" + ;; + "jammy") + echo "humble" + ;; + "noble") + echo "jazzy" + ;; + *) + return 1 + ;; + esac +} + +get_atos_venv_path() { + echo "$HOME/.local/share/atos/venv" } # Function that checks if command failed @@ -39,7 +91,7 @@ update_symlink() { add_source_line_if_needed() { local file="$1" local shell_type="$2" - local source_line="$3$shell_type" + local source_line="$3" if ! grep -qF "$source_line" "$file"; then # Ask the user only for interactive local shells. diff --git a/setup_atos.sh b/setup_atos.sh index 91ef83090..6ada7c3eb 100755 --- a/setup_atos.sh +++ b/setup_atos.sh @@ -9,28 +9,38 @@ source "scripts/installation/install_functions.sh" # Get this file location REPO_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" &> /dev/null && pwd)" -# Check if running on Ubuntu -if ! grep -q "Ubuntu" /etc/os-release; then - echo "This script is designed for Ubuntu systems only." +# Check if running on a supported Ubuntu variant +if is_ubuntu_core; then + echo "Ubuntu Core is not supported for native ATOS installation." + echo "Use Docker instead, or run this script on Ubuntu 20.04, 22.04, or 24.04 with apt available." exit 1 fi -# Set ROS_DISTRO based on Ubuntu distribution -case "$(get_ubuntu_codename)" in - "focal") - ;; - "jammy") - ;; - *) - echo "Unsupported Ubuntu distribution. Only 20.04 (focal) and 22.04 (jammy) are supported." +if ! is_standard_ubuntu; then + echo "This script is designed for standard Ubuntu systems only." exit 1 - ;; -esac +fi + +UBUNTU_CODENAME="$(get_ubuntu_codename)" +ROS_DISTRO="$(get_supported_ros_distro)" + +if [ -z "${UBUNTU_CODENAME}" ] || [ -z "${ROS_DISTRO}" ]; then + echo "Unsupported Ubuntu distribution. Supported releases are 20.04 (focal), 22.04 (jammy), and 24.04 (noble)." + exit 1 +fi + +if ! command -v apt-get >/dev/null 2>&1; then + echo "apt-get is required for native ATOS installation but was not found on this system." + exit 1 +fi + +export ROS_DISTRO # Add -h/--help option if [ "$1" == "-h" ] || [ "$1" == "--help" ]; then echo "Usage: ./setup_atos.sh [single option]" - echo "This script will install all necessary dependencies, setup the ROS workspace at ~/atos_ws and install ATOS. Please open and inspect this script for further details." + echo "This script will install all necessary dependencies, setup the ROS workspace at ~/atos_ws and install ATOS." + echo "Supported native targets: Ubuntu 20.04 with ROS 2 Foxy, Ubuntu 22.04 with ROS 2 Humble, and Ubuntu 24.04 with ROS 2 Jazzy." echo "Options:" echo " -h, --help Show this help message and exit" echo " -r Reinstall dependencies" From cc20e84a17075377d38092698e6a21f389596e91 Mon Sep 17 00:00:00 2001 From: Sebastian Loh Lindholm Date: Mon, 11 May 2026 13:09:00 +0200 Subject: [PATCH 23/33] Timestep to esminiadapter --- .../EsminiAdapter/src/esminiadapter.cpp | 21 ++++++++++++++++--- .../ObjectControl/src/objectcontrol.cpp | 8 +++---- 2 files changed, 22 insertions(+), 7 deletions(-) diff --git a/atos/modules/EsminiAdapter/src/esminiadapter.cpp b/atos/modules/EsminiAdapter/src/esminiadapter.cpp index 72691b879..9f2702ff8 100644 --- a/atos/modules/EsminiAdapter/src/esminiadapter.cpp +++ b/atos/modules/EsminiAdapter/src/esminiadapter.cpp @@ -12,6 +12,7 @@ #include #include #include +#include #include "atos_interfaces/msg/cartesian_trajectory.hpp" #include "rclcpp/wait_for_message.hpp" @@ -27,6 +28,20 @@ using std::placeholders::_1; using std::placeholders::_2; using namespace std::chrono_literals; +namespace +{ +template +int reportObjectPos(ReportObjectPosFn reportFn, int objectId, double timestamp, double x, double y, double z, double h, double p, double r) +{ + if constexpr (std::is_invocable_v) { + return reportFn(objectId, static_cast(timestamp), static_cast(x), static_cast(y), static_cast(z), + static_cast(h), static_cast(p), static_cast(r)); + } else { + return reportFn(objectId, x, y, z, h, p, r); + } +} +} + @@ -52,9 +67,9 @@ EsminiAdapter::EsminiAdapter() : Module(moduleName), oscFilePathClient_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); objectIdsClient_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); oscFilePathClient_ = create_client( - ServiceNames::getOpenScenarioFilePath, rclcpp::ServicesQoS(), oscFilePathClient_cb_group_); + ServiceNames::getOpenScenarioFilePath, rmw_qos_profile_services_default, oscFilePathClient_cb_group_); objectIdsClient_ = create_client( - ServiceNames::getObjectIds, rclcpp::ServicesQoS(), objectIdsClient_cb_group_); + ServiceNames::getObjectIds, rmw_qos_profile_services_default, objectIdsClient_cb_group_); declare_parameter("timestep", 0.1); } @@ -232,7 +247,7 @@ void EsminiAdapter::reportObjectPosition(const Monitor::message_type::SharedPtr auto speed = monr->velocity.twist.linear; // Reporting to Esmini - SE_ReportObjectPos(esminiObjectId, pos.x, pos.y, pos.z, yaw, pitch, roll); + reportObjectPos(&SE_ReportObjectPos, esminiObjectId, SE_GetSimulationTime(), pos.x, pos.y, pos.z, yaw, pitch, roll); SE_ReportObjectSpeed(esminiObjectId, speed.x); } diff --git a/atos/modules/ObjectControl/src/objectcontrol.cpp b/atos/modules/ObjectControl/src/objectcontrol.cpp index 0d1b44b71..35f761f8e 100644 --- a/atos/modules/ObjectControl/src/objectcontrol.cpp +++ b/atos/modules/ObjectControl/src/objectcontrol.cpp @@ -56,13 +56,13 @@ ObjectControl::ObjectControl(std::shared_ptrdeclare_parameter("max_missing_heartbeats", 100); objectsConnectedTimer = create_wall_timer(1000ms, std::bind(&ObjectControl::publishObjectIds, this)); idClient = create_client( - ServiceNames::getObjectIds, rclcpp::ServicesQoS(), id_client_cb_group_); + ServiceNames::getObjectIds, rmw_qos_profile_services_default, id_client_cb_group_); originClient = create_client( - ServiceNames::getTestOrigin, rclcpp::ServicesQoS(), origin_client_cb_group_); + ServiceNames::getTestOrigin, rmw_qos_profile_services_default, origin_client_cb_group_); trajectoryClient = create_client( - ServiceNames::getObjectTrajectory, rclcpp::ServicesQoS(), traj_client_cb_group_); + ServiceNames::getObjectTrajectory, rmw_qos_profile_services_default, traj_client_cb_group_); ipClient = create_client( - ServiceNames::getObjectIp, rclcpp::ServicesQoS(), ip_client_cb_group_); + ServiceNames::getObjectIp, rmw_qos_profile_services_default, ip_client_cb_group_); returnTrajectoryClient = create_client(ServiceNames::getObjectReturnTrajectory); stateService = create_service( From 630f5aad4b8ea2511a26ffa9600a4bc58f69dfd5 Mon Sep 17 00:00:00 2001 From: Sebastian Loh Lindholm Date: Thu, 4 Jun 2026 12:40:39 +0200 Subject: [PATCH 24/33] NiceGUI running in normal single server process --- atos_gui/atos_gui/main.py | 48 +++++++++++++++++++++++---------------- 1 file changed, 28 insertions(+), 20 deletions(-) diff --git a/atos_gui/atos_gui/main.py b/atos_gui/atos_gui/main.py index eb1cd5e85..adfdabdf0 100644 --- a/atos_gui/atos_gui/main.py +++ b/atos_gui/atos_gui/main.py @@ -28,13 +28,13 @@ FLEET_STATE_LOCK = threading.Lock() FLEET_TRUCK_STATES: dict[str, dict] = {} FLEET_GEOJSON_CACHE: dict[str, dict] = {} +APP_CONFIGURED = False def main() -> None: - # NOTE: This function is defined as the ROS entry point in setup.py, - # but it's empty to enable NiceGUI auto-reloading. - pass + configure_app() + ui.run(**build_ui_run_args()) def _candidate_conf_dirs() -> list[Path]: @@ -273,25 +273,33 @@ def print_access_hint() -> None: print(f"TruckObjectGUI ready. Open {scheme}://localhost:8420", flush=True) -# Start the ROS node logic in a thread managed by nicegui. -app.on_startup(lambda: threading.Thread(target=ros_main, daemon=True).start()) -app.on_startup(print_access_hint) +def configure_app() -> None: + global APP_CONFIGURED + if APP_CONFIGURED: + return -ui_run.APP_IMPORT_STRING = f"{__name__}:app" # ROS2 uses non-standard module naming. + app.on_startup(lambda: threading.Thread(target=ros_main, daemon=True).start()) + app.on_startup(print_access_hint) + APP_CONFIGURED = True -uvicorn_args = { - "uvicorn_reload_dirs": str(Path(__file__).parent.resolve()), - "host": "0.0.0.0", - "port": 8420, - "show": False, - "title": "TruckObjectGUI" if FLEET_MODE else "ATOS GUI", -} -if USE_SSL: - uvicorn_args["ssl_keyfile"] = Path.home() / ".astazero/ATOS/certs/selfsigned.key" - uvicorn_args["ssl_certfile"] = Path.home() / ".astazero/ATOS/certs/selfsigned.crt" +def build_ui_run_args() -> dict: + ui_run.APP_IMPORT_STRING = f"{__name__}:app" + + uvicorn_args = { + "host": "0.0.0.0", + "port": 8420, + "reload": False, + "show": False, + "title": "TruckObjectGUI" if FLEET_MODE else "ATOS GUI", + } + + if USE_SSL: + uvicorn_args["ssl_keyfile"] = Path.home() / ".astazero/ATOS/certs/selfsigned.key" + uvicorn_args["ssl_certfile"] = Path.home() / ".astazero/ATOS/certs/selfsigned.crt" + + return uvicorn_args -ui.run(**uvicorn_args) -if USE_SSL: - print("ATTENTION: Using SSL, use https://localhost:8420 to access the GUI instead", flush=True) +if __name__ in {"__main__", "__mp_main__"}: + main() From 2c6db55eb4cf53f57c3d679a9f1cd210d89b5dd4 Mon Sep 17 00:00:00 2001 From: Sebastian Loh Lindholm Date: Thu, 18 Jun 2026 14:11:55 +0200 Subject: [PATCH 25/33] Compile error fixed after rebasing --- atos/modules/MQTTBridge/inc/Imqtt2ros.hpp | 2 +- atos/modules/ObjectControl/inc/objectcontrol.hpp | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/atos/modules/MQTTBridge/inc/Imqtt2ros.hpp b/atos/modules/MQTTBridge/inc/Imqtt2ros.hpp index 52daa363f..cb619d830 100644 --- a/atos/modules/MQTTBridge/inc/Imqtt2ros.hpp +++ b/atos/modules/MQTTBridge/inc/Imqtt2ros.hpp @@ -15,7 +15,7 @@ struct Mqtt2RosInterface { struct { std::string topic; ///< ROS topic std::string msg_type; ///< message type of publisher - rclcpp::GenericPublisher::SharedPtr publisher; + rclcpp::Publisher::SharedPtr publisher; int queue_size = 1; ///< ROS publisher queue size bool is_stale = false; ///< whether a new generic publisher/subscriber is required } ros; ///< ROS-related variables diff --git a/atos/modules/ObjectControl/inc/objectcontrol.hpp b/atos/modules/ObjectControl/inc/objectcontrol.hpp index 88347aee3..d7ac83641 100644 --- a/atos/modules/ObjectControl/inc/objectcontrol.hpp +++ b/atos/modules/ObjectControl/inc/objectcontrol.hpp @@ -270,6 +270,7 @@ class ObjectControl : public Module { //! \brief Read the configured object and trajectory files and load related data //! into the ScenarioHandler. bool loadScenario(); + void loadObjectFiles(); //! \brief Transform the scenario trajectories relative to the trajectory of the //! specified object. void transformScenarioRelativeTo(const uint32_t objectID); From 1a1b46cdc52de4d1c00c03f20e1cb05ffc716cac Mon Sep 17 00:00:00 2001 From: Sebastian Loh Lindholm Date: Thu, 18 Jun 2026 14:12:29 +0200 Subject: [PATCH 26/33] format --- atos/common/module.hpp | 128 ++- atos/common/roschannels/roschannel.hpp | 35 +- atos/launch/launch_atosfleetmanagement.py | 330 ++++---- .../DirectControl/src/directcontrol.cpp | 186 ++--- .../EsminiAdapter/src/esminiadapter.cpp | 380 +++++---- .../src/scenarioexecution.cpp | 68 +- atos/modules/MQTTBridge/src/mqttbridge.cpp | 769 +++++++++--------- .../ObjectControl/inc/relativeanchor.hpp | 6 +- .../ObjectControl/inc/relativetestobject.hpp | 22 +- .../ObjectControl/src/objectlistener.cpp | 12 +- atos_gui/atos_gui/main.py | 39 +- atos_gui/atos_gui/objectpanel/objectpanel.py | 42 +- 12 files changed, 1010 insertions(+), 1007 deletions(-) diff --git a/atos/common/module.hpp b/atos/common/module.hpp index ff62b2e27..a2ba23ffe 100644 --- a/atos/common/module.hpp +++ b/atos/common/module.hpp @@ -8,27 +8,27 @@ #include #include -#include #include "roschannels/commandchannels.hpp" +#include namespace ServiceNames { -const std::string initDataDict = "init_data_dictionary"; -const std::string getObjectIds = "get_object_ids"; -const std::string getObjectIp = "get_object_ip"; -const std::string getTestOrigin = "get_test_origin"; -const std::string getObjectTrajectory = "get_object_trajectory"; -const std::string getObjectTriggerStart = "get_object_trigger_start"; -const std::string getObjectControlState = "get_object_control_state"; +const std::string initDataDict = "init_data_dictionary"; +const std::string getObjectIds = "get_object_ids"; +const std::string getObjectIp = "get_object_ip"; +const std::string getTestOrigin = "get_test_origin"; +const std::string getObjectTrajectory = "get_object_trajectory"; +const std::string getObjectTriggerStart = "get_object_trigger_start"; +const std::string getObjectControlState = "get_object_control_state"; const std::string getObjectReturnTrajectory = "get_object_return_trajectory"; -const std::string getOpenScenarioFilePath = "get_open_scenario_file_path"; -} +const std::string getOpenScenarioFilePath = "get_open_scenario_file_path"; +} // namespace ServiceNames // TODO move somewhere else? also make generic to allow more args (variadic template)? /*! * \brief Facilitates one-line intialization * of a ros message with one argument. */ -template +template Msg_T msgCtr1(MsgData_T data) { Msg_T ret; ret.data = data; @@ -43,51 +43,51 @@ Msg_T msgCtr1(MsgData_T data) { * \param name name of the module. Is passed to the ROS node. */ class Module : public rclcpp::Node { - public: - Module(const std::string name) : Node(name), getStatusResponsePub(*this) {}; +public: + Module(const std::string name) : + Node(name), + getStatusResponsePub(*this) {}; Module() = delete; bool shouldExit(); - protected: - bool quit=false; +protected: + bool quit = false; ROSChannels::GetStatusResponse::Pub getStatusResponsePub; - virtual void onFailureMessage(const ROSChannels::Failure::message_type::SharedPtr){}; - virtual void onGetStatusResponse(const ROSChannels::GetStatusResponse::message_type::SharedPtr){}; + virtual void onFailureMessage(const ROSChannels::Failure::message_type::SharedPtr) {}; + virtual void onGetStatusResponse(const ROSChannels::GetStatusResponse::message_type::SharedPtr) {}; virtual void onGetStatusMessage(const ROSChannels::GetStatus::message_type::SharedPtr) { auto msg = std_msgs::msg::String(); msg.data = this->get_name(); getStatusResponsePub.publish(msg); }; - virtual void onInitMessage(const ROSChannels::Init::message_type::SharedPtr){}; - virtual void onConnectMessage(const ROSChannels::Connect::message_type::SharedPtr){}; - virtual void onDisconnectMessage(const ROSChannels::Disconnect::message_type::SharedPtr){}; - virtual void onArmMessage(const ROSChannels::Arm::message_type::SharedPtr){}; - virtual void onDisarmMessage(const ROSChannels::Disarm::message_type::SharedPtr){}; - virtual void onObjectsConnectedMessage(const ROSChannels::ObjectsConnected::message_type::SharedPtr){}; - virtual void onAllClearMessage(const ROSChannels::AllClear::message_type::SharedPtr){}; - virtual void onStartMessage(const ROSChannels::Start::message_type::SharedPtr){}; - virtual void onStartObjectMessage(const ROSChannels::StartObject::message_type::SharedPtr){}; - virtual void onStopMessage(const ROSChannels::Stop::message_type::SharedPtr){}; - virtual void onAbortMessage(const ROSChannels::Abort::message_type::SharedPtr){}; - virtual void onReplayMessage(const ROSChannels::Replay::message_type::SharedPtr){}; + virtual void onInitMessage(const ROSChannels::Init::message_type::SharedPtr) {}; + virtual void onConnectMessage(const ROSChannels::Connect::message_type::SharedPtr) {}; + virtual void onDisconnectMessage(const ROSChannels::Disconnect::message_type::SharedPtr) {}; + virtual void onArmMessage(const ROSChannels::Arm::message_type::SharedPtr) {}; + virtual void onDisarmMessage(const ROSChannels::Disarm::message_type::SharedPtr) {}; + virtual void onObjectsConnectedMessage(const ROSChannels::ObjectsConnected::message_type::SharedPtr) {}; + virtual void onAllClearMessage(const ROSChannels::AllClear::message_type::SharedPtr) {}; + virtual void onStartMessage(const ROSChannels::Start::message_type::SharedPtr) {}; + virtual void onStartObjectMessage(const ROSChannels::StartObject::message_type::SharedPtr) {}; + virtual void onStopMessage(const ROSChannels::Stop::message_type::SharedPtr) {}; + virtual void onAbortMessage(const ROSChannels::Abort::message_type::SharedPtr) {}; + virtual void onReplayMessage(const ROSChannels::Replay::message_type::SharedPtr) {}; virtual void onExitMessage(const ROSChannels::Exit::message_type::SharedPtr); - virtual void onResetTestObjectsMessage(const ROSChannels::ResetTestObjects::message_type::SharedPtr){}; - virtual void onReloadObjectSettingsMessage(const ROSChannels::ReloadObjectSettings::message_type::SharedPtr){}; - + virtual void onResetTestObjectsMessage(const ROSChannels::ResetTestObjects::message_type::SharedPtr) {}; + virtual void onReloadObjectSettingsMessage(const ROSChannels::ReloadObjectSettings::message_type::SharedPtr) {}; static void tryHandleMessage(std::function tryExecute, std::function executeIfFail, const std::string& topic, const rclcpp::Logger& logger); - /** * @brief This helper function performs a service call given a client and yields a response. This * function is used when you want to specify the request instead of sending an empty request. - * + * * @tparam Srv Srv The name of the service to request. * @param timeout The timeout for the service call. * @param client The client to use to request the service. @@ -95,12 +95,11 @@ class Module : public rclcpp::Node { * @param request The request of the service, with the data to be sent. Defaults to an empty request. * @return The response of the service. */ - template - bool callService( const std::chrono::duration< double > &timeout, - std::shared_ptr> &client, - std::shared_ptr &response, - std::shared_ptr request = std::make_shared()) -{ + template + bool callService(const std::chrono::duration& timeout, + std::shared_ptr>& client, + std::shared_ptr& response, + std::shared_ptr request = std::make_shared()) { auto promise = client->async_send_request(request); if (rclcpp::spin_until_future_complete(get_node_base_interface(), promise, timeout) == rclcpp::FutureReturnCode::SUCCESS) { @@ -113,17 +112,16 @@ class Module : public rclcpp::Node { } /*! \brief This helper function waits for a service to become available and returns a client. - * \tparam Srv The name of the service to request. - * \param n The number of times to retry. - * \param timeout The timeout to wait for the service. - * \param serviceName The name of the service to request. - * \return The response of the service. - */ - template + * \tparam Srv The name of the service to request. + * \param n The number of times to retry. + * \param timeout The timeout to wait for the service. + * \param serviceName The name of the service to request. + * \return The response of the service. + */ + template typename std::shared_ptr> nTimesWaitForService(int n, - const std::chrono::duration< double > &timeout, - const std::string &serviceName) -{ + const std::chrono::duration& timeout, + const std::string& serviceName) { int retries = 0; auto client = create_client(serviceName); @@ -140,25 +138,23 @@ class Module : public rclcpp::Node { throw std::runtime_error("Failed to initialize service " + serviceName); } - /*! \brief This helper function is used by rosnodes to request a service. - * \tparam Srv The name of the service to request. - * \param n Number of times to retry until accepting the service is not available. - * \param timeout The timeout to wait for the service. - * \param serviceName The name of the service to request. - * \param response Response of the service, to be returned. - * \return The response of the service. - */ - template - bool nShotServiceRequest(int n, - const std::chrono::duration< double > &timeout, - const std::string &serviceName, - std::shared_ptr &response) { + * \tparam Srv The name of the service to request. + * \param n Number of times to retry until accepting the service is not available. + * \param timeout The timeout to wait for the service. + * \param serviceName The name of the service to request. + * \param response Response of the service, to be returned. + * \return The response of the service. + */ + template + bool nShotServiceRequest(int n, + const std::chrono::duration& timeout, + const std::string& serviceName, + std::shared_ptr& response) { std::shared_ptr> client; - try{ + try { client = nTimesWaitForService(n, timeout, serviceName); - } - catch (std::runtime_error &e){ + } catch (std::runtime_error& e) { RCLCPP_ERROR(get_logger(), "Failed to initialize service %s", serviceName.c_str()); return false; } diff --git a/atos/common/roschannels/roschannel.hpp b/atos/common/roschannels/roschannel.hpp index 8e608f4aa..adb4399ef 100644 --- a/atos/common/roschannels/roschannel.hpp +++ b/atos/common/roschannels/roschannel.hpp @@ -5,36 +5,37 @@ */ #pragma once +#include #include #include -#include namespace ROSChannels { template class BasePub { public: - BasePub(rclcpp::Node& node, - const std::string& topicName, - const rclcpp::QoS& qos = rclcpp::QoS(rclcpp::KeepAll())) - : pub(node.create_publisher(topicName, qos)) {} - BasePub() = delete; - virtual ~BasePub() = default; - typename rclcpp::Publisher::SharedPtr pub; - inline virtual void publish(const T& msg) { assert(pub); pub->publish(msg); }; + BasePub(rclcpp::Node& node, const std::string& topicName, const rclcpp::QoS& qos = rclcpp::QoS(rclcpp::KeepAll())) : + pub(node.create_publisher(topicName, qos)) {} + BasePub() = delete; + virtual ~BasePub() = default; + typename rclcpp::Publisher::SharedPtr pub; + inline virtual void publish(const T& msg) { + assert(pub); + pub->publish(msg); + }; }; template class BaseSub { public: - BaseSub(rclcpp::Node& node, - const std::string& topicName, - std::function callback, - const rclcpp::QoS& qos = rclcpp::QoS(rclcpp::KeepAll())) - : sub(node.create_subscription(topicName, qos, callback)) {} - BaseSub() = delete; - virtual ~BaseSub() = default; - typename rclcpp::Subscription::SharedPtr sub; + BaseSub(rclcpp::Node& node, + const std::string& topicName, + std::function callback, + const rclcpp::QoS& qos = rclcpp::QoS(rclcpp::KeepAll())) : + sub(node.create_subscription(topicName, qos, callback)) {} + BaseSub() = delete; + virtual ~BaseSub() = default; + typename rclcpp::Subscription::SharedPtr sub; }; } // namespace ROSChannels diff --git a/atos/launch/launch_atosfleetmanagement.py b/atos/launch/launch_atosfleetmanagement.py index 72bf26486..0d2c1054a 100644 --- a/atos/launch/launch_atosfleetmanagement.py +++ b/atos/launch/launch_atosfleetmanagement.py @@ -9,169 +9,193 @@ from launch_ros.actions import Node # Need to modify sys.path since we launch from the ros2 installed path. -sys.path.insert(0, os.path.join(get_package_prefix('atos'), 'share', 'atos', 'launch')) +sys.path.insert(0, os.path.join(get_package_prefix("atos"), "share", "atos", "launch")) def generate_launch_description(): - insecure_websockets = LaunchConfiguration('insecure') - foxbridge = LaunchConfiguration('foxbridge') - with_simulator = LaunchConfiguration('with_truck_simulator') - cot_tls_require_client_cert = LaunchConfiguration('cot_tls_require_client_cert') - cot_tls_cert_path = LaunchConfiguration('cot_tls_cert_path') - cot_tls_key_path = LaunchConfiguration('cot_tls_key_path') - cot_tls_ca_path = LaunchConfiguration('cot_tls_ca_path') + insecure_websockets = LaunchConfiguration("insecure") + foxbridge = LaunchConfiguration("foxbridge") + with_simulator = LaunchConfiguration("with_truck_simulator") + cot_tls_require_client_cert = LaunchConfiguration("cot_tls_require_client_cert") + cot_tls_cert_path = LaunchConfiguration("cot_tls_cert_path") + cot_tls_key_path = LaunchConfiguration("cot_tls_key_path") + cot_tls_ca_path = LaunchConfiguration("cot_tls_ca_path") - insecure_launch_arg = DeclareLaunchArgument('insecure', default_value='False') - foxbridge_launch_arg = DeclareLaunchArgument('foxbridge', default_value='True') - simulator_launch_arg = DeclareLaunchArgument('with_truck_simulator', default_value='False') - cot_tls_require_client_cert_launch_arg = DeclareLaunchArgument('cot_tls_require_client_cert', default_value='False') - cot_tls_cert_path_launch_arg = DeclareLaunchArgument('cot_tls_cert_path', default_value='') - cot_tls_key_path_launch_arg = DeclareLaunchArgument('cot_tls_key_path', default_value='') - cot_tls_ca_path_launch_arg = DeclareLaunchArgument('cot_tls_ca_path', default_value='') + insecure_launch_arg = DeclareLaunchArgument("insecure", default_value="False") + foxbridge_launch_arg = DeclareLaunchArgument("foxbridge", default_value="True") + simulator_launch_arg = DeclareLaunchArgument( + "with_truck_simulator", default_value="False" + ) + cot_tls_require_client_cert_launch_arg = DeclareLaunchArgument( + "cot_tls_require_client_cert", default_value="False" + ) + cot_tls_cert_path_launch_arg = DeclareLaunchArgument( + "cot_tls_cert_path", default_value="" + ) + cot_tls_key_path_launch_arg = DeclareLaunchArgument( + "cot_tls_key_path", default_value="" + ) + cot_tls_ca_path_launch_arg = DeclareLaunchArgument( + "cot_tls_ca_path", default_value="" + ) fox_tls_bridge_params = [ - {'port': 8765}, - {'retry_startup_delay': 5.0}, - {'tls': True}, - {'fragment_timeout': 600}, - {'max_message_size': 10000000}, - {'unregister_timeout': 10.0}, - {'use_compression': False}, + {"port": 8765}, + {"retry_startup_delay": 5.0}, + {"tls": True}, + {"fragment_timeout": 600}, + {"max_message_size": 10000000}, + {"unregister_timeout": 10.0}, + {"use_compression": False}, ] ros_tls_bridge_params = [ - {'port': 9090}, - {'retry_startup_delay': 5.0}, - {'tls': True}, - {'fragment_timeout': 600}, - {'max_message_size': 10000000}, - {'unregister_timeout': 10.0}, - {'use_compression': False}, + {"port": 9090}, + {"retry_startup_delay": 5.0}, + {"tls": True}, + {"fragment_timeout": 600}, + {"max_message_size": 10000000}, + {"unregister_timeout": 10.0}, + {"use_compression": False}, ] fox_bridge_params = [dict(item) for item in fox_tls_bridge_params] - fox_bridge_params[2] = {'tls': False} + fox_bridge_params[2] = {"tls": False} ros_bridge_params = [dict(item) for item in ros_tls_bridge_params] - ros_bridge_params[1] = {'retry_startup_delay': 5.0} - ros_bridge_params[2] = {'tls': False} + ros_bridge_params[1] = {"retry_startup_delay": 5.0} + ros_bridge_params[2] = {"tls": False} - return LaunchDescription([ - foxbridge_launch_arg, - insecure_launch_arg, - simulator_launch_arg, - cot_tls_require_client_cert_launch_arg, - cot_tls_cert_path_launch_arg, - cot_tls_key_path_launch_arg, - cot_tls_ca_path_launch_arg, - Node( - condition=IfCondition(PythonExpression(['not ', LaunchConfiguration('insecure')])), - package='atos_gui', - namespace='atos', - executable='truck_object_gui', - name='truck_object_gui', - output='screen', - arguments=['True', 'atosfleetmanagement'], - ), - Node( - condition=IfCondition(LaunchConfiguration('insecure')), - package='atos_gui', - namespace='atos', - executable='truck_object_gui', - name='truck_object_gui', - output='screen', - arguments=['False', 'atosfleetmanagement'], - ), - Node( - package='atos', - namespace='atos', - executable='truck_object_control', - name='truck_object_control', - output='screen', - parameters=[ - {'cot_tls_require_client_cert': cot_tls_require_client_cert}, - {'cot_tls_cert_path': cot_tls_cert_path}, - {'cot_tls_key_path': cot_tls_key_path}, - {'cot_tls_ca_path': cot_tls_ca_path}, - ], - ), - Node( - condition=IfCondition(with_simulator), - package='atos', - namespace='atos', - executable='atos_truck_simulator', - name='atos_truck_simulator_1', - output='screen', - parameters=[ - {'uid': 'L5S-TRUCK-SIM-1'}, - {'start_index': 0}, - {'target_speed_kmh': 80.0}, - {'acceleration_mps2': 2.0}, - {'ignore_warning_speed_commands': True}, - ], - ), - Node( - condition=IfCondition(with_simulator), - package='atos', - namespace='atos', - executable='atos_truck_simulator', - name='atos_truck_simulator_2', - output='screen', - parameters=[ - {'uid': 'L5S-TRUCK-SIM-2'}, - {'start_index': 250}, - {'target_speed_kmh': 40.0}, - {'acceleration_mps2': 2.0}, - ], - ), - Node( - condition=IfCondition(with_simulator), - package='atos', - namespace='atos', - executable='atos_truck_simulator', - name='atos_truck_simulator_3', - output='screen', - parameters=[ - {'uid': 'L5S-TRUCK-SIM-3'}, - {'start_index': 500}, - {'target_speed_kmh': 40.0}, - {'acceleration_mps2': 2.0}, - ], - ), - Node( - condition=IfCondition(PythonExpression(['not ', foxbridge])), - name='rosapi', - package='rosapi', - executable='rosapi_node', - ), - Node( - condition=IfCondition(PythonExpression([insecure_websockets, ' and ', foxbridge])), - package='foxglove_bridge', - executable='foxglove_bridge', - name='foxglove_bridge', - output={'both': 'log'}, - parameters=fox_bridge_params, - ), - Node( - condition=IfCondition(PythonExpression(['not ', insecure_websockets, ' and ', foxbridge])), - package='foxglove_bridge', - executable='foxglove_bridge', - name='foxglove_bridge', - output={'both': 'log'}, - parameters=fox_tls_bridge_params, - ), - Node( - condition=IfCondition(PythonExpression([insecure_websockets, ' and not ', foxbridge])), - package='rosbridge_server', - executable='rosbridge_websocket', - name='ros_bridge', - output={'both': 'log'}, - parameters=ros_bridge_params, - ), - Node( - condition=IfCondition(PythonExpression(['not ', insecure_websockets, ' and not ', foxbridge])), - package='rosbridge_server', - executable='rosbridge_websocket', - name='ros_bridge', - output={'both': 'log'}, - parameters=ros_tls_bridge_params, - ), - ]) + return LaunchDescription( + [ + foxbridge_launch_arg, + insecure_launch_arg, + simulator_launch_arg, + cot_tls_require_client_cert_launch_arg, + cot_tls_cert_path_launch_arg, + cot_tls_key_path_launch_arg, + cot_tls_ca_path_launch_arg, + Node( + condition=IfCondition( + PythonExpression(["not ", LaunchConfiguration("insecure")]) + ), + package="atos_gui", + namespace="atos", + executable="truck_object_gui", + name="truck_object_gui", + output="screen", + arguments=["True", "atosfleetmanagement"], + ), + Node( + condition=IfCondition(LaunchConfiguration("insecure")), + package="atos_gui", + namespace="atos", + executable="truck_object_gui", + name="truck_object_gui", + output="screen", + arguments=["False", "atosfleetmanagement"], + ), + Node( + package="atos", + namespace="atos", + executable="truck_object_control", + name="truck_object_control", + output="screen", + parameters=[ + {"cot_tls_require_client_cert": cot_tls_require_client_cert}, + {"cot_tls_cert_path": cot_tls_cert_path}, + {"cot_tls_key_path": cot_tls_key_path}, + {"cot_tls_ca_path": cot_tls_ca_path}, + ], + ), + Node( + condition=IfCondition(with_simulator), + package="atos", + namespace="atos", + executable="atos_truck_simulator", + name="atos_truck_simulator_1", + output="screen", + parameters=[ + {"uid": "L5S-TRUCK-SIM-1"}, + {"start_index": 0}, + {"target_speed_kmh": 80.0}, + {"acceleration_mps2": 2.0}, + {"ignore_warning_speed_commands": True}, + ], + ), + Node( + condition=IfCondition(with_simulator), + package="atos", + namespace="atos", + executable="atos_truck_simulator", + name="atos_truck_simulator_2", + output="screen", + parameters=[ + {"uid": "L5S-TRUCK-SIM-2"}, + {"start_index": 250}, + {"target_speed_kmh": 40.0}, + {"acceleration_mps2": 2.0}, + ], + ), + Node( + condition=IfCondition(with_simulator), + package="atos", + namespace="atos", + executable="atos_truck_simulator", + name="atos_truck_simulator_3", + output="screen", + parameters=[ + {"uid": "L5S-TRUCK-SIM-3"}, + {"start_index": 500}, + {"target_speed_kmh": 40.0}, + {"acceleration_mps2": 2.0}, + ], + ), + Node( + condition=IfCondition(PythonExpression(["not ", foxbridge])), + name="rosapi", + package="rosapi", + executable="rosapi_node", + ), + Node( + condition=IfCondition( + PythonExpression([insecure_websockets, " and ", foxbridge]) + ), + package="foxglove_bridge", + executable="foxglove_bridge", + name="foxglove_bridge", + output={"both": "log"}, + parameters=fox_bridge_params, + ), + Node( + condition=IfCondition( + PythonExpression(["not ", insecure_websockets, " and ", foxbridge]) + ), + package="foxglove_bridge", + executable="foxglove_bridge", + name="foxglove_bridge", + output={"both": "log"}, + parameters=fox_tls_bridge_params, + ), + Node( + condition=IfCondition( + PythonExpression([insecure_websockets, " and not ", foxbridge]) + ), + package="rosbridge_server", + executable="rosbridge_websocket", + name="ros_bridge", + output={"both": "log"}, + parameters=ros_bridge_params, + ), + Node( + condition=IfCondition( + PythonExpression( + ["not ", insecure_websockets, " and not ", foxbridge] + ) + ), + package="rosbridge_server", + executable="rosbridge_websocket", + name="ros_bridge", + output={"both": "log"}, + parameters=ros_tls_bridge_params, + ), + ] + ) diff --git a/atos/modules/DirectControl/src/directcontrol.cpp b/atos/modules/DirectControl/src/directcontrol.cpp index 52f15125f..6525e0aa3 100644 --- a/atos/modules/DirectControl/src/directcontrol.cpp +++ b/atos/modules/DirectControl/src/directcontrol.cpp @@ -3,12 +3,12 @@ * License, v. 2.0. If a copy of the MPL was not distributed with this * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ -#include #include "directcontrol.hpp" -#include "util.h" -#include "tcphandler.hpp" #include "atosTime.h" #include "atos_interfaces/msg/control_signal_percentage.hpp" +#include "tcphandler.hpp" +#include "util.h" +#include using atos_interfaces::msg::ControlSignalPercentage; using namespace ROSChannels; @@ -19,39 +19,41 @@ using namespace ROSChannels; * @brief Driver Model message, used in esmini to send * control signals to a vehicle from a driver model. */ -class DmMsg{ +class DmMsg { public: - DmMsg(){}; + DmMsg() {}; - ControlSignalPercentage toATOSMsg(){ + ControlSignalPercentage toATOSMsg() { ControlSignalPercentage cspmsg = ControlSignalPercentage(); - cspmsg.atos_header.object_id = this->objectId; - // Convert throttle brake and steering angle to integers between 0,100 and -100,100 respectively. - cspmsg.throttle = round(this->throttle * 100); - cspmsg.brake = round(this->brake * 100); + cspmsg.atos_header.object_id = this->objectId; + // Convert throttle brake and steering angle to integers between 0,100 and -100,100 respectively. + cspmsg.throttle = round(this->throttle * 100); + cspmsg.brake = round(this->brake * 100); cspmsg.steering_angle = round(this->steeringAngle * 100); return cspmsg; } /*! - * \brief Decodes a DM message from esmini compatible sender. - * if the message is not of type inputMode=1 (DRIVER_INPUT) - * the function returns - * - * \param bytes vector of (char) bytes - * \param DMMsg the data structure to be populated with contents of the message - * \return number of bytes on successfully parsed message, -1 otherwise - */ - int parseFromBytes(const std::vector& bytes){ - int idx=0; - decodeNextXBytes(4,this->version,idx,bytes); - decodeNextXBytes(4,this->inputMode,idx,bytes); - if (this->inputMode != 1) { return 0;} - decodeNextXBytes(4,this->objectId,idx,bytes); - decodeNextXBytes(4,this->frameNumber,idx,bytes); - decodeNextXBytes(8,this->throttle,idx,bytes); - decodeNextXBytes(8,this->brake,idx,bytes); - decodeNextXBytes(8,this->steeringAngle,idx,bytes); + * \brief Decodes a DM message from esmini compatible sender. + * if the message is not of type inputMode=1 (DRIVER_INPUT) + * the function returns + * + * \param bytes vector of (char) bytes + * \param DMMsg the data structure to be populated with contents of the message + * \return number of bytes on successfully parsed message, -1 otherwise + */ + int parseFromBytes(const std::vector& bytes) { + int idx = 0; + decodeNextXBytes(4, this->version, idx, bytes); + decodeNextXBytes(4, this->inputMode, idx, bytes); + if (this->inputMode != 1) { + return 0; + } + decodeNextXBytes(4, this->objectId, idx, bytes); + decodeNextXBytes(4, this->frameNumber, idx, bytes); + decodeNextXBytes(8, this->throttle, idx, bytes); + decodeNextXBytes(8, this->brake, idx, bytes); + decodeNextXBytes(8, this->steeringAngle, idx, bytes); return idx; } @@ -60,34 +62,31 @@ class DmMsg{ unsigned int inputMode; unsigned int objectId; unsigned int frameNumber; - double throttle; // range [0, 1] - double brake; // range [0, 1] - double steeringAngle; // range [-pi/2, pi/2] + double throttle; // range [0, 1] + double brake; // range [0, 1] + double steeringAngle; // range [-pi/2, pi/2] /*! - * \brief transfers x bytes, on the interval from idx to idx+x, - * from a vector of bytes into a variable - * - * \param x number of bytes to transfer - * \param field variable to receive bytes - * \param idx starting byte - * \param bytes vector of (char) bytes - */ + * \brief transfers x bytes, on the interval from idx to idx+x, + * from a vector of bytes into a variable + * + * \param x number of bytes to transfer + * \param field variable to receive bytes + * \param idx starting byte + * \param bytes vector of (char) bytes + */ template - void decodeNextXBytes(int x, T& field, int& idx, const std::vector& bytes){ + void decodeNextXBytes(int x, T& field, int& idx, const std::vector& bytes) { std::memcpy(&field, &(bytes[idx]), sizeof(T)); - if (sizeof(T) == 2){ + if (sizeof(T) == 2) { le16toh(field); - } - else if (sizeof(T) == 4){ + } else if (sizeof(T) == 4) { le32toh(field); - } - else if (sizeof(T) == 8){ + } else if (sizeof(T) == 8) { le64toh(field); - } - idx+=x; + } + idx += x; } - }; //! Message queue callbacks @@ -99,82 +98,80 @@ void DirectControl::onAllClearMessage(const AllClear::message_type::SharedPtr) { //! Class methods DirectControl::DirectControl() : - Module(moduleName), - controlSignalPub(*this), - tcpHandler(TCPPort, "", "off", 1, O_NONBLOCK), - udpServer("0.0.0.0",UDPPort) {} + Module(moduleName), + controlSignalPub(*this), + tcpHandler(TCPPort, "", "off", 1, O_NONBLOCK), + udpServer("0.0.0.0", UDPPort) {} void DirectControl::startThreads() { - receiveThread=std::make_unique(&DirectControl::readTCPSocketData, this); - receiveThreadUDP=std::make_unique(&DirectControl::readUDPSocketData, this); + receiveThread = std::make_unique(&DirectControl::readTCPSocketData, this); + receiveThreadUDP = std::make_unique(&DirectControl::readUDPSocketData, this); } -void DirectControl::joinThreads(){ - //Tear down connections - if(this->tcpHandler.isConnected()) this->tcpHandler.TCPHandlerclose(); +void DirectControl::joinThreads() { + // Tear down connections + if (this->tcpHandler.isConnected()) + this->tcpHandler.TCPHandlerclose(); this->udpServer.close(); - //Join threads + // Join threads receiveThread->join(); receiveThreadUDP->join(); } - /*! * \brief Listens for UDP data and sends a control signal on ros topic when recevied */ void DirectControl::readUDPSocketData() { - RCLCPP_INFO(get_logger(),"Listening on UDP port %d",UDPPort); - - while (!this->quit){ - try{ + RCLCPP_INFO(get_logger(), "Listening on UDP port %d", UDPPort); + + while (!this->quit) { + try { auto [data, remote] = udpServer.recvfrom(); DmMsg dmMsg; auto bytesParsed = dmMsg.parseFromBytes(data); - // TODO: add re-ordering with frame numbers? - // Have to think abt if/what kind of re-ordering makes sense for + // TODO: add re-ordering with frame numbers? + // Have to think abt if/what kind of re-ordering makes sense for // the application (driver model). - if (bytesParsed != -1){ + if (bytesParsed != -1) { ControlSignalPercentage cspmsg = dmMsg.toATOSMsg(); controlSignalPub.publish(cspmsg); } - } - catch(const SocketErrors::DisconnectedError& error){ - //If we get a disconnected exception when we do not intend to exit, propagate said exception - if (!this->quit){ + } catch (const SocketErrors::DisconnectedError& error) { + // If we get a disconnected exception when we do not intend to exit, propagate said exception + if (!this->quit) { throw error; } } } } - void DirectControl::readTCPSocketData() { std::vector data(TCP_BUFFER_SIZE); int recvData = 0; - RCLCPP_INFO(get_logger(),"Awaiting TCP connection..."); + RCLCPP_INFO(get_logger(), "Awaiting TCP connection..."); this->tcpHandler.CreateServer(); while (!this->quit) { // TODO set up TCP connection (wait until connected before continue) this->tcpHandler.TCPHandlerAccept(1000); - if (this->tcpHandler.isConnected()){ - RCLCPP_INFO(get_logger(),"Connected"); - + if (this->tcpHandler.isConnected()) { + RCLCPP_INFO(get_logger(), "Connected"); + while (!this->quit && tcpHandler.isConnected()) { data.resize(TCP_BUFFER_SIZE); std::fill(data.begin(), data.end(), 0); recvData = tcpHandler.receiveTCP(data, 0); if (recvData == TCPHandler::FAILURE) { this->tcpHandler.TCPHandlerclose(); - RCLCPP_INFO(get_logger(),"TCP connection closed unexpectedly..."); - RCLCPP_INFO(get_logger(),"Awaiting new TCP connection...");; + RCLCPP_INFO(get_logger(), "TCP connection closed unexpectedly..."); + RCLCPP_INFO(get_logger(), "Awaiting new TCP connection..."); + ; this->tcpHandler.CreateServer(); break; - } - else if (recvData > 0) { + } else if (recvData > 0) { try { this->handleISOMessage(data, static_cast(recvData)); } catch (std::invalid_argument& e) { @@ -187,45 +184,40 @@ void DirectControl::readTCPSocketData() { } } -void DirectControl::handleISOMessage( - std::vector& byteData, - size_t receivedBytes) { +void DirectControl::handleISOMessage(std::vector& byteData, size_t receivedBytes) { while (receivedBytes > 0) { ISOMessageID recvMessage = getISOMessageType(byteData.data(), byteData.size(), false); // TODO check for RDCI (optional) // TODO if RDCI, respond with DCTI (optional) switch (recvMessage) { - case MESSAGE_ID_INVALID: - throw std::invalid_argument("Received invalid ISO message"); - case MESSAGE_ID_VENDOR_SPECIFIC_ASTAZERO_RDCA: - receivedBytes -= this->handleRDCAMessage(byteData); - break; - default: - throw std::invalid_argument("Received unhandled ISO message"); + case MESSAGE_ID_INVALID: + throw std::invalid_argument("Received invalid ISO message"); + case MESSAGE_ID_VENDOR_SPECIFIC_ASTAZERO_RDCA: + receivedBytes -= this->handleRDCAMessage(byteData); + break; + default: + throw std::invalid_argument("Received unhandled ISO message"); } } } -size_t DirectControl::handleRDCAMessage( - std::vector &byteData) { +size_t DirectControl::handleRDCAMessage(std::vector& byteData) { RequestControlActionType recvMessage; struct timeval currentTime; TimeSetToCurrentSystemTime(¤tTime); ssize_t bytesRead = decodeRDCAMessage(byteData.data(), &recvMessage, byteData.size(), currentTime, false); if (bytesRead >= 0) { byteData.erase(byteData.begin(), byteData.begin() + bytesRead); - //TODO: Implement equivalent of below line in using ROS2. - //DataDictionarySetRequestedControlAction(recvMessage.executingID, &recvMessage); + // TODO: Implement equivalent of below line in using ROS2. + // DataDictionarySetRequestedControlAction(recvMessage.executingID, &recvMessage); return static_cast(bytesRead); - } - else { + } else { // TODO respond with error (optional) throw std::invalid_argument("Failed to decode RDCA message"); } } -size_t DirectControl::handleUnknownMessage( - std::vector &byteData) { +size_t DirectControl::handleUnknownMessage(std::vector& byteData) { std::fill(byteData.begin(), byteData.end(), 0); return byteData.size(); } diff --git a/atos/modules/EsminiAdapter/src/esminiadapter.cpp b/atos/modules/EsminiAdapter/src/esminiadapter.cpp index 9f2702ff8..46321a5b0 100644 --- a/atos/modules/EsminiAdapter/src/esminiadapter.cpp +++ b/atos/modules/EsminiAdapter/src/esminiadapter.cpp @@ -4,107 +4,111 @@ * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ #include "esminiadapter.hpp" -#include -#include #include -#include #include #include -#include +#include #include +#include +#include #include #include "atos_interfaces/msg/cartesian_trajectory.hpp" #include "rclcpp/wait_for_message.hpp" -#include "trajectory.hpp" #include "string_utility.hpp" - +#include "trajectory.hpp" using namespace ROSChannels; -using TestOriginSrv = atos_interfaces::srv::GetTestOrigin; +using TestOriginSrv = atos_interfaces::srv::GetTestOrigin; using ObjectTrajectorySrv = atos_interfaces::srv::GetObjectTrajectory; -using ObjectTriggerSrv = atos_interfaces::srv::GetObjectTriggerStart; +using ObjectTriggerSrv = atos_interfaces::srv::GetObjectTriggerStart; using std::placeholders::_1; using std::placeholders::_2; using namespace std::chrono_literals; -namespace -{ -template -int reportObjectPos(ReportObjectPosFn reportFn, int objectId, double timestamp, double x, double y, double z, double h, double p, double r) -{ +namespace { +template +int reportObjectPos(ReportObjectPosFn reportFn, + int objectId, + double timestamp, + double x, + double y, + double z, + double h, + double p, + double r) { if constexpr (std::is_invocable_v) { - return reportFn(objectId, static_cast(timestamp), static_cast(x), static_cast(y), static_cast(z), - static_cast(h), static_cast(p), static_cast(r)); + return reportFn(objectId, + static_cast(timestamp), + static_cast(x), + static_cast(y), + static_cast(z), + static_cast(h), + static_cast(p), + static_cast(r)); } else { return reportFn(objectId, x, y, z, h, p, r); } } -} +} // namespace - - - -std::shared_ptr EsminiAdapter::me = nullptr; +std::shared_ptr EsminiAdapter::me = nullptr; std::unordered_map EsminiAdapter::atosIDToObjectName = std::unordered_map(); std::unordered_map EsminiAdapter::objectNameToAtosId = std::unordered_map(); -std::map EsminiAdapter::atosObjectIdToTraj = std::map(); -std::unordered_map EsminiAdapter::atosIdToEsminiId = std::unordered_map(); - -std::unordered_map> EsminiAdapter::monrSubscribers = std::unordered_map>(); -std::shared_ptr> EsminiAdapter::objectTrajectoryService = std::shared_ptr>(); -std::shared_ptr> EsminiAdapter::testOriginService = std::shared_ptr>(); +std::map EsminiAdapter::atosObjectIdToTraj = std::map(); +std::unordered_map EsminiAdapter::atosIdToEsminiId = std::unordered_map(); + +std::unordered_map> EsminiAdapter::monrSubscribers = + std::unordered_map>(); +std::shared_ptr> EsminiAdapter::objectTrajectoryService = + std::shared_ptr>(); +std::shared_ptr> EsminiAdapter::testOriginService = + std::shared_ptr>(); geographic_msgs::msg::GeoPose EsminiAdapter::testOrigin = geographic_msgs::msg::GeoPose(); -EsminiAdapter::EsminiAdapter() : Module(moduleName), - storyBoardElementStateChangePub(*this), - connectedObjectIdsSub(*this, &EsminiAdapter::onConnectedObjectIdsMessage), - exitSub(*this, &EsminiAdapter::onStaticExitMessage), - stateChangeSub(*this, &EsminiAdapter::onStaticStateChangeMessage), - applyTrajTransform(false), - testOriginSet(false) - { +EsminiAdapter::EsminiAdapter() : + Module(moduleName), + storyBoardElementStateChangePub(*this), + connectedObjectIdsSub(*this, &EsminiAdapter::onConnectedObjectIdsMessage), + exitSub(*this, &EsminiAdapter::onStaticExitMessage), + stateChangeSub(*this, &EsminiAdapter::onStaticStateChangeMessage), + applyTrajTransform(false), + testOriginSet(false) { oscFilePathClient_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); - objectIdsClient_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); - oscFilePathClient_ = create_client( - ServiceNames::getOpenScenarioFilePath, rmw_qos_profile_services_default, oscFilePathClient_cb_group_); + objectIdsClient_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + oscFilePathClient_ = create_client( + ServiceNames::getOpenScenarioFilePath, rmw_qos_profile_services_default, oscFilePathClient_cb_group_); objectIdsClient_ = create_client( ServiceNames::getObjectIds, rmw_qos_profile_services_default, objectIdsClient_cb_group_); declare_parameter("timestep", 0.1); - } /*! * \brief Fetches the open drive file path from the open scenario file parameter * \return Configured path -*/ -std::filesystem::path EsminiAdapter::getOpenDriveFile() -{ + */ +std::filesystem::path EsminiAdapter::getOpenDriveFile() { std::filesystem::path odrFilePath; if (SE_GetODRFilename() != nullptr) { odrFilePath = std::filesystem::path(SE_GetODRFilename()); RCLCPP_INFO(me->get_logger(), "Got ODR file %s from scenario", odrFilePath.string().c_str()); - } - else { + } else { RCLCPP_DEBUG(me->get_logger(), "No ODR file found"); } if (odrFilePath.is_absolute()) { return odrFilePath; - } - else { + } else { // Look for the file relative the scenario file (ie root openx dir) - auto odrCatalog = std::filesystem::path(me->oscFilePath).parent_path(); + auto odrCatalog = std::filesystem::path(me->oscFilePath).parent_path(); std::filesystem::path joinedPath = odrCatalog / odrFilePath; if (!std::filesystem::exists(joinedPath)) { throw std::runtime_error("ODR file " + joinedPath.string() + " does not exist"); - } - else { + } else { RCLCPP_INFO(me->get_logger(), "Found ODR file at %s", joinedPath.string().c_str()); } return joinedPath; } - } /*! @@ -115,22 +119,24 @@ std::shared_ptr EsminiAdapter::instance() { if (me == nullptr) { me = std::shared_ptr(new EsminiAdapter()); // Start listening to connected object ids - me->connectedObjectIdsSub = ROSChannels::ConnectedObjectIds::Sub(*me,&EsminiAdapter::onConnectedObjectIdsMessage); - me->exitSub = ROSChannels::Exit::Sub(*me,&EsminiAdapter::onStaticExitMessage); - me->stateChangeSub = ROSChannels::StateChange::Sub(*me,&EsminiAdapter::onStaticStateChangeMessage); - + me->connectedObjectIdsSub = + ROSChannels::ConnectedObjectIds::Sub(*me, &EsminiAdapter::onConnectedObjectIdsMessage); + me->exitSub = ROSChannels::Exit::Sub(*me, &EsminiAdapter::onStaticExitMessage); + me->stateChangeSub = ROSChannels::StateChange::Sub(*me, &EsminiAdapter::onStaticStateChangeMessage); } return me; } /*! - * \brief This callback is executed when objects are connected, and creates Monitor subscribers for all connected objects. + * \brief This callback is executed when objects are connected, and creates Monitor subscribers for all connected + * objects. * \param msg Array of object IDs */ void EsminiAdapter::onConnectedObjectIdsMessage(const ConnectedObjectIds::message_type::SharedPtr msg) { for (uint32_t id : msg->ids) { - if (me->monrSubscribers.find(id) == me->monrSubscribers.end()){ - me->monrSubscribers[id] = std::make_shared(*me, id, std::bind(&EsminiAdapter::onMonitorMessage, me, _1, id)); + if (me->monrSubscribers.find(id) == me->monrSubscribers.end()) { + me->monrSubscribers[id] = + std::make_shared(*me, id, std::bind(&EsminiAdapter::onMonitorMessage, me, _1, id)); } } } @@ -143,56 +149,50 @@ void EsminiAdapter::onConnectedObjectIdsMessage(const ConnectedObjectIds::messag * @param msg StateChange message */ void EsminiAdapter::onStaticStateChangeMessage(const ROSChannels::StateChange::message_type::SharedPtr msg) { - int prevState = msg->prev_state; + int prevState = msg->prev_state; int currentState = msg->current_state; if (prevState == OBCState_t::OBC_STATE_ARMED && currentState == OBCState_t::OBC_STATE_RUNNING) { me->handleStartCommand(); - } - else if (currentState == OBCState_t::OBC_STATE_ABORTING) { + } else if (currentState == OBCState_t::OBC_STATE_ABORTING) { me->handleAbortCommand(); } } - void EsminiAdapter::handleAbortCommand() { SE_Close(); RCLCPP_INFO(me->get_logger(), "Esmini ScenarioEngine stopped due to Abort"); } -void EsminiAdapter::fetchOSCFilePath() -{ +void EsminiAdapter::fetchOSCFilePath() { // Get the file path of xosc file std::promise done; auto callback = [&](rclcpp::Client::SharedFuture future) { - auto response = future.get(); + auto response = future.get(); me->oscFilePath = response->path; if (response->md5hash != me->scenarioFileMd5hash) { me->scenarioFileMd5hash = response->md5hash; - me->runSimulation = true; + me->runSimulation = true; } me->scenarioFileMd5hash = response->md5hash; done.set_value(true); }; auto request = std::make_shared(); - auto future = me->oscFilePathClient_->async_send_request(request, std::move(callback)); + auto future = me->oscFilePathClient_->async_send_request(request, std::move(callback)); if (done.get_future().wait_for(250ms) == std::future_status::timeout) { RCLCPP_ERROR(me->get_logger(), "Failed to fetch open scenario file path"); } - } -void EsminiAdapter::onStaticExitMessage(const ROSChannels::Exit::message_type::SharedPtr) -{ +void EsminiAdapter::onStaticExitMessage(const ROSChannels::Exit::message_type::SharedPtr) { SE_Close(); - RCLCPP_DEBUG(me->get_logger(),"Received exit command"); + RCLCPP_DEBUG(me->get_logger(), "Received exit command"); rclcpp::shutdown(); } -void EsminiAdapter::handleStartCommand() -{ - if (SE_Init(me->oscFilePath.c_str(),0,0,0,0) < 0) { +void EsminiAdapter::handleStartCommand() { + if (SE_Init(me->oscFilePath.c_str(), 0, 0, 0, 0) < 0) { throw std::runtime_error("Failed to initialize esmini with scenario file " + me->oscFilePath.string()); } // Handle triggers and story board element changes @@ -206,21 +206,22 @@ void EsminiAdapter::handleStartCommand() * \brief Callback to be executed by esmini when story board state changes. * If story board element is an action, and the action is supported, the action is run. * \param name Name of the StoryBoardElement whose state has changed. - * \param type Possible values: STORY_BOARD = 1, STORY = 2, ACT = 3, MANEUVER_GROUP = 4, MANEUVER = 5, EVENT = 6, ACTION = 7, UNDEFINED_ELEMENT_TYPE = 0. + * \param type Possible values: STORY_BOARD = 1, STORY = 2, ACT = 3, MANEUVER_GROUP = 4, MANEUVER = 5, EVENT = 6, ACTION + * = 7, UNDEFINED_ELEMENT_TYPE = 0. * \param state new state, possible values: STANDBY = 1, RUNNING = 2, COMPLETE = 3, UNDEFINED_ELEMENT_STATE = 0. */ -void EsminiAdapter::handleStoryBoardElementChange( - const char *name, - int type, - int state, - const char *full_path) -{ - RCLCPP_INFO(me->get_logger(), "Storyboard state changed! Name: %s, Type: %d, State: %d, Full path: %s", name, type, state, full_path); +void EsminiAdapter::handleStoryBoardElementChange(const char* name, int type, int state, const char* full_path) { + RCLCPP_INFO(me->get_logger(), + "Storyboard state changed! Name: %s, Type: %d, State: %d, Full path: %s", + name, + type, + state, + full_path); atos_interfaces::msg::StoryBoardElementStateChange msg; - msg.name = name; - msg.type = type; - msg.state = state; + msg.name = name; + msg.type = type; + msg.state = state; msg.full_path = full_path; me->storyBoardElementStateChangePub.publish(msg); @@ -232,9 +233,9 @@ void EsminiAdapter::handleStoryBoardElementChange( * \param monr ROS Monitor message of an object * \param id The object ID to which the monr belongs */ -void EsminiAdapter::reportObjectPosition(const Monitor::message_type::SharedPtr monr, uint32_t esminiObjectId){ +void EsminiAdapter::reportObjectPosition(const Monitor::message_type::SharedPtr monr, uint32_t esminiObjectId) { // Conversions from ROS to Esmini - auto ori = monr->pose.pose.orientation; + auto ori = monr->pose.pose.orientation; auto quat = tf2::Quaternion(ori.x, ori.y, ori.z, ori.w); tf2::Matrix3x3 m(quat); double roll, pitch, yaw; @@ -255,14 +256,16 @@ void EsminiAdapter::reportObjectPosition(const Monitor::message_type::SharedPtr * \brief Callback for MONR messages, reports the object position to esmini and advances the simulation time * \param monr ROS Monitor message of an object * \param id The object ID to which the monr belongs -*/ + */ void EsminiAdapter::onMonitorMessage(const Monitor::message_type::SharedPtr monr, uint32_t ATOSObjectId) { if (auto idMapping = atosIdToEsminiId.find(ATOSObjectId); idMapping != atosIdToEsminiId.end()) { reportObjectPosition(monr, idMapping->second); // Report object position to esmini - SE_Step(); // Advance the "simulation world"-time - } - else { - RCLCPP_WARN(me->get_logger(), "Received MONR message for object with ATOS Object ID %d, but no such object exists in the scenario", ATOSObjectId); + SE_Step(); // Advance the "simulation world"-time + } else { + RCLCPP_WARN( + me->get_logger(), + "Received MONR message for object with ATOS Object ID %d, but no such object exists in the scenario", + ATOSObjectId); } } @@ -274,10 +277,7 @@ void EsminiAdapter::onMonitorMessage(const Monitor::message_type::SharedPtr monr * \param states A vector of object states * \return A trajectory consisting of trajectory points, one for each state. */ -ATOS::Trajectory EsminiAdapter::getTrajectoryFromObjectState( - uint32_t id, - std::vector& states) -{ +ATOS::Trajectory EsminiAdapter::getTrajectoryFromObjectState(uint32_t id, std::vector& states) { ATOS::Trajectory trajectory(me->get_logger()); trajectory.name = "Esmini Trajectory for object " + std::to_string(id); if (states.empty()) { @@ -303,8 +303,7 @@ ATOS::Trajectory EsminiAdapter::getTrajectoryFromObjectState( if (state.timestamp != prevState.timestamp) { tp.setLongitudinalAcceleration((currLonVel - prevLonVel) / (state.timestamp - prevState.timestamp)); tp.setLateralAcceleration((currLatVel - prevLatVel) / (state.timestamp - prevState.timestamp)); - } - else { + } else { tp.setLongitudinalAcceleration(0); tp.setLateralAcceleration(0); } @@ -313,12 +312,12 @@ ATOS::Trajectory EsminiAdapter::getTrajectoryFromObjectState( }; if (states.size() > 1) { - for (auto it = states.begin()+1; it != states.end(); ++it) { - if (it->x == (it-1)->x && it->y == (it-1)->y && // Nothing interesting happens within 1 timestep, skip - it->z == (it-1)->z && it->h == (it-1)->h) { + for (auto it = states.begin() + 1; it != states.end(); ++it) { + if (it->x == (it - 1)->x && it->y == (it - 1)->y && // Nothing interesting happens within 1 timestep, skip + it->z == (it - 1)->z && it->h == (it - 1)->h) { continue; } - saveTp(*it,*(it-1)); // Next timestep is different, save current one. + saveTp(*it, *(it - 1)); // Next timestep is different, save current one. } } if (trajectory.points.size() == 0) { @@ -327,7 +326,7 @@ ATOS::Trajectory EsminiAdapter::getTrajectoryFromObjectState( auto startTime = trajectory.points.front().getTime(); // Subtract start time from all timesteps - for (auto& tp : trajectory.points){ + for (auto& tp : trajectory.points) { tp.setTime(tp.getTime() - startTime); } return trajectory; @@ -342,8 +341,7 @@ std::string EsminiAdapter::projStrFromGeoReference(RM_GeoReference& geoRef) { std::string projStringFrom = "+proj="; if (strlen(geoRef.proj_) != 0) { projStringFrom += std::string(geoRef.proj_) + " "; - } - else { + } else { throw std::runtime_error("No projection found in geo reference"); } if (!std::isnan(geoRef.lat_0_)) { @@ -413,14 +411,11 @@ std::string EsminiAdapter::projStrFromGeoReference(RM_GeoReference& geoRef) { * \param states The return map of ids mapping to the respective object states at different timesteps * \return A map of object states, where the key is the object ID and the value is a vector of states */ -void EsminiAdapter::getObjectStates( - double timeStep, - std::map>& states) -{ - double accumTime = 0.0; +void EsminiAdapter::getObjectStates(double timeStep, std::map>& states) { + double accumTime = 0.0; auto pushCurrentState = [&](auto& st, auto& index) { auto id = SE_GetId(index); - //SE_SetAlignModeZ(id, 0); // Disable Z-alignment, not implemented in esmini yet + // SE_SetAlignModeZ(id, 0); // Disable Z-alignment, not implemented in esmini yet SE_ScenarioObjectState s; SE_GetObjectState(id, &s); s.timestamp = accumTime; @@ -430,13 +425,13 @@ void EsminiAdapter::getObjectStates( // Populate States map with vector for each object containing the initial state SE_StepDT(timeStep); accumTime += timeStep; - for (int j = 0; j < SE_GetNumberOfObjects(); j++){ + for (int j = 0; j < SE_GetNumberOfObjects(); j++) { states[SE_GetId(j)] = std::vector(); pushCurrentState(states, j); } constexpr double MIN_SCENARIO_TIME = 10.0; constexpr double MAX_SCENARIO_TIME = 3600.0; - bool stopSimulation = false; + bool stopSimulation = false; while (!stopSimulation) { if (SE_GetQuitFlag() != 0) { break; @@ -444,51 +439,48 @@ void EsminiAdapter::getObjectStates( SE_StepDT(timeStep); accumTime += timeStep; - for (int j = 0; j < SE_GetNumberOfObjects(); j++){ + for (int j = 0; j < SE_GetNumberOfObjects(); j++) { pushCurrentState(states, j); } - bool noMovement = std::all_of(states.begin(), states.end(), [&](auto& pair) { - return pair.second.back().speed < 0.1; - }); - bool atLeastMinTimePassed = accumTime > MIN_SCENARIO_TIME; + bool noMovement = + std::all_of(states.begin(), states.end(), [&](auto& pair) { return pair.second.back().speed < 0.1; }); + bool atLeastMinTimePassed = accumTime > MIN_SCENARIO_TIME; bool moreThanMaxTimePassed = accumTime > MAX_SCENARIO_TIME; - stopSimulation = (noMovement && atLeastMinTimePassed) || moreThanMaxTimePassed; + stopSimulation = (noMovement && atLeastMinTimePassed) || moreThanMaxTimePassed; } if (accumTime > MAX_SCENARIO_TIME) { RCLCPP_WARN(me->get_logger(), "Scenario time limit reached, stopping simulation"); - } - else if (accumTime < MIN_SCENARIO_TIME + timeStep) { - RCLCPP_WARN(me->get_logger(), "Ran scenario for the minimum time %.2f, possibly no movement in scenario", MIN_SCENARIO_TIME); + } else if (accumTime < MIN_SCENARIO_TIME + timeStep) { + RCLCPP_WARN(me->get_logger(), + "Ran scenario for the minimum time %.2f, possibly no movement in scenario", + MIN_SCENARIO_TIME); } RCLCPP_INFO(me->get_logger(), "Finished %f s simulation", accumTime); } - /*! * \brief Runs the esmini simulator with the xosc file and returns the trajectories for each object * \param timeStep Time step to use for generating the trajectories * \return A map of ids mapping to the respective trajectories */ -std::map EsminiAdapter::extractTrajectories( - double timeStep) -{ - std::map esminiObjectIdToTraj = std::map(); +std::map EsminiAdapter::extractTrajectories(double timeStep) { + std::map esminiObjectIdToTraj = std::map(); // Get object states - std::map> esminiIdToStates; + std::map> esminiIdToStates; getObjectStates(timeStep, esminiIdToStates); // Extract trajectories for (auto& os : esminiIdToStates) { - auto id = os.first; + auto id = os.first; auto objectStates = os.second; - auto traj = getTrajectoryFromObjectState(id, objectStates); + auto traj = getTrajectoryFromObjectState(id, objectStates); // Apply CRS transform if OpenDrive CRS Transformation is defined - if (me->applyTrajTransform){ + if (me->applyTrajTransform) { RCLCPP_DEBUG(me->get_logger(), "Applying CRS transformation to trajectory for object %d", id); me->crsTransformation->apply(traj.points); } - esminiObjectIdToTraj.insert(std::pair(id, traj)); + esminiObjectIdToTraj.insert(std::pair(id, traj)); } return esminiObjectIdToTraj; } @@ -497,8 +489,7 @@ std::map EsminiAdapter::extractTrajectories( * \brief Initialize the esmini simulator and perform subsequent setup tasks. * Can be called many times, each time the test is initialized. */ -void EsminiAdapter::runEsminiSimulation() -{ +void EsminiAdapter::runEsminiSimulation() { me->atosObjectIdToTraj.clear(); me->atosIdToEsminiId.clear(); me->atosIDToObjectName.clear(); @@ -511,36 +502,35 @@ void EsminiAdapter::runEsminiSimulation() RM_Close(); // Stop RoadManager in case it is running RCLCPP_INFO(me->get_logger(), "Initializing esmini with scenario file %s", me->oscFilePath.c_str()); - if (SE_Init(me->oscFilePath.c_str(),1,0,0,0) < 0) { // Disable controllers, let DefaultController be used - throw std::runtime_error("Failed to initialize esmini with scenario file " + me->oscFilePath.string() + ". For more information, see " + logFilePath + "."); + if (SE_Init(me->oscFilePath.c_str(), 1, 0, 0, 0) < 0) { // Disable controllers, let DefaultController be used + throw std::runtime_error("Failed to initialize esmini with scenario file " + me->oscFilePath.string() + + ". For more information, see " + logFilePath + "."); } auto odrFile = getOpenDriveFile(); if (RM_Init(odrFile.c_str()) < 0) { throw std::runtime_error(std::string("Failed to initialize with odr file ").append(odrFile)); } - // Call RM_GetOpenDriveGeoReference to get the RM_GeoReference struct RM_GeoReference geoRef; if (RM_GetOpenDriveGeoReference(&geoRef) == 0) { try { std::string projStringFrom = projStrFromGeoReference(geoRef); - std::string toDatum = "WGS84"; - auto llh_0 = CRSTransformation::projToLLH(projStringFrom, toDatum); + std::string toDatum = "WGS84"; + auto llh_0 = CRSTransformation::projToLLH(projStringFrom, toDatum); RCLCPP_INFO(me->get_logger(), "llh origin: %lf, %lf, %lf", llh_0[0], llh_0[1], llh_0[2]); - me->testOrigin.position.latitude = llh_0[0]; + me->testOrigin.position.latitude = llh_0[0]; me->testOrigin.position.longitude = llh_0[1]; - me->testOrigin.position.altitude = llh_0[2]; - me->testOriginSet = true; + me->testOrigin.position.altitude = llh_0[2]; + me->testOriginSet = true; std::string projStringTo = "+proj=tmerc +lat_0=" + std::to_string(llh_0[0]) + - " +lon_0=" + std::to_string(llh_0[1]) + - " +datum="+ toDatum + " +units=m +no_defs"; + " +lon_0=" + std::to_string(llh_0[1]) + " +datum=" + toDatum + + " +units=m +no_defs"; - me->crsTransformation = std::make_shared(projStringFrom, projStringTo); + me->crsTransformation = std::make_shared(projStringFrom, projStringTo); me->applyTrajTransform = true; - } - catch (std::exception& e) { + } catch (std::exception& e) { RCLCPP_ERROR(me->get_logger(), "%s", e.what()); return; } @@ -550,17 +540,18 @@ void EsminiAdapter::runEsminiSimulation() RM_Close(); auto response = std::make_shared(); - auto request = std::make_shared(); + auto request = std::make_shared(); std::promise idsFetched; - auto objectNameAndAtosIDsCallback = [&](rclcpp::Client::SharedFutureWithRequest future) { - auto response = future.get(); - for (size_t i = 0; i < response.second->ids.size(); ++i) { - me->atosIDToObjectName[response.second->ids[i]] = response.second->names[i]; - me->objectNameToAtosId[response.second->names[i]] = response.second->ids[i]; - } - idsFetched.set_value(); - }; + auto objectNameAndAtosIDsCallback = + [&](rclcpp::Client::SharedFutureWithRequest future) { + auto response = future.get(); + for (size_t i = 0; i < response.second->ids.size(); ++i) { + me->atosIDToObjectName[response.second->ids[i]] = response.second->names[i]; + me->objectNameToAtosId[response.second->names[i]] = response.second->ids[i]; + } + idsFetched.set_value(); + }; auto future = me->objectIdsClient_->async_send_request(request, std::move(objectNameAndAtosIDsCallback)); if (idsFetched.get_future().wait_for(250ms) == std::future_status::timeout) { @@ -569,41 +560,45 @@ void EsminiAdapter::runEsminiSimulation() } RCLCPP_INFO(me->get_logger(), "Starting extracting trajectories"); - double timeStep = me->get_parameter("timestep").as_double(); - std::map esminiIdToTraj = me->extractTrajectories(timeStep); + double timeStep = me->get_parameter("timestep").as_double(); + std::map esminiIdToTraj = me->extractTrajectories(timeStep); // Fill atosIdToTraj with the extracted trajectories for (auto& it : esminiIdToTraj) { - auto esminiId = it.first; - auto traj = it.second; + auto esminiId = it.first; + auto traj = it.second; auto objectName = SE_GetObjectName(esminiId); - if (auto idMapping = me->objectNameToAtosId.find(objectName); idMapping != me->objectNameToAtosId.end()) { - auto atos_id = idMapping->second; - me->atosObjectIdToTraj.emplace(atos_id, traj); - me->atosIdToEsminiId.emplace(atos_id, esminiId); - RCLCPP_INFO( - me->get_logger(), "Extracted trajectory for object %s with size %zu", objectName, traj.points.size()); - - me->pathPublishers.emplace(atos_id, ROSChannels::Path::Pub(*me, atos_id)); - me->pathPublishers.at(atos_id).publish(traj.toPath()); - std::array llh_0 = {me->testOrigin.position.latitude, me->testOrigin.position.longitude, me->testOrigin.position.altitude}; - me->gnssPathPublishers.emplace(atos_id, ROSChannels::GNSSPath::Pub(*me, atos_id)); - me->gnssPathPublishers.at(atos_id).publish(traj.toGeoJSON(llh_0)); - } - else { - RCLCPP_DEBUG(me->get_logger(), "Object %s is not an active object in the scenario", objectName); - } + if (auto idMapping = me->objectNameToAtosId.find(objectName); idMapping != me->objectNameToAtosId.end()) { + auto atos_id = idMapping->second; + me->atosObjectIdToTraj.emplace(atos_id, traj); + me->atosIdToEsminiId.emplace(atos_id, esminiId); + RCLCPP_INFO( + me->get_logger(), "Extracted trajectory for object %s with size %zu", objectName, traj.points.size()); + + me->pathPublishers.emplace(atos_id, ROSChannels::Path::Pub(*me, atos_id)); + me->pathPublishers.at(atos_id).publish(traj.toPath()); + std::array llh_0 = { + me->testOrigin.position.latitude, me->testOrigin.position.longitude, me->testOrigin.position.altitude}; + me->gnssPathPublishers.emplace(atos_id, ROSChannels::GNSSPath::Pub(*me, atos_id)); + me->gnssPathPublishers.at(atos_id).publish(traj.toGeoJSON(llh_0)); + } else { + RCLCPP_DEBUG(me->get_logger(), "Object %s is not an active object in the scenario", objectName); + } - for (auto& tp : traj.points){ - RCLCPP_DEBUG(me->get_logger(), "Trajectory point: %lf, %lf, %lf, %lf, %ld", tp.getXCoord(), tp.getYCoord(), tp.getZCoord(), tp.getHeading(), tp.getTime().count()); + for (auto& tp : traj.points) { + RCLCPP_DEBUG(me->get_logger(), + "Trajectory point: %lf, %lf, %lf, %lf, %ld", + tp.getXCoord(), + tp.getYCoord(), + tp.getZCoord(), + tp.getHeading(), + tp.getTime().count()); } } SE_Close(); // Stop ScenarioEngine } -void EsminiAdapter::onRequestObjectTrajectory( - const std::shared_ptr req, - std::shared_ptr res) -{ +void EsminiAdapter::onRequestObjectTrajectory(const std::shared_ptr req, + std::shared_ptr res) { me->fetchOSCFilePath(); if (me->runSimulation || me->atosObjectIdToTraj.find(req->id) == me->atosObjectIdToTraj.end()) { me->runEsminiSimulation(); @@ -612,25 +607,22 @@ void EsminiAdapter::onRequestObjectTrajectory( try { res->trajectory = me->atosObjectIdToTraj.at(req->id).toCartesianTrajectory(); - res->success = true; - } - catch (std::out_of_range& e){ + res->success = true; + } catch (std::out_of_range& e) { RCLCPP_ERROR(me->get_logger(), "Esmini trajectory service called, no trajectory found for object %d", req->id); res->success = false; } } -void EsminiAdapter::onRequestTestOrigin( - const std::shared_ptr, - std::shared_ptr res) -{ - while (me->testOriginSet == false){ +void EsminiAdapter::onRequestTestOrigin(const std::shared_ptr, + std::shared_ptr res) { + while (me->testOriginSet == false) { RCLCPP_DEBUG(me->get_logger(), "Waiting for test origin to be available"); std::this_thread::sleep_for(std::chrono::milliseconds(20)); } - res->origin.position.latitude = me->testOrigin.position.latitude; + res->origin.position.latitude = me->testOrigin.position.latitude; res->origin.position.longitude = me->testOrigin.position.longitude; - res->origin.position.altitude = me->testOrigin.position.altitude; + res->origin.position.altitude = me->testOrigin.position.altitude; } /*! @@ -642,10 +634,10 @@ void EsminiAdapter::onRequestTestOrigin( int EsminiAdapter::initializeModule() { int retval = 0; - me->objectTrajectoryService = me->create_service(ServiceNames::getObjectTrajectory, - std::bind(&EsminiAdapter::onRequestObjectTrajectory, _1, _2)); - me->testOriginService = me->create_service(ServiceNames::getTestOrigin, - std::bind(&EsminiAdapter::onRequestTestOrigin, _1, _2)); + me->objectTrajectoryService = me->create_service( + ServiceNames::getObjectTrajectory, std::bind(&EsminiAdapter::onRequestObjectTrajectory, _1, _2)); + me->testOriginService = me->create_service( + ServiceNames::getTestOrigin, std::bind(&EsminiAdapter::onRequestTestOrigin, _1, _2)); return retval; } diff --git a/atos/modules/IntegrationTesting/src/scenarioexecution.cpp b/atos/modules/IntegrationTesting/src/scenarioexecution.cpp index 7a62cd7a2..acea2bde8 100644 --- a/atos/modules/IntegrationTesting/src/scenarioexecution.cpp +++ b/atos/modules/IntegrationTesting/src/scenarioexecution.cpp @@ -3,36 +3,36 @@ * License, v. 2.0. If a copy of the MPL was not distributed with this * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ -#include #include "scenarioexecution.hpp" #include "rclcpp/wait_for_message.hpp" +#include using namespace std::chrono_literals; /** * @brief An integration test for scenario execution. This test will run a scenario execution for one object, * and check that all states are correct and that it follows the trajectory. - * + * */ -ScenarioExecution::ScenarioExecution() : IntegrationTesting(testName) { +ScenarioExecution::ScenarioExecution() : + IntegrationTesting(testName) { RCLCPP_INFO(get_logger(), "Started integration test: %s", testName.c_str()); - getObjectTrajectoryClient = this->create_client(atosNamespace + ServiceNames::getObjectTrajectory); + getObjectTrajectoryClient = + this->create_client(atosNamespace + ServiceNames::getObjectTrajectory); runIntegrationTest(); } - /** * @brief Destructor. - * + * */ ScenarioExecution::~ScenarioExecution() {} - /** * @brief Run the integration test. This integration test will initialize a scenario, connect to object 1, * arm the object, and then start it. It will check that all states are correct, and that the object follows * the trajectory. - * + * */ void ScenarioExecution::runIntegrationTest() { RCLCPP_INFO(get_logger(), "Running scenario"); @@ -41,7 +41,7 @@ void ScenarioExecution::runIntegrationTest() { initPub->publish(msg); std::this_thread::sleep_for(1s); // sleep to allow the object to initialize checkState(initTopic); - + connectPub->publish(msg); std::this_thread::sleep_for(1s); // sleep to allow the object to connect checkState(connectTopic); @@ -56,17 +56,16 @@ void ScenarioExecution::runIntegrationTest() { checkObjectStoppedAtLastPoint(); printResult(); -} - +} /** * @brief Get the x- and y-coordinates of the trajectory points for object 1. - * + * * @return std::vector> Trajectory points. */ std::vector> ScenarioExecution::getTrajectoryPoints() { auto request = std::make_shared(); - request->id = 1; + request->id = 1; std::shared_ptr response; this->callService(1000ms, getObjectTrajectoryClient, response, request); @@ -77,43 +76,47 @@ std::vector> ScenarioExecution::getTrajectoryPoints() return trajectory; } - /** * @brief Check that the object ends up near the end point of the trajectory. - * + * */ void ScenarioExecution::checkObjectStoppedAtLastPoint() { auto trajectory = getTrajectoryPoints(); std::pair lastPoint; auto isObjectMoving = true; - while(isObjectMoving) { + while (isObjectMoving) { atos_interfaces::msg::Monitor monr; - monitorSub = this->create_subscription("/atos/object_1/object_monitor", 10, std::bind(&ScenarioExecution::placeholderCallback, this, std::placeholders::_1)); + monitorSub = this->create_subscription( + "/atos/object_1/object_monitor", + 10, + std::bind(&ScenarioExecution::placeholderCallback, this, std::placeholders::_1)); rclcpp::wait_for_message(monr, monitorSub, this->get_node_base_interface()->get_context(), 50ms); double speedThreshold = 0.1; if (abs(monr.velocity.twist.linear.x) < speedThreshold && abs(monr.velocity.twist.linear.y) < speedThreshold) { - lastPoint = std::make_pair(monr.pose.pose.position.x, monr.pose.pose.position.y); + lastPoint = std::make_pair(monr.pose.pose.position.x, monr.pose.pose.position.y); isObjectMoving = false; } } - auto distanceX = lastPoint.first - trajectory.back().first; - auto distanceY = lastPoint.second - trajectory.back().second; + auto distanceX = lastPoint.first - trajectory.back().first; + auto distanceY = lastPoint.second - trajectory.back().second; auto distanceThreshold = 1.0; if (abs(distanceX) < distanceThreshold && abs(distanceY) < distanceThreshold) { followedTrajectory = true; RCLCPP_INFO(get_logger(), "Object followed trajectory succesfully"); - } - else { + } else { followedTrajectory = false; - RCLCPP_ERROR(get_logger(), "Object did not follow trajectory succesfully. Distance from end point of trajectory is x: %f, y: %f", distanceX, distanceY); + RCLCPP_ERROR( + get_logger(), + "Object did not follow trajectory succesfully. Distance from end point of trajectory is x: %f, y: %f", + distanceX, + distanceY); } } - /** * @brief Print the result of the integration test. - * + * */ void ScenarioExecution::printResult() { std::stringstream ss; @@ -121,21 +124,19 @@ void ScenarioExecution::printResult() { ss << "\nIntegration test - Scenario Excecution results:\n"; ss << "State change results table:\n"; ss << std::setfill('-') << std::setw(width * 3) << "-" << std::endl; - ss << std::left << std::setfill(' ') << std::setw(width) << "State" - << std::setw(width) << "Expected state" - << std::setw(width) << "Pass" << std::endl; + ss << std::left << std::setfill(' ') << std::setw(width) << "State" << std::setw(width) << "Expected state" + << std::setw(width) << "Pass" << std::endl; ss << std::setfill('-') << std::setw(width * 3) << "-" << std::endl; bool allStatesCorrect = true; for (auto const& [state, expectedState] : stateResult) { const bool passed = (state == expectedState); - const char *pass = passed ? "OK" : "NOT OK"; + const char* pass = passed ? "OK" : "NOT OK"; if (!passed) { allStatesCorrect = false; } - ss << std::left << std::setfill(' ') << std::setw(width) << state - << std::setw(width) << expectedState - << std::setw(width) << pass << std::endl; + ss << std::left << std::setfill(' ') << std::setw(width) << state << std::setw(width) << expectedState + << std::setw(width) << pass << std::endl; } ss << std::setfill('-') << std::setw(width * 3) << "-" << std::endl; ss << std::setfill(' '); @@ -148,10 +149,9 @@ void ScenarioExecution::printResult() { RCLCPP_INFO(get_logger(), "%s", result.c_str()); } - /** * @brief This is a placeholder callback function. It is needed in order to create a topic subscriber. - * + * * @param msg Message. */ void ScenarioExecution::placeholderCallback(const atos_interfaces::msg::Monitor::SharedPtr msg) { diff --git a/atos/modules/MQTTBridge/src/mqttbridge.cpp b/atos/modules/MQTTBridge/src/mqttbridge.cpp index 5f109f168..aad5be3a4 100644 --- a/atos/modules/MQTTBridge/src/mqttbridge.cpp +++ b/atos/modules/MQTTBridge/src/mqttbridge.cpp @@ -10,142 +10,132 @@ using namespace ROSChannels; using std::placeholders::_1; -MqttBridge::MqttBridge() - : Module(MqttBridge::moduleName), - customCommandActionMsgSub( - *this, std::bind(&MqttBridge::onCustomCommandActionMsg, this, _1)), - obcStateChangeSub(*this, - std::bind(&MqttBridge::onObcStateChangeMsg, this, _1)) { - this->loadParameters(); - this->initialize(); +MqttBridge::MqttBridge() : + Module(MqttBridge::moduleName), + customCommandActionMsgSub(*this, std::bind(&MqttBridge::onCustomCommandActionMsg, this, _1)), + obcStateChangeSub(*this, std::bind(&MqttBridge::onObcStateChangeMsg, this, _1)) { + this->loadParameters(); + this->initialize(); } void MqttBridge::loadParameters() { - declare_parameter("broker.host", ""); - declare_parameter("broker.port", 1883); - declare_parameter("client.username", ""); - declare_parameter("client.password", ""); - declare_parameter("client.id", ""); - declare_parameter("topic_prefix", "atos"); - - get_parameter("broker.host", brokerIP); - get_parameter("broker.port", port); - get_parameter("client.username", username); - get_parameter("client.password", password); - get_parameter("client.id", clientId); - get_parameter("topic_prefix", topic_prefix); - - rcl_interfaces::msg::ParameterDescriptor param_desc; - param_desc.description = "The list of topics to bridge from MQTT to ROS"; - const auto mqtt2ros_mqtt_topics = declare_parameter>( - "mqtt2ros.mqtt_topics", std::vector(), param_desc); - for (const auto &mqtt_topic : mqtt2ros_mqtt_topics) { - param_desc.description = - "ROS topic on which corresponding MQTT messages are published"; - declare_parameter(fmt::format("mqtt2ros.{}.ros_topic", mqtt_topic), - rclcpp::ParameterType::PARAMETER_STRING, param_desc); - param_desc.description = "MQTT QoS value"; - declare_parameter(fmt::format("mqtt2ros.{}.advanced.mqtt.qos", mqtt_topic), - rclcpp::ParameterType::PARAMETER_INTEGER, param_desc); - param_desc.description = "ROS publisher queue size"; - declare_parameter( - fmt::format("mqtt2ros.{}.advanced.ros.queue_size", mqtt_topic), - rclcpp::ParameterType::PARAMETER_INTEGER, param_desc); - } - - param_desc.description = "The list of topics to bridge from ROS to MQTT"; - const auto ros2mqtt_ros_topics = declare_parameter>( - "ros2mqtt.ros_topics", std::vector(), param_desc); - for (const auto &ros_topic : ros2mqtt_ros_topics) { - param_desc.description = "MQTT topic on which the corresponding ROS " - "messages are sent to the broker"; - declare_parameter(fmt::format("ros2mqtt.{}.mqtt_topic", ros_topic), - rclcpp::ParameterType::PARAMETER_STRING, param_desc); - param_desc.description = "ROS subscriber queue size"; - declare_parameter( - fmt::format("ros2mqtt.{}.advanced.ros.queue_size", ros_topic), - rclcpp::ParameterType::PARAMETER_INTEGER, param_desc); - param_desc.description = "MQTT QoS value"; - declare_parameter(fmt::format("ros2mqtt.{}.advanced.mqtt.qos", ros_topic), - rclcpp::ParameterType::PARAMETER_INTEGER, param_desc); - } - - // mqtt2ros - for (const auto &mqtt_topic : mqtt2ros_mqtt_topics) { - - rclcpp::Parameter ros_topic_param; - if (get_parameter(fmt::format("mqtt2ros.{}.ros_topic", mqtt_topic), - ros_topic_param)) { - - // mqtt2ros[k]/mqtt_topic and mqtt2ros[k]/ros_topic - const std::string ros_topic = ros_topic_param.as_string(); - Mqtt2RosInterface &mqtt2ros = mqtt2ros_[mqtt_topic]; - mqtt2ros.ros.topic = ros_topic; - - // mqtt2ros[k]/advanced/mqtt/qos - rclcpp::Parameter qos_param; - if (get_parameter( - fmt::format("mqtt2ros.{}.advanced.mqtt.qos", mqtt_topic), - qos_param)) { - mqtt2ros.mqtt.qos = qos_param.as_int(); - } - - // mqtt2ros[k]/advanced/ros/queue_size - rclcpp::Parameter queue_size_param; - if (get_parameter( - fmt::format("mqtt2ros.{}.advanced.ros.queue_size", mqtt_topic), - queue_size_param)) { - mqtt2ros.ros.queue_size = queue_size_param.as_int(); - } - - RCLCPP_INFO(get_logger(), "Bridging MQTT topic '%s' to ROS topic '%s'", - mqtt_topic.c_str(), mqtt2ros.ros.topic.c_str()); - } else { - const auto warning = fmt::format( - "Parameter 'mqtt2ros.{}' is missing subparameter 'ros_topic', will " - "be ignored", - mqtt_topic); - RCLCPP_WARN(get_logger(), - "%s", warning.c_str()); - } - } - - // ros2mqtt - for (const auto &ros_topic : ros2mqtt_ros_topics) { - - rclcpp::Parameter mqtt_topic_param; - if (get_parameter(fmt::format("ros2mqtt.{}.mqtt_topic", ros_topic), - mqtt_topic_param)) { - - // ros2mqtt[k]/ros_topic and ros2mqtt[k]/mqtt_topic - const std::string mqtt_topic = mqtt_topic_param.as_string(); - Ros2MqttInterface &ros2mqtt = ros2mqtt_[ros_topic]; - ros2mqtt.mqtt.topic = mqtt_topic; - - // ros2mqtt[k]/advanced/ros/queue_size - rclcpp::Parameter queue_size_param; - if (get_parameter( - fmt::format("ros2mqtt.{}.advanced.ros.queue_size", ros_topic), - queue_size_param)) - ros2mqtt.ros.queue_size = queue_size_param.as_int(); - - // ros2mqtt[k]/advanced/mqtt/qos - rclcpp::Parameter qos_param; - if (get_parameter(fmt::format("ros2mqtt.{}.advanced.mqtt.qos", ros_topic), - qos_param)) - ros2mqtt.mqtt.qos = qos_param.as_int(); - - RCLCPP_INFO(get_logger(), "Bridging ROS topic '%s' to MQTT topic '%s'", - ros_topic.c_str(), ros2mqtt.mqtt.topic.c_str()); - } else { - const auto warning = fmt::format( - "Parameter 'ros2mqtt.{}' is missing subparameter 'mqtt_topic', will " - "be ignored", - ros_topic); - RCLCPP_WARN(get_logger(), - "%s", warning.c_str()); - } - } + declare_parameter("broker.host", ""); + declare_parameter("broker.port", 1883); + declare_parameter("client.username", ""); + declare_parameter("client.password", ""); + declare_parameter("client.id", ""); + declare_parameter("topic_prefix", "atos"); + + get_parameter("broker.host", brokerIP); + get_parameter("broker.port", port); + get_parameter("client.username", username); + get_parameter("client.password", password); + get_parameter("client.id", clientId); + get_parameter("topic_prefix", topic_prefix); + + rcl_interfaces::msg::ParameterDescriptor param_desc; + param_desc.description = "The list of topics to bridge from MQTT to ROS"; + const auto mqtt2ros_mqtt_topics = + declare_parameter>("mqtt2ros.mqtt_topics", std::vector(), param_desc); + for (const auto& mqtt_topic : mqtt2ros_mqtt_topics) { + param_desc.description = "ROS topic on which corresponding MQTT messages are published"; + declare_parameter( + fmt::format("mqtt2ros.{}.ros_topic", mqtt_topic), rclcpp::ParameterType::PARAMETER_STRING, param_desc); + param_desc.description = "MQTT QoS value"; + declare_parameter(fmt::format("mqtt2ros.{}.advanced.mqtt.qos", mqtt_topic), + rclcpp::ParameterType::PARAMETER_INTEGER, + param_desc); + param_desc.description = "ROS publisher queue size"; + declare_parameter(fmt::format("mqtt2ros.{}.advanced.ros.queue_size", mqtt_topic), + rclcpp::ParameterType::PARAMETER_INTEGER, + param_desc); + } + + param_desc.description = "The list of topics to bridge from ROS to MQTT"; + const auto ros2mqtt_ros_topics = + declare_parameter>("ros2mqtt.ros_topics", std::vector(), param_desc); + for (const auto& ros_topic : ros2mqtt_ros_topics) { + param_desc.description = "MQTT topic on which the corresponding ROS " + "messages are sent to the broker"; + declare_parameter( + fmt::format("ros2mqtt.{}.mqtt_topic", ros_topic), rclcpp::ParameterType::PARAMETER_STRING, param_desc); + param_desc.description = "ROS subscriber queue size"; + declare_parameter(fmt::format("ros2mqtt.{}.advanced.ros.queue_size", ros_topic), + rclcpp::ParameterType::PARAMETER_INTEGER, + param_desc); + param_desc.description = "MQTT QoS value"; + declare_parameter(fmt::format("ros2mqtt.{}.advanced.mqtt.qos", ros_topic), + rclcpp::ParameterType::PARAMETER_INTEGER, + param_desc); + } + + // mqtt2ros + for (const auto& mqtt_topic : mqtt2ros_mqtt_topics) { + + rclcpp::Parameter ros_topic_param; + if (get_parameter(fmt::format("mqtt2ros.{}.ros_topic", mqtt_topic), ros_topic_param)) { + + // mqtt2ros[k]/mqtt_topic and mqtt2ros[k]/ros_topic + const std::string ros_topic = ros_topic_param.as_string(); + Mqtt2RosInterface& mqtt2ros = mqtt2ros_[mqtt_topic]; + mqtt2ros.ros.topic = ros_topic; + + // mqtt2ros[k]/advanced/mqtt/qos + rclcpp::Parameter qos_param; + if (get_parameter(fmt::format("mqtt2ros.{}.advanced.mqtt.qos", mqtt_topic), qos_param)) { + mqtt2ros.mqtt.qos = qos_param.as_int(); + } + + // mqtt2ros[k]/advanced/ros/queue_size + rclcpp::Parameter queue_size_param; + if (get_parameter(fmt::format("mqtt2ros.{}.advanced.ros.queue_size", mqtt_topic), queue_size_param)) { + mqtt2ros.ros.queue_size = queue_size_param.as_int(); + } + + RCLCPP_INFO(get_logger(), + "Bridging MQTT topic '%s' to ROS topic '%s'", + mqtt_topic.c_str(), + mqtt2ros.ros.topic.c_str()); + } else { + const auto warning = fmt::format("Parameter 'mqtt2ros.{}' is missing subparameter 'ros_topic', will " + "be ignored", + mqtt_topic); + RCLCPP_WARN(get_logger(), "%s", warning.c_str()); + } + } + + // ros2mqtt + for (const auto& ros_topic : ros2mqtt_ros_topics) { + + rclcpp::Parameter mqtt_topic_param; + if (get_parameter(fmt::format("ros2mqtt.{}.mqtt_topic", ros_topic), mqtt_topic_param)) { + + // ros2mqtt[k]/ros_topic and ros2mqtt[k]/mqtt_topic + const std::string mqtt_topic = mqtt_topic_param.as_string(); + Ros2MqttInterface& ros2mqtt = ros2mqtt_[ros_topic]; + ros2mqtt.mqtt.topic = mqtt_topic; + + // ros2mqtt[k]/advanced/ros/queue_size + rclcpp::Parameter queue_size_param; + if (get_parameter(fmt::format("ros2mqtt.{}.advanced.ros.queue_size", ros_topic), queue_size_param)) + ros2mqtt.ros.queue_size = queue_size_param.as_int(); + + // ros2mqtt[k]/advanced/mqtt/qos + rclcpp::Parameter qos_param; + if (get_parameter(fmt::format("ros2mqtt.{}.advanced.mqtt.qos", ros_topic), qos_param)) + ros2mqtt.mqtt.qos = qos_param.as_int(); + + RCLCPP_INFO(get_logger(), + "Bridging ROS topic '%s' to MQTT topic '%s'", + ros_topic.c_str(), + ros2mqtt.mqtt.topic.c_str()); + } else { + const auto warning = fmt::format("Parameter 'ros2mqtt.{}' is missing subparameter 'mqtt_topic', will " + "be ignored", + ros_topic); + RCLCPP_WARN(get_logger(), "%s", warning.c_str()); + } + } } /*! @@ -153,323 +143,294 @@ void MqttBridge::loadParameters() { * with ros parameter settings. */ void MqttBridge::initialize() { - if (this->brokerIP.empty()) { - RCLCPP_INFO(this->get_logger(), - "No Broker IP provided in configuration. Shutting down..."); - rclcpp::shutdown(); - } else { - this->setupClient(); - this->connect(); - // Wait for all ros topics to be announced - rclcpp::sleep_for(std::chrono::seconds(1)); - this->setupMqtt2RosBridge(); - this->setupRos2MqttBridge(); - } + if (this->brokerIP.empty()) { + RCLCPP_INFO(this->get_logger(), "No Broker IP provided in configuration. Shutting down..."); + rclcpp::shutdown(); + } else { + this->setupClient(); + this->connect(); + // Wait for all ros topics to be announced + rclcpp::sleep_for(std::chrono::seconds(1)); + this->setupMqtt2RosBridge(); + this->setupRos2MqttBridge(); + } } void MqttBridge::setupMqtt2RosBridge() { - new_mqtt2ros_bridge_service_ = - create_service( - "~/new_mqtt2ros_bridge", - std::bind(&MqttBridge::newMqtt2RosBridge, this, std::placeholders::_1, - std::placeholders::_2)); - - // Create a request and response for each mqtt2ros bridge and put into a - // map - auto serviceCallMap = std::map< - std::shared_ptr, - std::shared_ptr>(); - for (const auto &mqtt2ros : mqtt2ros_) { - atos_interfaces::srv::NewMqtt2RosBridge::Request::SharedPtr request = - std::make_shared(); - request->mqtt_topic = mqtt2ros.first; - request->ros_topic = mqtt2ros.second.ros.topic; - request->mqtt_qos = mqtt2ros.second.mqtt.qos; - request->ros_queue_size = mqtt2ros.second.ros.queue_size; - atos_interfaces::srv::NewMqtt2RosBridge::Response::SharedPtr response = - std::make_shared(); - serviceCallMap[response] = request; - } - - // Loop through the map until all response objects are true - while (std::any_of(serviceCallMap.begin(), serviceCallMap.end(), - [](const auto &pair) { return !pair.first->success; })) { - // Get all unsuccesful responses - auto failedResponses = std::vector< - std::shared_ptr>(); - for (const auto &pair : serviceCallMap) { - if (!pair.first->success) { - failedResponses.push_back(pair.first); - } - } - // Retry all failed responses - for (const auto &response : failedResponses) { - newMqtt2RosBridge(serviceCallMap[response], response); - } - // Wait for a second before retrying - rclcpp::sleep_for(std::chrono::seconds(1)); - } + new_mqtt2ros_bridge_service_ = create_service( + "~/new_mqtt2ros_bridge", + std::bind(&MqttBridge::newMqtt2RosBridge, this, std::placeholders::_1, std::placeholders::_2)); + + // Create a request and response for each mqtt2ros bridge and put into a + // map + auto serviceCallMap = std::map, + std::shared_ptr>(); + for (const auto& mqtt2ros : mqtt2ros_) { + atos_interfaces::srv::NewMqtt2RosBridge::Request::SharedPtr request = + std::make_shared(); + request->mqtt_topic = mqtt2ros.first; + request->ros_topic = mqtt2ros.second.ros.topic; + request->mqtt_qos = mqtt2ros.second.mqtt.qos; + request->ros_queue_size = mqtt2ros.second.ros.queue_size; + atos_interfaces::srv::NewMqtt2RosBridge::Response::SharedPtr response = + std::make_shared(); + serviceCallMap[response] = request; + } + + // Loop through the map until all response objects are true + while (std::any_of( + serviceCallMap.begin(), serviceCallMap.end(), [](const auto& pair) { return !pair.first->success; })) { + // Get all unsuccesful responses + auto failedResponses = std::vector>(); + for (const auto& pair : serviceCallMap) { + if (!pair.first->success) { + failedResponses.push_back(pair.first); + } + } + // Retry all failed responses + for (const auto& response : failedResponses) { + newMqtt2RosBridge(serviceCallMap[response], response); + } + // Wait for a second before retrying + rclcpp::sleep_for(std::chrono::seconds(1)); + } } void MqttBridge::setupRos2MqttBridge() { - new_ros2mqtt_bridge_service_ = - create_service( - "~/new_ros2mqtt_bridge", - std::bind(&MqttBridge::newRos2MqttBridge, this, std::placeholders::_1, - std::placeholders::_2)); - - // setup subscribers - this->setupSubscriptions(); + new_ros2mqtt_bridge_service_ = create_service( + "~/new_ros2mqtt_bridge", + std::bind(&MqttBridge::newRos2MqttBridge, this, std::placeholders::_1, std::placeholders::_2)); + + // setup subscribers + this->setupSubscriptions(); } void MqttBridge::setupSubscriptions() { - // get info of all topics - const auto all_topics_and_types = get_topic_names_and_types(); - // check for ros2mqtt topics - for (auto &[ros_topic, ros2mqtt] : ros2mqtt_) { - if (all_topics_and_types.count(ros_topic)) { - // check if message type has changed or if mapping is stale - const std::string &msg_type = all_topics_and_types.at(ros_topic)[0]; - if (msg_type == ros2mqtt.ros.msg_type && !ros2mqtt.ros.is_stale) - continue; - ros2mqtt.ros.is_stale = false; - ros2mqtt.ros.msg_type = msg_type; - - // create new generic subscription, if message type has changed - std::function msg)> - bound_callback_func = std::bind(&MqttBridge::ros2mqtt, this, - std::placeholders::_1, ros_topic); - try { - ros2mqtt.ros.subscriber = create_generic_subscription( - ros_topic, msg_type, ros2mqtt.ros.queue_size, bound_callback_func); - } catch (rclcpp::exceptions::RCLError &e) { - RCLCPP_ERROR(get_logger(), "Failed to create generic subscriber: %s", - e.what()); - return; - } - RCLCPP_INFO(get_logger(), "Subscribed to ROS topic '%s' of type '%s'", - ros_topic.c_str(), msg_type.c_str()); - } - } + // get info of all topics + const auto all_topics_and_types = get_topic_names_and_types(); + // check for ros2mqtt topics + for (auto& [ros_topic, ros2mqtt] : ros2mqtt_) { + if (all_topics_and_types.count(ros_topic)) { + // check if message type has changed or if mapping is stale + const std::string& msg_type = all_topics_and_types.at(ros_topic)[0]; + if (msg_type == ros2mqtt.ros.msg_type && !ros2mqtt.ros.is_stale) + continue; + ros2mqtt.ros.is_stale = false; + ros2mqtt.ros.msg_type = msg_type; + + // create new generic subscription, if message type has changed + std::function msg)> bound_callback_func = + std::bind(&MqttBridge::ros2mqtt, this, std::placeholders::_1, ros_topic); + try { + ros2mqtt.ros.subscriber = + create_generic_subscription(ros_topic, msg_type, ros2mqtt.ros.queue_size, bound_callback_func); + } catch (rclcpp::exceptions::RCLError& e) { + RCLCPP_ERROR(get_logger(), "Failed to create generic subscriber: %s", e.what()); + return; + } + RCLCPP_INFO(get_logger(), "Subscribed to ROS topic '%s' of type '%s'", ros_topic.c_str(), msg_type.c_str()); + } + } } -void MqttBridge::ros2mqtt( - const std::shared_ptr &serialized_msg, - const std::string &ros_topic) { - (void)serialized_msg; - - Ros2MqttInterface &ros2mqtt = ros2mqtt_[ros_topic]; - std::string mqtt_topic = ros2mqtt.mqtt.topic; - std::vector payload_buffer; - - RCLCPP_INFO(get_logger(), "Received ROS message on topic '%s'", - ros_topic.c_str()); - - try { - mqtt::message_ptr mqtt_msg = - mqtt::make_message(mqtt_topic, payload_buffer.data(), - payload_buffer.size(), ros2mqtt.mqtt.qos, true); - client_->publish(mqtt_msg); - RCLCPP_INFO(get_logger(), "Published ROS message to MQTT topic '%s'", - mqtt_topic.c_str()); - } catch (const mqtt::exception &e) { - RCLCPP_WARN( - get_logger(), - "Publishing ROS message type information to MQTT topic '%s' failed: %s", - mqtt_topic.c_str(), e.what()); - } +void MqttBridge::ros2mqtt(const std::shared_ptr& serialized_msg, + const std::string& ros_topic) { + (void)serialized_msg; + + Ros2MqttInterface& ros2mqtt = ros2mqtt_[ros_topic]; + std::string mqtt_topic = ros2mqtt.mqtt.topic; + std::vector payload_buffer; + + RCLCPP_INFO(get_logger(), "Received ROS message on topic '%s'", ros_topic.c_str()); + + try { + mqtt::message_ptr mqtt_msg = + mqtt::make_message(mqtt_topic, payload_buffer.data(), payload_buffer.size(), ros2mqtt.mqtt.qos, true); + client_->publish(mqtt_msg); + RCLCPP_INFO(get_logger(), "Published ROS message to MQTT topic '%s'", mqtt_topic.c_str()); + } catch (const mqtt::exception& e) { + RCLCPP_WARN(get_logger(), + "Publishing ROS message type information to MQTT topic '%s' failed: %s", + mqtt_topic.c_str(), + e.what()); + } } void MqttBridge::setupClient() { - RCLCPP_INFO(this->get_logger(), - "Setting up connection with clientID: %s, and broker IP: %s", - clientId.c_str(), brokerIP.c_str()); - - // basic client connection options - connect_options_.set_automatic_reconnect(true); - connect_options_.set_clean_session(true); - connect_options_.set_keep_alive_interval(5); - - // user authentication - if (!username.empty()) { - connect_options_.set_user_name(username); - connect_options_.set_password(password); - } - - auto timeNanoseconds = - std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()) - .count(); - auto id = clientId + "_" + std::to_string(timeNanoseconds); - - const std::string uri = fmt::format("{}://{}:{}", "tcp", brokerIP, port); - try { - client_ = - std::shared_ptr(new mqtt::async_client(uri, id)); - } catch (const mqtt::exception &e) { - RCLCPP_ERROR(get_logger(), "Client could not be initialized: %s", e.what()); - exit(EXIT_FAILURE); - } - - // setup MQTT callbacks - client_->set_callback(*this); + RCLCPP_INFO(this->get_logger(), + "Setting up connection with clientID: %s, and broker IP: %s", + clientId.c_str(), + brokerIP.c_str()); + + // basic client connection options + connect_options_.set_automatic_reconnect(true); + connect_options_.set_clean_session(true); + connect_options_.set_keep_alive_interval(5); + + // user authentication + if (!username.empty()) { + connect_options_.set_user_name(username); + connect_options_.set_password(password); + } + + auto timeNanoseconds = + std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + auto id = clientId + "_" + std::to_string(timeNanoseconds); + + const std::string uri = fmt::format("{}://{}:{}", "tcp", brokerIP, port); + try { + client_ = std::shared_ptr(new mqtt::async_client(uri, id)); + } catch (const mqtt::exception& e) { + RCLCPP_ERROR(get_logger(), "Client could not be initialized: %s", e.what()); + exit(EXIT_FAILURE); + } + + // setup MQTT callbacks + client_->set_callback(*this); } void MqttBridge::connect() { - std::string as_client = - clientId.empty() ? "" - : std::string(" as '") + clientId + std::string("'"); - RCLCPP_INFO(get_logger(), "Connecting to broker at '%s'%s ...", - client_->get_server_uri().c_str(), as_client.c_str()); - - try { - client_->connect(connect_options_, nullptr, *this); - } catch (const mqtt::exception &e) { - RCLCPP_ERROR(get_logger(), "Connection to broker failed: %s", e.what()); - exit(EXIT_FAILURE); - } + std::string as_client = clientId.empty() ? "" : std::string(" as '") + clientId + std::string("'"); + RCLCPP_INFO( + get_logger(), "Connecting to broker at '%s'%s ...", client_->get_server_uri().c_str(), as_client.c_str()); + + try { + client_->connect(connect_options_, nullptr, *this); + } catch (const mqtt::exception& e) { + RCLCPP_ERROR(get_logger(), "Connection to broker failed: %s", e.what()); + exit(EXIT_FAILURE); + } } -void MqttBridge::newMqtt2RosBridge( - atos_interfaces::srv::NewMqtt2RosBridge::Request::SharedPtr request, - atos_interfaces::srv::NewMqtt2RosBridge::Response::SharedPtr response) { - - // add mapping definition to mqtt2ros_ - Mqtt2RosInterface &mqtt2ros = mqtt2ros_[request->mqtt_topic]; - mqtt2ros.ros.is_stale = true; - mqtt2ros.ros.topic = request->ros_topic; - mqtt2ros.mqtt.qos = request->mqtt_qos; - mqtt2ros.ros.publisher = this->create_publisher( - mqtt2ros.ros.topic, request->ros_queue_size); - mqtt2ros.ros.queue_size = request->ros_queue_size; - - RCLCPP_DEBUG(get_logger(), "Bridging MQTT topic '%s' to ROS topic '%s'", - request->mqtt_topic.c_str(), mqtt2ros.ros.topic.c_str()); - - // subscribe to the MQTT topic - std::string mqtt_topic_to_subscribe = request->mqtt_topic; - try { - client_->subscribe(mqtt_topic_to_subscribe, mqtt2ros.mqtt.qos); - RCLCPP_INFO(get_logger(), "Subscribed to MQTT topic '%s'", - mqtt_topic_to_subscribe.c_str()); - response->success = true; - } - // Catch exception if the topic is already subscribed - catch (const mqtt::exception &e) { - RCLCPP_WARN(get_logger(), "Failed to subscribe MQTT topic '%s': %s", - mqtt_topic_to_subscribe.c_str(), e.what()); - response->success = false; - } +void MqttBridge::newMqtt2RosBridge(atos_interfaces::srv::NewMqtt2RosBridge::Request::SharedPtr request, + atos_interfaces::srv::NewMqtt2RosBridge::Response::SharedPtr response) { + + // add mapping definition to mqtt2ros_ + Mqtt2RosInterface& mqtt2ros = mqtt2ros_[request->mqtt_topic]; + mqtt2ros.ros.is_stale = true; + mqtt2ros.ros.topic = request->ros_topic; + mqtt2ros.mqtt.qos = request->mqtt_qos; + mqtt2ros.ros.publisher = this->create_publisher(mqtt2ros.ros.topic, request->ros_queue_size); + mqtt2ros.ros.queue_size = request->ros_queue_size; + + RCLCPP_DEBUG(get_logger(), + "Bridging MQTT topic '%s' to ROS topic '%s'", + request->mqtt_topic.c_str(), + mqtt2ros.ros.topic.c_str()); + + // subscribe to the MQTT topic + std::string mqtt_topic_to_subscribe = request->mqtt_topic; + try { + client_->subscribe(mqtt_topic_to_subscribe, mqtt2ros.mqtt.qos); + RCLCPP_INFO(get_logger(), "Subscribed to MQTT topic '%s'", mqtt_topic_to_subscribe.c_str()); + response->success = true; + } + // Catch exception if the topic is already subscribed + catch (const mqtt::exception& e) { + RCLCPP_WARN(get_logger(), "Failed to subscribe MQTT topic '%s': %s", mqtt_topic_to_subscribe.c_str(), e.what()); + response->success = false; + } } -void MqttBridge::newRos2MqttBridge( - atos_interfaces::srv::NewRos2MqttBridge::Request::SharedPtr request, - atos_interfaces::srv::NewRos2MqttBridge::Response::SharedPtr response) { +void MqttBridge::newRos2MqttBridge(atos_interfaces::srv::NewRos2MqttBridge::Request::SharedPtr request, + atos_interfaces::srv::NewRos2MqttBridge::Response::SharedPtr response) { - // add mapping definition to ros2mqtt_ - Ros2MqttInterface &ros2mqtt = ros2mqtt_[request->ros_topic]; - ros2mqtt.ros.is_stale = true; - ros2mqtt.ros.topic = request->ros_topic; - ros2mqtt.mqtt.topic = request->mqtt_topic; - ros2mqtt.mqtt.qos = request->mqtt_qos; + // add mapping definition to ros2mqtt_ + Ros2MqttInterface& ros2mqtt = ros2mqtt_[request->ros_topic]; + ros2mqtt.ros.is_stale = true; + ros2mqtt.ros.topic = request->ros_topic; + ros2mqtt.mqtt.topic = request->mqtt_topic; + ros2mqtt.mqtt.qos = request->mqtt_qos; - RCLCPP_DEBUG(get_logger(), "Bridging ROS topic '%s' to MQTT topic '%s'", - ros2mqtt.ros.topic.c_str(), ros2mqtt.mqtt.topic.c_str()); + RCLCPP_DEBUG(get_logger(), + "Bridging ROS topic '%s' to MQTT topic '%s'", + ros2mqtt.ros.topic.c_str(), + ros2mqtt.mqtt.topic.c_str()); - // setup ROS subscriptions - setupSubscriptions(); + // setup ROS subscriptions + setupSubscriptions(); - response->success = true; + response->success = true; } void MqttBridge::onCustomCommandActionMsg( - const CustomCommandAction::message_type::SharedPtr - custom_command_action_msg) { - if (custom_command_action_msg->type == "v2x") { - RCLCPP_INFO(this->get_logger(), "Received V2X message on %s topic", - CustomCommandAction::topicName.c_str()); - this->onMessage(custom_command_action_msg->content, "v2x", - v2xToJson); - } + const CustomCommandAction::message_type::SharedPtr custom_command_action_msg) { + if (custom_command_action_msg->type == "v2x") { + RCLCPP_INFO(this->get_logger(), "Received V2X message on %s topic", CustomCommandAction::topicName.c_str()); + this->onMessage(custom_command_action_msg->content, "v2x", v2xToJson); + } } -void MqttBridge::onObcStateChangeMsg( - const StateChange::message_type::SharedPtr obc_msg) { - this->onMessage(obc_msg, "state", - obcStateChangeToJson); +void MqttBridge::onObcStateChangeMsg(const StateChange::message_type::SharedPtr obc_msg) { + this->onMessage(obc_msg, "state", obcStateChangeToJson); } void MqttBridge::message_arrived(mqtt::const_message_ptr mqtt_msg) { - std::string mqtt_topic = mqtt_msg->get_topic(); - RCLCPP_DEBUG(get_logger(), "Received MQTT message on topic '%s'", - mqtt_topic.c_str()); - Mqtt2RosInterface &mqtt2ros = mqtt2ros_[mqtt_topic]; - - // Publish empty message to ROS topic - mqtt2ros.ros.publisher->publish(std_msgs::msg::Empty()); - RCLCPP_DEBUG(get_logger(), "Published empty message to ROS topic '%s'", - mqtt2ros.ros.topic.c_str()); + std::string mqtt_topic = mqtt_msg->get_topic(); + RCLCPP_DEBUG(get_logger(), "Received MQTT message on topic '%s'", mqtt_topic.c_str()); + Mqtt2RosInterface& mqtt2ros = mqtt2ros_[mqtt_topic]; + + // Publish empty message to ROS topic + mqtt2ros.ros.publisher->publish(std_msgs::msg::Empty()); + RCLCPP_DEBUG(get_logger(), "Published empty message to ROS topic '%s'", mqtt2ros.ros.topic.c_str()); } -template -void MqttBridge::onMessage(T msg, std::string mqtt_topic, - std::function convertFunc) { - json payload = convertFunc(msg); - try { - RCLCPP_DEBUG(this->get_logger(), "Publishing MQTT msg to broker %s", - payload.dump().c_str()); - client_->publish(mqtt_topic, payload.dump().c_str(), payload.dump().size()); - } catch (std::runtime_error &) { - RCLCPP_ERROR(this->get_logger(), "Failed to publish MQTT message"); - } +template +void MqttBridge::onMessage(T msg, std::string mqtt_topic, std::function convertFunc) { + json payload = convertFunc(msg); + try { + RCLCPP_DEBUG(this->get_logger(), "Publishing MQTT msg to broker %s", payload.dump().c_str()); + client_->publish(mqtt_topic, payload.dump().c_str(), payload.dump().size()); + } catch (std::runtime_error&) { + RCLCPP_ERROR(this->get_logger(), "Failed to publish MQTT message"); + } } json MqttBridge::v2xToJson(std::string msg_content) { - std::replace(msg_content.begin(), msg_content.end(), '\'', - '"'); // Replace ROS single quotes string with double quotes to - // make it a valid JSON string - json j; - // Parse the string to a json object - try { - j = json::parse(msg_content); - } catch (json::parse_error &e) { - std::cerr << "Failed to parse JSON object: " << e.what() << std::endl; - } - - if (j.find("message_type") == j.end()) { - std::cerr << "No message_type field in JSON object" << std::endl; - return j; - } - if (j["message_type"] == "DENM") { - j["detection_time"] = - std::chrono::system_clock::now().time_since_epoch().count(); - } - return j; + std::replace(msg_content.begin(), + msg_content.end(), + '\'', + '"'); // Replace ROS single quotes string with double quotes to + // make it a valid JSON string + json j; + // Parse the string to a json object + try { + j = json::parse(msg_content); + } catch (json::parse_error& e) { + std::cerr << "Failed to parse JSON object: " << e.what() << std::endl; + } + + if (j.find("message_type") == j.end()) { + std::cerr << "No message_type field in JSON object" << std::endl; + return j; + } + if (j["message_type"] == "DENM") { + j["detection_time"] = std::chrono::system_clock::now().time_since_epoch().count(); + } + return j; } -json MqttBridge::obcStateChangeToJson( - const StateChange::message_type::SharedPtr obc_msg) { - json j; - j["current_state"] = obc_msg->current_state; - j["prev_state"] = obc_msg->prev_state; - return j; +json MqttBridge::obcStateChangeToJson(const StateChange::message_type::SharedPtr obc_msg) { + json j; + j["current_state"] = obc_msg->current_state; + j["prev_state"] = obc_msg->prev_state; + return j; } -void MqttBridge::on_success(const mqtt::token &token) { +void MqttBridge::on_success(const mqtt::token& token) { - (void)token; // Avoid compiler warning for unused parameter. - RCLCPP_INFO(get_logger(), "Connected to broker successfully"); - is_connected_ = true; + (void)token; // Avoid compiler warning for unused parameter. + RCLCPP_INFO(get_logger(), "Connected to broker successfully"); + is_connected_ = true; } -void MqttBridge::on_failure(const mqtt::token &token) { +void MqttBridge::on_failure(const mqtt::token& token) { - RCLCPP_ERROR( - get_logger(), - "Connection to broker failed (return code %d), will automatically " - "retry...", - token.get_return_code()); - is_connected_ = false; + RCLCPP_ERROR(get_logger(), + "Connection to broker failed (return code %d), will automatically " + "retry...", + token.get_return_code()); + is_connected_ = false; } diff --git a/atos/modules/ObjectControl/inc/relativeanchor.hpp b/atos/modules/ObjectControl/inc/relativeanchor.hpp index 885e5abeb..46e8197a1 100644 --- a/atos/modules/ObjectControl/inc/relativeanchor.hpp +++ b/atos/modules/ObjectControl/inc/relativeanchor.hpp @@ -15,10 +15,10 @@ class RelativeAnchor : public TestObject { RelativeAnchor(RelativeAnchor&&); RelativeAnchor& operator=(const RelativeAnchor&) = delete; - RelativeAnchor& operator=(RelativeAnchor&&) = delete; + RelativeAnchor& operator=(RelativeAnchor&&) = delete; - virtual void publishMonr(const ROSChannels::Monitor::message_type) override; + virtual void publishMonr(const ROSChannels::Monitor::message_type) override; private: - ROSChannels::Monitor::AnchorPub anchorPub; + ROSChannels::Monitor::AnchorPub anchorPub; }; diff --git a/atos/modules/ObjectControl/inc/relativetestobject.hpp b/atos/modules/ObjectControl/inc/relativetestobject.hpp index 72ada9bef..3c1f33366 100644 --- a/atos/modules/ObjectControl/inc/relativetestobject.hpp +++ b/atos/modules/ObjectControl/inc/relativetestobject.hpp @@ -5,9 +5,9 @@ */ #pragma once -#include "testobject.hpp" -#include "roschannels/monitorchannel.hpp" #include "positioning.h" +#include "roschannels/monitorchannel.hpp" +#include "testobject.hpp" class RelativeTestObject : public TestObject { public: @@ -16,15 +16,15 @@ class RelativeTestObject : public TestObject { RelativeTestObject(RelativeTestObject&&); RelativeTestObject& operator=(const RelativeTestObject&) = delete; - RelativeTestObject& operator=(RelativeTestObject&&) = delete; + RelativeTestObject& operator=(RelativeTestObject&&) = delete; private: - virtual ObjectMonitorType transformCoordinate(const ObjectMonitorType& point, - const ObjectMonitorType& anchor, - const bool debug); - virtual MonitorMessage readMonitorMessage() override; - - ROSChannels::Monitor::AnchorSub anchorSub; - ObjectMonitorType lastAnchorMonr; - void updateAnchor(const ROSChannels::Monitor::message_type::SharedPtr); + virtual ObjectMonitorType transformCoordinate(const ObjectMonitorType& point, + const ObjectMonitorType& anchor, + const bool debug); + virtual MonitorMessage readMonitorMessage() override; + + ROSChannels::Monitor::AnchorSub anchorSub; + ObjectMonitorType lastAnchorMonr; + void updateAnchor(const ROSChannels::Monitor::message_type::SharedPtr); }; diff --git a/atos/modules/ObjectControl/src/objectlistener.cpp b/atos/modules/ObjectControl/src/objectlistener.cpp index e67a05cee..54a286809 100644 --- a/atos/modules/ObjectControl/src/objectlistener.cpp +++ b/atos/modules/ObjectControl/src/objectlistener.cpp @@ -6,10 +6,10 @@ #include "objectlistener.hpp" #include "objectcontrol.hpp" -ObjectListener::ObjectListener(ObjectControl* sh, std::shared_ptr ob, rclcpp::Logger log) : - Loggable(log), - obj(ob), - handler(sh) { +ObjectListener::ObjectListener(ObjectControl* sh, std::shared_ptr ob, rclcpp::Logger log) : + Loggable(log), + obj(ob), + handler(sh) { if (!obj->isConnected()) { throw std::invalid_argument("Attempted to start listener for disconnected object"); @@ -31,12 +31,12 @@ ObjectListener::~ObjectListener() { void ObjectListener::listen() { try { while (!this->quit) { - //handle incoming iso22133 messages + // handle incoming iso22133 messages obj->handleISOMessage(true); } } catch (std::invalid_argument& e) { RCLCPP_ERROR(get_logger(), "%s", e.what()); // TODO: add comment explaining this case.. - } catch (std::range_error& e){ + } catch (std::range_error& e) { RCLCPP_DEBUG(get_logger(), "%s", e.what()); // Socket was interrupted intentionally, exit gracefully } catch (std::runtime_error& e) { RCLCPP_ERROR(get_logger(), "%s", e.what()); diff --git a/atos_gui/atos_gui/main.py b/atos_gui/atos_gui/main.py index adfdabdf0..d69f53277 100644 --- a/atos_gui/atos_gui/main.py +++ b/atos_gui/atos_gui/main.py @@ -31,7 +31,6 @@ APP_CONFIGURED = False - def main() -> None: configure_app() ui.run(**build_ui_run_args()) @@ -128,13 +127,17 @@ def _fleet_map_payload_json() -> str: class FleetStateNode(Node): def __init__(self) -> None: super().__init__("truck_object_gui_state_bridge") - self._state_sub = self.create_subscription(String, "truck_objects/state", self._on_state, 100) + self._state_sub = self.create_subscription( + String, "truck_objects/state", self._on_state, 100 + ) def _on_state(self, msg: String) -> None: try: payload = json.loads(msg.data) except Exception: - self.get_logger().warning("Failed to parse truck_objects/state payload as JSON") + self.get_logger().warning( + "Failed to parse truck_objects/state payload as JSON" + ) return uid = payload.get("uid") @@ -154,7 +157,9 @@ def render_atosfleetmanagement_pages() -> None: @ui.page(path="/", title="TruckObjectGUI") def render_home() -> None: - ui.add_head_html(f'') + ui.add_head_html( + f'' + ) ui.label("TruckObjectGUI (ATOSFleetManagement mode)").classes("text-h4") with ui.tabs().classes("w-full") as tabs: @@ -190,13 +195,22 @@ def render_home() -> None: ui.label("Path visualization by COT path_name").classes("text-h5") if not default_geojson: - ui.label("Could not load default geojson file.").classes("text-red-600") - ui.markdown("Searched conf dirs:\n" + "\n".join([f"- `{p}`" for p in _candidate_conf_dirs()])) + ui.label("Could not load default geojson file.").classes( + "text-red-600" + ) + ui.markdown( + "Searched conf dirs:\n" + + "\n".join([f"- `{p}`" for p in _candidate_conf_dirs()]) + ) return - ui.label(f"Default source: {source_path}").classes("text-sm text-gray-600") + ui.label(f"Default source: {source_path}").classes( + "text-sm text-gray-600" + ) map_id = "fleet-road-map" - ui.html(f'
    ') + ui.html( + f'
    ' + ) ui.timer( 0.2, @@ -268,6 +282,7 @@ def ros_main() -> None: node.destroy_node() rclpy.shutdown() + def print_access_hint() -> None: scheme = "https" if USE_SSL else "http" print(f"TruckObjectGUI ready. Open {scheme}://localhost:8420", flush=True) @@ -295,8 +310,12 @@ def build_ui_run_args() -> dict: } if USE_SSL: - uvicorn_args["ssl_keyfile"] = Path.home() / ".astazero/ATOS/certs/selfsigned.key" - uvicorn_args["ssl_certfile"] = Path.home() / ".astazero/ATOS/certs/selfsigned.crt" + uvicorn_args["ssl_keyfile"] = ( + Path.home() / ".astazero/ATOS/certs/selfsigned.key" + ) + uvicorn_args["ssl_certfile"] = ( + Path.home() / ".astazero/ATOS/certs/selfsigned.crt" + ) return uvicorn_args diff --git a/atos_gui/atos_gui/objectpanel/objectpanel.py b/atos_gui/atos_gui/objectpanel/objectpanel.py index 111ec6d91..f12d1eb70 100644 --- a/atos_gui/atos_gui/objectpanel/objectpanel.py +++ b/atos_gui/atos_gui/objectpanel/objectpanel.py @@ -13,8 +13,8 @@ def _safe_notify(self, message: str) -> None: except RuntimeError: self.get_logger().warning(message) - """This node is responsible for rendering the object panel and visualize the IP address for each object ID in the scenario.""" + def __init__(self) -> None: """Initializes the node, fetches all object IDs and their IPs in the current scenario and visualizes them.""" super().__init__("object_panel") @@ -55,8 +55,12 @@ def get_object_ids(self): "Get object ID service not available, waiting again..." ) if service_timeout_counter > MAX_TIMEOUT: - self._safe_notify(f'Get object ID service not available after {MAX_TIMEOUT} seconds') - self.get_logger().info(f'Get object ID service not available after {MAX_TIMEOUT} seconds') + self._safe_notify( + f"Get object ID service not available after {MAX_TIMEOUT} seconds" + ) + self.get_logger().info( + f"Get object ID service not available after {MAX_TIMEOUT} seconds" + ) return future = self.get_id_client.call_async(self.object_ids_req) future.add_done_callback( @@ -77,8 +81,12 @@ def get_object_ips(self, object_ids): "Get object IP service not available, waiting again..." ) if service_timeout_counter > MAX_TIMEOUT: - self._safe_notify(f'Get object IP service not available after {MAX_TIMEOUT} seconds') - self.get_logger().info(f'Get object IP service not available after {MAX_TIMEOUT} seconds') + self._safe_notify( + f"Get object IP service not available after {MAX_TIMEOUT} seconds" + ) + self.get_logger().info( + f"Get object IP service not available after {MAX_TIMEOUT} seconds" + ) return for object_id in object_ids: self.object_id_ip_map[object_id] = "" @@ -105,15 +113,19 @@ def update_object_ip(self, object_id, object_ip): object_ip (str): New IP address for the given object ID. """ - self._safe_notify(f'Setting object {object_id} IP: {object_ip}') + self._safe_notify(f"Setting object {object_id} IP: {object_ip}") service_timeout_counter = 0 while not self.set_ip_client.wait_for_service(timeout_sec=1.0): self.get_logger().debug( "Set object IP service not available, waiting again..." ) if service_timeout_counter > MAX_TIMEOUT: - self._safe_notify(f'Set object IP service not available after {MAX_TIMEOUT} seconds') - self.get_logger().info(f'Set object IP service not available after {MAX_TIMEOUT} seconds') + self._safe_notify( + f"Set object IP service not available after {MAX_TIMEOUT} seconds" + ) + self.get_logger().info( + f"Set object IP service not available after {MAX_TIMEOUT} seconds" + ) return future = self.set_ip_client.call_async( SetObjectIp.Request(id=object_id, ip=object_ip) @@ -131,13 +143,19 @@ def update_object_ip_result(self, result): """ if result.success: with self.refresh_row: - self._safe_notify(f'IP set to {result.ip} for object {result.id} was successful') - self.get_logger().info(f'IP set to {result.ip} for object {result.id} was successful') + self._safe_notify( + f"IP set to {result.ip} for object {result.id} was successful" + ) + self.get_logger().info( + f"IP set to {result.ip} for object {result.id} was successful" + ) self.object_id_ip_map[result.id] = result.ip else: with self.refresh_row: - self._safe_notify(f'Failed to set object {result.id} IP to {result.ip}') - self.get_logger().info(f'Failed to set object {result.id} IP to {result.ip}') + self._safe_notify(f"Failed to set object {result.id} IP to {result.ip}") + self.get_logger().info( + f"Failed to set object {result.id} IP to {result.ip}" + ) def refresh(self): """Refreshes the object panel by fetching all object IDs and their IPs in the current scenario.""" From 67543087bea70572a95fbb53530cee3d49217852 Mon Sep 17 00:00:00 2001 From: Sebastian Loh Lindholm Date: Thu, 18 Jun 2026 14:25:04 +0200 Subject: [PATCH 27/33] fix venv for CI --- Dockerfile | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/Dockerfile b/Dockerfile index 75a805678..c4ecc4431 100644 --- a/Dockerfile +++ b/Dockerfile @@ -19,11 +19,9 @@ COPY ./atos_gui/requirements.txt ./atos_gui/requirements.txt COPY ./atos_gui/package.xml ./atos_gui/package.xml COPY ./atos_interfaces/package.xml ./atos_interfaces/package.xml COPY ./atos/package.xml ./atos/package.xml - -RUN --mount=type=cache,target=/var/cache/apt \ - ./scripts/installation/install_deps.sh ${REPO_DIR} COPY . . -RUN ./scripts/installation/install_atos.sh ${REPO_DIR} +RUN --mount=type=cache,target=/var/cache/apt \ + ./setup_atos.sh WORKDIR /root/atos_ws RUN chmod +x /root/atos_git/scripts/run_atosfleetmanagement.sh From 3253226643cac0ea000fa74279a165b1740b07f3 Mon Sep 17 00:00:00 2001 From: Sebastian Loh Lindholm Date: Thu, 18 Jun 2026 14:36:38 +0200 Subject: [PATCH 28/33] recreating venv in ci --- scripts/installation/install_deps.sh | 25 ++++++++++++++++++------- 1 file changed, 18 insertions(+), 7 deletions(-) diff --git a/scripts/installation/install_deps.sh b/scripts/installation/install_deps.sh index 32787af0c..72b8e112f 100755 --- a/scripts/installation/install_deps.sh +++ b/scripts/installation/install_deps.sh @@ -23,6 +23,8 @@ if [ -z "${ROS_DISTRO:-}" ]; then echo "ROS_DISTRO is not set. Source /opt/ros//setup.bash before running this script." exit 1 fi + +ATOS_VENV_PATH="$(get_atos_venv_path)" apt_update_retry() { local attempts=5 local delay=5 @@ -55,18 +57,27 @@ apt_install_retry() { return 1 } -PIP_INSTALL_CMD=(python3 -m pip install) -if python3 -m pip help install 2>/dev/null | grep -q -- "--break-system-packages"; then - PIP_INSTALL_CMD+=(--break-system-packages) -fi - # Update and install required dependencies specified in dependencies.txt and requirements.txt file apt_deps=$(cat ${ATOS_REPO_PATH}/scripts/installation/dependencies.txt | tr '\n' ' ') echo "Installing dependencies... $apt_deps" apt_update_retry apt_install_retry ${apt_deps} apt_install_retry python3-pip -"${PIP_INSTALL_CMD[@]}" -r ${ATOS_REPO_PATH}/scripts/installation/requirements.txt +apt_install_retry python3-venv + +if [ -d "${ATOS_VENV_PATH}" ] && [ "$REINSTALL" = true ]; then + echo "Removing existing ATOS Python virtual environment at ${ATOS_VENV_PATH}..." + rm -rf "${ATOS_VENV_PATH}" +fi + +if [ ! -f "${ATOS_VENV_PATH}/bin/activate" ]; then + echo "Creating ATOS Python virtual environment at ${ATOS_VENV_PATH}..." + mkdir -p "$(dirname "${ATOS_VENV_PATH}")" + python3 -m venv "${ATOS_VENV_PATH}" +fi + +"${ATOS_VENV_PATH}/bin/python" -m pip install --upgrade pip +"${ATOS_VENV_PATH}/bin/python" -m pip install -r ${ATOS_REPO_PATH}/scripts/installation/requirements.txt # Check if apt failed to install dependencies check_command_failed $? "Failed to install dependencies." @@ -110,7 +121,7 @@ check_command_failed $? "Failed to install ROS2 dependencies." ######## Install ATOS GUI dependencies ######## ############################################### -"${PIP_INSTALL_CMD[@]}" -r ${ATOS_REPO_PATH}/atos_gui/requirements.txt +"${ATOS_VENV_PATH}/bin/python" -m pip install -r ${ATOS_REPO_PATH}/atos_gui/requirements.txt ########################################### ###### Install some deps from source ###### From ee7c228752b1663cb8eb70ec8ccfdf5630620015 Mon Sep 17 00:00:00 2001 From: Sebastian Loh Lindholm Date: Thu, 18 Jun 2026 14:44:14 +0200 Subject: [PATCH 29/33] fix for error in CI: ModuleNotFoundError: No module named 'em --- scripts/installation/install_deps.sh | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/scripts/installation/install_deps.sh b/scripts/installation/install_deps.sh index 72b8e112f..dd54c26da 100755 --- a/scripts/installation/install_deps.sh +++ b/scripts/installation/install_deps.sh @@ -73,7 +73,9 @@ fi if [ ! -f "${ATOS_VENV_PATH}/bin/activate" ]; then echo "Creating ATOS Python virtual environment at ${ATOS_VENV_PATH}..." mkdir -p "$(dirname "${ATOS_VENV_PATH}")" - python3 -m venv "${ATOS_VENV_PATH}" + # ROS Python tooling depends on distro packages such as `empy` being importable + # while the ATOS venv is active during colcon builds. + python3 -m venv --system-site-packages "${ATOS_VENV_PATH}" fi "${ATOS_VENV_PATH}/bin/python" -m pip install --upgrade pip From d2cac02f37650213b3e27f2a5063cd06c541c81c Mon Sep 17 00:00:00 2001 From: Sebastian Loh Lindholm Date: Thu, 18 Jun 2026 21:21:36 +0200 Subject: [PATCH 30/33] trying to fix ModuleNotFoundError: No module named 'OpenSSL' --- docker-compose-bridge.yml | 4 ++-- docker-compose-test.yml | 2 +- docker-compose.yml | 4 ++-- scripts/run_atosfleetmanagement.sh | 1 + 4 files changed, 6 insertions(+), 5 deletions(-) diff --git a/docker-compose-bridge.yml b/docker-compose-bridge.yml index 79fc37b52..36b4c68e9 100644 --- a/docker-compose-bridge.yml +++ b/docker-compose-bridge.yml @@ -28,7 +28,7 @@ services: - "9090:9090" - "8765:8765" - "8000:8000" - command: bash -c "source /root/atos_ws/install/setup.sh ; ros2 launch atos launch_basic.py insecure:=True" + command: bash -c "source /root/.local/share/atos/venv/bin/activate ; source /root/atos_ws/install/setup.sh ; ros2 launch atos launch_basic.py insecure:=True" foxglove-studio: image: ghcr.io/foxglove/studio:1.76 networks: @@ -36,4 +36,4 @@ services: ports: - 8080:8080 volumes: - - ./plugins/foxglove/Map, 3D and control layout.json:/foxglove/default-layout.json \ No newline at end of file + - ./plugins/foxglove/Map, 3D and control layout.json:/foxglove/default-layout.json diff --git a/docker-compose-test.yml b/docker-compose-test.yml index 254ece545..640a45fcb 100644 --- a/docker-compose-test.yml +++ b/docker-compose-test.yml @@ -15,7 +15,7 @@ services: - ./conf/conf/:/root/.astazero/ATOS/conf - ./conf/odr/:/root/.astazero/ATOS/odr - ./conf/osc/:/root/.astazero/ATOS/osc - command: bash -c "source /root/atos_ws/install/setup.sh ; python3 -m pytest /root/atos_ws/src/atos/scripts/integration_testing/run_scenario_test.py" + command: bash -c "source /root/.local/share/atos/venv/bin/activate ; source /root/atos_ws/install/setup.sh ; python3 -m pytest /root/atos_ws/src/atos/scripts/integration_testing/run_scenario_test.py" isoObject: image: astazero/iso_object_demo:latest networks: diff --git a/docker-compose.yml b/docker-compose.yml index 50b53ca8c..14c7370f0 100644 --- a/docker-compose.yml +++ b/docker-compose.yml @@ -14,9 +14,9 @@ services: volumes: - ~/.astazero/ATOS/:/root/.astazero/ATOS/ network_mode: "host" - command: bash -c "source /root/atos_ws/install/setup.sh ; ros2 launch atos launch_basic.py insecure:=True" + command: bash -c "source /root/.local/share/atos/venv/bin/activate ; source /root/atos_ws/install/setup.sh ; ros2 launch atos launch_basic.py insecure:=True" foxglove-studio: image: ghcr.io/foxglove/studio:1.76 network_mode: "host" volumes: - - ./plugins/foxglove/Map, 3D and control layout.json:/foxglove/default-layout.json \ No newline at end of file + - ./plugins/foxglove/Map, 3D and control layout.json:/foxglove/default-layout.json diff --git a/scripts/run_atosfleetmanagement.sh b/scripts/run_atosfleetmanagement.sh index f3534925a..3d5e60068 100644 --- a/scripts/run_atosfleetmanagement.sh +++ b/scripts/run_atosfleetmanagement.sh @@ -12,6 +12,7 @@ COT_TLS_CA_PATH="${COT_TLS_CA_PATH:-}" # setup.sh from colcon references vars that may be unset; avoid nounset during sourcing. set +u +source /root/.local/share/atos/venv/bin/activate source /root/atos_ws/install/setup.sh set -u From d8395b4dbb5693dc1f666763f558f6b5275641dd Mon Sep 17 00:00:00 2001 From: Sebastian Loh Lindholm Date: Thu, 18 Jun 2026 21:38:01 +0200 Subject: [PATCH 31/33] trying to fix ModuleNotFoundError: No module named 'OpenSSL' --- scripts/installation/dependencies.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/scripts/installation/dependencies.txt b/scripts/installation/dependencies.txt index 15765b353..976e58269 100644 --- a/scripts/installation/dependencies.txt +++ b/scripts/installation/dependencies.txt @@ -11,6 +11,7 @@ gnupg2 lsb-release python3-pip + python3-openssl proj-bin clang libstdc++-12-dev From f324bc7094ce30d17842db0f98ab82fb6c8c1de8 Mon Sep 17 00:00:00 2001 From: Sebastian Loh Lindholm Date: Thu, 18 Jun 2026 22:57:55 +0200 Subject: [PATCH 32/33] Skip GUI in integration test launch --- atos/launch/launch_integration_testing.py | 2 +- atos/launch/launch_utils/launch_base.py | 21 ++++++++++++++------- 2 files changed, 15 insertions(+), 8 deletions(-) diff --git a/atos/launch/launch_integration_testing.py b/atos/launch/launch_integration_testing.py index 875357b7f..28979e33e 100644 --- a/atos/launch/launch_integration_testing.py +++ b/atos/launch/launch_integration_testing.py @@ -27,7 +27,7 @@ def get_integration_test_nodes(): def generate_launch_description(): - base_nodes = launch_base.get_base_nodes() + base_nodes = launch_base.get_base_nodes(include_gui=False) integration_test_nodes = get_integration_test_nodes() diff --git a/atos/launch/launch_utils/launch_base.py b/atos/launch/launch_utils/launch_base.py index 9cedf320f..3a6faccf8 100644 --- a/atos/launch/launch_utils/launch_base.py +++ b/atos/launch/launch_utils/launch_base.py @@ -24,7 +24,7 @@ def get_files(): return validate_atos_dir() -def get_base_nodes(): +def get_base_nodes(include_gui=True): files = get_files() insecure_websockets = LaunchConfiguration("insecure") @@ -52,14 +52,9 @@ def get_base_nodes(): ros_bridge_params = copy.deepcopy(fox_bridge_params) ros_bridge_params[0] = {"port": 9090} - return [ + base_nodes = [ foxbridge_launch_arg, insecure_launch_arg, - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory("atos_gui"), "launch/gui.py") - ) - ), Node( package="atos", namespace="atos", @@ -135,3 +130,15 @@ def get_base_nodes(): parameters=ros_tls_bridge_params, ), ] + + if include_gui: + base_nodes.insert( + 2, + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory("atos_gui"), "launch/gui.py") + ) + ), + ) + + return base_nodes From 8d5438c4aa4656b8b0cb1a2da46b3cf5258f31c3 Mon Sep 17 00:00:00 2001 From: Sebastian Loh Lindholm Date: Thu, 18 Jun 2026 22:59:02 +0200 Subject: [PATCH 33/33] format --- atos/launch/launch_utils/launch_base.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/atos/launch/launch_utils/launch_base.py b/atos/launch/launch_utils/launch_base.py index 3a6faccf8..0b8ea4c58 100644 --- a/atos/launch/launch_utils/launch_base.py +++ b/atos/launch/launch_utils/launch_base.py @@ -136,7 +136,9 @@ def get_base_nodes(include_gui=True): 2, IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory("atos_gui"), "launch/gui.py") + os.path.join( + get_package_share_directory("atos_gui"), "launch/gui.py" + ) ) ), )